diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 098de552ff..c2e6ef750e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -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; } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index fac9c34e9f..6dd151c27b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 5d5750e82e..a6c26ad6eb 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index aed96e91cf..87edf0f0c0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -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