Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: remove instance id from EK2 external interface 

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-08 09:47:04 +10:00 committed by Andrew Tridgell
parent c770a2bd58
commit c1c18331f9
1 changed files with 24 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -492,7 +492,7 @@ void AP_AHRS::update_EKF2(void)
if (active_EKF_type() == EKFType::TWO) { if (active_EKF_type() == EKFType::TWO) {
Vector3f eulers; Vector3f eulers;
EKF2.getRotationBodyToNED(_dcm_matrix); EKF2.getRotationBodyToNED(_dcm_matrix);
EKF2.getEulerAngles(-1,eulers); EKF2.getEulerAngles(eulers);
roll = eulers.x; roll = eulers.x;
pitch = eulers.y; pitch = eulers.y;
yaw = eulers.z; yaw = eulers.z;
@ -508,7 +508,7 @@ void AP_AHRS::update_EKF2(void)
// get gyro bias for primary EKF and change sign to give gyro drift // get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth // Note sign convention used by EKF is bias = measurement - truth
_gyro_drift.zero(); _gyro_drift.zero();
EKF2.getGyroBias(-1,_gyro_drift); EKF2.getGyroBias(_gyro_drift);
_gyro_drift = -_gyro_drift; _gyro_drift = -_gyro_drift;
// calculate corrected gyro estimate for get_gyro() // calculate corrected gyro estimate for get_gyro()
@ -522,7 +522,7 @@ void AP_AHRS::update_EKF2(void)
// get z accel bias estimate from active EKF (this is usually for the primary IMU) // get z accel bias estimate from active EKF (this is usually for the primary IMU)
float &abias = _accel_bias.z; float &abias = _accel_bias.z;
EKF2.getAccelZBias(-1,abias); EKF2.getAccelZBias(abias);
// This EKF is currently using primary_imu, and abias applies to only that IMU // This EKF is currently using primary_imu, and abias applies to only that IMU
for (uint8_t i=0; i<_ins.get_accel_count(); i++) { for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
@ -536,7 +536,7 @@ void AP_AHRS::update_EKF2(void)
} }
_accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()]; _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
nav_filter_status filt_state; nav_filter_status filt_state;
EKF2.getFilterStatus(-1,filt_state); EKF2.getFilterStatus(filt_state);
update_notify_from_filter_status(filt_state); update_notify_from_filter_status(filt_state);
} }
} }
@ -822,7 +822,7 @@ Vector3f AP_AHRS::wind_estimate(void) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
EKF2.getWind(-1,wind); EKF2.getWind(wind);
break; break;
#endif #endif
@ -987,7 +987,7 @@ bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const
break; break;
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
return EKF2.getAirSpdVec(-1, vec); return EKF2.getAirSpdVec(vec);
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
@ -1063,7 +1063,7 @@ bool AP_AHRS::get_quaternion(Quaternion &quat) const
if (!_ekf2_started) { if (!_ekf2_started) {
return false; return false;
} }
EKF2.getQuaternion(-1, quat); EKF2.getQuaternion(quat);
break; break;
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
@ -1116,7 +1116,7 @@ bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
// EKF2 is secondary // EKF2 is secondary
EKF2.getEulerAngles(-1, eulers); EKF2.getEulerAngles(eulers);
return _ekf2_started; return _ekf2_started;
#endif #endif
@ -1174,7 +1174,7 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const
if (!_ekf2_started) { if (!_ekf2_started) {
return false; return false;
} }
EKF2.getQuaternion(-1, quat); EKF2.getQuaternion(quat);
break; break;
#endif #endif
@ -1263,7 +1263,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
EKF2.getVelNED(-1,vec); EKF2.getVelNED(vec);
return Vector2f(vec.x, vec.y); return Vector2f(vec.x, vec.y);
#endif #endif
@ -1376,7 +1376,7 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
EKF2.getVelNED(-1,vec); EKF2.getVelNED(vec);
return true; return true;
#endif #endif
@ -1413,7 +1413,7 @@ bool AP_AHRS::get_mag_field_NED(Vector3f &vec) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
EKF2.getMagNED(-1,vec); EKF2.getMagNED(vec);
return true; return true;
#endif #endif
@ -1444,7 +1444,7 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
EKF2.getMagXYZ(-1,vec); EKF2.getMagXYZ(vec);
return true; return true;
#endif #endif
@ -1477,7 +1477,7 @@ bool AP_AHRS::get_vert_pos_rate(float &velocity) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
velocity = EKF2.getPosDownDerivative(-1); velocity = EKF2.getPosDownDerivative();
return true; return true;
#endif #endif
@ -1555,7 +1555,7 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
case EKFType::TWO: { case EKFType::TWO: {
Vector2f posNE; Vector2f posNE;
float posD; float posD;
if (EKF2.getPosNE(-1,posNE) && EKF2.getPosD(-1,posD)) { if (EKF2.getPosNE(posNE) && EKF2.getPosD(posD)) {
// position is valid // position is valid
vec.x = posNE.x; vec.x = posNE.x;
vec.y = posNE.y; vec.y = posNE.y;
@ -1646,7 +1646,7 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: { case EKFType::TWO: {
bool position_is_valid = EKF2.getPosNE(-1,posNE); bool position_is_valid = EKF2.getPosNE(posNE);
return position_is_valid; return position_is_valid;
} }
#endif #endif
@ -1716,7 +1716,7 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: { case EKFType::TWO: {
bool position_is_valid = EKF2.getPosD(-1,posD); bool position_is_valid = EKF2.getPosD(posD);
return position_is_valid; return position_is_valid;
} }
#endif #endif
@ -1841,7 +1841,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
} }
if (always_use_EKF()) { if (always_use_EKF()) {
uint16_t ekf2_faults; uint16_t ekf2_faults;
EKF2.getFilterFaults(-1,ekf2_faults); EKF2.getFilterFaults(ekf2_faults);
if (ekf2_faults == 0) { if (ekf2_faults == 0) {
ret = EKFType::TWO; ret = EKFType::TWO;
} }
@ -1897,7 +1897,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
nav_filter_status filt_state; nav_filter_status filt_state;
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
if (ret == EKFType::TWO) { if (ret == EKFType::TWO) {
EKF2.getFilterStatus(-1,filt_state); EKF2.getFilterStatus(filt_state);
should_use_gps = EKF2.configuredToUseGPSForPosXY(); should_use_gps = EKF2.configuredToUseGPSForPosXY();
} }
#endif #endif
@ -2155,7 +2155,7 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
EKF2.getFilterStatus(-1,status); EKF2.getFilterStatus(status);
return true; return true;
#endif #endif
@ -2345,7 +2345,7 @@ void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
imu_idx = EKF2.getPrimaryCoreIMUIndex(); imu_idx = EKF2.getPrimaryCoreIMUIndex();
EKF2.getAccelZBias(-1,accel_bias.z); EKF2.getAccelZBias(accel_bias.z);
break; break;
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
@ -2703,7 +2703,7 @@ bool AP_AHRS::get_origin(Location &ret) const
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
if (!EKF2.getOriginLLH(-1,ret)) { if (!EKF2.getOriginLLH(ret)) {
return false; return false;
} }
return true; return true;
@ -2811,7 +2811,7 @@ bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
// use EKF to get innovations // use EKF to get innovations
return EKF2.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); return EKF2.getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
@ -2877,7 +2877,7 @@ bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3
case EKFType::TWO: { case EKFType::TWO: {
// use EKF to get variance // use EKF to get variance
Vector2f offset; Vector2f offset;
return EKF2.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); return EKF2.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
} }
#endif #endif