From 40d7b07789c325eb863ced098c4cdde65ac7d7b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell 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 | 106 ++++++++++++++++++++++++++++--------- ArduCopter/planner.pde | 6 +-- 4 files changed, 112 insertions(+), 51 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 8a949023d6..5d12c30de3 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 6a1fc4182c..607fe197dd 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 bdd0f217cd..445716b517 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) } } -void -GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) +// see if we should send a stream now. Called at 50Hz +bool GCS_MAVLINK::stream_trigger(enum streams stream_num) { - if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { + AP_Int16 *stream_rates = &streamRateRawSensors; + uint8_t rate = (uint8_t)stream_rates[stream_num].get(); - if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ + 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(void) +{ + if (waypoint_receiving || waypoint_sending) { + // don't interfere with mission transfer + return; + } + + 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 72c780eeaf..c8bc1d30b2 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); }