aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: remove instance id from EK3 external interface

Browse Source 
Removes passing of instance id in interfaces where -1 was the only value
ever passed in
This commit is contained in:
Peter Barker 2022-04-05 13:33:33 +10:00 committed by Andrew Tridgell
parent b762aac6ce
commit adf9c21d48
2 changed files with 83 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1154,55 +1154,49 @@ int8_t NavEKF3::getPrimaryCoreIMUIndex(void) const
// Write the last calculated NE position relative to the reference point (m). // Write the last calculated NE position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false // If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // If false returned, do not use for flight control
bool NavEKF3::getPosNE(int8_t instance, Vector2f &posNE) const bool NavEKF3::getPosNE(Vector2f &posNE) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (!core) { if (!core) {
return false; return false;
} }
return core[instance].getPosNE(posNE); return core[primary].getPosNE(posNE);
} }
// Write the last calculated D position relative to the reference point (m). // Write the last calculated D position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false // If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // If false returned, do not use for flight control
bool NavEKF3::getPosD(int8_t instance, float &posD) const bool NavEKF3::getPosD(float &posD) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (!core) { if (!core) {
return false; return false;
} }
return core[instance].getPosD(posD); return core[primary].getPosD(posD);
} }
// return NED velocity in m/s // return NED velocity in m/s
void NavEKF3::getVelNED(int8_t instance, Vector3f &vel) const void NavEKF3::getVelNED(Vector3f &vel) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getVelNED(vel); core[primary].getVelNED(vel);
} }
} }
// return estimate of true airspeed vector in body frame in m/s for the specified instance // return estimate of true airspeed vector in body frame in m/s
// An out of range instance (eg -1) returns data for the primary instance
// returns false if estimate is unavailable // returns false if estimate is unavailable
bool NavEKF3::getAirSpdVec(int8_t instance, Vector3f &vel) const bool NavEKF3::getAirSpdVec(Vector3f &vel) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
return core[instance].getAirSpdVec(vel); return core[primary].getAirSpdVec(vel);
} }
return false; return false;
} }
// Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
float NavEKF3::getPosDownDerivative(int8_t instance) const float NavEKF3::getPosDownDerivative() const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
// return the value calculated from a complementary filter applied to the EKF height and vertical acceleration // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
if (core) { if (core) {
return core[instance].getPosDownDerivative(); return core[primary].getPosDownDerivative();
} }
return 0.0f; return 0.0f;
} }
@ -1273,40 +1267,35 @@ void NavEKF3::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
// returns true if wind state estimation is active // returns true if wind state estimation is active
bool NavEKF3::getWind(int8_t instance, Vector3f &wind) const bool NavEKF3::getWind(Vector3f &wind) const
{ {
bool ret = false; if (core == nullptr) {
if (instance < 0 || instance >= num_cores) instance = primary; return false;
if (core) {
ret = core[instance].getWind(wind);
} }
return ret; return core[primary].getWind(wind);
} }
// return earth magnetic field estimates in measurement units / 1000 // return earth magnetic field estimates in measurement units / 1000
void NavEKF3::getMagNED(int8_t instance, Vector3f &magNED) const void NavEKF3::getMagNED(Vector3f &magNED) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getMagNED(magNED); core[primary].getMagNED(magNED);
} }
} }
// return body magnetic field estimates in measurement units / 1000 // return body magnetic field estimates in measurement units / 1000
void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ) const void NavEKF3::getMagXYZ(Vector3f &magXYZ) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getMagXYZ(magXYZ); core[primary].getMagXYZ(magXYZ);
} }
} }
// return the airspeed sensor in use for the specified instance // return the airspeed sensor in use
uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const uint8_t NavEKF3::getActiveAirspeed() const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
return core[instance].getActiveAirspeed(); return core[primary].getActiveAirspeed();
} else { } else {
return 255; return 255;
} }
@ -1343,17 +1332,15 @@ bool NavEKF3::getLLH(struct Location &loc) const
return core[primary].getLLH(loc); return core[primary].getLLH(loc);
} }
// Return the latitude and longitude and height used to set the NED origin for the specified instance // Return the latitude and longitude and height used to set the NED origin
// An out of range instance (eg -1) returns data for the primary instance
// All NED positions calculated by the filter are relative to this location // All NED positions calculated by the filter are relative to this location
// Returns false if the origin has not been set // Returns false if the origin has not been set
bool NavEKF3::getOriginLLH(int8_t instance, struct Location &loc) const bool NavEKF3::getOriginLLH(struct Location &loc) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (!core) { if (!core) {
return false; return false;
} }
return core[instance].getOriginLLH(loc); return core[primary].getOriginLLH(loc);
} }
// set the latitude and longitude and height used to set the NED origin // set the latitude and longitude and height used to set the NED origin
@ -1393,12 +1380,11 @@ bool NavEKF3::getHAGL(float &HAGL) const
return core[primary].getHAGL(HAGL); return core[primary].getHAGL(HAGL);
} }
// return the Euler roll, pitch and yaw angle in radians for the specified instance // return the Euler roll, pitch and yaw angle in radians
void NavEKF3::getEulerAngles(int8_t instance, Vector3f &eulers) const void NavEKF3::getEulerAngles(Vector3f &eulers) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getEulerAngles(eulers); core[primary].getEulerAngles(eulers);
} }
} }
@ -1422,45 +1408,39 @@ void NavEKF3::getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const
} }
// return the quaternions defining the rotation from NED to XYZ (autopilot) axes // return the quaternions defining the rotation from NED to XYZ (autopilot) axes
void NavEKF3::getQuaternion(int8_t instance, Quaternion &quat) const void NavEKF3::getQuaternion(Quaternion &quat) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getQuaternion(quat); core[primary].getQuaternion(quat);
} }
} }
// return the innovations for the specified instance // return the innovations
bool NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const bool NavEKF3::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
{ {
if (core == nullptr) { if (core == nullptr) {
return false; return false;
} }
if (instance < 0 || instance >= num_cores) instance = primary;
return core[instance].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); return core[primary].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
} }
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
bool NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const bool NavEKF3::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{ {
if (core == nullptr) { if (core == nullptr) {
return false; return false;
} }
if (instance < 0 || instance >= num_cores) instance = primary;
return core[instance].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); return core[primary].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 // get a source's velocity innovations
// returns true on success and results are placed in innovations and variances arguments // returns true on success and results are placed in innovations and variances arguments
bool NavEKF3::getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const bool NavEKF3::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
{ {
if (instance < 0 || instance >= num_cores) {
instance = primary;
}
if (core) { if (core) {
return core[instance].getVelInnovationsAndVariancesForSource(source, innovations, variances); return core[primary].getVelInnovationsAndVariancesForSource(source, innovations, variances);
} }
return false; return false;
} }
@ -1754,11 +1734,10 @@ void NavEKF3::setTerrainHgtStable(bool val)
6 = badly conditioned synthetic sideslip fusion 6 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised 7 = filter is not initialised
*/ */
void NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults) const void NavEKF3::getFilterFaults(uint16_t &faults) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getFilterFaults(faults); core[primary].getFilterFaults(faults);
} else { } else {
faults = 0; faults = 0;
} }
@ -1767,11 +1746,10 @@ void NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults) const
/* /*
return filter status flags return filter status flags
*/ */
void NavEKF3::getFilterStatus(int8_t instance, nav_filter_status &status) const void NavEKF3::getFilterStatus(nav_filter_status &status) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getFilterStatus(status); core[primary].getFilterStatus(status);
} else { } else {
memset(&status, 0, sizeof(status)); memset(&status, 0, sizeof(status));
} }
@ -2019,14 +1997,11 @@ bool NavEKF3::yawAlignmentComplete(void) const
return core[primary].have_aligned_yaw(); return core[primary].have_aligned_yaw();
} }
// returns true when the state estimates for the selected core are significantly degraded by vibration // returns true when the state estimates are significantly degraded by vibration
bool NavEKF3::isVibrationAffected(int8_t instance) const bool NavEKF3::isVibrationAffected() const
{ {
if (instance < 0 || instance >= num_cores) {
instance = primary;
}
if (core) { if (core) {
return core[instance].isVibrationAffected(); return core[primary].isVibrationAffected();
} }
return false; return false;
} }

90
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -68,32 +68,27 @@ public:
// return -1 if no primary core selected // return -1 if no primary core selected
int8_t getPrimaryCoreIMUIndex(void) const; int8_t getPrimaryCoreIMUIndex(void) const;
// Write the last calculated NE position relative to the reference point (m) for the specified instance. // Write the last calculated NE position relative to the reference point (m)
// An out of range instance (eg -1) returns data for the primary instance
// If a calculated solution is not available, use the best available data and return false // If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // If false returned, do not use for flight control
bool getPosNE(int8_t instance, Vector2f &posNE) const; bool getPosNE(Vector2f &posNE) const;
// Write the last calculated D position relative to the reference point (m) for the specified instance. // Write the last calculated D position relative to the reference point (m)
// An out of range instance (eg -1) returns data for the primary instance
// If a calculated solution is not available, use the best available data and return false // If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // If false returned, do not use for flight control
bool getPosD(int8_t instance, float &posD) const; bool getPosD(float &posD) const;
// return NED velocity in m/s for the specified instance // return NED velocity in m/s
// An out of range instance (eg -1) returns data for the primary instance void getVelNED(Vector3f &vel) const;
void getVelNED(int8_t instance, Vector3f &vel) const;
// return estimate of true airspeed vector in body frame in m/s for the specified instance // return estimate of true airspeed vector in body frame in m/s
// An out of range instance (eg -1) returns data for the primary instance
// returns false if estimate is unavailable // returns false if estimate is unavailable
bool getAirSpdVec(int8_t instance, Vector3f &vel) const; bool getAirSpdVec(Vector3f &vel) const;
// Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s for the specified instance // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
// An out of range instance (eg -1) returns data for the primary instance
// This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
// but will always be kinematically consistent with the z component of the EKF position state // but will always be kinematically consistent with the z component of the EKF position state
float getPosDownDerivative(int8_t instance) const; float getPosDownDerivative() const;
// return body axis gyro bias estimates in rad/sec for the specified instance // return body axis gyro bias estimates in rad/sec for the specified instance
// An out of range instance (eg -1) returns data for the primary instance // An out of range instance (eg -1) returns data for the primary instance
@ -117,22 +112,19 @@ public:
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) // return the NED wind speed estimates in m/s (positive is air
// An out of range instance (eg -1) returns data for the the primary instance // moving in the direction of the axis) returns true if wind state
// returns true if wind state estimation is active // estimation is active
bool getWind(int8_t instance, Vector3f &wind) const; bool getWind(Vector3f &wind) const;
// return earth magnetic field estimates in measurement units / 1000 for the specified instance // return earth magnetic field estimates in measurement units / 1000
// An out of range instance (eg -1) returns data for the primary instance void getMagNED(Vector3f &magNED) const;
void getMagNED(int8_t instance, Vector3f &magNED) const;
// return body magnetic field estimates in measurement units / 1000 for the specified instance // return body magnetic field estimates in measurement units / 1000
// An out of range instance (eg -1) returns data for the primary instance void getMagXYZ(Vector3f &magXYZ) const;
void getMagXYZ(int8_t instance, Vector3f &magXYZ) const;
// return the sensor in use for the specified instance // return the airspeed sensor in use
// An out of range instance (eg -1) returns data for the primary instance uint8_t getActiveAirspeed() const;
uint8_t getActiveAirspeed(int8_t instance) const;
// Return estimated magnetometer offsets // Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid // Return true if magnetometer offsets are valid
@ -144,11 +136,10 @@ public:
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
bool getLLH(struct Location &loc) const; bool getLLH(struct Location &loc) const;
// Return the latitude and longitude and height used to set the NED origin for the specified instance // Return the latitude and longitude and height used to set the NED origin
// An out of range instance (eg -1) returns data for the primary instance
// All NED positions calculated by the filter are relative to this location // All NED positions calculated by the filter are relative to this location
// Returns false if the origin has not been set // Returns false if the origin has not been set
bool getOriginLLH(int8_t instance, struct Location &loc) const; bool getOriginLLH(struct Location &loc) const;
// set the latitude and longitude and height used to set the NED origin // set the latitude and longitude and height used to set the NED origin
// All NED positions calculated by the filter will be relative to this location // All NED positions calculated by the filter will be relative to this location
@ -160,9 +151,8 @@ public:
// return false if ground height is not being estimated. // return false if ground height is not being estimated.
bool getHAGL(float &HAGL) const; bool getHAGL(float &HAGL) const;
// return the Euler roll, pitch and yaw angle in radians for the specified instance // return the Euler roll, pitch and yaw angle in radians
// An out of range instance (eg -1) returns data for the primary instance void getEulerAngles(Vector3f &eulers) const;
void getEulerAngles(int8_t instance, Vector3f &eulers) const;
// return the transformation matrix from XYZ (body) to NED axes // return the transformation matrix from XYZ (body) to NED axes
void getRotationBodyToNED(Matrix3f &mat) const; void getRotationBodyToNED(Matrix3f &mat) const;
@ -171,19 +161,17 @@ public:
void getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const; void getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const;
// return the quaternions defining the rotation from NED to XYZ (autopilot) axes // return the quaternions defining the rotation from NED to XYZ (autopilot) axes
void getQuaternion(int8_t instance, Quaternion &quat) const; void getQuaternion(Quaternion &quat) const;
// return the innovations for the specified instance // return the innovations
// An out of range instance (eg -1) returns data for the primary instance bool getInnovations(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 // return the innovation consistency test ratios
// An out of range instance (eg -1) returns data for the primary instance bool getVariances(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 // get a source's velocity innovations
// returns true on success and results are placed in innovations and variances arguments // returns true on success and results are placed in innovations and variances arguments
bool getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED; bool getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
// should we use the compass? This is public so it can be used for // should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass() // reporting via ahrs.use_compass()
@ -271,8 +259,7 @@ public:
void setTerrainHgtStable(bool val); void setTerrainHgtStable(bool val);
/* /*
return the filter fault status as a bitmasked integer for the specified instance return the filter fault status as a bitmasked integer
An out of range instance (eg -1) returns data for the primary instance
0 = quaternions are NaN 0 = quaternions are NaN
1 = velocities are NaN 1 = velocities are NaN
2 = badly conditioned X magnetometer fusion 2 = badly conditioned X magnetometer fusion
@ -282,13 +269,12 @@ public:
6 = badly conditioned synthetic sideslip fusion 6 = badly conditioned synthetic sideslip fusion
7 = filter is not initialised 7 = filter is not initialised
*/ */
void getFilterFaults(int8_t instance, uint16_t &faults) const; void getFilterFaults(uint16_t &faults) const;
/* /*
return filter status flags for the specified instance return filter status flags
An out of range instance (eg -1) returns data for the primary instance
*/ */
void getFilterStatus(int8_t instance, nav_filter_status &status) const; void getFilterStatus(nav_filter_status &status) const;
// send an EKF_STATUS_REPORT message to GCS // send an EKF_STATUS_REPORT message to GCS
void send_status_report(mavlink_channel_t chan) const; void send_status_report(mavlink_channel_t chan) const;
@ -358,9 +344,9 @@ public:
// returns true when the yaw angle has been aligned // returns true when the yaw angle has been aligned
bool yawAlignmentComplete(void) const; bool yawAlignmentComplete(void) const;
// returns true when the state estimates for the selected core are significantly degraded by vibration // returns true when the state estimates are significantly
// if instance < 0, the primary instance will be used // degraded by vibration
bool isVibrationAffected(int8_t instance) const; bool isVibrationAffected() const;
// get a yaw estimator instance // get a yaw estimator instance
const EKFGSF_yaw *get_yawEstimator(void) const; const EKFGSF_yaw *get_yawEstimator(void) const;