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

AP_NavEKF3: pass NavEKF failures back up to callers

This commit is contained in:
Peter Barker 2021-07-19 21:27:38 +10:00 committed by Andrew Tridgell
parent 9f03f5a9a9
commit 156a220d5b
4 changed files with 22 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1422,21 +1422,25 @@ void NavEKF3::getQuaternion(int8_t instance, Quaternion &quat) const
}
// return the innovations for the specified instance
void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
bool NavEKF3::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 NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
bool NavEKF3::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);
}
// get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance

4
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -174,11 +174,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;
// get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance
// returns true on success and results are placed in innovations and variances arguments

9
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -405,7 +405,7 @@ uint8_t NavEKF3_core::getActiveAirspeed() const
}
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
void NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
bool NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
{
velInnov.x = innovVelPos[0];
velInnov.y = innovVelPos[1];
@ -418,6 +418,7 @@ void NavEKF3_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 synthetic air data drag and sideslip innovations
@ -433,7 +434,7 @@ void NavEKF3_core::getSynthAirDataInnovations(Vector2f &dragInnov, float &betaIn
// 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 NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
bool NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
velVar = sqrtF(velTestRatio);
posVar = sqrtF(posTestRatio);
@ -444,6 +445,8 @@ void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve
magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtF(tasTestRatio);
offset = posResetNE.tofloat();
return true;
}
// get a particular source's velocity innovations
@ -550,7 +553,7 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_core.h
View File

@ -209,13 +209,13 @@ 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 synthetic air data drag and sideslip innovations
void getSynthAirDataInnovations(Vector2f &dragInnov, float &betaInnov) 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;
// get a particular source's velocity innovations
// returns true on success and results are placed in innovations and variances arguments