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

AP_NavEKF2: remove instance id from EK2 external interface 

Removes passing of instance id in interfaces where -1 was the only value
ever passed in
apm_2208
Peter Barker 3 years ago committed by Andrew Tridgell
parent
c1c18331f9
commit
04a64a20fa
2 changed files with 61 additions and 99 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 102
      libraries/AP_NavEKF2/AP_NavEKF2.cpp
  2. 58
      libraries/AP_NavEKF2/AP_NavEKF2.h

102
libraries/AP_NavEKF2/AP_NavEKF2.cpp
Unescape View File

@ -913,74 +913,59 @@ int8_t NavEKF2::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 NavEKF2::getPosNE(int8_t instance, Vector2f &posNE) const bool NavEKF2::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 NavEKF2::getPosD(int8_t instance, float &posD) const bool NavEKF2::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 NavEKF2::getVelNED(int8_t instance, Vector3f &vel) const void NavEKF2::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 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
// returns false if estimate is unavailable // returns false if estimate is unavailable
bool NavEKF2::getAirSpdVec(int8_t instance, Vector3f &vel) const bool NavEKF2::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 NavEKF2::getPosDownDerivative(int8_t instance) const float NavEKF2::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;
} }
// return body axis gyro bias estimates in rad/sec // return body axis gyro bias estimates in rad/sec
void NavEKF2::getGyroBias(int8_t instance, Vector3f &gyroBias) const void NavEKF2::getGyroBias(Vector3f &gyroBias) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getGyroBias(gyroBias); core[primary].getGyroBias(gyroBias);
}
}
// return body axis gyro scale factor error as a percentage
void NavEKF2::getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getGyroScaleErrorPercentage(gyroScale);
} }
} }
@ -1031,38 +1016,34 @@ void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
} }
// return the individual Z-accel bias estimates in m/s^2 // return the individual Z-accel bias estimates in m/s^2
void NavEKF2::getAccelZBias(int8_t instance, float &zbias) const void NavEKF2::getAccelZBias(float &zbias) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getAccelZBias(zbias); core[primary].getAccelZBias(zbias);
} }
} }
// 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)
void NavEKF2::getWind(int8_t instance, Vector3f &wind) const void NavEKF2::getWind(Vector3f &wind) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getWind(wind); core[primary].getWind(wind);
} }
} }
// return earth magnetic field estimates in measurement units / 1000 // return earth magnetic field estimates in measurement units / 1000
void NavEKF2::getMagNED(int8_t instance, Vector3f &magNED) const void NavEKF2::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 NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ) const void NavEKF2::getMagXYZ(Vector3f &magXYZ) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getMagXYZ(magXYZ); core[primary].getMagXYZ(magXYZ);
} }
} }
@ -1101,13 +1082,12 @@ bool NavEKF2::getLLH(struct Location &loc) const
// 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
// 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 NavEKF2::getOriginLLH(int8_t instance, struct Location &loc) const bool NavEKF2::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
@ -1148,11 +1128,10 @@ bool NavEKF2::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 for the specified instance
void NavEKF2::getEulerAngles(int8_t instance, Vector3f &eulers) const void NavEKF2::getEulerAngles(Vector3f &eulers) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getEulerAngles(eulers); core[primary].getEulerAngles(eulers);
} }
} }
@ -1176,35 +1155,31 @@ void NavEKF2::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 NavEKF2::getQuaternion(int8_t instance, Quaternion &quat) const void NavEKF2::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 for the specified instance
bool NavEKF2::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const bool NavEKF2::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[primary].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
return core[instance].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 NavEKF2::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const bool NavEKF2::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);
} }
// 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
@ -1265,11 +1240,10 @@ void NavEKF2::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 NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults) const void NavEKF2::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;
} }
@ -1278,11 +1252,10 @@ void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults) const
/* /*
return filter status flags return filter status flags
*/ */
void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status) const void NavEKF2::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));
} }
@ -1291,11 +1264,10 @@ void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status) const
/* /*
return filter gps quality check status return filter gps quality check status
*/ */
void NavEKF2::getFilterGpsStatus(int8_t instance, nav_gps_status &status) const void NavEKF2::getFilterGpsStatus(nav_gps_status &status) const
{ {
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) { if (core) {
core[instance].getFilterGpsStatus(status); core[primary].getFilterGpsStatus(status);
} else { } else {
memset(&status, 0, sizeof(status)); memset(&status, 0, sizeof(status));
} }
@ -1601,7 +1573,7 @@ void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt
} }
} }
// get a yaw estimator instance // get yaw estimator instance
const EKFGSF_yaw *NavEKF2::get_yawEstimator(void) const const EKFGSF_yaw *NavEKF2::get_yawEstimator(void) const
{ {
if (core) { if (core) {

58
libraries/AP_NavEKF2/AP_NavEKF2.h
Unescape View File

@ -68,40 +68,30 @@ 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
// An out of range instance (eg -1) returns data for the primary instance
void getGyroBias(int8_t instance, Vector3f &gyroBias) const;
// return body axis gyro scale factor error as a percentage for the specified instance // return body axis gyro bias estimates in rad/sec
// An out of range instance (eg -1) returns data for the primary instance void getGyroBias(Vector3f &gyroBias) const;
void getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) const;
// reset body axis gyro bias estimates // reset body axis gyro bias estimates
void resetGyroBias(void); void resetGyroBias(void);
@ -119,19 +109,19 @@ public:
// return the Z-accel bias estimate in m/s^2 for the specified instance // return the Z-accel bias estimate in m/s^2 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
void getAccelZBias(int8_t instance, float &zbias) const; void getAccelZBias(float &zbias) 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 moving in the direction of the axis)
// 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
void getWind(int8_t instance, Vector3f &wind) const; void 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 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
void getMagNED(int8_t instance, Vector3f &magNED) const; void getMagNED(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 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
void getMagXYZ(int8_t instance, Vector3f &magXYZ) const; void getMagXYZ(Vector3f &magXYZ) const;
// Return estimated magnetometer offsets // Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid // Return true if magnetometer offsets are valid
@ -147,7 +137,7 @@ public:
// 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
// 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
@ -161,7 +151,7 @@ public:
// return the Euler roll, pitch and yaw angle in radians for the specified instance // return the Euler roll, pitch and yaw angle in radians 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
void getEulerAngles(int8_t instance, Vector3f &eulers) const; void getEulerAngles(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;
@ -170,15 +160,15 @@ 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 autopilot axes // return the quaternions defining the rotation from NED to 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 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
bool getInnovations(int8_t index, 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 specified instance // return the innovation consistency test ratios 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
bool getVariances(int8_t instance, 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 // should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass() // reporting via ahrs.use_compass()
@ -211,19 +201,19 @@ 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 gps quality check status for the specified instance return filter gps quality check status 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
*/ */
void getFilterGpsStatus(int8_t instance, nav_gps_status &faults) const; void getFilterGpsStatus(nav_gps_status &faults) const;
/* /*
return filter status flags for the specified instance return filter status flags 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
*/ */
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;

Write Preview
Loading…
Cancel
Save