From 8251d21ece3fd29d072608925d9ee5720a5076ea Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 23 Dec 2024 17:17:18 +1100 Subject: [PATCH] AP_AHRS: add and use active_estimates pointer --- libraries/AP_AHRS/AP_AHRS.cpp | 195 ++++++++++++++-------------------- libraries/AP_AHRS/AP_AHRS.h | 1 + 2 files changed, 79 insertions(+), 117 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 350a951d2f8ca..9287159bb2ea7 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -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(); @@ -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 @@ -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)) { @@ -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 @@ -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: @@ -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; } @@ -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: { @@ -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(); } @@ -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); @@ -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; } @@ -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: @@ -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; @@ -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); @@ -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; @@ -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; @@ -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; @@ -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); @@ -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; @@ -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: @@ -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 } } @@ -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: @@ -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; } @@ -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: @@ -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; } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 04ce489dad7e3..7d481b6963066 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -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;