diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index d5d33f09b1..52dae7c7f5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 3a451f77f8..882a276e00 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 10d8b2a18b..79f4e0bd6f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -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); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 2ce09595a1..2a89f1a5ab 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -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