Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * main loop scheduler for APM
18 : * Author: Andrew Tridgell, January 2013
19 : *
20 : */
21 : #pragma once
22 :
23 : #include "AP_Scheduler_config.h"
24 :
25 : #if AP_SCHEDULER_ENABLED
26 :
27 : #include <AP_Param/AP_Param.h>
28 : #include <AP_HAL/Semaphores.h>
29 : #include <AP_HAL/Util.h>
30 : #include <AP_Math/AP_Math.h>
31 : #include "PerfInfo.h" // loop perf monitoring
32 : #include <AP_InternalError/AP_InternalError.h>
33 :
34 : #if AP_SCHEDULER_EXTENDED_TASKINFO_ENABLED
35 : #define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name,
36 : #define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name "*",
37 : #else
38 : #define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_name,
39 : #define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_name "*",
40 : #endif
41 : #define LOOP_RATE 0
42 :
43 : /*
44 : useful macro for creating scheduler task table
45 : */
46 : #define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros, _priority) { \
47 : .function = FUNCTOR_BIND(classptr, &classname::func, void),\
48 : AP_SCHEDULER_NAME_INITIALIZER(classname, func)\
49 : .rate_hz = _rate_hz,\
50 : .max_time_micros = _max_time_micros, \
51 : .priority = _priority \
52 : }
53 :
54 : /*
55 : useful macro for creating the fastloop task table
56 : */
57 : #define FAST_TASK_CLASS(classname, classptr, func) { \
58 : .function = FUNCTOR_BIND(classptr, &classname::func, void),\
59 : AP_FAST_NAME_INITIALIZER(classname, func)\
60 : .rate_hz = 0,\
61 : .max_time_micros = 0,\
62 : .priority = AP_Scheduler::FAST_TASK_PRI0 \
63 : }
64 :
65 : /*
66 : A task scheduler for APM main loops
67 :
68 : Sketches should call scheduler.init() on startup, then call
69 : scheduler.tick() at regular intervals (typically every 10ms).
70 :
71 : To run tasks use scheduler.run(), passing the amount of time that
72 : the scheduler is allowed to use before it must return
73 : */
74 :
75 : class AP_Scheduler
76 : {
77 : public:
78 : AP_Scheduler();
79 :
80 : /* Do not allow copies */
81 : CLASS_NO_COPY(AP_Scheduler);
82 :
83 : static AP_Scheduler *get_singleton();
84 : static AP_Scheduler *_singleton;
85 :
86 : FUNCTOR_TYPEDEF(task_fn_t, void);
87 :
88 : struct Task {
89 : task_fn_t function;
90 : const char *name;
91 : float rate_hz;
92 : uint16_t max_time_micros;
93 : uint8_t priority; // task priority
94 : };
95 :
96 : enum class Options : uint8_t {
97 : RECORD_TASK_INFO = 1 << 0
98 : };
99 :
100 : enum FastTaskPriorities {
101 : FAST_TASK_PRI0 = 0,
102 : FAST_TASK_PRI1 = 1,
103 : FAST_TASK_PRI2 = 2,
104 : MAX_FAST_TASK_PRIORITIES = 3
105 : };
106 :
107 : // initialise scheduler
108 : void init(const Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit);
109 :
110 : // called by vehicle's main loop - which should be the only thing
111 : // that function does
112 : void loop();
113 :
114 : // call to update any logging the scheduler might do; call at 1Hz
115 : void update_logging();
116 :
117 : // write out PERF message to logger
118 : void Log_Write_Performance();
119 :
120 : // call when one tick has passed
121 : void tick(void);
122 :
123 : // return current tick counter
124 594903611 : uint16_t ticks() const { return _tick_counter; }
125 135108679 : uint32_t ticks32() const { return _tick_counter32; }
126 :
127 : // run the tasks. Call this once per 'tick'.
128 : // time_available is the amount of time available to run
129 : // tasks in microseconds
130 : void run(uint32_t time_available);
131 :
132 : // return the number of microseconds available for the current task
133 : uint16_t time_available_usec(void) const;
134 :
135 : // return debug parameter
136 19414 : uint8_t debug_flags(void) { return _debug; }
137 :
138 : // return load average, as a number between 0 and 1. 1 means
139 : // 100% load. Calculated from how much spare time we have at the
140 : // end of a run()
141 : float load_average();
142 :
143 : // get the active main loop rate
144 26936950 : uint16_t get_loop_rate_hz(void) {
145 26936950 : if (_active_loop_rate_hz == 0) {
146 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
147 0 : _active_loop_rate_hz = _loop_rate_hz;
148 : }
149 26936950 : return _active_loop_rate_hz;
150 : }
151 : // get the time-allowed-per-loop in microseconds
152 574109816 : uint32_t get_loop_period_us() {
153 574109816 : if (_loop_period_us == 0) {
154 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
155 0 : _loop_period_us = 1000000UL / _loop_rate_hz;
156 : }
157 574109816 : return _loop_period_us;
158 : }
159 : // get the time-allowed-per-loop in seconds
160 105885425 : float get_loop_period_s() {
161 105885425 : if (is_zero(_loop_period_s)) {
162 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
163 0 : _loop_period_s = 1.0f / _loop_rate_hz;
164 : }
165 105885425 : return _loop_period_s;
166 : }
167 :
168 : // get the filtered main loop time in seconds
169 4364 : float get_filtered_loop_time(void) const {
170 4364 : return perf_info.get_filtered_time();
171 : }
172 :
173 : // get the filtered active main loop rate
174 1584089 : float get_filtered_loop_rate_hz() {
175 1584089 : return perf_info.get_filtered_loop_rate_hz();
176 : }
177 :
178 : // get the time in seconds that the last loop took
179 38750376 : float get_last_loop_time_s(void) const {
180 38750376 : return _last_loop_time_s;
181 : }
182 :
183 : // get the time in microseconds that the current loop started
184 3480899 : uint64_t get_loop_start_time_us(void) const {
185 3480899 : return _loop_sample_time_us;
186 : }
187 :
188 : // get the amount of extra time being added on each loop
189 291100 : uint32_t get_extra_loop_us(void) const {
190 291100 : return extra_loop_us;
191 : }
192 :
193 336124 : HAL_Semaphore &get_semaphore(void) { return _rsem; }
194 :
195 : void task_info(ExpandingString &str);
196 :
197 : static const struct AP_Param::GroupInfo var_info[];
198 :
199 : // loop performance monitoring:
200 : AP::PerfInfo perf_info;
201 :
202 : private:
203 : // used to enable scheduler debugging
204 : AP_Int8 _debug;
205 :
206 : // overall scheduling rate in Hz
207 : AP_Int16 _loop_rate_hz;
208 :
209 : // loop rate in Hz as set at startup
210 : uint16_t _active_loop_rate_hz;
211 :
212 : // scheduler options
213 : AP_Int8 _options;
214 :
215 : // calculated loop period in usec
216 : uint16_t _loop_period_us;
217 :
218 : // calculated loop period in seconds
219 : float _loop_period_s;
220 :
221 : // list of tasks to run
222 : const struct Task *_vehicle_tasks;
223 : uint8_t _num_vehicle_tasks;
224 :
225 : // list of common tasks to run
226 : const struct Task *_common_tasks;
227 : uint8_t _num_common_tasks;
228 :
229 : // total number of tasks in _tasks and _common_tasks list
230 : uint8_t _num_tasks;
231 :
232 : // number of 'ticks' that have passed (number of times that
233 : // tick() has been called
234 : uint16_t _tick_counter;
235 : uint32_t _tick_counter32;
236 :
237 : // tick counter at the time we last ran each task
238 : uint16_t *_last_run;
239 :
240 : // number of microseconds allowed for the current task
241 : uint32_t _task_time_allowed;
242 :
243 : // the time in microseconds when the task started
244 : uint32_t _task_time_started;
245 :
246 : // number of spare microseconds accumulated
247 : uint32_t _spare_micros;
248 :
249 : // number of ticks that _spare_micros is counted over
250 : uint8_t _spare_ticks;
251 :
252 : // start of previous loop
253 : uint32_t _loop_timer_start_us;
254 :
255 : // time of last loop in seconds
256 : float _last_loop_time_s;
257 :
258 : // start of current loop
259 : uint64_t _loop_sample_time_us;
260 :
261 : // bitmask bit which indicates if we should log PERF message
262 : uint32_t _log_performance_bit;
263 :
264 : // maximum task slowdown compared to desired task rate before we
265 : // start giving extra time per loop
266 : static constexpr uint8_t max_task_slowdown = 4;
267 :
268 : // counters to handle dynamically adjusting extra loop time to
269 : // cope with low CPU conditions
270 : uint32_t task_not_achieved;
271 : uint32_t task_all_achieved;
272 :
273 : // extra time available for each loop - used to dynamically adjust
274 : // the loop rate in case we are well over budget
275 : uint32_t extra_loop_us;
276 :
277 :
278 : // semaphore that is held while not waiting for ins samples
279 : HAL_Semaphore _rsem;
280 : };
281 :
282 : namespace AP {
283 : AP_Scheduler &scheduler();
284 : };
285 :
286 : #endif // AP_SCHEDULER_ENABLED
|
