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 // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { 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) innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m) 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 // called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect // causes the EKF to compensate for expected barometer errors due to ground effect

15
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 // 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 the states have not been initialised or we have not received any beacon updates then return zeros
if (!statesInitialised || N_beacons == 0) { if (!statesInitialised || N_beacons == 0) {
ID = 0; return false;
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 // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
@ -103,6 +93,7 @@ void NavEKF2_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
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 offsetLow = bcnPosOffsetMin; // beacon system vertical pos offset lower estimate
rngBcnFuseDataReportIndex++; rngBcnFuseDataReportIndex++;
return true;
} }
// provides the height limit to be observed by the control loops // 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 testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m) 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 // called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect // causes the EKF to compensate for expected barometer errors due to ground effect