Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduSub: allow specification of Scheduler table priorities

Browse Source
This commit is contained in:
Peter Barker 2021-11-14 14:49:44 +11:00 committed by Andrew Tridgell
parent 17395f362e
commit 422bb3cfbb
1 changed files with 56 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
ArduSub/ArduSub.cpp
View File

@ -17,67 +17,88 @@
#include "Sub.h" #include "Sub.h"
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros) #define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros, priority)
/* /*
scheduler table for fast CPUs - all regular tasks apart from the fast_loop() scheduler table - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called (in hz) should be listed here.
and the maximum time they are expected to take (in microseconds)
All entries in this table must be ordered by priority.
This table is interleaved with the table in AP_Vehicle to determine
the order in which tasks are run. Convenience methods SCHED_TASK
and SCHED_TASK_CLASS are provided to build entries in this structure:
SCHED_TASK arguments:
- name of static function to call
- rate (in Hertz) at which the function should be called
- expected time (in MicroSeconds) that the function should take to run
- priority (0 through 255, lower number meaning higher priority)
SCHED_TASK_CLASS arguments:
- class name of method to be called
- instance on which to call the method
- method to call on that instance
- rate (in Hertz) at which the method should be called
- expected time (in MicroSeconds) that the method should take to run
- priority (0 through 255, lower number meaning higher priority)
*/ */
const AP_Scheduler::Task Sub::scheduler_tasks[] = { const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(fifty_hz_loop, 50, 75), SCHED_TASK(fifty_hz_loop, 50, 75, 3),
SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200), SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200, 6),
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
SCHED_TASK_CLASS(OpticalFlow, &sub.optflow, update, 200, 160), SCHED_TASK_CLASS(OpticalFlow, &sub.optflow, update, 200, 160, 9),
#endif #endif
SCHED_TASK(update_batt_compass, 10, 120), SCHED_TASK(update_batt_compass, 10, 120, 12),
SCHED_TASK(read_rangefinder, 20, 100), SCHED_TASK(read_rangefinder, 20, 100, 15),
SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(update_altitude, 10, 100, 18),
SCHED_TASK(three_hz_loop, 3, 75), SCHED_TASK(three_hz_loop, 3, 75, 21),
SCHED_TASK(update_turn_counter, 10, 50), SCHED_TASK(update_turn_counter, 10, 50, 24),
SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90), SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90, 27),
SCHED_TASK_CLASS(AP_Notify, &sub.notify, update, 50, 90), SCHED_TASK_CLASS(AP_Notify, &sub.notify, update, 50, 90, 30),
SCHED_TASK(one_hz_loop, 1, 100), SCHED_TASK(one_hz_loop, 1, 100, 33),
SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180), SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180, 36),
SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_send, 400, 550), SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_send, 400, 550, 39),
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
SCHED_TASK_CLASS(AC_Fence, &sub.fence, update, 10, 100), SCHED_TASK_CLASS(AC_Fence, &sub.fence, update, 10, 100, 42),
#endif #endif
#if HAL_MOUNT_ENABLED #if HAL_MOUNT_ENABLED
SCHED_TASK_CLASS(AP_Mount, &sub.camera_mount, update, 50, 75), SCHED_TASK_CLASS(AP_Mount, &sub.camera_mount, update, 50, 75, 45),
#endif #endif
#if CAMERA == ENABLED #if CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Camera, &sub.camera, update, 50, 75), SCHED_TASK_CLASS(AP_Camera, &sub.camera, update, 50, 75, 48),
#endif #endif
SCHED_TASK(ten_hz_logging_loop, 10, 350), SCHED_TASK(ten_hz_logging_loop, 10, 350, 51),
SCHED_TASK(twentyfive_hz_logging, 25, 110), SCHED_TASK(twentyfive_hz_logging, 25, 110, 54),
SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300), SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300, 57),
SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50), SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50, 60),
SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75), SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75, 63),
#if RPM_ENABLED == ENABLED #if RPM_ENABLED == ENABLED
SCHED_TASK(rpm_update, 10, 200), SCHED_TASK(rpm_update, 10, 200, 66),
#endif #endif
SCHED_TASK_CLASS(Compass, &sub.compass, cal_update, 100, 100), SCHED_TASK_CLASS(Compass, &sub.compass, cal_update, 100, 100, 69),
SCHED_TASK(terrain_update, 10, 100), SCHED_TASK(terrain_update, 10, 100, 72),
#if GRIPPER_ENABLED == ENABLED #if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &sub.g2.gripper, update, 10, 75), SCHED_TASK_CLASS(AP_Gripper, &sub.g2.gripper, update, 10, 75, 75),
#endif #endif
#ifdef USERHOOK_FASTLOOP #ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75), SCHED_TASK(userhook_FastLoop, 100, 75, 78),
#endif #endif
#ifdef USERHOOK_50HZLOOP #ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75), SCHED_TASK(userhook_50Hz, 50, 75, 81),
#endif #endif
#ifdef USERHOOK_MEDIUMLOOP #ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75), SCHED_TASK(userhook_MediumLoop, 10, 75, 84),
#endif #endif
#ifdef USERHOOK_SLOWLOOP #ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75), SCHED_TASK(userhook_SlowLoop, 3.3, 75, 87),
#endif #endif
#ifdef USERHOOK_SUPERSLOWLOOP #ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75), SCHED_TASK(userhook_SuperSlowLoop, 1, 75, 90),
#endif #endif
SCHED_TASK(read_airspeed, 10, 100), SCHED_TASK(read_airspeed, 10, 100, 93),
}; };
void Sub::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, void Sub::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,