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

AP_NavEKF2: getRangeBeaconDebug returns false on failure to get beacon data

This commit is contained in:
Randy Mackay 2016-11-30 15:54:55 +09:00
parent 0ec8f0932a
commit 66a9093cd0
4 changed files with 11 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1077,11 +1077,13 @@ void NavEKF2::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl
}
// return data for debugging range beacon fusion
void NavEKF2::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
bool NavEKF2::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow);
return core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow);
} else {
return false;
}
}

3
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -216,8 +216,9 @@ public:
innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m)
returns true if data could be found, false if it could not
*/
void getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect

17
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -71,21 +71,11 @@ 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)
bool 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;
return false;
}
// Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
@ -100,9 +90,10 @@ void NavEKF2_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar; // innovation variance (m^2)
testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio; // innovation consistency test ratio
beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon NED position
offsetHigh = bcnPosOffsetMax; // beacon system vertical pos offset upper estimate
offsetHigh = bcnPosOffsetMax; // beacon system vertical pos offset upper estimate
offsetLow = bcnPosOffsetMin; // beacon system vertical pos offset lower estimate
rngBcnFuseDataReportIndex++;
return true;
}
// provides the height limit to be observed by the control loops

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -203,7 +203,7 @@ public:
testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m)
*/
void getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect