Rover: removed fast_loop() and use scheduler for all tasks

this also fixes a parameter download speed issue

APMrover2/APMrover2.pde

@@ -548,27 +548,34 @@ static uint16_t			mainLoop_count;
 ////////////////////////////////////////////////////////////////////////////////
 
 /*
-  scheduler table - all regular tasks apart from the fast_loop()
+  scheduler table - all regular tasks should be listed here, along
-  should be listed here, along with how often they should be called
+  with how often they should be called (in 20ms units) and the maximum
-  (in 20ms units) and the maximum time they are expected to take (in
+  time they are expected to take (in microseconds)
-  microseconds)
  */
 static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
+	{ read_radio,             1,   1000 },
+    { ahrs_update,            1,   6400 },
+    { read_sonars,            1,   2000 },
+    { update_current_mode,    1,   1000 },
+    { set_servos,             1,   1000 },
     { update_GPS,             5,   2500 },
     { navigate,               5,   1600 },
     { update_compass,         5,   2000 },
     { update_commands,        5,   1000 },
     { update_logging,         5,   1000 },
+    { gcs_retry_deferred,     1,   1000 },
+    { gcs_update,             1,   1700 },
+    { gcs_data_stream_send,   1,   3000 },
+    { read_control_switch,   15,   1000 },
+    { read_trim_switch,       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 },
+    { mount_update,           1,    600 },
-    { gcs_failsafe_check,     5,    500 },
+    { gcs_failsafe_check,     5,    600 },
     { compass_accumulate,     1,    900 },
-    { update_notify,          1,    100 },
+    { update_notify,          1,    300 },
     { one_second_loop,       50,   3000 }
 };
 
@@ -616,60 +623,33 @@ void loop()
     G_Dt                = (float)delta_ms_fast_loop / 1000.f;
     fast_loopTimer      = timer;
 
-    mainLoop_count++;
+	if (delta_ms_fast_loop > G_Dt_max)
+		G_Dt_max = delta_ms_fast_loop;
 
-    // Execute the fast loop
+    mainLoop_count++;
-    // ---------------------
-    fast_loop();
 
     // tell the scheduler one tick has passed
     scheduler.tick();
     fast_loopTimeStamp = millis();
 
-    scheduler.run(19000U);
+    scheduler.run(19500U);
 }
 
-// Main loop 50Hz
+// update AHRS system
-static void fast_loop()
+static void ahrs_update()
 {
-	// This is the fast loop - we want it to execute at 50Hz if possible
+#if HIL_MODE != HIL_MODE_DISABLED
-	// -----------------------------------------------------------------
+    // update hil before AHRS update
-	if (delta_ms_fast_loop > G_Dt_max)
+    gcs_update();
-		G_Dt_max = delta_ms_fast_loop;
+#endif
 
-	// Read radio
+    ahrs.update();
-	// ----------
-	read_radio();
-
-    // try to send any deferred messages if the serial port now has
-    // some space available
-    gcs_send_message(MSG_RETRY_DEFERRED);
-
-	#if HIL_MODE != HIL_MODE_DISABLED
-		// update hil before dcm update
-		gcs_update();
-	#endif
-
-	ahrs.update();
-
-    read_sonars();
 
     if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
         Log_Write_Attitude();
 
     if (g.log_bitmask & MASK_LOG_IMU)
         DataFlash.Log_Write_IMU(&ins);
-
-	// custom code/exceptions for flight modes
-	// ---------------------------------------
-	update_current_mode();
-
-	// write out the servo PWM values
-	// ------------------------------
-	set_servos();
-
-    gcs_update();
-    gcs_data_stream_send();
 }
 
 /*

APMrover2/GCS_Mavlink.pde

@@ -9,6 +9,9 @@ static bool in_mavlink_delay;
 // true when we have received at least 1 MAVLink packet
 static bool mavlink_active;
 
+// true if we are out of time in our event timeslice
+static bool	gcs_out_of_time;
+
 // check if a message will fit in the payload space available
 #define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
 
@@ -479,6 +482,14 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         return false;
     }
 
+    // if we don't have at least 1ms remaining before the main loop
+    // wants to fire then don't send a mavlink message. We want to
+    // prioritise the main flight control loop over communications
+    if (!in_mavlink_delay && scheduler.time_available_usec() < 1200) {
+        gcs_out_of_time = true;
+        return false;
+    }
+
     switch (id) {
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
@@ -769,8 +780,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
-    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRates[8],  5),
+    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRates[8],  10),
-
     AP_GROUPEND
 };
 
@@ -871,7 +881,8 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
 
     // send at a much lower rate while handling waypoints and
     // parameter sends
-    if (waypoint_receiving || _queued_parameter != NULL) {
+    if ((stream_num != STREAM_PARAMS) && 
+        (waypoint_receiving || _queued_parameter != NULL)) {
         rate *= 0.25;
     }
 
@@ -896,18 +907,19 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
 void
 GCS_MAVLINK::data_stream_send(void)
 {
+    gcs_out_of_time = false;
+
     if (_queued_parameter != NULL) {
         if (streamRates[STREAM_PARAMS].get() <= 0) {
-            streamRates[STREAM_PARAMS].set(5);
+            streamRates[STREAM_PARAMS].set(10);
-        }
-        if (streamRates[STREAM_PARAMS].get() > 5) {
-            streamRates[STREAM_PARAMS].set(5);
         }
         if (stream_trigger(STREAM_PARAMS)) {
             send_message(MSG_NEXT_PARAM);
         }
     }
 
+    if (gcs_out_of_time) return;
+
     if (in_mavlink_delay) {
 #if HIL_MODE != HIL_MODE_DISABLED
         // in HIL we need to keep sending servo values to ensure
@@ -924,11 +936,15 @@ GCS_MAVLINK::data_stream_send(void)
         return;
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_RAW_SENSORS)) {
         send_message(MSG_RAW_IMU1);
         send_message(MSG_RAW_IMU3);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_EXTENDED_STATUS)) {
         send_message(MSG_EXTENDED_STATUS1);
         send_message(MSG_EXTENDED_STATUS2);
@@ -937,29 +953,41 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_NAV_CONTROLLER_OUTPUT);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_POSITION)) {
         // sent with GPS read
         send_message(MSG_LOCATION);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_RAW_CONTROLLER)) {
         send_message(MSG_SERVO_OUT);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_RC_CHANNELS)) {
         send_message(MSG_RADIO_OUT);
         send_message(MSG_RADIO_IN);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_EXTRA1)) {
         send_message(MSG_ATTITUDE);
         send_message(MSG_SIMSTATE);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_EXTRA2)) {
         send_message(MSG_VFR_HUD);
     }
 
+    if (gcs_out_of_time) return;
+
     if (stream_trigger(STREAM_EXTRA3)) {
         send_message(MSG_AHRS);
         send_message(MSG_HWSTATUS);
@@ -2021,3 +2049,11 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...)
     }
 }
 
+
+/**
+   retry any deferred messages
+ */
+static void gcs_retry_deferred(void)
+{
+    gcs_send_message(MSG_RETRY_DEFERRED);
+}