From 17395f362ee4911315f17991738a167e6dc9f0ff Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 14 Nov 2021 14:49:44 +1100 Subject: [PATCH] ArduPlane: allow specification of Scheduler table priorities --- ArduPlane/ArduPlane.cpp | 136 +++++++++++++++++++++++----------------- 1 file changed, 78 insertions(+), 58 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index cdc62dfa36..2734a4e8de 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -22,96 +22,116 @@ #include "Plane.h" -#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros) +#define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority) /* - scheduler table - all regular tasks are listed here, along with how - often they should be called (in Hz) and the maximum time - they are expected to take (in microseconds) + scheduler table - all regular tasks should be listed here. + + All entries in this table must be ordered by priority. + + This table is interleaved with the table presnet in each of the + vehicles 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 Plane::scheduler_tasks[] = { // Units: Hz us - SCHED_TASK(ahrs_update, 400, 400), - SCHED_TASK(read_radio, 50, 100), - SCHED_TASK(check_short_failsafe, 50, 100), - SCHED_TASK(update_speed_height, 50, 200), - SCHED_TASK(update_control_mode, 400, 100), - SCHED_TASK(stabilize, 400, 100), - SCHED_TASK(set_servos, 400, 100), - SCHED_TASK(update_throttle_hover, 100, 90), - SCHED_TASK(read_control_switch, 7, 100), - SCHED_TASK(update_GPS_50Hz, 50, 300), - SCHED_TASK(update_GPS_10Hz, 10, 400), - SCHED_TASK(navigate, 10, 150), - SCHED_TASK(update_compass, 10, 200), - SCHED_TASK(read_airspeed, 10, 100), - SCHED_TASK(update_alt, 10, 200), - SCHED_TASK(adjust_altitude_target, 10, 200), + SCHED_TASK(ahrs_update, 400, 400, 3), + SCHED_TASK(read_radio, 50, 100, 6), + SCHED_TASK(check_short_failsafe, 50, 100, 9), + SCHED_TASK(update_speed_height, 50, 200, 12), + SCHED_TASK(update_control_mode, 400, 100, 15), + SCHED_TASK(stabilize, 400, 100, 18), + SCHED_TASK(set_servos, 400, 100, 21), + SCHED_TASK(update_throttle_hover, 100, 90, 24), + SCHED_TASK(read_control_switch, 7, 100, 27), + SCHED_TASK(update_GPS_50Hz, 50, 300, 30), + SCHED_TASK(update_GPS_10Hz, 10, 400, 33), + SCHED_TASK(navigate, 10, 150, 36), + SCHED_TASK(update_compass, 10, 200, 39), + SCHED_TASK(read_airspeed, 10, 100, 42), + SCHED_TASK(update_alt, 10, 200, 45), + SCHED_TASK(adjust_altitude_target, 10, 200, 48), #if ADVANCED_FAILSAFE == ENABLED - SCHED_TASK(afs_fs_check, 10, 100), + SCHED_TASK(afs_fs_check, 10, 100, 51), #endif - SCHED_TASK(ekf_check, 10, 75), - SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500), - SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750), - SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150), - SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300), - SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150), - SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300), + SCHED_TASK(ekf_check, 10, 75, 54), + SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500, 57), + SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750, 60), + SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63), + SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66), + SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150, 69), + SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300, 72), #if AC_FENCE == ENABLED - SCHED_TASK_CLASS(AC_Fence, &plane.fence, update, 10, 100), + SCHED_TASK_CLASS(AC_Fence, &plane.fence, update, 10, 100, 75), #endif - SCHED_TASK(read_rangefinder, 50, 100), - SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100), - SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50), + SCHED_TASK(read_rangefinder, 50, 100, 78), + SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81), + SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50, 84), #if OPTFLOW == ENABLED - SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50), + SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50, 87), #endif - SCHED_TASK(one_second_loop, 1, 400), - SCHED_TASK(three_hz_loop, 3, 75), - SCHED_TASK(check_long_failsafe, 3, 400), - SCHED_TASK(rpm_update, 10, 100), + SCHED_TASK(one_second_loop, 1, 400, 90), + SCHED_TASK(three_hz_loop, 3, 75, 93), + SCHED_TASK(check_long_failsafe, 3, 400, 96), + SCHED_TASK(rpm_update, 10, 100, 99), #if AP_AIRSPEED_AUTOCAL_ENABLE - SCHED_TASK(airspeed_ratio_update, 1, 100), + SCHED_TASK(airspeed_ratio_update, 1, 100, 102), #endif // AP_AIRSPEED_AUTOCAL_ENABLE #if HAL_MOUNT_ENABLED - SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100), + SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100, 105), #endif // HAL_MOUNT_ENABLED #if CAMERA == ENABLED - SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100), + SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100, 108), #endif // CAMERA == ENABLED - SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100), - SCHED_TASK(compass_save, 0.1, 200), - SCHED_TASK(Log_Write_Fast, 400, 300), - SCHED_TASK(update_logging1, 25, 300), - SCHED_TASK(update_logging2, 25, 300), + SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111), + SCHED_TASK(compass_save, 0.1, 200, 114), + SCHED_TASK(Log_Write_Fast, 400, 300, 117), + SCHED_TASK(update_logging1, 25, 300, 120), + SCHED_TASK(update_logging2, 25, 300, 123), #if HAL_SOARING_ENABLED - SCHED_TASK(update_soaring, 50, 400), + SCHED_TASK(update_soaring, 50, 400, 126), #endif - SCHED_TASK(parachute_check, 10, 200), + SCHED_TASK(parachute_check, 10, 200, 129), #if AP_TERRAIN_AVAILABLE - SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200), + SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200, 132), #endif // AP_TERRAIN_AVAILABLE - SCHED_TASK(update_is_flying_5Hz, 5, 100), + SCHED_TASK(update_is_flying_5Hz, 5, 100, 135), #if LOGGING_ENABLED == ENABLED - SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400), + SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400, 138), #endif - SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50), -#if HAL_ADSB_ENABLED - SCHED_TASK(avoidance_adsb_update, 10, 100), + SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50, 141), +#if HAL_ADSB_ENABLED + SCHED_TASK(avoidance_adsb_update, 10, 100, 144), #endif - SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200), + SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200, 147), #if HAL_BUTTON_ENABLED - SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100), + SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150), #endif #if STATS_ENABLED == ENABLED - SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100), + SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100, 153), #endif #if GRIPPER_ENABLED == ENABLED - SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75), + SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75, 156), #endif #if LANDING_GEAR_ENABLED == ENABLED - SCHED_TASK(landing_gear_update, 5, 50), + SCHED_TASK(landing_gear_update, 5, 50, 159), #endif };