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

AP_AHRS: return estimates from state structure 

this lowers CPU usage and fixes race conditions
This commit is contained in:
Andrew Tridgell 2023-08-17 10:58:53 +10:00
parent fcf6cc0eff
commit 725a2227ca
3 changed files with 311 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

224
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -196,7 +196,7 @@ AP_AHRS::AP_AHRS(uint8_t flags) :
// Copter and Sub force the use of EKF // Copter and Sub force the use of EKF
_ekf_flags |= AP_AHRS::FLAG_ALWAYS_USE_EKF; _ekf_flags |= AP_AHRS::FLAG_ALWAYS_USE_EKF;
#endif #endif
_dcm_matrix.identity(); state.dcm_matrix.identity();
// initialise the controller-to-autopilot-body trim state: // initialise the controller-to-autopilot-body trim state:
_last_trim = _trim.get(); _last_trim = _trim.get();
@ -274,22 +274,25 @@ void AP_AHRS::update_trim_rotation_matrices()
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed(); _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
} }
// return the smoothed gyro vector corrected for drift // return a Quaternion representing our current attitude in NED frame
const Vector3f &AP_AHRS::get_gyro(void) const void AP_AHRS::get_quat_body_to_ned(Quaternion &quat) const
{ {
return _gyro_estimate; quat.from_rotation_matrix(get_rotation_body_to_ned());
} }
const Matrix3f &AP_AHRS::get_rotation_body_to_ned(void) const // convert a vector from body to earth frame
Vector3f AP_AHRS::body_to_earth(const Vector3f &v) const
{ {
return _dcm_matrix; return get_rotation_body_to_ned() * v;
} }
const Vector3f &AP_AHRS::get_gyro_drift(void) const // convert a vector from earth to body frame
Vector3f AP_AHRS::earth_to_body(const Vector3f &v) const
{ {
return _gyro_drift; return get_rotation_body_to_ned().mul_transpose(v);
} }
// reset the current gyro drift estimate // reset the current gyro drift estimate
// should be called if gyro offsets are recalculated // should be called if gyro offsets are recalculated
void AP_AHRS::reset_gyro_drift(void) void AP_AHRS::reset_gyro_drift(void)
@ -312,6 +315,32 @@ void AP_AHRS::reset_gyro_drift(void)
#endif #endif
} }
/*
update state structure after each update()
*/
void AP_AHRS::update_state(void)
{
state.primary_IMU = _get_primary_IMU_index();
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.EAS2TAS = dcm.get_EAS2TAS();
state.airspeed_ok = _airspeed_estimate(state.airspeed);
state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true);
state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec);
state.quat_ok = _get_quaternion(state.quat);
state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude);
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 = _groundspeed();
_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);
}
void AP_AHRS::update(bool skip_ins_update) void AP_AHRS::update(bool skip_ins_update)
{ {
// periodically checks to see if we should update the AHRS // periodically checks to see if we should update the AHRS
@ -393,11 +422,11 @@ void AP_AHRS::update(bool skip_ins_update)
update_AOA_SSA(); update_AOA_SSA();
#if HAL_GCS_ENABLED #if HAL_GCS_ENABLED
EKFType active = active_EKF_type(); state.active_EKF = _active_EKF_type();
if (active != last_active_ekf_type) { if (state.active_EKF != last_active_ekf_type) {
last_active_ekf_type = active; last_active_ekf_type = state.active_EKF;
const char *shortname = "???"; const char *shortname = "???";
switch ((EKFType)active) { switch ((EKFType)state.active_EKF) {
case EKFType::NONE: case EKFType::NONE:
shortname = "DCM"; shortname = "DCM";
break; break;
@ -426,6 +455,9 @@ void AP_AHRS::update(bool skip_ins_update)
} }
#endif // HAL_GCS_ENABLED #endif // HAL_GCS_ENABLED
// update published state
update_state();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
/* /*
add timing jitter to simulate slow EKF response add timing jitter to simulate slow EKF response
@ -448,13 +480,13 @@ void AP_AHRS::copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estim
pitch = results.pitch_rad; pitch = results.pitch_rad;
yaw = results.yaw_rad; yaw = results.yaw_rad;
_dcm_matrix = results.dcm_matrix; state.dcm_matrix = results.dcm_matrix;
_gyro_estimate = results.gyro_estimate; state.gyro_estimate = results.gyro_estimate;
_gyro_drift = results.gyro_drift; state.gyro_drift = results.gyro_drift;
_accel_ef = results.accel_ef; state.accel_ef = results.accel_ef;
_accel_bias = results.accel_bias; state.accel_bias = results.accel_bias;
update_cd_values(); update_cd_values();
update_trig(); update_trig();
@ -481,7 +513,7 @@ void AP_AHRS::update_SITL(void)
sim.update(); sim.update();
sim.get_results(sim_estimates); sim.get_results(sim_estimates);
if (active_EKF_type() == EKFType::SIM) { if (_active_EKF_type() == EKFType::SIM) {
copy_estimates_from_backend_estimates(sim_estimates); copy_estimates_from_backend_estimates(sim_estimates);
} }
} }
@ -518,9 +550,9 @@ void AP_AHRS::update_EKF2(void)
} }
if (_ekf2_started) { if (_ekf2_started) {
EKF2.UpdateFilter(); EKF2.UpdateFilter();
if (active_EKF_type() == EKFType::TWO) { if (_active_EKF_type() == EKFType::TWO) {
Vector3f eulers; Vector3f eulers;
EKF2.getRotationBodyToNED(_dcm_matrix); EKF2.getRotationBodyToNED(state.dcm_matrix);
EKF2.getEulerAngles(eulers); EKF2.getEulerAngles(eulers);
roll = eulers.x; roll = eulers.x;
pitch = eulers.y; pitch = eulers.y;
@ -537,21 +569,21 @@ 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(); Vector3f drift;
EKF2.getGyroBias(_gyro_drift); EKF2.getGyroBias(drift);
_gyro_drift = -_gyro_drift; state.gyro_drift = -drift;
// use the same IMU as the primary EKF and correct for gyro drift // use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_gyro) + _gyro_drift; state.gyro_estimate = _ins.get_gyro(primary_gyro) + state.gyro_drift;
// 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 = state.accel_bias.z;
EKF2.getAccelZBias(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
Vector3f accel = _ins.get_accel(primary_accel); Vector3f accel = _ins.get_accel(primary_accel);
accel.z -= abias; accel.z -= abias;
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; state.accel_ef = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
nav_filter_status filt_state; nav_filter_status filt_state;
EKF2.getFilterStatus(filt_state); EKF2.getFilterStatus(filt_state);
@ -584,9 +616,9 @@ void AP_AHRS::update_EKF3(void)
} }
if (_ekf3_started) { if (_ekf3_started) {
EKF3.UpdateFilter(); EKF3.UpdateFilter();
if (active_EKF_type() == EKFType::THREE) { if (_active_EKF_type() == EKFType::THREE) {
Vector3f eulers; Vector3f eulers;
EKF3.getRotationBodyToNED(_dcm_matrix); EKF3.getRotationBodyToNED(state.dcm_matrix);
EKF3.getEulerAngles(eulers); EKF3.getEulerAngles(eulers);
roll = eulers.x; roll = eulers.x;
pitch = eulers.y; pitch = eulers.y;
@ -604,21 +636,21 @@ void AP_AHRS::update_EKF3(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(); Vector3f drift;
EKF3.getGyroBias(-1,_gyro_drift); EKF3.getGyroBias(-1, drift);
_gyro_drift = -_gyro_drift; state.gyro_drift = -drift;
// use the same IMU as the primary EKF and correct for gyro drift // use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_gyro) + _gyro_drift; state.gyro_estimate = _ins.get_gyro(primary_gyro) + state.gyro_drift;
// get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU) // get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU)
Vector3f &abias = _accel_bias; Vector3f &abias = state.accel_bias;
EKF3.getAccelBias(-1,abias); EKF3.getAccelBias(-1,abias);
// use the primary IMU for accel earth frame // use the primary IMU for accel earth frame
Vector3f accel = _ins.get_accel(primary_accel); Vector3f accel = _ins.get_accel(primary_accel);
accel -= abias; accel -= abias;
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; state.accel_ef = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
nav_filter_status filt_state; nav_filter_status filt_state;
EKF3.getFilterStatus(filt_state); EKF3.getFilterStatus(filt_state);
@ -634,7 +666,7 @@ void AP_AHRS::update_external(void)
external.update(); external.update();
external.get_results(external_estimates); external.get_results(external_estimates);
if (active_EKF_type() == EKFType::EXTERNAL) { if (_active_EKF_type() == EKFType::EXTERNAL) {
copy_estimates_from_backend_estimates(external_estimates); copy_estimates_from_backend_estimates(external_estimates);
} }
} }
@ -667,7 +699,7 @@ void AP_AHRS::reset()
} }
// dead-reckoning support // dead-reckoning support
bool AP_AHRS::get_location(Location &loc) const bool AP_AHRS::_get_location(Location &loc) const
{ {
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
@ -720,7 +752,7 @@ float AP_AHRS::get_error_yaw(void) const
} }
// return a wind estimation vector, in m/s // return a wind estimation vector, in m/s
bool AP_AHRS::wind_estimate(Vector3f &wind) const bool AP_AHRS::_wind_estimate(Vector3f &wind) const
{ {
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
@ -749,12 +781,6 @@ bool AP_AHRS::wind_estimate(Vector3f &wind) const
} }
return false; return false;
} }
Vector3f AP_AHRS::wind_estimate(void) const
{
Vector3f ret;
UNUSED_RESULT(wind_estimate(ret));
return ret;
}
/* /*
return true if a real airspeed sensor is enabled return true if a real airspeed sensor is enabled
@ -780,7 +806,7 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // if we have an estimate
bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
{ {
bool ret = false; bool ret = false;
@ -861,7 +887,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
return ret; return ret;
} }
bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const
{ {
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
@ -890,7 +916,7 @@ bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const
// return estimate of true airspeed vector in body frame in m/s // return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable // returns false if estimate is unavailable
bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const bool AP_AHRS::_airspeed_vector_true(Vector3f &vec) const
{ {
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
@ -987,7 +1013,7 @@ bool AP_AHRS::use_compass(void)
} }
// return the quaternion defining the rotation from NED to XYZ (body) axes // return the quaternion defining the rotation from NED to XYZ (body) axes
bool AP_AHRS::get_quaternion(Quaternion &quat) const bool AP_AHRS::_get_quaternion(Quaternion &quat) const
{ {
// backends always return in autopilot XYZ frame; rotate result // backends always return in autopilot XYZ frame; rotate result
// according to trim // according to trim
@ -1033,10 +1059,10 @@ bool AP_AHRS::get_quaternion(Quaternion &quat) const
} }
// return secondary attitude solution if available, as eulers in radians // return secondary attitude solution if available, as eulers in radians
bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const bool AP_AHRS::_get_secondary_attitude(Vector3f &eulers) const
{ {
EKFType secondary_ekf_type; EKFType secondary_ekf_type;
if (!get_secondary_EKF_type(secondary_ekf_type)) { if (!_get_secondary_EKF_type(secondary_ekf_type)) {
return false; return false;
} }
@ -1086,10 +1112,10 @@ bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const
// return secondary attitude solution if available, as quaternion // return secondary attitude solution if available, as quaternion
bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const bool AP_AHRS::_get_secondary_quaternion(Quaternion &quat) const
{ {
EKFType secondary_ekf_type; EKFType secondary_ekf_type;
if (!get_secondary_EKF_type(secondary_ekf_type)) { if (!_get_secondary_EKF_type(secondary_ekf_type)) {
return false; return false;
} }
@ -1141,10 +1167,10 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const
} }
// return secondary position solution if available // return secondary position solution if available
bool AP_AHRS::get_secondary_position(Location &loc) const bool AP_AHRS::_get_secondary_position(Location &loc) const
{ {
EKFType secondary_ekf_type; EKFType secondary_ekf_type;
if (!get_secondary_EKF_type(secondary_ekf_type)) { if (!_get_secondary_EKF_type(secondary_ekf_type)) {
return false; return false;
} }
@ -1187,7 +1213,7 @@ bool AP_AHRS::get_secondary_position(Location &loc) const
} }
// EKF has a better ground speed vector estimate // EKF has a better ground speed vector estimate
Vector2f AP_AHRS::groundspeed_vector(void) Vector2f AP_AHRS::_groundspeed_vector(void)
{ {
Vector3f vec; Vector3f vec;
@ -1220,7 +1246,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
return dcm.groundspeed_vector(); return dcm.groundspeed_vector();
} }
float AP_AHRS::groundspeed(void) float AP_AHRS::_groundspeed(void)
{ {
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
@ -1307,7 +1333,7 @@ bool AP_AHRS::have_inertial_nav(void) const
// return a ground velocity in meters/second, North/East/Down // return a ground velocity in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true // order. Must only be called if have_inertial_nav() is true
bool AP_AHRS::get_velocity_NED(Vector3f &vec) const bool AP_AHRS::_get_velocity_NED(Vector3f &vec) const
{ {
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
@ -1637,7 +1663,7 @@ void AP_AHRS::get_relative_position_D_home(float &posD) const
Location originLLH; Location originLLH;
float originD; float originD;
if (!get_relative_position_D_origin(originD) || if (!get_relative_position_D_origin(originD) ||
!get_origin(originLLH)) { !_get_origin(originLLH)) {
const auto &gps = AP::gps(); const auto &gps = AP::gps();
if (_gps_use == GPSUse::EnableWithHeight && if (_gps_use == GPSUse::EnableWithHeight &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) { gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
@ -1697,7 +1723,7 @@ AP_AHRS::EKFType AP_AHRS::ekf_type(void) const
#endif #endif
} }
AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
{ {
EKFType ret = EKFType::NONE; EKFType ret = EKFType::NONE;
@ -1837,7 +1863,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
} }
// get secondary EKF type. returns false if no secondary (i.e. only using DCM) // get secondary EKF type. returns false if no secondary (i.e. only using DCM)
bool AP_AHRS::get_secondary_EKF_type(EKFType &secondary_ekf_type) const bool AP_AHRS::_get_secondary_EKF_type(EKFType &secondary_ekf_type) const
{ {
switch (active_EKF_type()) { switch (active_EKF_type()) {
@ -2216,7 +2242,7 @@ bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
} }
// Retrieves the NED delta velocity corrected // Retrieves the NED delta velocity corrected
void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const void AP_AHRS::_getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
{ {
int8_t imu_idx = -1; int8_t imu_idx = -1;
Vector3f accel_bias; Vector3f accel_bias;
@ -2251,7 +2277,7 @@ void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
ret.zero(); ret.zero();
AP::ins().get_delta_velocity((uint8_t)imu_idx, ret, dt); AP::ins().get_delta_velocity((uint8_t)imu_idx, ret, dt);
ret -= accel_bias*dt; ret -= accel_bias*dt;
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret; ret = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
ret.z += GRAVITY_MSS*dt; ret.z += GRAVITY_MSS*dt;
} }
@ -2565,7 +2591,7 @@ void AP_AHRS::send_ekf_status_report(GCS_MAVLINK &link) const
} }
// return origin for a specified EKF type // return origin for a specified EKF type
bool AP_AHRS::get_origin(EKFType type, Location &ret) const bool AP_AHRS::_get_origin(EKFType type, Location &ret) const
{ {
switch (type) { switch (type) {
case EKFType::NONE: case EKFType::NONE:
@ -2601,12 +2627,12 @@ bool AP_AHRS::get_origin(EKFType type, Location &ret) const
This copes with users force arming a plane that is running on DCM as This copes with users force arming a plane that is running on DCM as
the EKF has not fully initialised the EKF has not fully initialised
*/ */
bool AP_AHRS::get_origin(Location &ret) const bool AP_AHRS::_get_origin(Location &ret) const
{ {
if (get_origin(ekf_type(), ret)) { if (_get_origin(ekf_type(), ret)) {
return true; return true;
} }
if (hal.util->get_soft_armed() && get_origin(active_EKF_type(), ret)) { if (hal.util->get_soft_armed() && _get_origin(active_EKF_type(), ret)) {
return true; return true;
} }
return false; return false;
@ -2838,7 +2864,7 @@ uint8_t AP_AHRS::get_active_airspeed_index() const
} }
// get the index of the current primary IMU // get the index of the current primary IMU
uint8_t AP_AHRS::get_primary_IMU_index() const uint8_t AP_AHRS::_get_primary_IMU_index() const
{ {
int8_t imu = -1; int8_t imu = -1;
switch (active_EKF_type()) { switch (active_EKF_type()) {
@ -2873,7 +2899,7 @@ uint8_t AP_AHRS::get_primary_IMU_index() const
} }
// return the index of the primary core or -1 if no primary core selected // return the index of the primary core or -1 if no primary core selected
int8_t AP_AHRS::get_primary_core_index() const int8_t AP_AHRS::_get_primary_core_index() const
{ {
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
@ -2903,7 +2929,7 @@ int8_t AP_AHRS::get_primary_core_index() const
} }
// get the index of the current primary accelerometer sensor // get the index of the current primary accelerometer sensor
uint8_t AP_AHRS::get_primary_accel_index(void) const uint8_t AP_AHRS::_get_primary_accel_index(void) const
{ {
if (ekf_type() != EKFType::NONE) { if (ekf_type() != EKFType::NONE) {
return get_primary_IMU_index(); return get_primary_IMU_index();
@ -2912,7 +2938,7 @@ uint8_t AP_AHRS::get_primary_accel_index(void) const
} }
// get the index of the current primary gyro sensor // get the index of the current primary gyro sensor
uint8_t AP_AHRS::get_primary_gyro_index(void) const uint8_t AP_AHRS::_get_primary_gyro_index(void) const
{ {
if (ekf_type() != EKFType::NONE) { if (ekf_type() != EKFType::NONE) {
return get_primary_IMU_index(); return get_primary_IMU_index();
@ -3103,6 +3129,66 @@ const EKFGSF_yaw *AP_AHRS::get_yaw_estimator(void) const
return nullptr; return nullptr;
} }
// get current location estimate
bool AP_AHRS::get_location(Location &loc) const
{
loc = state.location;
return state.location_ok;
}
// return a wind estimation vector, in m/s; returns 0,0,0 on failure
bool AP_AHRS::wind_estimate(Vector3f &wind) const
{
wind = state.wind_estimate;
return state.wind_estimate_ok;
}
// return an airspeed estimate if available. return true
// if we have an estimate
bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
{
airspeed_ret = state.airspeed;
return state.airspeed_ok;
}
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const
{
airspeed_ret = state.airspeed_true;
return state.airspeed_true_ok;
}
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const
{
vec = state.airspeed_vec;
return state.airspeed_vec_ok;
}
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool AP_AHRS::get_quaternion(Quaternion &quat) const
{
quat = state.quat;
return state.quat_ok;
}
// returns the inertial navigation origin in lat/lon/alt
bool AP_AHRS::get_origin(Location &ret) const
{
ret = state.origin;
return state.origin_ok;
}
// 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
{
vec = state.velocity_NED;
return state.velocity_NED_ok;
}
// singleton instance // singleton instance
AP_AHRS *AP_AHRS::_singleton; AP_AHRS *AP_AHRS::_singleton;

197
libraries/AP_AHRS/AP_AHRS.h
View File

@ -76,10 +76,10 @@ public:
} }
// return the smoothed gyro vector corrected for drift // return the smoothed gyro vector corrected for drift
const Vector3f &get_gyro(void) const; const Vector3f &get_gyro(void) const { return state.gyro_estimate; }
// return the current drift correction integrator value // return the current drift correction integrator value
const Vector3f &get_gyro_drift(void) const; const Vector3f &get_gyro_drift(void) const { return state.gyro_drift; }
// reset the current gyro drift estimate // reset the current gyro drift estimate
// should be called if gyro offsets are recalculated // should be called if gyro offsets are recalculated
@ -88,7 +88,7 @@ public:
void update(bool skip_ins_update=false); void update(bool skip_ins_update=false);
void reset(); void reset();
// dead-reckoning support // get current location estimate
bool get_location(Location &loc) const; bool get_location(Location &loc) const;
// get latest altitude estimate above ground level in meters and validity flag // get latest altitude estimate above ground level in meters and validity flag
@ -109,7 +109,10 @@ public:
bool get_wind_estimation_enabled() const { return wind_estimation_enabled; } bool get_wind_estimation_enabled() const { return wind_estimation_enabled; }
// return a wind estimation vector, in m/s; returns 0,0,0 on failure // return a wind estimation vector, in m/s; returns 0,0,0 on failure
Vector3f wind_estimate() const; const Vector3f &wind_estimate() const { return state.wind_estimate; }
// return a wind estimation vector, in m/s; returns 0,0,0 on failure
bool wind_estimate(Vector3f &wind) const;
// instruct DCM to update its wind estimate: // instruct DCM to update its wind estimate:
void estimate_wind() { dcm.estimate_wind(); } void estimate_wind() { dcm.estimate_wind(); }
@ -125,13 +128,13 @@ public:
// get apparent to true airspeed ratio // get apparent to true airspeed ratio
float get_EAS2TAS(void) const { float get_EAS2TAS(void) const {
// FIXME: make this is a method on the active backend return state.EAS2TAS;
return dcm.get_EAS2TAS();
} }
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const; bool airspeed_estimate(float &airspeed_ret) const;
// return a true airspeed estimate (navigation airspeed) if // return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate // available. return true if we have an estimate
bool airspeed_estimate_true(float &airspeed_ret) const; bool airspeed_estimate_true(float &airspeed_ret) const;
@ -168,26 +171,38 @@ public:
bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED;
// return secondary attitude solution if available, as eulers in radians // return secondary attitude solution if available, as eulers in radians
bool get_secondary_attitude(Vector3f &eulers) const; bool get_secondary_attitude(Vector3f &eulers) const {
eulers = state.secondary_attitude;
return state.secondary_attitude_ok;
}
// return secondary attitude solution if available, as quaternion // return secondary attitude solution if available, as quaternion
bool get_secondary_quaternion(Quaternion &quat) const; bool get_secondary_quaternion(Quaternion &quat) const {
quat = state.secondary_quat;
return state.secondary_quat_ok;
}
// return secondary position solution if available // return secondary position solution if available
bool get_secondary_position(Location &loc) const; bool get_secondary_position(Location &loc) const {
loc = state.secondary_pos;
return state.secondary_pos_ok;
}
// EKF has a better ground speed vector estimate // EKF has a better ground speed vector estimate
Vector2f groundspeed_vector(); const Vector2f &groundspeed_vector() const { return state.ground_speed_vec; }
// return ground speed estimate in meters/second. Used by ground vehicles. // return ground speed estimate in meters/second. Used by ground vehicles.
float groundspeed(void); float groundspeed(void) const { return state.ground_speed; }
const Vector3f &get_accel_ef() const { const Vector3f &get_accel_ef() const {
return _accel_ef; return state.accel_ef;
} }
// Retrieves the corrected NED delta velocity in use by the inertial navigation // Retrieves the corrected NED delta velocity in use by the inertial navigation
void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const; void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const {
ret = state.corrected_dv;
dt = state.corrected_dv_dt;
}
// set the EKF's origin location in 10e7 degrees. This should only // 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) // be called when the EKF has no absolute position reference (i.e. GPS)
@ -343,13 +358,13 @@ public:
uint8_t get_active_airspeed_index() const; uint8_t get_active_airspeed_index() const;
// return the index of the primary core or -1 if no primary core selected // return the index of the primary core or -1 if no primary core selected
int8_t get_primary_core_index() const; int8_t get_primary_core_index() const { return state.primary_core; }
// get the index of the current primary accelerometer sensor // get the index of the current primary accelerometer sensor
uint8_t get_primary_accel_index(void) const; uint8_t get_primary_accel_index(void) const { return state.primary_accel; }
// get the index of the current primary gyro sensor // get the index of the current primary gyro sensor
uint8_t get_primary_gyro_index(void) const; uint8_t get_primary_gyro_index(void) const { return state.primary_gyro; }
// see if EKF lane switching is possible to avoid EKF failsafe // see if EKF lane switching is possible to avoid EKF failsafe
void check_lane_switch(void); void check_lane_switch(void);
@ -507,12 +522,10 @@ public:
int32_t pitch_sensor; int32_t pitch_sensor;
int32_t yaw_sensor; int32_t yaw_sensor;
const Matrix3f &get_rotation_body_to_ned(void) const; const Matrix3f &get_rotation_body_to_ned(void) const { return state.dcm_matrix; }
// return a Quaternion representing our current attitude in NED frame // return a Quaternion representing our current attitude in NED frame
void get_quat_body_to_ned(Quaternion &quat) const { void get_quat_body_to_ned(Quaternion &quat) const;
quat.from_rotation_matrix(get_rotation_body_to_ned());
}
// get rotation matrix specifically from DCM backend (used for // get rotation matrix specifically from DCM backend (used for
// compass calibrator) // compass calibrator)
@ -529,14 +542,10 @@ public:
Vector2f body_to_earth2D(const Vector2f &bf) const; Vector2f body_to_earth2D(const Vector2f &bf) const;
// convert a vector from body to earth frame // convert a vector from body to earth frame
Vector3f body_to_earth(const Vector3f &v) const { Vector3f body_to_earth(const Vector3f &v) const;
return get_rotation_body_to_ned() * v;
}
// convert a vector from earth to body frame // convert a vector from earth to body frame
Vector3f earth_to_body(const Vector3f &v) const { Vector3f earth_to_body(const Vector3f &v) const;
return get_rotation_body_to_ned().mul_transpose(v);
}
/* /*
* methods for the benefit of LUA bindings * methods for the benefit of LUA bindings
@ -553,7 +562,7 @@ public:
// get_accel() vector to get best current body frame accel // get_accel() vector to get best current body frame accel
// estimate // estimate
const Vector3f &get_accel_bias(void) const { const Vector3f &get_accel_bias(void) const {
return _accel_bias; return state.accel_bias;
} }
/* /*
@ -660,11 +669,7 @@ private:
EXTERNAL = 11, EXTERNAL = 11,
#endif #endif
}; };
EKFType active_EKF_type(void) const; EKFType active_EKF_type(void) const { return state.active_EKF; }
// if successful returns true and sets secondary_ekf_type to None (for DCM), EKF3 or EKF3
// returns false if no secondary (i.e. only using DCM)
bool get_secondary_EKF_type(EKFType &secondary_ekf_type) const;
bool always_use_EKF() const { bool always_use_EKF() const {
return _ekf_flags & FLAG_ALWAYS_USE_EKF; return _ekf_flags & FLAG_ALWAYS_USE_EKF;
@ -690,9 +695,6 @@ private:
// update roll_sensor, pitch_sensor and yaw_sensor // update roll_sensor, pitch_sensor and yaw_sensor
void update_cd_values(void); void update_cd_values(void);
// return origin for a specified EKF type
bool get_origin(EKFType type, Location &ret) const;
// helper trig variables // helper trig variables
float _cos_roll{1.0f}; float _cos_roll{1.0f};
float _cos_pitch{1.0f}; float _cos_pitch{1.0f};
@ -711,12 +713,7 @@ private:
#endif #endif
// rotation from vehicle body to NED frame // rotation from vehicle body to NED frame
Matrix3f _dcm_matrix;
Vector3f _gyro_drift;
Vector3f _gyro_estimate;
Vector3f _accel_ef;
Vector3f _accel_bias;
const uint16_t startup_delay_ms = 1000; const uint16_t startup_delay_ms = 1000;
uint32_t start_time_ms; uint32_t start_time_ms;
@ -726,7 +723,7 @@ private:
void update_DCM(); void update_DCM();
// get the index of the current primary IMU // get the index of the current primary IMU
uint8_t get_primary_IMU_index(void) const; uint8_t get_primary_IMU_index(void) const { return state.primary_IMU; }
/* /*
* home-related state * home-related state
@ -799,9 +796,6 @@ private:
*/ */
bool wind_estimation_enabled; bool wind_estimation_enabled;
// return a wind estimation vector, in m/s
bool wind_estimate(Vector3f &wind) const WARN_IF_UNUSED;
/* /*
* fly_forward is set by the vehicles to indicate the vehicle * fly_forward is set by the vehicles to indicate the vehicle
* should generally be moving in the direction of its heading. * should generally be moving in the direction of its heading.
@ -843,6 +837,121 @@ private:
void Write_AHRS2(void) const; void Write_AHRS2(void) const;
// write POS (canonical vehicle position) message out: // write POS (canonical vehicle position) message out:
void Write_POS(void) const; void Write_POS(void) const;
// return an airspeed estimate if available. return true
// if we have an estimate
bool _airspeed_estimate(float &airspeed_ret) const;
// return secondary attitude solution if available, as eulers in radians
bool _get_secondary_attitude(Vector3f &eulers) const;
// return secondary attitude solution if available, as quaternion
bool _get_secondary_quaternion(Quaternion &quat) const;
// get ground speed 2D
Vector2f _groundspeed_vector(void);
// 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;
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
bool _airspeed_vector_true(Vector3f &vec) const;
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED;
// return secondary position solution if available
bool _get_secondary_position(Location &loc) const;
// return ground speed estimate in meters/second. Used by ground vehicles.
float _groundspeed(void);
// Retrieves the corrected NED delta velocity in use by the inertial navigation
void _getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const;
// returns the inertial navigation origin in lat/lon/alt
bool _get_origin(Location &ret) const WARN_IF_UNUSED;
// return origin for a specified EKF type
bool _get_origin(EKFType type, Location &ret) const;
// return a ground velocity in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool _get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED;
// get secondary EKF type. returns false if no secondary (i.e. only using DCM)
bool _get_secondary_EKF_type(EKFType &secondary_ekf_type) const;
// return the index of the primary core or -1 if no primary core selected
int8_t _get_primary_core_index() const;
// get the index of the current primary accelerometer sensor
uint8_t _get_primary_accel_index(void) const;
// get the index of the current primary gyro sensor
uint8_t _get_primary_gyro_index(void) const;
// get the index of the current primary IMU
uint8_t _get_primary_IMU_index(void) const;
// get current location estimate
bool _get_location(Location &loc) const;
/*
update state structure
*/
void update_state(void);
/*
state updated at the end of each update() call
*/
struct {
EKFType active_EKF;
uint8_t primary_IMU;
uint8_t primary_gyro;
uint8_t primary_accel;
uint8_t primary_core;
Vector3f gyro_estimate;
Matrix3f dcm_matrix;
Vector3f gyro_drift;
Vector3f accel_ef;
Vector3f accel_bias;
Vector3f wind_estimate;
bool wind_estimate_ok;
float EAS2TAS;
bool airspeed_ok;
float airspeed;
bool airspeed_true_ok;
float airspeed_true;
Vector3f airspeed_vec;
bool airspeed_vec_ok;
Quaternion quat;
bool quat_ok;
Vector3f secondary_attitude;
bool secondary_attitude_ok;
Quaternion secondary_quat;
bool secondary_quat_ok;
Location location;
bool location_ok;
Location secondary_pos;
bool secondary_pos_ok;
Vector2f ground_speed_vec;
float ground_speed;
Vector3f corrected_dv;
float corrected_dv_dt;
Location origin;
bool origin_ok;
Vector3f velocity_NED;
bool velocity_NED_ok;
} state;
}; };
namespace AP { namespace AP {

6
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -106,9 +106,9 @@ void AP_AHRS::write_video_stabilisation() const
const struct log_Video_Stabilisation pkt { const struct log_Video_Stabilisation pkt {
LOG_PACKET_HEADER_INIT(LOG_VIDEO_STABILISATION_MSG), LOG_PACKET_HEADER_INIT(LOG_VIDEO_STABILISATION_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
gyro_x : _gyro_estimate.x, gyro_x : state.gyro_estimate.x,
gyro_y : _gyro_estimate.y, gyro_y : state.gyro_estimate.y,
gyro_z : _gyro_estimate.z, gyro_z : state.gyro_estimate.z,
accel_x : accel.x, accel_x : accel.x,
accel_y : accel.y, accel_y : accel.y,
accel_z : accel.z, accel_z : accel.z,