aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Frsky_Telem: fix for protocol=4 GAlt=0 and GSPd=0

Browse Source 
This fixes protocol 4 GPS altitude and speed error, both were reported as constant 0
for cal_gps_position() was never called
This commit is contained in:
yaapu 2020-03-08 15:29:08 -07:00 committed by Peter Barker
parent a3e2e156e7
commit fed8ecc3b0
2 changed files with 11 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -294,17 +294,13 @@ void AP_Frsky_Telem::send_SPort(void)
if (numc == 0) { if (numc == 0) {
// no serial data to process do bg tasks // no serial data to process do bg tasks
switch (_SPort.next_sensor_id) { if (_SPort.vario_refresh) {
case SENSOR_ID_VARIO: calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent
calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent _SPort.vario_refresh = false;
break; }
case SENSOR_ID_FAS: if (_SPort.gps_refresh) {
break; calc_gps_position(); // gps data is not recalculated until all of it has been sent
case SENSOR_ID_GPS: _SPort.gps_refresh = false;
calc_gps_position(); // gps data is not recalculated until all of it has been sent
break;
case SENSOR_ID_SP2UR:
break;
} }
return; return;
} }
@ -328,8 +324,7 @@ void AP_Frsky_Telem::send_SPort(void)
break; break;
case 2: case 2:
send_uint32(SPORT_DATA_FRAME, DATA_ID_VARIO, _SPort_data.vario_vspd); // send vspeed m/s 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.vario_refresh = true;
_SPort.next_sensor_id = SENSOR_ID_FAS;
break; break;
} }
if (++_SPort.vario_call > 2) { 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 send_uint32(SPORT_DATA_FRAME, DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
break; break;
} }
// update gps data in next idle serial port loop
_SPort.next_sensor_id = SENSOR_ID_GPS;
break; break;
} }
if (++_SPort.fas_call > 2) { if (++_SPort.fas_call > 2) {
@ -383,8 +376,7 @@ void AP_Frsky_Telem::send_SPort(void)
break; break;
case 6: 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 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.gps_refresh = true;
_SPort.next_sensor_id = SENSOR_ID_SP2UR;
break; break;
} }
if (++_SPort.gps_call > 6) { if (++_SPort.gps_call > 6) {
@ -398,8 +390,6 @@ void AP_Frsky_Telem::send_SPort(void)
break; break;
case 1: case 1:
send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode 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; break;
} }
if (++_SPort.various_call > 1) { if (++_SPort.various_call > 1) {

3
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -206,11 +206,12 @@ private:
struct struct
{ {
bool sport_status; bool sport_status;
bool gps_refresh;
bool vario_refresh;
uint8_t fas_call; uint8_t fas_call;
uint8_t gps_call; uint8_t gps_call;
uint8_t vario_call; uint8_t vario_call;
uint8_t various_call; uint8_t various_call;
uint8_t next_sensor_id;
} _SPort; } _SPort;
struct struct