diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index b5d5431b89..ab6e8f6d01 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 6fec6576cb..27c03ebd2c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -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() diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index d7716e85e3..21bcd3eb8b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -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); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 3ff0952b34..1b36283990 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -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()