Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: SBF: avoid FPE when undulation is DNF

This commit is contained in:
Peter Barker 2024-08-19 13:20:44 +10:00 committed by Peter Barker
parent c9f648b97c
commit 3bde446538
1 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -423,6 +423,15 @@ AP_GPS_SBF::parse(uint8_t temp)
return false;
}
static bool is_DNU(double value)
{
constexpr double DNU = -2e-10f;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values
return value != DNU;
#pragma GCC diagnostic pop
}
bool
AP_GPS_SBF::process_message(void)
{
@ -466,9 +475,13 @@ AP_GPS_SBF::process_message(void)
if (temp.Latitude > -200000) {
state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7);
state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7);
state.have_undulation = true;
state.undulation = -temp.Undulation;
set_alt_amsl_cm(state, ((float)temp.Height - temp.Undulation) * 1e2f);
state.have_undulation = !is_DNU(temp.Undulation);
double height = temp.Height; // in metres
if (state.have_undulation) {
height -= temp.Undulation;
state.undulation = -temp.Undulation;
}
set_alt_amsl_cm(state, (float)height * 1e2f); // m -> cm
}
state.num_sats = temp.NrSV;