Skip to content

Commit

Permalink
AP_AHRS: active_backend and estimates
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Nov 23, 2023
1 parent cf6f31d commit 1eae3ff
Show file tree
Hide file tree
Showing 2 changed files with 125 additions and 146 deletions.
263 changes: 120 additions & 143 deletions libraries/AP_AHRS/AP_AHRS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -202,6 +202,29 @@ AP_AHRS::AP_AHRS(uint8_t flags) :
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, _last_trim.z);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();

// ensure we always have an active backend:
#if AP_AHRS_DCM_ENABLED
active_backend = &dcm;
active_estimates = &dcm_estimates;
#elif HAL_NAVEKF2_AVAILABLE
active_backend = &ekf2;
active_estimates = &ekf2_estimates;
#elif HAL_NAVEKF3_AVAILABLE
active_backend = &EKF3;
active_estimates = &ekf3_estimates;
#elif AP_AHRS_SIM_ENABLED
active_backend = ∼
active_estimates = &sim_estimates;
#elif HAL_EXTERNAL_AHRS_ENABLED
active_backend = &external;
active_estimates = &external_estimates;
#else
// you do have to work hard to get this error. This can't be
// compile-time as sanity checks ensure we should be able to
// compile with no backend compiled in in other places.
AP_HAL::panic("No backend available");
#endif
}

// init sets up INS board orientation
Expand Down Expand Up @@ -328,7 +351,7 @@ void AP_AHRS::update_state(void)
state.primary_gyro = _get_primary_gyro_index();
state.primary_accel = _get_primary_accel_index();
state.primary_core = _get_primary_core_index();
state.wind_estimate_ok = _wind_estimate(state.wind_estimate);
state.wind_estimate_ok = active_estimates->wind_estimate(state.wind_estimate);
state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS();
state.airspeed_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type);
state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true);
Expand All @@ -338,11 +361,11 @@ void AP_AHRS::update_state(void)
state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat);
state.location_ok = _get_location(state.location);
state.secondary_pos_ok = _get_secondary_position(state.secondary_pos);
state.ground_speed_vec = _groundspeed_vector();
state.ground_speed_vec = active_estimates->groundspeed_vector;
state.ground_speed = state.ground_speed_vec.length();
_getCorrectedDeltaVelocityNED(state.corrected_dv, state.corrected_dv_dt);
state.origin_ok = _get_origin(state.origin);
state.velocity_NED_ok = _get_velocity_NED(state.velocity_NED);
state.velocity_NED_ok = active_estimates->get_velocity_NED(state.velocity_NED);
}

void AP_AHRS::update(bool skip_ins_update)
Expand Down Expand Up @@ -429,6 +452,8 @@ void AP_AHRS::update(bool skip_ins_update)

#if HAL_GCS_ENABLED
state.active_EKF = _active_EKF_type();
active_backend = &_active_backend();
active_estimates = &_active_estimates();
if (state.active_EKF != last_active_ekf_type) {
last_active_ekf_type = state.active_EKF;
const char *shortname = "???";
Expand Down Expand Up @@ -761,6 +786,96 @@ void AP_AHRS::reset()
#endif
}

AP_AHRS_Backend &AP_AHRS::_active_backend()
{
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm;
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
return EKF2;
#endif

#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
return EKF3;
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim;
#endif

#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
return external;
#endif
}

#if AP_AHRS_DCM_ENABLED
return dcm;
#endif
#if HAL_NAVEKF2_AVAILABLE
return EKF2;
#endif
#if HAL_NAVEKF3_AVAILABLE
return EKF3;
#endif
#if AP_AHRS_SIM_ENABLED
return sim;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
return external;
#endif
}

AP_AHRS_Backend::Estimates &AP_AHRS::_active_estimates()
{
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm_estimates;
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
return ekf2_estimates;
#endif

#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
return ekf3_estimates;
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates;
#endif

#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
return external_estimates;
#endif
}

#if AP_AHRS_DCM_ENABLED
return dcm_estimates;
#endif
#if HAL_NAVEKF2_AVAILABLE
return ekf2_estimates;
#endif
#if HAL_NAVEKF3_AVAILABLE
return ekf3_estimates;
#endif
#if AP_AHRS_SIM_ENABLED
return sim_estimates;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
return external_estimates;
#endif
}

// dead-reckoning support
bool AP_AHRS::_get_location(Location &loc) const
{
Expand Down Expand Up @@ -823,39 +938,6 @@ float AP_AHRS::get_error_yaw(void) const
return 0;
}

// return a wind estimation vector, in m/s
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 HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
EKF2.getWind(wind);
return true;
#endif

#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
return EKF3.wind_estimate(wind);
#endif

#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
return external_estimates.wind_estimate(wind);
#endif
}
return false;
}

/*
return true if the current AHRS airspeed estimate is directly derived from an airspeed sensor
*/
Expand Down Expand Up @@ -1132,43 +1214,8 @@ bool AP_AHRS::use_compass(void)
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool AP_AHRS::_get_quaternion(Quaternion &quat) const
{
// backends always return in autopilot XYZ frame; rotate result
// according to trim
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
if (!dcm_estimates.get_quaternion(quat)) {
return false;
}
break;
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
if (!_ekf2_started) {
return false;
}
EKF2.getQuaternion(quat);
break;
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
if (!ekf3_estimates.get_quaternion(quat)) {
return false;
}
break;
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
if (!sim_estimates.get_quaternion(quat)) {
return false;
}
break;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
// we assume the external AHRS isn't trimmed with the autopilot!
return external_estimates.get_quaternion(quat);
#endif
if (!active_estimates->get_quaternion(quat)) {
return false;
}

quat.rotate(-_trim.get());
Expand Down Expand Up @@ -1338,41 +1385,6 @@ bool AP_AHRS::_get_secondary_position(Location &loc) const
return false;
}

// EKF has a better ground speed vector estimate
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 HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
Vector3f vec;
EKF2.getVelNED(vec);
return vec.xy();
}
#endif

#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
return ekf3_estimates.groundspeed_vector;
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.groundspeed_vector;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL: {
return external_estimates.groundspeed_vector;
}
#endif
}
return Vector2f();
}

// set the EKF's origin location in 10e7 degrees. This should only
// be called when the EKF has no absolute position reference (i.e. GPS)
// from which to decide the origin on its own
Expand Down Expand Up @@ -1438,41 +1450,6 @@ bool AP_AHRS::have_inertial_nav(void) const
return true;
}

// return a ground velocity in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS::_get_velocity_NED(Vector3f &vec) const
{
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
break;
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
EKF2.getVelNED(vec);
return true;
#endif

#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
return ekf3_estimates.get_velocity_NED(vec);
#endif

#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
return sim_estimates.get_velocity_NED(vec);
#endif
#if HAL_EXTERNAL_AHRS_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;
}

// returns the expected NED magnetic field
bool AP_AHRS::get_mag_field_NED(Vector3f &vec) const
{
Expand Down
8 changes: 5 additions & 3 deletions libraries/AP_AHRS/AP_AHRS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -859,9 +859,6 @@ class AP_AHRS {
// get active EKF type
EKFType _active_EKF_type(void) const;

// return a wind estimation vector, in m/s
bool _wind_estimate(Vector3f &wind) const WARN_IF_UNUSED;

// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool _airspeed_estimate_true(float &airspeed_ret) const;
Expand Down Expand Up @@ -989,6 +986,11 @@ class AP_AHRS {
struct AP_AHRS_Backend::Estimates external_estimates;
#endif

AP_AHRS_Backend &_active_backend();
AP_AHRS_Backend::Estimates &_active_estimates();
AP_AHRS_Backend *active_backend;
AP_AHRS_Backend::Estimates *active_estimates;

};

namespace AP {
Expand Down

0 comments on commit 1eae3ff

Please sign in to comment.