Rover: convert to use AP_Scheduler

this gives us better timing information and scheduling, while also
making the code easier to read

APMrover2/APMrover2.pde

@@ -75,6 +75,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <DataFlash.h>
 #include <AP_RCMapper.h>        // RC input mapping library
 #include <SITL.h>
+#include <AP_Scheduler.h>       // main loop scheduler
 #include <stdarg.h>
 
 #include <AP_HAL_AVR.h>
@@ -116,6 +117,9 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
 //
 static Parameters      g;
 
+// main loop scheduler
+static AP_Scheduler scheduler;
+
 // mapping between input channels
 static RCMapper rcmap;
 
@@ -532,20 +536,6 @@ static uint8_t 		delta_ms_fast_loop;
 // Counter of main loop executions.  Used for performance monitoring and failsafe processing
 static uint16_t			mainLoop_count;
 
-// Time in miliseconds of start of medium control loop.  Milliseconds
-static uint32_t 	medium_loopTimer;
-// Counters for branching from main control loop to slower loops
-static uint8_t 			medium_loopCounter;	
-// Number of milliseconds used in last medium loop cycle
-static uint8_t			delta_ms_medium_loop;
-
-// Counters for branching from medium control loop to slower loops
-static uint8_t 			slow_loopCounter;
-// Counter to trigger execution of very low rate processes
-static uint8_t 			superslow_loopCounter;
-// Counter to trigger execution of 1 Hz processes
-static uint8_t				counter_one_herz;
-
 // % MCU cycles used
 static float 			load;
 
@@ -553,6 +543,33 @@ static float 			load;
 // Top-level logic
 ////////////////////////////////////////////////////////////////////////////////
 
+/*
+  scheduler table - all regular tasks apart from the fast_loop()
+  should be listed here, along with how often they should be called
+  (in 20ms units) and the maximum time they are expected to take (in
+  microseconds)
+ */
+static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
+    { update_GPS,             5,    900 },
+    { navigate,               5,   1000 },
+    { update_compass,         5,   1000 },
+    { update_commands,        5,   1000 },
+    { update_logging,         5,   1000 },
+    { read_battery,           5,   1000 },
+    { read_receiver_rssi,     5,   1000 },
+    { read_trim_switch,       5,   1000 },
+    { read_control_switch,   15,   1000 },
+    { update_events,         15,   1000 },
+    { check_usb_mux,         15,   1000 },
+    { mount_update,           1,    500 },
+    { failsafe_check,         5,    500 },
+    { one_second_loop,       50,   1000 }
+};
+
+
+/*
+  setup is called when the sketch starts
+ */
 void setup() {
 	memcheck_init();
     cliSerial = hal.console;
@@ -566,10 +583,18 @@ void setup() {
     batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
 
 	init_ardupilot();
+
+    // initialise the main loop scheduler
+    scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
 }
 
+/*
+  loop() is called rapidly while the sketch is running
+ */
 void loop()
 {
+    uint32_t timer = millis();
+
     // We want this to execute at 50Hz, but synchronised with the gyro/accel
     uint16_t num_samples = ins.num_samples_available();
     if (num_samples >= 1) {
@@ -584,35 +609,14 @@ void loop()
 		// ---------------------
 		fast_loop();
 
-		// Execute the medium loop
+        // tell the scheduler one tick has passed
-		// -----------------------
+        scheduler.tick();
-		medium_loop();
-
-		counter_one_herz++;
-		if(counter_one_herz == 50){
-			one_second_loop();
-			counter_one_herz = 0;
-		}
-
-		if (millis() - perf_mon_timer > 20000) {
-			if (mainLoop_count != 0) {
-				if (g.log_bitmask & MASK_LOG_PM)
-					#if HIL_MODE != HIL_MODE_ATTITUDE
-					Log_Write_Performance();
-					#endif
-				resetPerfData();
-			}
-		}
-
 		fast_loopTimeStamp = millis();
-    } else if (millis() - fast_loopTimeStamp < 19) {
+    } else {
-        // less than 19ms has passed. We have at least one millisecond
+        uint16_t dt = timer - fast_loopTimer;
-        // of free time. The most useful thing to do with that time is
+        if (dt < 20) {
-        // to accumulate some sensor readings, specifically the
+            uint16_t time_to_next_loop = 20 - dt;
-        // compass, which is often very noisy but is not interrupt
+            scheduler.run(time_to_next_loop * 1000U);
-        // driven, so it can't accumulate readings by itself
-        if (g.compass_enabled) {
-            compass.accumulate();
         }
     }
 }
@@ -665,135 +669,60 @@ static void fast_loop()
     gcs_data_stream_send();
 }
 
-static void medium_loop()
+/*
+  update camera mount - 50Hz
+ */
+static void mount_update(void)
 {
 #if MOUNT == ENABLED
 	camera_mount.update_mount_position();
 #endif
-
-	// This is the start of the medium (10 Hz) loop pieces
-	// -----------------------------------------
-	switch(medium_loopCounter) {
-
-		// This case deals with the GPS
-		//-------------------------------
-		case 0:
-            failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000);
-			medium_loopCounter++;
-            update_GPS();
-            
-			#if HIL_MODE != HIL_MODE_ATTITUDE
-            if (g.compass_enabled && compass.read()) {
-                ahrs.set_compass(&compass);
-                // Calculate heading
-                compass.null_offsets();
-                if (g.log_bitmask & MASK_LOG_COMPASS) {
-                    Log_Write_Compass();
-                }
-            } else {
-                ahrs.set_compass(NULL);
-            }
-			#endif
-			break;
-
-		// This case performs some navigation computations
-		//------------------------------------------------
-		case 1:
-			medium_loopCounter++;
-            navigate();
-			break;
-
-		// command processing
-		//------------------------------
-		case 2:
-			medium_loopCounter++;
-
-            read_receiver_rssi();
-
-			// perform next command
-			// --------------------
-			update_commands();
-			break;
-
-		// This case deals with sending high rate telemetry
-		//-------------------------------------------------
-		case 3:
-			medium_loopCounter++;
-			#if HIL_MODE != HIL_MODE_ATTITUDE
-				if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
-					Log_Write_Attitude();
-
-				if (g.log_bitmask & MASK_LOG_CTUN)
-					Log_Write_Control_Tuning();
-			#endif
-
-			if (g.log_bitmask & MASK_LOG_NTUN)
-				Log_Write_Nav_Tuning();
-			break;
-
-		// This case controls the slow loop
-		//---------------------------------
-		case 4:
-			medium_loopCounter = 0;
-			delta_ms_medium_loop	= millis() - medium_loopTimer;
-			medium_loopTimer      	= millis();
-
-			if (g.battery_monitoring != 0){
-				read_battery();
-			}
-
-			read_trim_switch();
-
-			slow_loop();
-			break;
-	}
 }
 
-static void slow_loop()
+/*
+  check for GCS failsafe - 10Hz
+ */
+static void failsafe_check(void)
 {
-	// This is the slow (3 1/3 Hz) loop pieces
+    failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000);
-	//----------------------------------------
-	switch (slow_loopCounter){
-		case 0:
-			slow_loopCounter++;
-			superslow_loopCounter++;
-			if(superslow_loopCounter >=200) {				//	200 = Execute every minute
-				#if HIL_MODE != HIL_MODE_ATTITUDE
-					if(g.compass_enabled) {
-						compass.save_offsets();
-					}
-				#endif
-				superslow_loopCounter = 0;
-			}
-			break;
-
-		case 1:
-			slow_loopCounter++;
-
-			// Read 3-position switch on radio
-			// -------------------------------
-			read_control_switch();
-
-			update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
-
-#if MOUNT == ENABLED
-			camera_mount.update_mount_type();
-#endif
-			break;
-
-		case 2:
-			slow_loopCounter = 0;
-                        
-			update_events();
-
-            mavlink_system.sysid = g.sysid_this_mav;		// This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
-
-            check_usb_mux();
-			break;
-	}
 }
 
-static void one_second_loop()
+/*
+  check for new compass data - 10Hz
+ */
+static void update_compass(void)
+{
+    if (g.compass_enabled && compass.read()) {
+        ahrs.set_compass(&compass);
+        // update offsets
+        compass.null_offsets();
+        if (g.log_bitmask & MASK_LOG_COMPASS) {
+            Log_Write_Compass();
+        }
+    } else {
+        ahrs.set_compass(NULL);
+    }
+}
+
+/*
+  log some key data - 10Hz
+ */
+static void update_logging(void)
+{
+    if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
+        Log_Write_Attitude();
+    
+    if (g.log_bitmask & MASK_LOG_CTUN)
+        Log_Write_Control_Tuning();
+
+    if (g.log_bitmask & MASK_LOG_NTUN)
+        Log_Write_Nav_Tuning();
+}
+
+/*
+  once a second events
+ */
+static void one_second_loop(void)
 {
 	if (g.log_bitmask & MASK_LOG_CURRENT)
 		Log_Write_Current();
@@ -804,6 +733,35 @@ static void one_second_loop()
     ahrs.set_orientation();
 
     set_control_channels();
+
+    // cope with changes to aux functions
+    update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
+
+#if MOUNT == ENABLED
+    camera_mount.update_mount_type();
+#endif
+
+    // cope with changes to mavlink system ID
+    mavlink_system.sysid = g.sysid_this_mav;
+
+    static uint8_t counter;
+
+    counter++;
+
+    // write perf data every 20s
+    if (counter == 20) {
+        if (g.log_bitmask & MASK_LOG_PM)
+            Log_Write_Performance();
+        resetPerfData();
+    }
+
+    // save compass offsets once a minute
+    if (counter >= 60) {				
+        if (g.compass_enabled) {
+            compass.save_offsets();
+        }
+        counter = 0;
+    }
 }
 
 static void update_GPS(void)

APMrover2/Parameters.h

@@ -32,6 +32,7 @@ public:
         k_param_num_resets,
         k_param_reset_switch_chan,
         k_param_initial_mode,
+        k_param_scheduler,
 
         // IO pins
         k_param_rssi_pin = 20,

APMrover2/Parameters.pde

@@ -416,6 +416,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Compass/Compass.cpp
 	GOBJECT(compass,                "COMPASS_",	Compass),
 
+    // @Group: SCHED_
+    // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
+    GOBJECT(scheduler, "SCHED_", AP_Scheduler),
+
     // @Group: RCMAP_
     // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
     GOBJECT(rcmap,                 "RCMAP_",         RCMapper),

APMrover2/sensors.pde

@@ -18,6 +18,9 @@ void ReadSCP1000(void) {}
 
 #endif // HIL_MODE != HIL_MODE_ATTITUDE
 
+/*
+  read and update the battery
+ */
 static void read_battery(void)
 {
 	if(g.battery_monitoring == 0) {
@@ -30,11 +33,19 @@ static void read_battery(void)
         batt_volt_pin->set_pin(g.battery_volt_pin);
         battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin);
     }
-    if(g.battery_monitoring == 4) {
+
-        // this copes with changing the pin at runtime
+    if (g.battery_monitoring == 4) {
-        batt_curr_pin->set_pin(g.battery_curr_pin);
+        static uint32_t last_time_ms;
-        current_amps1    = CURRENT_AMPS(batt_curr_pin);
+        uint32_t tnow = hal.scheduler->millis();
-        current_total1   += current_amps1 * (float)delta_ms_medium_loop * 0.0002778;                                    // .0002778 is 1/3600 (conversion to hours)
+        float dt = tnow - last_time_ms;
+        if (last_time_ms != 0 && dt < 2000) {
+            // this copes with changing the pin at runtime
+            batt_curr_pin->set_pin(g.battery_curr_pin);
+            current_amps1    = CURRENT_AMPS(batt_curr_pin);
+            // .0002778 is 1/3600 (conversion to hours)
+            current_total1   += current_amps1 * dt * 0.0002778; 
+        }
+        last_time_ms = tnow;
     }
 }
 

APMrover2/system.pde

@@ -294,6 +294,10 @@ static void startup_ground(void)
 	// -----------------------
 	demo_servos(3);
 
+    hal.uartA->set_blocking_writes(false);
+    hal.uartB->set_blocking_writes(false);
+    hal.uartC->set_blocking_writes(false);
+
 	gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive."));
 }
 

APMrover2/test.pde

@@ -233,7 +233,6 @@ test_battery(uint8_t argc, const Menu::arg *argv)
 {
 if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
 	print_hit_enter();
-	delta_ms_medium_loop = 100;
 
 	while(1){
 		delay(100);
@@ -423,6 +422,8 @@ test_ins(uint8_t argc, const Menu::arg *argv)
 	print_hit_enter();
 	delay(1000);
 
+    uint8_t medium_loopCounter = 0;
+
 	while(1){
 		delay(20);
 		if (millis() - fast_loopTimer > 19) {
@@ -436,7 +437,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
 
 			if(g.compass_enabled) {
 				medium_loopCounter++;
-				if(medium_loopCounter == 5){
+				if(medium_loopCounter >= 5){
 					compass.read();
                     medium_loopCounter = 0;
 				}
@@ -489,6 +490,8 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 
     print_hit_enter();
 
+    uint8_t medium_loopCounter = 0;
+
     while(1) {
 		delay(20);
 		if (millis() - fast_loopTimer > 19) {
@@ -501,7 +504,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 			ahrs.update();
 
             medium_loopCounter++;
-            if(medium_loopCounter == 5){
+            if(medium_loopCounter >= 5){
                 if (compass.read()) {
                     // Calculate heading
                     Matrix3f m = ahrs.get_dcm_matrix();