From 40d7b07789c325eb863ced098c4cdde65ac7d7b9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 2 Apr 2012 11:18:42 +1000
Subject: [PATCH] MAVLink: port the new adaptive flow control to ArduCopter

This allows for arbitrary stream rates, and supports flow control if
you are using a 3DR radio
---
 ArduCopter/ArduCopter.pde  |  12 +----
 ArduCopter/GCS.h           |  39 ++++++++++----
 ArduCopter/GCS_Mavlink.pde | 104 +++++++++++++++++++++++++++++--------
 ArduCopter/planner.pde     |   6 +--
 4 files changed, 111 insertions(+), 50 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 8a949023d..5d12c30de 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1067,11 +1067,6 @@ static void medium_loop()
 				if (g.log_bitmask & MASK_LOG_MOTORS)
 					Log_Write_Motors();
             }
-
-			// 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
@@ -1163,7 +1158,7 @@ static void fifty_hz_loop()
 
 	// kick the GCS to process uplink data
 	gcs_update();
-    gcs_data_stream_send(45,1000);
+    gcs_data_stream_send();
 
 	#if FRAME_CONFIG == TRI_FRAME
 		// servo Yaw
@@ -1218,10 +1213,6 @@ static void slow_loop()
 			// blink if we are armed
 			update_lights();
 
-            // send all requested output streams with rates requested
-            // between 3 and 5 Hz
-            gcs_data_stream_send(3,5);
-
 			if(g.radio_tuning > 0)
 				tuning();
 
@@ -1262,7 +1253,6 @@ static void super_slow_loop()
 	}
 
     gcs_send_message(MSG_HEARTBEAT);
-    gcs_data_stream_send(1,3);
 
 	// agmatthews - USERHOOKS
 	#ifdef USERHOOK_SUPERSLOWLOOP
diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h
index 6a1fc4182..607fe197d 100644
--- a/ArduCopter/GCS.h
+++ b/ArduCopter/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,29 @@ 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,
+                  STREAM_PARAMS,
+                  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 +177,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 +187,13 @@ private:
 	AP_Int16 streamRateExtra1;
 	AP_Int16 streamRateExtra2;
 	AP_Int16 streamRateExtra3;
+	AP_Int16 streamRateParams;
+
+    // number of 50Hz ticks until we next send this stream
+    uint8_t stream_ticks[NUM_STREAMS];
+
+    // number of extra ticks to add to slow things down for the radio
+    uint8_t stream_slowdown;
 };
 
 #endif // __GCS_H
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index bdd0f217c..445716b51 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -589,6 +589,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
 	AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK, streamRateExtra1),
 	AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK, streamRateExtra2),
 	AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK, streamRateExtra3),
+	AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK, streamRateParams),
     AP_GROUPEND
 };
 
@@ -653,11 +654,6 @@ GCS_MAVLINK::update(void)
 	// Update packet drops counter
 	packet_drops += status.packet_rx_drop_count;
 
-    // send out queued params/ waypoints
-    if (NULL != _queued_parameter) {
-        send_message(MSG_NEXT_PARAM);
-    }
-
     if (!waypoint_receiving && !waypoint_sending) {
         return;
     }
@@ -665,8 +661,8 @@ GCS_MAVLINK::update(void)
     uint32_t tnow = millis();
 
     if (waypoint_receiving &&
-        waypoint_request_i < (unsigned)g.command_total &&
-        tnow > waypoint_timelast_request + 500) {
+        waypoint_request_i <= (unsigned)g.command_total &&
+        tnow > waypoint_timelast_request + 500 + (stream_slowdown*20)) {
         waypoint_timelast_request = tnow;
         send_message(MSG_NEXT_WAYPOINT);
     }
@@ -682,19 +678,57 @@ 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) + stream_slowdown;
+        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_receiving || waypoint_sending) {
+        // don't interfere with mission transfer
+        return;
+    }
 
-		if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
+    if (_queued_parameter != NULL) {
+        if (streamRateParams.get() <= 0) {
+            streamRateParams.set(50);
+        }
+        if (stream_trigger(STREAM_PARAMS)) {
+            send_message(MSG_NEXT_PARAM);
+        }
+        // don't send anything else at the same time as parameters
+        return;
+    }
+
+    if (stream_trigger(STREAM_RAW_SENSORS)) {
 			send_message(MSG_RAW_IMU1);
 			send_message(MSG_RAW_IMU2);
 			send_message(MSG_RAW_IMU3);
 			//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
 		}
 
-		if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
+    if (stream_trigger(STREAM_EXTENDED_STATUS)) {
 			send_message(MSG_EXTENDED_STATUS1);
 			send_message(MSG_EXTENDED_STATUS2);
 			send_message(MSG_CURRENT_WAYPOINT);
@@ -711,7 +745,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
             }
 		}
 
-		if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
+    if (stream_trigger(STREAM_POSITION)) {
 			// sent with GPS read
 #if HIL_MODE == HIL_MODE_ATTITUDE
 			send_message(MSG_LOCATION);
@@ -719,35 +753,35 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 			//Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
 		}
 
-		if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
-			// This is used for HIL.  Do not change without discussing with HIL maintainers
+    if (stream_trigger(STREAM_RAW_CONTROLLER)) {
 			send_message(MSG_SERVO_OUT);
 			//Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
 		}
 
-		if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
+    if (stream_trigger(STREAM_RC_CHANNELS)) {
 			send_message(MSG_RADIO_OUT);
 			send_message(MSG_RADIO_IN);
 			//Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
 		}
 
-		if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){	 // Use Extra 1 for AHRS info
+    if (stream_trigger(STREAM_EXTRA1)) {
 			send_message(MSG_ATTITUDE);
 			send_message(MSG_SIMSTATE);
 			//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
 		}
 
-		if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){		// Use Extra 2 for additional HIL info
+    if (stream_trigger(STREAM_EXTRA2)) {
 			send_message(MSG_VFR_HUD);
 			//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
 		}
 
-		if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
+    if (stream_trigger(STREAM_EXTRA3)) {
 			send_message(MSG_AHRS);
 			send_message(MSG_HWSTATUS);
 		}
 	}
-}
+
+
 
 void
 GCS_MAVLINK::send_message(enum ap_message id)
@@ -1679,6 +1713,30 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 #endif // HIL_MODE
+
+    case MAVLINK_MSG_ID_RADIO:
+        {
+            mavlink_radio_t packet;
+            mavlink_msg_radio_decode(msg, &packet);
+            // use the state of the transmit buffer in the radio to
+            // control the stream rate, giving us adaptive software
+            // flow control
+            if (packet.txbuf < 20 && stream_slowdown < 100) {
+                // we are very low on space - slow down a lot
+                stream_slowdown += 3;
+            } else if (packet.txbuf < 50 && stream_slowdown < 100) {
+                // we are a bit low on space, slow down slightly
+                stream_slowdown += 1;
+            } else if (packet.txbuf > 95 && stream_slowdown > 10) {
+                // the buffer has plenty of space, speed up a lot
+                stream_slowdown -= 2;
+            } else if (packet.txbuf > 90 && stream_slowdown != 0) {
+                // the buffer has enough space, speed up a bit
+                stream_slowdown--;
+            }
+            break;
+        }
+
 	} // end switch
 } // end handle mavlink
 
@@ -1803,12 +1861,12 @@ 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();
+    }
 }
 
 /*
diff --git a/ArduCopter/planner.pde b/ArduCopter/planner.pde
index 72c780eea..c8bc1d30b 100644
--- a/ArduCopter/planner.pde
+++ b/ArduCopter/planner.pde
@@ -38,13 +38,9 @@ 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);
             }
 
-- 
GitLab