From fed8ecc3b0bb22d035ec1530c9d73aa31a04bac1 Mon Sep 17 00:00:00 2001 From: yaapu Date: Sun, 8 Mar 2020 15:29:08 -0700 Subject: [PATCH] AP_Frsky_Telem: fix for protocol=4 GAlt=0 and GSPd=0 This fixes protocol 4 GPS altitude and speed error, both were reported as constant 0 for cal_gps_position() was never called --- libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 28 +++++++-------------- libraries/AP_Frsky_Telem/AP_Frsky_Telem.h | 3 ++- 2 files changed, 11 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 091e78a275..b8102dabec 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -294,17 +294,13 @@ void AP_Frsky_Telem::send_SPort(void) if (numc == 0) { // no serial data to process do bg tasks - switch (_SPort.next_sensor_id) { - case SENSOR_ID_VARIO: - calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent - break; - case SENSOR_ID_FAS: - break; - case SENSOR_ID_GPS: - calc_gps_position(); // gps data is not recalculated until all of it has been sent - break; - case SENSOR_ID_SP2UR: - break; + if (_SPort.vario_refresh) { + calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent + _SPort.vario_refresh = false; + } + if (_SPort.gps_refresh) { + calc_gps_position(); // gps data is not recalculated until all of it has been sent + _SPort.gps_refresh = false; } return; } @@ -328,8 +324,7 @@ void AP_Frsky_Telem::send_SPort(void) break; case 2: send_uint32(SPORT_DATA_FRAME, DATA_ID_VARIO, _SPort_data.vario_vspd); // send vspeed m/s - // update vfas data in next idle serial port loop - _SPort.next_sensor_id = SENSOR_ID_FAS; + _SPort.vario_refresh = true; break; } if (++_SPort.vario_call > 2) { @@ -353,8 +348,6 @@ void AP_Frsky_Telem::send_SPort(void) send_uint32(SPORT_DATA_FRAME, DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption break; } - // update gps data in next idle serial port loop - _SPort.next_sensor_id = SENSOR_ID_GPS; break; } if (++_SPort.fas_call > 2) { @@ -383,8 +376,7 @@ void AP_Frsky_Telem::send_SPort(void) break; case 6: send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_COURS_BP, _SPort_data.yaw); // send heading in degree based on AHRS and not GPS - // update SPUR data in next idle serial port loop - _SPort.next_sensor_id = SENSOR_ID_SP2UR; + _SPort.gps_refresh = true; break; } if (++_SPort.gps_call > 6) { @@ -398,8 +390,6 @@ void AP_Frsky_Telem::send_SPort(void) break; case 1: send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode - // update vario data in next idle serial port loop - _SPort.next_sensor_id = SENSOR_ID_VARIO; break; } if (++_SPort.various_call > 1) { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index cccdbcb55d..dbd6265cc7 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -206,11 +206,12 @@ private: struct { bool sport_status; + bool gps_refresh; + bool vario_refresh; uint8_t fas_call; uint8_t gps_call; uint8_t vario_call; uint8_t various_call; - uint8_t next_sensor_id; } _SPort; struct