Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: pass NavEKF failures back up to callers

Browse Source
This commit is contained in:
Peter Barker 2021-07-19 21:27:37 +10:00 committed by Andrew Tridgell
parent 141e2aae91
commit b5f165ce2e
4 changed files with 24 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1189,21 +1189,26 @@ void NavEKF2::getQuaternion(int8_t instance, Quaternion &quat) const
}
// return the innovations for the specified instance
void NavEKF2::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
bool NavEKF2::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
if (core == nullptr) {
return false;
}
if (instance < 0 || instance >= num_cores) instance = primary;
return core[instance].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void NavEKF2::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
bool NavEKF2::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
if (core == nullptr) {
return false;
}
if (instance < 0 || instance >= num_cores) instance = primary;
return core[instance].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
}
// should we use the compass? This is public so it can be used for

4
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -173,11 +173,11 @@ public:
// return the innovations for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
bool getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
// return the innovation consistency test ratios for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
bool getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()

10
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -399,7 +399,7 @@ bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
}
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
void NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
bool NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
{
velInnov.x = innovVelPos[0];
velInnov.y = innovVelPos[1];
@ -412,12 +412,14 @@ void NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vecto
magInnov.z = 1e3f*innovMag[2]; // Convert back to sensor units
tasInnov = innovVtas;
yawInnov = innovYaw;
return true;
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
// this indicates the amount of margin available when tuning the various error traps
// also return the delta in position due to the last position reset
void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
bool NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
velVar = sqrtF(velTestRatio);
posVar = sqrtF(posTestRatio);
@ -428,6 +430,8 @@ void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve
magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtF(tasTestRatio);
offset = posResetNE.tofloat();
return true;
}
@ -532,7 +536,7 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) const
}
// get variances
float velVar, posVar, hgtVar, tasVar;
float velVar = 0, posVar = 0, hgtVar = 0, tasVar = 0;
Vector3f magVar;
Vector2f offset;
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -184,10 +184,10 @@ public:
void getQuaternion(Quaternion &quat) const;
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
void getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
bool getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
bool getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()