diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index bdc0e169e4..6c36dd7eb5 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -17,67 +17,88 @@ #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() - should be 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 apart from the fast_loop() + should be listed here. + + 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[] = { - SCHED_TASK(fifty_hz_loop, 50, 75), - SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200), + SCHED_TASK(fifty_hz_loop, 50, 75, 3), + SCHED_TASK_CLASS(AP_GPS, &sub.gps, update, 50, 200, 6), #if OPTFLOW == ENABLED - SCHED_TASK_CLASS(OpticalFlow, &sub.optflow, update, 200, 160), + SCHED_TASK_CLASS(OpticalFlow, &sub.optflow, update, 200, 160, 9), #endif - SCHED_TASK(update_batt_compass, 10, 120), - SCHED_TASK(read_rangefinder, 20, 100), - SCHED_TASK(update_altitude, 10, 100), - SCHED_TASK(three_hz_loop, 3, 75), - SCHED_TASK(update_turn_counter, 10, 50), - SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90), - SCHED_TASK_CLASS(AP_Notify, &sub.notify, update, 50, 90), - SCHED_TASK(one_hz_loop, 1, 100), - SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180), - SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_send, 400, 550), + SCHED_TASK(update_batt_compass, 10, 120, 12), + SCHED_TASK(read_rangefinder, 20, 100, 15), + SCHED_TASK(update_altitude, 10, 100, 18), + SCHED_TASK(three_hz_loop, 3, 75, 21), + SCHED_TASK(update_turn_counter, 10, 50, 24), + SCHED_TASK_CLASS(AP_Baro, &sub.barometer, accumulate, 50, 90, 27), + SCHED_TASK_CLASS(AP_Notify, &sub.notify, update, 50, 90, 30), + SCHED_TASK(one_hz_loop, 1, 100, 33), + SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_receive, 400, 180, 36), + SCHED_TASK_CLASS(GCS, (GCS*)&sub._gcs, update_send, 400, 550, 39), #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 #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 #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 - SCHED_TASK(ten_hz_logging_loop, 10, 350), - SCHED_TASK(twentyfive_hz_logging, 25, 110), - SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300), - SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50), - SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75), + SCHED_TASK(ten_hz_logging_loop, 10, 350, 51), + SCHED_TASK(twentyfive_hz_logging, 25, 110, 54), + SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300, 57), + SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50, 60), + SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75, 63), #if RPM_ENABLED == ENABLED - SCHED_TASK(rpm_update, 10, 200), + SCHED_TASK(rpm_update, 10, 200, 66), #endif - SCHED_TASK_CLASS(Compass, &sub.compass, cal_update, 100, 100), - SCHED_TASK(terrain_update, 10, 100), + SCHED_TASK_CLASS(Compass, &sub.compass, cal_update, 100, 100, 69), + SCHED_TASK(terrain_update, 10, 100, 72), #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 #ifdef USERHOOK_FASTLOOP - SCHED_TASK(userhook_FastLoop, 100, 75), + SCHED_TASK(userhook_FastLoop, 100, 75, 78), #endif #ifdef USERHOOK_50HZLOOP - SCHED_TASK(userhook_50Hz, 50, 75), + SCHED_TASK(userhook_50Hz, 50, 75, 81), #endif #ifdef USERHOOK_MEDIUMLOOP - SCHED_TASK(userhook_MediumLoop, 10, 75), + SCHED_TASK(userhook_MediumLoop, 10, 75, 84), #endif #ifdef USERHOOK_SLOWLOOP - SCHED_TASK(userhook_SlowLoop, 3.3, 75), + SCHED_TASK(userhook_SlowLoop, 3.3, 75, 87), #endif #ifdef USERHOOK_SUPERSLOWLOOP - SCHED_TASK(userhook_SuperSlowLoop, 1, 75), + SCHED_TASK(userhook_SuperSlowLoop, 1, 75, 90), #endif - SCHED_TASK(read_airspeed, 10, 100), + SCHED_TASK(read_airspeed, 10, 100, 93), }; void Sub::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,