zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF3: pass NavEKF failures back up to callers

gps-1.3.1
Peter Barker 4 years ago committed by Andrew Tridgell
parent
b5f165ce2e
commit
795f412264
4 changed files with 22 additions and 15 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 4
      libraries/AP_NavEKF3/AP_NavEKF3.h
  3. 9
      libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
  4. 4
      libraries/AP_NavEKF3/AP_NavEKF3_core.h

20
libraries/AP_NavEKF3/AP_NavEKF3.cpp
Unescape View File

@ -1422,21 +1422,25 @@ void NavEKF3::getQuaternion(int8_t instance, Quaternion &quat) const @@ -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
Unescape View File

@ -174,11 +174,11 @@ public: @@ -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
Unescape View File

@ -405,7 +405,7 @@ uint8_t NavEKF3_core::getActiveAirspeed() const @@ -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 @@ -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 @@ -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 @@ -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 @@ -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
Unescape View File

@ -209,13 +209,13 @@ public: @@ -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

Write Preview
Loading…
Cancel
Save