mirror of https://github.com/ArduPilot/ardupilot
275 lines
7.6 KiB
C++
275 lines
7.6 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
/*
|
|
* main loop scheduler for APM
|
|
* Author: Andrew Tridgell, January 2013
|
|
*
|
|
*/
|
|
#pragma once
|
|
|
|
#include <AP_HAL/AP_HAL_Boards.h>
|
|
|
|
#ifndef AP_SCHEDULER_ENABLED
|
|
#define AP_SCHEDULER_ENABLED 1
|
|
#endif
|
|
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <AP_HAL/Semaphores.h>
|
|
#include <AP_HAL/Util.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include "PerfInfo.h" // loop perf monitoring
|
|
|
|
#if HAL_MINIMIZE_FEATURES
|
|
#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_name,
|
|
#define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_name "*",
|
|
#else
|
|
#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name,
|
|
#define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name "*",
|
|
#endif
|
|
#define LOOP_RATE 0
|
|
|
|
/*
|
|
useful macro for creating scheduler task table
|
|
*/
|
|
#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros, _priority) { \
|
|
.function = FUNCTOR_BIND(classptr, &classname::func, void),\
|
|
AP_SCHEDULER_NAME_INITIALIZER(classname, func)\
|
|
.rate_hz = _rate_hz,\
|
|
.max_time_micros = _max_time_micros, \
|
|
.priority = _priority \
|
|
}
|
|
|
|
/*
|
|
useful macro for creating the fastloop task table
|
|
*/
|
|
#define FAST_TASK_CLASS(classname, classptr, func) { \
|
|
.function = FUNCTOR_BIND(classptr, &classname::func, void),\
|
|
AP_FAST_NAME_INITIALIZER(classname, func)\
|
|
.rate_hz = 0,\
|
|
.max_time_micros = 0,\
|
|
.priority = AP_Scheduler::FAST_TASK_PRI0 \
|
|
}
|
|
|
|
/*
|
|
A task scheduler for APM main loops
|
|
|
|
Sketches should call scheduler.init() on startup, then call
|
|
scheduler.tick() at regular intervals (typically every 10ms).
|
|
|
|
To run tasks use scheduler.run(), passing the amount of time that
|
|
the scheduler is allowed to use before it must return
|
|
*/
|
|
|
|
class AP_Scheduler
|
|
{
|
|
public:
|
|
AP_Scheduler();
|
|
|
|
/* Do not allow copies */
|
|
CLASS_NO_COPY(AP_Scheduler);
|
|
|
|
static AP_Scheduler *get_singleton();
|
|
static AP_Scheduler *_singleton;
|
|
|
|
FUNCTOR_TYPEDEF(task_fn_t, void);
|
|
|
|
struct Task {
|
|
task_fn_t function;
|
|
const char *name;
|
|
float rate_hz;
|
|
uint16_t max_time_micros;
|
|
uint8_t priority; // task priority
|
|
};
|
|
|
|
enum class Options : uint8_t {
|
|
RECORD_TASK_INFO = 1 << 0
|
|
};
|
|
|
|
enum FastTaskPriorities {
|
|
FAST_TASK_PRI0 = 0,
|
|
FAST_TASK_PRI1 = 1,
|
|
FAST_TASK_PRI2 = 2,
|
|
MAX_FAST_TASK_PRIORITIES = 3
|
|
};
|
|
|
|
// initialise scheduler
|
|
void init(const Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit);
|
|
|
|
// called by vehicle's main loop - which should be the only thing
|
|
// that function does
|
|
void loop();
|
|
|
|
// call to update any logging the scheduler might do; call at 1Hz
|
|
void update_logging();
|
|
|
|
// write out PERF message to logger
|
|
void Log_Write_Performance();
|
|
|
|
// call when one tick has passed
|
|
void tick(void);
|
|
|
|
// return current tick counter
|
|
uint16_t ticks() const { return _tick_counter; }
|
|
uint32_t ticks32() const { return _tick_counter32; }
|
|
|
|
// run the tasks. Call this once per 'tick'.
|
|
// time_available is the amount of time available to run
|
|
// tasks in microseconds
|
|
void run(uint32_t time_available);
|
|
|
|
// return the number of microseconds available for the current task
|
|
uint16_t time_available_usec(void) const;
|
|
|
|
// return debug parameter
|
|
uint8_t debug_flags(void) { return _debug; }
|
|
|
|
// return load average, as a number between 0 and 1. 1 means
|
|
// 100% load. Calculated from how much spare time we have at the
|
|
// end of a run()
|
|
float load_average();
|
|
|
|
// get the active main loop rate
|
|
uint16_t get_loop_rate_hz(void) {
|
|
if (_active_loop_rate_hz == 0) {
|
|
_active_loop_rate_hz = _loop_rate_hz;
|
|
}
|
|
return _active_loop_rate_hz;
|
|
}
|
|
// get the time-allowed-per-loop in microseconds
|
|
uint32_t get_loop_period_us() {
|
|
if (_loop_period_us == 0) {
|
|
_loop_period_us = 1000000UL / _loop_rate_hz;
|
|
}
|
|
return _loop_period_us;
|
|
}
|
|
// get the time-allowed-per-loop in seconds
|
|
float get_loop_period_s() {
|
|
if (is_zero(_loop_period_s)) {
|
|
_loop_period_s = 1.0f / _loop_rate_hz;
|
|
}
|
|
return _loop_period_s;
|
|
}
|
|
|
|
// get the filtered main loop time in seconds
|
|
float get_filtered_loop_time(void) const {
|
|
return perf_info.get_filtered_time();
|
|
}
|
|
|
|
// get the filtered active main loop rate
|
|
float get_filtered_loop_rate_hz() {
|
|
return perf_info.get_filtered_loop_rate_hz();
|
|
}
|
|
|
|
// get the time in seconds that the last loop took
|
|
float get_last_loop_time_s(void) const {
|
|
return _last_loop_time_s;
|
|
}
|
|
|
|
// get the amount of extra time being added on each loop
|
|
uint32_t get_extra_loop_us(void) const {
|
|
return extra_loop_us;
|
|
}
|
|
|
|
HAL_Semaphore &get_semaphore(void) { return _rsem; }
|
|
|
|
void task_info(ExpandingString &str);
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
// loop performance monitoring:
|
|
AP::PerfInfo perf_info;
|
|
|
|
private:
|
|
// used to enable scheduler debugging
|
|
AP_Int8 _debug;
|
|
|
|
// overall scheduling rate in Hz
|
|
AP_Int16 _loop_rate_hz;
|
|
|
|
// loop rate in Hz as set at startup
|
|
AP_Int16 _active_loop_rate_hz;
|
|
|
|
// scheduler options
|
|
AP_Int8 _options;
|
|
|
|
// calculated loop period in usec
|
|
uint16_t _loop_period_us;
|
|
|
|
// calculated loop period in seconds
|
|
float _loop_period_s;
|
|
|
|
// list of tasks to run
|
|
const struct Task *_vehicle_tasks;
|
|
uint8_t _num_vehicle_tasks;
|
|
|
|
// list of common tasks to run
|
|
const struct Task *_common_tasks;
|
|
uint8_t _num_common_tasks;
|
|
|
|
// total number of tasks in _tasks and _common_tasks list
|
|
uint8_t _num_tasks;
|
|
|
|
// number of 'ticks' that have passed (number of times that
|
|
// tick() has been called
|
|
uint16_t _tick_counter;
|
|
uint32_t _tick_counter32;
|
|
|
|
// tick counter at the time we last ran each task
|
|
uint16_t *_last_run;
|
|
|
|
// number of microseconds allowed for the current task
|
|
uint32_t _task_time_allowed;
|
|
|
|
// the time in microseconds when the task started
|
|
uint32_t _task_time_started;
|
|
|
|
// number of spare microseconds accumulated
|
|
uint32_t _spare_micros;
|
|
|
|
// number of ticks that _spare_micros is counted over
|
|
uint8_t _spare_ticks;
|
|
|
|
// start of loop timing
|
|
uint32_t _loop_timer_start_us;
|
|
|
|
// time of last loop in seconds
|
|
float _last_loop_time_s;
|
|
|
|
// bitmask bit which indicates if we should log PERF message
|
|
uint32_t _log_performance_bit;
|
|
|
|
// maximum task slowdown compared to desired task rate before we
|
|
// start giving extra time per loop
|
|
const uint8_t max_task_slowdown = 4;
|
|
|
|
// counters to handle dynamically adjusting extra loop time to
|
|
// cope with low CPU conditions
|
|
uint32_t task_not_achieved;
|
|
uint32_t task_all_achieved;
|
|
|
|
// extra time available for each loop - used to dynamically adjust
|
|
// the loop rate in case we are well over budget
|
|
uint32_t extra_loop_us;
|
|
|
|
|
|
// semaphore that is held while not waiting for ins samples
|
|
HAL_Semaphore _rsem;
|
|
};
|
|
|
|
namespace AP {
|
|
AP_Scheduler &scheduler();
|
|
};
|