mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-02 19:53:57 -04:00
AP_Scheduler: switch to Hz based scheduler config
This commit is contained in:
parent
6bc19cd44c
commit
1b1be7e4a2
@ -24,6 +24,15 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Progmem/AP_Progmem.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
|
||||
#define SCHEDULER_DEFAULT_LOOP_RATE 400
|
||||
#define SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER 0
|
||||
#else
|
||||
#define SCHEDULER_DEFAULT_LOOP_RATE 50
|
||||
#define SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER 1
|
||||
#endif
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
@ -36,9 +45,39 @@ const AP_Param::GroupInfo AP_Scheduler::var_info[] = {
|
||||
// @Values: 0:Disabled,2:ShowSlips,3:ShowOverruns
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("DEBUG", 0, AP_Scheduler, _debug, 0),
|
||||
|
||||
#if SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER
|
||||
// @Param: LOOP_RATE
|
||||
// @DisplayName: Scheduling main loop rate
|
||||
// @Description: This controls the rate of the main control loop in Hz. This should only be changed by developers. This only takes effect on restart
|
||||
// @Values: 50:50Hz,100:100Hz,200:200Hz,400:400Hz
|
||||
// @RebootRequired: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("LOOP_RATE", 1, AP_Scheduler, _loop_rate_hz, SCHEDULER_DEFAULT_LOOP_RATE),
|
||||
#endif
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
// constructor
|
||||
AP_Scheduler::AP_Scheduler(void)
|
||||
{
|
||||
_loop_rate_hz.set_default(SCHEDULER_DEFAULT_LOOP_RATE);
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
|
||||
// only allow specific values of main loop rate. This code is not
|
||||
// setup for arbitrary rates yet
|
||||
if (_loop_rate_hz <= 50) {
|
||||
_loop_rate_hz.set(50);
|
||||
} else if (_loop_rate_hz <= 100) {
|
||||
_loop_rate_hz.set(100);
|
||||
} else if (_loop_rate_hz <= 200) {
|
||||
_loop_rate_hz.set(200);
|
||||
} else {
|
||||
_loop_rate_hz.set(400);
|
||||
}
|
||||
}
|
||||
|
||||
// initialise the scheduler
|
||||
void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks)
|
||||
{
|
||||
@ -66,7 +105,10 @@ void AP_Scheduler::run(uint16_t time_available)
|
||||
|
||||
for (uint8_t i=0; i<_num_tasks; i++) {
|
||||
uint16_t dt = _tick_counter - _last_run[i];
|
||||
uint16_t interval_ticks = pgm_read_word(&_tasks[i].interval_ticks);
|
||||
uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz;
|
||||
if (interval_ticks < 1) {
|
||||
interval_ticks = 1;
|
||||
}
|
||||
if (dt >= interval_ticks) {
|
||||
// this task is due to run. Do we have enough time to run it?
|
||||
_task_time_allowed = pgm_read_word(&_tasks[i].max_time_micros);
|
||||
|
@ -27,6 +27,16 @@
|
||||
|
||||
#define AP_SCHEDULER_NAME_INITIALIZER(_name) .name = #_name,
|
||||
|
||||
/*
|
||||
useful macro for creating scheduler task table
|
||||
*/
|
||||
#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros) { \
|
||||
.function = FUNCTOR_BIND(classptr, &classname::func, void),\
|
||||
AP_SCHEDULER_NAME_INITIALIZER(func)\
|
||||
.rate_hz = _rate_hz,\
|
||||
.max_time_micros = _max_time_micros\
|
||||
}
|
||||
|
||||
/*
|
||||
A task scheduler for APM main loops
|
||||
|
||||
@ -43,12 +53,15 @@
|
||||
class AP_Scheduler
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
AP_Scheduler(void);
|
||||
|
||||
FUNCTOR_TYPEDEF(task_fn_t, void);
|
||||
|
||||
struct Task {
|
||||
task_fn_t function;
|
||||
const char *name;
|
||||
uint16_t interval_ticks;
|
||||
float rate_hz;
|
||||
uint16_t max_time_micros;
|
||||
};
|
||||
|
||||
@ -74,6 +87,11 @@ public:
|
||||
// end of a run()
|
||||
float load_average(uint32_t tick_time_usec) const;
|
||||
|
||||
// get the configured main loop rate
|
||||
uint16_t get_loop_rate_hz(void) const {
|
||||
return _loop_rate_hz;
|
||||
}
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// current running task, or -1 if none. Used to debug stuck tasks
|
||||
@ -83,6 +101,9 @@ private:
|
||||
// used to enable scheduler debugging
|
||||
AP_Int8 _debug;
|
||||
|
||||
// overall scheduling rate in Hz
|
||||
AP_Int16 _loop_rate_hz;
|
||||
|
||||
// progmem list of tasks to run
|
||||
const struct Task *_tasks;
|
||||
|
||||
|
@ -30,12 +30,7 @@ private:
|
||||
|
||||
static SchedTest schedtest;
|
||||
|
||||
#define SCHED_TASK(func, _interval_ticks, _max_time_micros) {\
|
||||
.function = FUNCTOR_BIND(&schedtest, &SchedTest::func, void),\
|
||||
AP_SCHEDULER_NAME_INITIALIZER(func)\
|
||||
.interval_ticks = _interval_ticks,\
|
||||
.max_time_micros = _max_time_micros,\
|
||||
}
|
||||
#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(SchedTest, &schedtest, func, _interval_ticks, _max_time_micros)
|
||||
|
||||
/*
|
||||
scheduler table - all regular tasks are listed here, along with how
|
||||
@ -43,16 +38,15 @@ static SchedTest schedtest;
|
||||
they are expected to take (in microseconds)
|
||||
*/
|
||||
const AP_Scheduler::Task SchedTest::scheduler_tasks[] = {
|
||||
SCHED_TASK(ins_update, 1, 1000),
|
||||
SCHED_TASK(one_hz_print, 50, 1000),
|
||||
SCHED_TASK(five_second_call, 250, 1800),
|
||||
SCHED_TASK(ins_update, 50, 1000),
|
||||
SCHED_TASK(one_hz_print, 1, 1000),
|
||||
SCHED_TASK(five_second_call, 0.2, 1800),
|
||||
};
|
||||
|
||||
|
||||
void SchedTest::setup(void)
|
||||
{
|
||||
// we
|
||||
ins.init(AP_InertialSensor::RATE_50HZ);
|
||||
ins.init(scheduler.get_loop_rate_hz());
|
||||
|
||||
// initialise the scheduler
|
||||
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
|
||||
|
Loading…
Reference in New Issue
Block a user