From e43b2a73da60b0c14d03d2cb97d00801919ff264 Mon Sep 17 00:00:00 2001 From: Florent Martel Date: Fri, 4 Nov 2016 13:09:44 -0500 Subject: [PATCH] AP_Frsky_Telem: increase sending params priority If there's a lot of messages in the queue, params would not be transmitted for a while, until the queue is empty, which can take a bit of time during init. --- libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 31 ++++++++++++--------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index a01d1d22d0..33188a88a4 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -94,8 +94,14 @@ 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 { // check if there's other data to send + } 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) && (!AP_Notify::flags.armed)) { + send_uint32(DIY_FIRST_ID+7, calc_param()); + _passthrough.params_timer = AP_HAL::millis(); + return; + } // build message queue for sensor_status_flags check_sensor_status_flags(); // build message queue for ekf_status @@ -105,35 +111,34 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void) send_uint32(DIY_FIRST_ID, _msg_chunk.chunk); return; } - // send other sensor data if it's time for them, and reset the corresponding timer if sent - uint32_t now = AP_HAL::millis(); - if (((now - _passthrough.params_timer) > 1000) && (!AP_Notify::flags.armed)) { - send_uint32(DIY_FIRST_ID+7, calc_param()); - _passthrough.params_timer = AP_HAL::millis(); - return; - } else if ((now - _passthrough.ap_status_timer) > 500) { + if ((now - _passthrough.ap_status_timer) > 500) { if (((*_ap.valuep) & AP_INITIALIZED_FLAG) > 0) { // 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; - } else if ((now - _passthrough.batt_timer) > 1000) { + } + if ((now - _passthrough.batt_timer) > 1000) { send_uint32(DIY_FIRST_ID+3, calc_batt()); _passthrough.batt_timer = AP_HAL::millis(); return; - } else if ((now - _passthrough.gps_status_timer) > 1000) { + } + if ((now - _passthrough.gps_status_timer) > 1000) { send_uint32(DIY_FIRST_ID+2, calc_gps_status()); _passthrough.gps_status_timer = AP_HAL::millis(); return; - } else if ((now - _passthrough.home_timer) > 500) { + } + if ((now - _passthrough.home_timer) > 500) { send_uint32(DIY_FIRST_ID+4, calc_home()); _passthrough.home_timer = AP_HAL::millis(); return; - } else if ((now - _passthrough.velandyaw_timer) > 500) { + } + if ((now - _passthrough.velandyaw_timer) > 500) { send_uint32(DIY_FIRST_ID+5, calc_velandyaw()); _passthrough.velandyaw_timer = AP_HAL::millis(); return; - } else if ((now - _passthrough.gps_latlng_timer) > 1000) { + } + 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();