Skip to content

Commit

Permalink
AP_AHRS: add ad 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 bac6648 commit 0e39121
Show file tree
Hide file tree
Showing 2 changed files with 57 additions and 84 deletions.
140 changes: 56 additions & 84 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
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 0e39121

Please sign in to comment.