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

AP_NavEKF2: Prevent output of NaN's in range beacon debug output 

This can happen if this accessor function is called before the EKF states are initialised
This commit is contained in:
priseborough 2016-11-28 20:00:05 +11:00 committed by Randy Mackay
parent fbecda7eeb
commit 0ec8f0932a
1 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -73,9 +73,27 @@ void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call
void NavEKF2_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
{
// if the states have not been initialised or we have not received any beacon updates then return zeros
if (!statesInitialised || N_beacons == 0) {
ID = 0;
rng = 0.0f;
innov = 0.0f;
innovVar = 0.0f;
testRatio = 0.0f;
Vector3f temp;
temp.zero();
beaconPosNED = temp;
offsetHigh = 0.0f;
offsetLow = 0.0f;
return;
}
// Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
if (rngBcnFuseDataReportIndex >= N_beacons) {
rngBcnFuseDataReportIndex = 0;
}
// Output the fusion status data for the specified beacon
ID = rngBcnFuseDataReportIndex; // beacon identifier
rng = rngBcnFusionReport[rngBcnFuseDataReportIndex].rng; // measured range to beacon (m)
innov = rngBcnFusionReport[rngBcnFuseDataReportIndex].innov; // range innovation (m)