From efdad1fca75deb05ac9f6da4b2e30dc5249969ac Mon Sep 17 00:00:00 2001 From: yaapu Date: Wed, 20 Feb 2019 09:37:08 +0100 Subject: [PATCH] AP_Frsky_Telem: fix for passthrough telemetry stall while sending message chunks This prevents the library from giving message chunks a too high priority leading to all telemetry packets but 0x5006(attitude) to starve. --- libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 106 +++++++++++--------- libraries/AP_Frsky_Telem/AP_Frsky_Telem.h | 1 + 2 files changed, 57 insertions(+), 50 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 13312d3e5d..b746876fe9 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -95,69 +95,75 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void) } if ((prev_byte == START_STOP_SPORT) && (_passthrough.new_byte == SENSOR_ID_28)) { // byte 0x7E is the header of each poll request - if (_passthrough.send_attiandrng) { // skip other data, send attitude (roll, pitch) and range only this iteration - _passthrough.send_attiandrng = false; // next iteration, check if we should send something other - } else { // send other sensor data if it's time for them, and reset the corresponding timer if sent - _passthrough.send_attiandrng = true; // next iteration, send attitude b/c it needs frequent updates to remain smooth - uint32_t now = AP_HAL::millis(); - if ((now - _passthrough.params_timer) >= 1000) { - send_uint32(DIY_FIRST_ID+7, calc_param()); - _passthrough.params_timer = AP_HAL::millis(); - return; + if (_passthrough.send_chunk) { // skip other data and send a message chunk during this iteration + _passthrough.send_chunk = false; + if (get_next_msg_chunk()) { + send_uint32(DIY_FIRST_ID, _msg_chunk.chunk); } + } else { // build message queue for sensor_status_flags check_sensor_status_flags(); // build message queue for ekf_status check_ekf_status(); - // if there's any message in the queue, start sending them chunk by chunk; three times each chunk - if (get_next_msg_chunk()) { - send_uint32(DIY_FIRST_ID, _msg_chunk.chunk); - return; + // if there are pending messages in the queue, send them during next iteration + if (!_statustext_queue.empty()) { + _passthrough.send_chunk = true; } - if ((now - _passthrough.ap_status_timer) >= 500) { - if (gcs().vehicle_initialised()) { // send ap status only once vehicle has been initialised - send_uint32(DIY_FIRST_ID+1, calc_ap_status()); - _passthrough.ap_status_timer = AP_HAL::millis(); + if (_passthrough.send_attiandrng) { // skip other data, send attitude (roll, pitch) and range only this iteration + _passthrough.send_attiandrng = false; // next iteration, check if we should send something other + } else { // send other sensor data if it's time for them, and reset the corresponding timer if sent + _passthrough.send_attiandrng = true; // next iteration, send attitude b/c it needs frequent updates to remain smooth + uint32_t now = AP_HAL::millis(); + if ((now - _passthrough.params_timer) >= 1000) { + send_uint32(DIY_FIRST_ID+7, calc_param()); + _passthrough.params_timer = AP_HAL::millis(); return; } - } - if ((now - _passthrough.batt_timer) >= 1000) { - send_uint32(DIY_FIRST_ID+3, calc_batt(0)); - _passthrough.batt_timer = AP_HAL::millis(); - return; - } - if (AP::battery().num_instances() > 1) { - if ((now - _passthrough.batt_timer2) >= 1000) { - send_uint32(DIY_FIRST_ID+8, calc_batt(1)); - _passthrough.batt_timer2 = AP_HAL::millis(); + if ((now - _passthrough.ap_status_timer) >= 500) { + if (gcs().vehicle_initialised()) { // send ap status only once vehicle has been initialised + send_uint32(DIY_FIRST_ID+1, calc_ap_status()); + _passthrough.ap_status_timer = AP_HAL::millis(); + return; + } + } + if ((now - _passthrough.batt_timer) >= 1000) { + send_uint32(DIY_FIRST_ID+3, calc_batt(0)); + _passthrough.batt_timer = AP_HAL::millis(); return; } - } - if ((now - _passthrough.gps_status_timer) >= 1000) { - send_uint32(DIY_FIRST_ID+2, calc_gps_status()); - _passthrough.gps_status_timer = AP_HAL::millis(); - return; - } - if ((now - _passthrough.home_timer) >= 500) { - send_uint32(DIY_FIRST_ID+4, calc_home()); - _passthrough.home_timer = AP_HAL::millis(); - return; - } - if ((now - _passthrough.velandyaw_timer) >= 500) { - send_uint32(DIY_FIRST_ID+5, calc_velandyaw()); - _passthrough.velandyaw_timer = AP_HAL::millis(); - return; - } - if ((now - _passthrough.gps_latlng_timer) >= 1000) { - send_uint32(GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude - if (!_passthrough.send_latitude) { // we've cycled and sent one each of longitude then latitude, so reset the timer - _passthrough.gps_latlng_timer = AP_HAL::millis(); + if (AP::battery().num_instances() > 1) { + if ((now - _passthrough.batt_timer2) >= 1000) { + send_uint32(DIY_FIRST_ID+8, calc_batt(1)); + _passthrough.batt_timer2 = AP_HAL::millis(); + return; + } + } + if ((now - _passthrough.gps_status_timer) >= 1000) { + send_uint32(DIY_FIRST_ID+2, calc_gps_status()); + _passthrough.gps_status_timer = AP_HAL::millis(); + return; + } + if ((now - _passthrough.home_timer) >= 500) { + send_uint32(DIY_FIRST_ID+4, calc_home()); + _passthrough.home_timer = AP_HAL::millis(); + return; + } + if ((now - _passthrough.velandyaw_timer) >= 500) { + send_uint32(DIY_FIRST_ID+5, calc_velandyaw()); + _passthrough.velandyaw_timer = AP_HAL::millis(); + return; + } + if ((now - _passthrough.gps_latlng_timer) >= 1000) { + send_uint32(GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude + if (!_passthrough.send_latitude) { // we've cycled and sent one each of longitude then latitude, so reset the timer + _passthrough.gps_latlng_timer = AP_HAL::millis(); + } + return; } - return; } + // if nothing else needed to be sent, send attitude (roll, pitch) and range data + send_uint32(DIY_FIRST_ID+6, calc_attiandrng()); } - // if nothing else needed to be sent, send attitude (roll, pitch) and range data - send_uint32(DIY_FIRST_ID+6, calc_attiandrng()); } } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 912edcafe2..64e855e3d7 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -162,6 +162,7 @@ private: uint8_t new_byte; bool send_attiandrng; bool send_latitude; + bool send_chunk; uint32_t params_timer; uint32_t ap_status_timer; uint32_t batt_timer;