Skip to content

Commit

Permalink
AP_AHRS: add and use active_estimates pointer
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Dec 23, 2024
1 parent 85cd79f commit 8251d21
Show file tree
Hide file tree
Showing 2 changed files with 79 additions and 117 deletions.
195 changes: 78 additions & 117 deletions libraries/AP_AHRS/AP_AHRS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -247,8 +247,6 @@ void AP_AHRS::init()
}
#endif

last_active_ekf_type = (EKFType)_ekf_type.get();

// init backends
#if AP_AHRS_DCM_ENABLED
dcm.init();
Expand Down Expand Up @@ -452,16 +450,19 @@ void AP_AHRS::update(bool skip_ins_update)
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
shortname = "DCM";
active_estimates = &dcm_estimates;
break;
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
shortname = "SIM";
active_estimates = &sim_estimates;
break;
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
shortname = "External";
active_estimates = &external_estimates;
break;
#endif
#if HAL_NAVEKF3_AVAILABLE
Expand Down Expand Up @@ -786,8 +787,14 @@ bool AP_AHRS::_get_location(Location &loc) const
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates.get_location(loc);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->get_location(loc);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
if (EKF2.getLLH(loc)) {
Expand All @@ -803,16 +810,6 @@ bool AP_AHRS::_get_location(Location &loc) const
}
break;
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.get_location(loc);
#endif

#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return external_estimates.get_location(loc);
#endif
}

#if AP_AHRS_DCM_ENABLED
Expand Down Expand Up @@ -848,13 +845,14 @@ bool AP_AHRS::_wind_estimate(Vector3f &wind) const
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates.wind_estimate(wind);
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.wind_estimate(wind);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->wind_estimate(wind);

#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
Expand All @@ -867,10 +865,6 @@ bool AP_AHRS::_wind_estimate(Vector3f &wind) const
return EKF3.getWind(wind);
#endif

#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return external_estimates.wind_estimate(wind);
#endif
}
return false;
}
Expand Down Expand Up @@ -1393,8 +1387,14 @@ Vector2f AP_AHRS::_groundspeed_vector(void)
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates.groundspeed_vector;
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->groundspeed_vector;

#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
Expand All @@ -1412,15 +1412,6 @@ Vector2f AP_AHRS::_groundspeed_vector(void)
}
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.groundspeed_vector;
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL: {
return external_estimates.groundspeed_vector;
}
#endif
}
return Vector2f();
}
Expand Down Expand Up @@ -1509,8 +1500,14 @@ bool AP_AHRS::_get_velocity_NED(Vector3f &vec) const
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
break;
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->get_velocity_NED(vec);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
EKF2.getVelNED(vec);
Expand All @@ -1523,18 +1520,7 @@ bool AP_AHRS::_get_velocity_NED(Vector3f &vec) const
return true;
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.get_velocity_NED(vec);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return external_estimates.get_velocity_NED(vec);
#endif
}
#if AP_AHRS_DCM_ENABLED
return dcm_estimates.get_velocity_NED(vec);
#endif
return false;
}

Expand Down Expand Up @@ -1610,8 +1596,14 @@ bool AP_AHRS::get_vert_pos_rate_D(float &velocity) const
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates.get_vert_pos_rate_D(velocity);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->get_vert_pos_rate_D(velocity);

#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
Expand All @@ -1624,15 +1616,6 @@ bool AP_AHRS::get_vert_pos_rate_D(float &velocity) const
velocity = EKF3.getPosDownDerivative();
return true;
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.get_vert_pos_rate_D(velocity);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return external_estimates.get_vert_pos_rate_D(velocity);
#endif
}
// since there is no default case above, this is unreachable
return false;
Expand All @@ -1644,8 +1627,14 @@ bool AP_AHRS::get_hagl(float &height) const
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates.get_hagl(height);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->get_hagl(height);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
return EKF2.getHAGL(height);
Expand All @@ -1655,15 +1644,6 @@ bool AP_AHRS::get_hagl(float &height) const
case EKFType::THREE:
return EKF3.getHAGL(height);
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.get_hagl(height);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return external_estimates.get_hagl(height);
#endif
}
// since there is no default case above, this is unreachable
return false;
Expand All @@ -1677,8 +1657,14 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates.get_relative_position_NED_origin(vec);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->get_relative_position_NED_origin(vec);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
Vector2f posNE;
Expand Down Expand Up @@ -1708,16 +1694,6 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
return false;
}
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.get_relative_position_NED_origin(vec);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL: {
return external_estimates.get_relative_position_NED_origin(vec);
}
#endif
}
// since there is no default case above, this is unreachable
return false;
Expand Down Expand Up @@ -1745,8 +1721,14 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates.get_relative_position_NE_origin(posNE);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->get_relative_position_NE_origin(posNE);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
bool position_is_valid = EKF2.getPosNE(posNE);
Expand All @@ -1760,16 +1742,6 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
return position_is_valid;
}
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM: {
return sim_estimates.get_relative_position_NE_origin(posNE);
}
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return external_estimates.get_relative_position_NE_origin(posNE);
#endif
}
// since there is no default case above, this is unreachable
return false;
Expand Down Expand Up @@ -2427,9 +2399,15 @@ void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
dcm_estimates.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
break;
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
active_estimates->get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
break;

#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
Expand All @@ -2442,18 +2420,6 @@ void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler
EKF3.getEkfControlLimits(ekfGndSpdLimit,ekfNavVelGainScaler);
break;
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
sim_estimates.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
break;
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
// no limit on gains, large vel limit
external_estimates.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
break;
#endif
}
}

Expand Down Expand Up @@ -2658,8 +2624,14 @@ uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng)

#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates.getLastYawResetAngle(yawAng);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->getLastYawResetAngle(yawAng);

#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
Expand All @@ -2670,15 +2642,6 @@ uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng)
case EKFType::THREE:
return EKF3.getLastYawResetAngle(yawAng);
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.getLastYawResetAngle(yawAng);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return external_estimates.getLastYawResetAngle(yawAng);
#endif
}
return 0;
}
Expand Down Expand Up @@ -2724,8 +2687,14 @@ uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const

#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return 0;
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
#endif
return active_estimates->getLastVelNorthEastReset(vel);

#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
Expand All @@ -2737,14 +2706,6 @@ uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const
return EKF3.getLastVelNorthEastReset(vel);
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.getLastVelNorthEastReset(vel);
#endif
#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return external_estimates.getLastVelNorthEastReset(vel);;
#endif
}
return 0;
}
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_AHRS/AP_AHRS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -725,6 +725,7 @@ class AP_AHRS {
AP_Int8 _gps_minsats;

EKFType active_EKF_type(void) const { return state.active_EKF; }
AP_AHRS_Backend::Estimates *active_estimates;

bool always_use_EKF() const {
return _ekf_flags & FLAG_ALWAYS_USE_EKF;
Expand Down

0 comments on commit 8251d21

Please sign in to comment.