From 58d9b030eacf9cb8780cb234c2012dce1288be3e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 1 Dec 2015 06:59:28 +1100
Subject: [PATCH] Plane: update for Hz based AP_Scheduler

---
 ArduPlane/ArduPlane.cpp | 94 ++++++++++++++++++++---------------------
 1 file changed, 45 insertions(+), 49 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 069444dccc..9c898a4c57 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -24,12 +24,8 @@
 
 #include "Plane.h"
 
-#define SCHED_TASK(func, _interval_ticks, _max_time_micros) {\
-    .function = FUNCTOR_BIND(&plane, &Plane::func, void),\
-    AP_SCHEDULER_NAME_INITIALIZER(func)\
-    .interval_ticks = _interval_ticks,\
-    .max_time_micros = _max_time_micros,\
-}
+#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros)
+
 
 /*
   scheduler table - all regular tasks are listed here, along with how
@@ -37,54 +33,54 @@
   they are expected to take (in microseconds)
  */
 const AP_Scheduler::Task Plane::scheduler_tasks[] = {
-    SCHED_TASK(read_radio,              1,    700),
-    SCHED_TASK(check_short_failsafe,    1,   1000),
-    SCHED_TASK(ahrs_update,             1,   6400),
-    SCHED_TASK(update_speed_height,     1,   1600),
-    SCHED_TASK(update_flight_mode,      1,   1400),
-    SCHED_TASK(stabilize,               1,   3500),
-    SCHED_TASK(set_servos,              1,   1600),
+    SCHED_TASK(read_radio,             50,    700),
+    SCHED_TASK(check_short_failsafe,   50,   1000),
+    SCHED_TASK(ahrs_update,            50,   6400),
+    SCHED_TASK(update_speed_height,    50,   1600),
+    SCHED_TASK(update_flight_mode,     50,   1400),
+    SCHED_TASK(stabilize,              50,   3500),
+    SCHED_TASK(set_servos,             50,   1600),
     SCHED_TASK(read_control_switch,     7,   1000),
-    SCHED_TASK(gcs_retry_deferred,      1,   1000),
-    SCHED_TASK(update_GPS_50Hz,         1,   2500),
-    SCHED_TASK(update_GPS_10Hz,         5,   2500),
-    SCHED_TASK(navigate,                5,   3000),
-    SCHED_TASK(update_compass,          5,   1200),
-    SCHED_TASK(read_airspeed,           5,   1200),
-    SCHED_TASK(update_alt,              5,   3400),
-    SCHED_TASK(adjust_altitude_target,  5,   1000),
-    SCHED_TASK(obc_fs_check,            5,   1000),
-    SCHED_TASK(gcs_update,              1,   1700),
-    SCHED_TASK(gcs_data_stream_send,    1,   3000),
-    SCHED_TASK(update_events,           1,   1500),
-    SCHED_TASK(check_usb_mux,           5,    300),
-    SCHED_TASK(read_battery,            5,   1000),
-    SCHED_TASK(compass_accumulate,      1,   1500),
-    SCHED_TASK(barometer_accumulate,    1,    900),
-    SCHED_TASK(update_notify,           1,    300),
-    SCHED_TASK(read_rangefinder,        1,    500),
-    SCHED_TASK(compass_cal_update,      1,    100),
+    SCHED_TASK(gcs_retry_deferred,     50,   1000),
+    SCHED_TASK(update_GPS_50Hz,        50,   2500),
+    SCHED_TASK(update_GPS_10Hz,        10,   2500),
+    SCHED_TASK(navigate,               10,   3000),
+    SCHED_TASK(update_compass,         10,   1200),
+    SCHED_TASK(read_airspeed,          10,   1200),
+    SCHED_TASK(update_alt,             10,   3400),
+    SCHED_TASK(adjust_altitude_target, 10,   1000),
+    SCHED_TASK(obc_fs_check,           10,   1000),
+    SCHED_TASK(gcs_update,             50,   1700),
+    SCHED_TASK(gcs_data_stream_send,   50,   3000),
+    SCHED_TASK(update_events,          50,   1500),
+    SCHED_TASK(check_usb_mux,          10,    300),
+    SCHED_TASK(read_battery,           10,   1000),
+    SCHED_TASK(compass_accumulate,     50,   1500),
+    SCHED_TASK(barometer_accumulate,   50,    900),
+    SCHED_TASK(update_notify,          50,    300),
+    SCHED_TASK(read_rangefinder,       50,    500),
+    SCHED_TASK(compass_cal_update,     50,    100),
 #if OPTFLOW == ENABLED
-    SCHED_TASK(update_optical_flow,     1,    500),
+    SCHED_TASK(update_optical_flow,    50,    500),
 #endif
-    SCHED_TASK(one_second_loop,        50,   1000),
-    SCHED_TASK(check_long_failsafe,    15,   1000),
-    SCHED_TASK(read_receiver_rssi,      5,   1000),
-    SCHED_TASK(rpm_update,              5,    200),
-    SCHED_TASK(airspeed_ratio_update,  50,   1000),
-    SCHED_TASK(update_mount,            1,   1500),
-    SCHED_TASK(log_perf_info,         500,   1000),
-    SCHED_TASK(compass_save,         3000,   2500),
-    SCHED_TASK(update_logging1,         5,   1700),
-    SCHED_TASK(update_logging2,         5,   1700),
-    SCHED_TASK(parachute_check,         5,    500),
+    SCHED_TASK(one_second_loop,         1,   1000),
+    SCHED_TASK(check_long_failsafe,     3,   1000),
+    SCHED_TASK(read_receiver_rssi,     10,   1000),
+    SCHED_TASK(rpm_update,             10,    200),
+    SCHED_TASK(airspeed_ratio_update,   1,   1000),
+    SCHED_TASK(update_mount,           50,   1500),
+    SCHED_TASK(log_perf_info,         0.1,   1000),
+    SCHED_TASK(compass_save,        0.016,   2500),
+    SCHED_TASK(update_logging1,        10,   1700),
+    SCHED_TASK(update_logging2,        10,   1700),
+    SCHED_TASK(parachute_check,        10,    500),
 #if FRSKY_TELEM_ENABLED == ENABLED
-    SCHED_TASK(frsky_telemetry_send,   10,    100),
+    SCHED_TASK(frsky_telemetry_send,    5,    100),
 #endif
-    SCHED_TASK(terrain_update,          5,    500),
-    SCHED_TASK(update_is_flying_5Hz,   10,    100),
-    SCHED_TASK(dataflash_periodic,      1,    300),
-    SCHED_TASK(adsb_update,            50,    500),
+    SCHED_TASK(terrain_update,         10,    500),
+    SCHED_TASK(update_is_flying_5Hz,    5,    100),
+    SCHED_TASK(dataflash_periodic,     50,    300),
+    SCHED_TASK(adsb_update,             1,    500),
 };
 
 void Plane::setup()