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

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

Browse Source 
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)