From 7e68ade3ef24bce1d2b46db47c4e809d360fa234 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 9 May 2024 08:29:11 +1000 Subject: [PATCH] dev: correct documentation of how-to-schedule-your-code document --- ...ng-your-new-code-to-run-intermittently.rst | 69 +++++++------------ 1 file changed, 24 insertions(+), 45 deletions(-) diff --git a/dev/source/docs/code-overview-scheduling-your-new-code-to-run-intermittently.rst b/dev/source/docs/code-overview-scheduling-your-new-code-to-run-intermittently.rst index 8fb17a066f..2855546924 100644 --- a/dev/source/docs/code-overview-scheduling-your-new-code-to-run-intermittently.rst +++ b/dev/source/docs/code-overview-scheduling-your-new-code-to-run-intermittently.rst @@ -15,65 +15,44 @@ scheduler. This can be done by adding your new function to the `scheduler_tasks `__ array in `Copter.cpp `__. -Note that there are actually two task lists, `the upper list `__ -is for high speed CPUs (i.e. Pixhawk) and the `lower list `__ -is for slow CPUs (i.e. APM2). -Adding a task is fairly simple, just create a new row in the list -(higher in the list means high priority). The first column holds the -function name, the 2nd is a number of 2.5ms units (or 10ms units in case -of APM2). So if you wanted the function executed at 400Hz this column -would contain "1", if you wanted 50Hz it would contain "8". The final -column holds the number of microseconds (i.e. millions of a second) the -function is expected to take. This helps the scheduler guess whether or -not there is enough time to run your function before the main loop -starts the next iteration. +Helpers are provided to create entries in the scheduler table; see the documentation referenced above. :: - /* - scheduler table - all regular tasks apart from the fast_loop() - should be listed here, along with how often they should be called - (in 10ms units) and the maximum time they are expected to take (in - microseconds) - */ - static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { - { update_GPS, 2, 900 }, - { update_nav_mode, 1, 400 }, - { medium_loop, 2, 700 }, - { update_altitude, 10, 1000 }, - { fifty_hz_loop, 2, 950 }, - { run_nav_updates, 10, 800 }, - { slow_loop, 10, 500 }, - { gcs_check_input, 2, 700 }, - { gcs_send_heartbeat, 100, 700 }, - { gcs_data_stream_send, 2, 1500 }, - { gcs_send_deferred, 2, 1200 }, - { compass_accumulate, 2, 700 }, - { barometer_accumulate, 2, 900 }, - { super_slow_loop, 100, 1100 }, - { my_new_function, 10, 200 }, - { perf_update, 1000, 500 } - }; + const AP_Scheduler::Task Copter::scheduler_tasks[] = { + // update INS immediately to get current gyro data populated + FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update), + // run low level rate controllers that only require IMU data + FAST_TASK(run_rate_controller), + #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED + FAST_TASK(run_custom_controller), + #endif + . + . + SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102), + SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550, 105), + #if HAL_MOUNT_ENABLED + SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75, 108), + #endif + #if AP_CAMERA_ENABLED + SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111), + . + . Running your code as part of one of the loops ============================================= -Instead of creating a new entry in the scheduler task list, you can add -your function to one of the existing timed loops. There is no real -advantage to this approach over the above approach except in the case of -the fast-loop. Adding your function to the fast-loop will mean that it -runs at the highest possible priority (i.e. it is nearly 100% guaranteed -to run at 400Hz). +Instead of creating a new entry in the scheduler task list, you can +add your function to one of the existing timed loops. There is no +real advantage to this approach over the above approach. -- `fast_loop `__ - : runs at 100Hz on APM2, 400Hz on Pixhawk - `fifty_hz_loop `__ : runs at 50Hz - `ten_hz_logging_loop `__: runs at 10Hz - `three_hz_loop `__: - runs at 3.3Hz + runs at 3Hz - `one_hz_loop `__ : runs at 1Hz