MAVLink: allow for find grained stream rate control

streams can now be requested at any multiple of 20ms. So if you ask
for a stream at 7Hz then you will get it at close to 7Hz.

ArduPlane/ArduPlane.pde

@@ -749,11 +749,8 @@ static void fast_loop()
 	// ------------------------------
 	set_servos();
 
-
-	// XXX is it appropriate to be doing the comms below on the fast loop?
-
     gcs_update();
-    gcs_data_stream_send(45,1000);
+    gcs_data_stream_send();
 }
 
 static void medium_loop()
@@ -854,10 +851,6 @@ Serial.println(tempaccel.z, DEC);
 
 			if (g.log_bitmask & MASK_LOG_GPS)
 				Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
-
-            // send all requested output streams with rates requested
-            // between 5 and 45 Hz
-            gcs_data_stream_send(5,45);
 			break;
 
 		// This case controls the slow loop
@@ -919,7 +912,6 @@ static void slow_loop()
 			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
-            gcs_data_stream_send(3,5);
 
 #if USB_MUX_PIN > 0
             check_usb_mux();
@@ -936,7 +928,6 @@ static void one_second_loop()
 
 	// send a heartbeat
 	gcs_send_message(MSG_HEARTBEAT);
-    gcs_data_stream_send(1,3);
 }
 
 static void update_GPS(void)

ArduPlane/GCS.h

@@ -76,16 +76,8 @@ public:
 	///
 	void		send_text(gcs_severity severity, const prog_char_t *str) {}
 
-    // test if frequency within range requested for loop
-    // used by data_stream_send
-    static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
-    {
-        if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
-        else return false;
-    }
-
     // send streams which match frequency range
-    void data_stream_send(uint16_t freqMin, uint16_t freqMax);
+    void data_stream_send(void);
 
     // set to true if this GCS link is active
     bool initialised;
@@ -118,12 +110,28 @@ public:
 	void	send_message(enum ap_message id);
 	void	send_text(gcs_severity severity, const char *str);
 	void	send_text(gcs_severity severity, const prog_char_t *str);
-    void    data_stream_send(uint16_t freqMin, uint16_t freqMax);
+    void    data_stream_send(void);
 	void    queued_param_send();
 	void    queued_waypoint_send();
 
     static const struct AP_Param::GroupInfo var_info[];
 
+    // NOTE! The streams enum below and the
+    // set of AP_Int16 stream rates _must_ be
+    // kept in the same order
+    enum streams {STREAM_RAW_SENSORS,
+                  STREAM_EXTENDED_STATUS,
+                  STREAM_RC_CHANNELS,
+                  STREAM_RAW_CONTROLLER,
+                  STREAM_POSITION,
+                  STREAM_EXTRA1,
+                  STREAM_EXTRA2,
+                  STREAM_EXTRA3,
+                  NUM_STREAMS};
+
+    // see if we should send a stream now. Called at 50Hz
+    bool stream_trigger(enum streams stream_num);
+
 private:
 	void 	handleMessage(mavlink_message_t * msg);
 
@@ -168,7 +176,8 @@ private:
 	uint16_t waypoint_send_timeout; // milliseconds
 	uint16_t waypoint_receive_timeout; // milliseconds
 
-	// data stream rates
+	// data stream rates. The code assumes that
+    // streamRateRawSensors is the first
 	AP_Int16 streamRateRawSensors;
 	AP_Int16 streamRateExtendedStatus;
 	AP_Int16 streamRateRCChannels;
@@ -177,6 +186,9 @@ private:
 	AP_Int16 streamRateExtra1;
 	AP_Int16 streamRateExtra2;
 	AP_Int16 streamRateExtra3;
+
+    // number of 50Hz ticks until we next send this stream
+    uint8_t stream_ticks[NUM_STREAMS];
 };
 
 #endif // __GCS_H

ArduPlane/GCS_Mavlink.pde

@@ -886,64 +886,89 @@ GCS_MAVLINK::update(void)
     }
 }
 
+// see if we should send a stream now. Called at 50Hz
+bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
+{
+    AP_Int16 *stream_rates = &streamRateRawSensors;
+    uint8_t rate = (uint8_t)stream_rates[stream_num].get();
+
+    if (rate == 0) {
+        return false;
+    }
+
+    if (stream_ticks[stream_num] == 0) {
+        // we're triggering now, setup the next trigger point
+        if (rate > 50) {
+            rate = 50;
+        }
+        stream_ticks[stream_num] = 50 / rate;
+        return true;
+    }
+
+    // count down at 50Hz
+    stream_ticks[stream_num]--;
+    return false;
+}
+
 void
-GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
+GCS_MAVLINK::data_stream_send(void)
 {
-	if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
+	if (waypoint_sending || waypoint_receiving || _queued_parameter != NULL) {
+        // don't send the streams when transferring bulk data
+        return;
+    }
 
-		if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
-			send_message(MSG_RAW_IMU1);
-			send_message(MSG_RAW_IMU2);
-			send_message(MSG_RAW_IMU3);
-		}
+    if (stream_trigger(STREAM_RAW_SENSORS)) {
+        send_message(MSG_RAW_IMU1);
+        send_message(MSG_RAW_IMU2);
+        send_message(MSG_RAW_IMU3);
+    }
 
-		if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
-			send_message(MSG_EXTENDED_STATUS1);
-			send_message(MSG_EXTENDED_STATUS2);
-			send_message(MSG_CURRENT_WAYPOINT);
-			send_message(MSG_GPS_RAW);            // TODO - remove this message after location message is working
-			send_message(MSG_NAV_CONTROLLER_OUTPUT);
-			send_message(MSG_FENCE_STATUS);
-
-            if (last_gps_satellites != g_gps->num_sats) {
-                // this message is mostly a huge waste of bandwidth,
-                // except it is the only message that gives the number
-                // of visible satellites. So only send it when that
-                // changes.
-                send_message(MSG_GPS_STATUS);
-                last_gps_satellites = g_gps->num_sats;
-            }
-		}
+    if (stream_trigger(STREAM_EXTENDED_STATUS)) {
+        send_message(MSG_EXTENDED_STATUS1);
+        send_message(MSG_EXTENDED_STATUS2);
+        send_message(MSG_CURRENT_WAYPOINT);
+        send_message(MSG_GPS_RAW);            // TODO - remove this message after location message is working
+        send_message(MSG_NAV_CONTROLLER_OUTPUT);
+        send_message(MSG_FENCE_STATUS);
+
+        if (last_gps_satellites != g_gps->num_sats) {
+            // this message is mostly a huge waste of bandwidth,
+            // except it is the only message that gives the number
+            // of visible satellites. So only send it when that
+            // changes.
+            send_message(MSG_GPS_STATUS);
+            last_gps_satellites = g_gps->num_sats;
+        }
+    }
 
-		if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
-			// sent with GPS read
-			send_message(MSG_LOCATION);
-		}
+    if (stream_trigger(STREAM_POSITION)) {
+        // sent with GPS read
+        send_message(MSG_LOCATION);
+    }
 
-		if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
-			// This is used for HIL.  Do not change without discussing with HIL maintainers
-			send_message(MSG_SERVO_OUT);
-		}
+    if (stream_trigger(STREAM_RAW_CONTROLLER)) {
+        send_message(MSG_SERVO_OUT);
+    }
 
-		if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
-			send_message(MSG_RADIO_OUT);
-			send_message(MSG_RADIO_IN);
-		}
+    if (stream_trigger(STREAM_RC_CHANNELS)) {
+        send_message(MSG_RADIO_OUT);
+        send_message(MSG_RADIO_IN);
+    }
 
-		if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){	 // Use Extra 1 for AHRS info
-			send_message(MSG_ATTITUDE);
-			send_message(MSG_SIMSTATE);
-		}
+    if (stream_trigger(STREAM_EXTRA1)) {
+        send_message(MSG_ATTITUDE);
+        send_message(MSG_SIMSTATE);
+    }
 
-		if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){		// Use Extra 2 for additional HIL info
-			send_message(MSG_VFR_HUD);
-		}
+    if (stream_trigger(STREAM_EXTRA2)) {
+        send_message(MSG_VFR_HUD);
+    }
 
-		if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
-			send_message(MSG_AHRS);
-			send_message(MSG_HWSTATUS);
-		}
-	}
+    if (stream_trigger(STREAM_EXTRA3)) {
+        send_message(MSG_AHRS);
+        send_message(MSG_HWSTATUS);
+    }
 }
 
 
@@ -2157,11 +2182,11 @@ static void gcs_send_message(enum ap_message id)
 /*
   send data streams in the given rate range on both links
  */
-static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
+static void gcs_data_stream_send(void)
 {
-    gcs0.data_stream_send(freqMin, freqMax);
+    gcs0.data_stream_send();
     if (gcs3.initialised) {
-        gcs3.data_stream_send(freqMin, freqMax);
+        gcs3.data_stream_send();
     }
 }
 

ArduPlane/planner.pde

@@ -41,11 +41,8 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
       
       read_radio();
       
-      gcs_data_stream_send(45,1000);
-      if ((loopcount % 5) == 0) // 10 hz
-          gcs_data_stream_send(5,45);
+      gcs_data_stream_send();
       if ((loopcount % 16) == 0) { // 3 hz
-          gcs_data_stream_send(1,5);
           gcs_send_message(MSG_HEARTBEAT);
       }
       loopcount++;