diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 99291d4251b4a5..9bebd14e1824ca 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -196,7 +196,7 @@ AP_AHRS::AP_AHRS(uint8_t flags) : // Copter and Sub force the use of EKF _ekf_flags |= AP_AHRS::FLAG_ALWAYS_USE_EKF; #endif - _dcm_matrix.identity(); + state.dcm_matrix.identity(); // initialise the controller-to-autopilot-body trim state: _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(); } -// return the smoothed gyro vector corrected for drift -const Vector3f &AP_AHRS::get_gyro(void) const +// return a Quaternion representing our current attitude in NED frame +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 // should be called if gyro offsets are recalculated void AP_AHRS::reset_gyro_drift(void) @@ -312,6 +315,32 @@ void AP_AHRS::reset_gyro_drift(void) #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) { // 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(); #if HAL_GCS_ENABLED - EKFType active = active_EKF_type(); - if (active != last_active_ekf_type) { - last_active_ekf_type = active; + state.active_EKF = _active_EKF_type(); + if (state.active_EKF != last_active_ekf_type) { + last_active_ekf_type = state.active_EKF; const char *shortname = "???"; - switch ((EKFType)active) { + switch ((EKFType)state.active_EKF) { case EKFType::NONE: shortname = "DCM"; break; @@ -426,6 +455,9 @@ void AP_AHRS::update(bool skip_ins_update) } #endif // HAL_GCS_ENABLED + // update published state + update_state(); + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL /* 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; yaw = results.yaw_rad; - _dcm_matrix = results.dcm_matrix; + state.dcm_matrix = results.dcm_matrix; - _gyro_estimate = results.gyro_estimate; - _gyro_drift = results.gyro_drift; + state.gyro_estimate = results.gyro_estimate; + state.gyro_drift = results.gyro_drift; - _accel_ef = results.accel_ef; - _accel_bias = results.accel_bias; + state.accel_ef = results.accel_ef; + state.accel_bias = results.accel_bias; update_cd_values(); update_trig(); @@ -481,7 +513,7 @@ void AP_AHRS::update_SITL(void) sim.update(); sim.get_results(sim_estimates); - if (active_EKF_type() == EKFType::SIM) { + if (_active_EKF_type() == EKFType::SIM) { copy_estimates_from_backend_estimates(sim_estimates); } } @@ -518,9 +550,9 @@ void AP_AHRS::update_EKF2(void) } if (_ekf2_started) { EKF2.UpdateFilter(); - if (active_EKF_type() == EKFType::TWO) { + if (_active_EKF_type() == EKFType::TWO) { Vector3f eulers; - EKF2.getRotationBodyToNED(_dcm_matrix); + EKF2.getRotationBodyToNED(state.dcm_matrix); EKF2.getEulerAngles(eulers); roll = eulers.x; 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 // Note sign convention used by EKF is bias = measurement - truth - _gyro_drift.zero(); - EKF2.getGyroBias(_gyro_drift); - _gyro_drift = -_gyro_drift; + Vector3f drift; + EKF2.getGyroBias(drift); + state.gyro_drift = -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) - float &abias = _accel_bias.z; + float &abias = state.accel_bias.z; EKF2.getAccelZBias(abias); // This EKF is currently using primary_imu, and abias applies to only that IMU Vector3f accel = _ins.get_accel(primary_accel); 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; EKF2.getFilterStatus(filt_state); @@ -584,9 +616,9 @@ void AP_AHRS::update_EKF3(void) } if (_ekf3_started) { EKF3.UpdateFilter(); - if (active_EKF_type() == EKFType::THREE) { + if (_active_EKF_type() == EKFType::THREE) { Vector3f eulers; - EKF3.getRotationBodyToNED(_dcm_matrix); + EKF3.getRotationBodyToNED(state.dcm_matrix); EKF3.getEulerAngles(eulers); roll = eulers.x; 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 // Note sign convention used by EKF is bias = measurement - truth - _gyro_drift.zero(); - EKF3.getGyroBias(-1,_gyro_drift); - _gyro_drift = -_gyro_drift; + Vector3f drift; + EKF3.getGyroBias(-1, drift); + state.gyro_drift = -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) - Vector3f &abias = _accel_bias; + Vector3f &abias = state.accel_bias; EKF3.getAccelBias(-1,abias); // use the primary IMU for accel earth frame Vector3f accel = _ins.get_accel(primary_accel); 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; EKF3.getFilterStatus(filt_state); @@ -634,7 +666,7 @@ void AP_AHRS::update_external(void) external.update(); external.get_results(external_estimates); - if (active_EKF_type() == EKFType::EXTERNAL) { + if (_active_EKF_type() == EKFType::EXTERNAL) { copy_estimates_from_backend_estimates(external_estimates); } } @@ -667,7 +699,7 @@ void AP_AHRS::reset() } // dead-reckoning support -bool AP_AHRS::get_location(Location &loc) const +bool AP_AHRS::_get_location(Location &loc) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -720,7 +752,7 @@ float AP_AHRS::get_error_yaw(void) const } // 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()) { case EKFType::NONE: @@ -749,12 +781,6 @@ bool AP_AHRS::wind_estimate(Vector3f &wind) const } 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 @@ -780,7 +806,7 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const // return an airspeed estimate if available. return true // 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; @@ -861,7 +887,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const 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()) { 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 // 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()) { 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 -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 // 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 -bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const +bool AP_AHRS::_get_secondary_attitude(Vector3f &eulers) const { EKFType secondary_ekf_type; - if (!get_secondary_EKF_type(secondary_ekf_type)) { + if (!_get_secondary_EKF_type(secondary_ekf_type)) { return false; } @@ -1086,10 +1112,10 @@ bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const // 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; - if (!get_secondary_EKF_type(secondary_ekf_type)) { + if (!_get_secondary_EKF_type(secondary_ekf_type)) { return false; } @@ -1141,10 +1167,10 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const } // 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; - if (!get_secondary_EKF_type(secondary_ekf_type)) { + if (!_get_secondary_EKF_type(secondary_ekf_type)) { return false; } @@ -1187,7 +1213,7 @@ bool AP_AHRS::get_secondary_position(Location &loc) const } // EKF has a better ground speed vector estimate -Vector2f AP_AHRS::groundspeed_vector(void) +Vector2f AP_AHRS::_groundspeed_vector(void) { Vector3f vec; @@ -1220,7 +1246,7 @@ Vector2f AP_AHRS::groundspeed_vector(void) return dcm.groundspeed_vector(); } -float AP_AHRS::groundspeed(void) +float AP_AHRS::_groundspeed(void) { switch (active_EKF_type()) { 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 // 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()) { case EKFType::NONE: @@ -1637,7 +1663,7 @@ void AP_AHRS::get_relative_position_D_home(float &posD) const Location originLLH; float originD; if (!get_relative_position_D_origin(originD) || - !get_origin(originLLH)) { + !_get_origin(originLLH)) { const auto &gps = AP::gps(); if (_gps_use == GPSUse::EnableWithHeight && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { @@ -1697,7 +1723,7 @@ AP_AHRS::EKFType AP_AHRS::ekf_type(void) const #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; @@ -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) -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()) { @@ -2216,7 +2242,7 @@ bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const } // 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; Vector3f accel_bias; @@ -2251,7 +2277,7 @@ void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const ret.zero(); AP::ins().get_delta_velocity((uint8_t)imu_idx, ret, 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; } @@ -2565,7 +2591,7 @@ void AP_AHRS::send_ekf_status_report(GCS_MAVLINK &link) const } // 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) { 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 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; } - 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 false; @@ -2838,7 +2864,7 @@ uint8_t AP_AHRS::get_active_airspeed_index() const } // 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; 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 -int8_t AP_AHRS::get_primary_core_index() const +int8_t AP_AHRS::_get_primary_core_index() const { switch (active_EKF_type()) { 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 -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) { 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 -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) { return get_primary_IMU_index(); @@ -3103,6 +3129,66 @@ const EKFGSF_yaw *AP_AHRS::get_yaw_estimator(void) const 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 AP_AHRS *AP_AHRS::_singleton; diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 9baf68b13d9adf..c75b268f0338ad 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -76,10 +76,10 @@ class AP_AHRS { } // 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 - const Vector3f &get_gyro_drift(void) const; + const Vector3f &get_gyro_drift(void) const { return state.gyro_drift; } // reset the current gyro drift estimate // should be called if gyro offsets are recalculated @@ -88,7 +88,7 @@ class AP_AHRS { void update(bool skip_ins_update=false); void reset(); - // dead-reckoning support + // get current location estimate bool get_location(Location &loc) const; // get latest altitude estimate above ground level in meters and validity flag @@ -109,7 +109,10 @@ class AP_AHRS { bool get_wind_estimation_enabled() const { return wind_estimation_enabled; } // 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: void estimate_wind() { dcm.estimate_wind(); } @@ -125,13 +128,13 @@ class AP_AHRS { // get apparent to true airspeed ratio float get_EAS2TAS(void) const { - // FIXME: make this is a method on the active backend - return dcm.get_EAS2TAS(); + return state.EAS2TAS; } // return an airspeed estimate if available. return true // if we have an estimate bool airspeed_estimate(float &airspeed_ret) const; + // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate bool airspeed_estimate_true(float &airspeed_ret) const; @@ -168,26 +171,38 @@ class AP_AHRS { bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; // 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 - 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 - 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 - Vector2f groundspeed_vector(); + const Vector2f &groundspeed_vector() const { return state.ground_speed_vec; } // 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 { - return _accel_ef; + return state.accel_ef; } // 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 // be called when the EKF has no absolute position reference (i.e. GPS) @@ -343,13 +358,13 @@ class AP_AHRS { uint8_t get_active_airspeed_index() const; // 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 - 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 - 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 void check_lane_switch(void); @@ -507,12 +522,10 @@ class AP_AHRS { int32_t pitch_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 - void get_quat_body_to_ned(Quaternion &quat) const { - quat.from_rotation_matrix(get_rotation_body_to_ned()); - } + void get_quat_body_to_ned(Quaternion &quat) const; // get rotation matrix specifically from DCM backend (used for // compass calibrator) @@ -529,14 +542,10 @@ class AP_AHRS { Vector2f body_to_earth2D(const Vector2f &bf) const; // convert a vector from body to earth frame - Vector3f body_to_earth(const Vector3f &v) const { - return get_rotation_body_to_ned() * v; - } + Vector3f body_to_earth(const Vector3f &v) const; // convert a vector from earth to body frame - Vector3f earth_to_body(const Vector3f &v) const { - return get_rotation_body_to_ned().mul_transpose(v); - } + Vector3f earth_to_body(const Vector3f &v) const; /* * methods for the benefit of LUA bindings @@ -553,7 +562,7 @@ class AP_AHRS { // get_accel() vector to get best current body frame accel // estimate const Vector3f &get_accel_bias(void) const { - return _accel_bias; + return state.accel_bias; } /* @@ -660,11 +669,7 @@ class AP_AHRS { EXTERNAL = 11, #endif }; - EKFType active_EKF_type(void) const; - - // 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; + EKFType active_EKF_type(void) const { return state.active_EKF; } bool always_use_EKF() const { return _ekf_flags & FLAG_ALWAYS_USE_EKF; @@ -690,9 +695,6 @@ class AP_AHRS { // update roll_sensor, pitch_sensor and yaw_sensor void update_cd_values(void); - // return origin for a specified EKF type - bool get_origin(EKFType type, Location &ret) const; - // helper trig variables float _cos_roll{1.0f}; float _cos_pitch{1.0f}; @@ -711,12 +713,7 @@ class AP_AHRS { #endif // 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; uint32_t start_time_ms; @@ -726,7 +723,7 @@ class AP_AHRS { void update_DCM(); // 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 @@ -799,9 +796,6 @@ class AP_AHRS { */ 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 * should generally be moving in the direction of its heading. @@ -843,6 +837,121 @@ class AP_AHRS { void Write_AHRS2(void) const; // write POS (canonical vehicle position) message out: 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 { diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 62d7a3f451c1c5..eb6fad02316346 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -106,9 +106,9 @@ void AP_AHRS::write_video_stabilisation() const const struct log_Video_Stabilisation pkt { LOG_PACKET_HEADER_INIT(LOG_VIDEO_STABILISATION_MSG), time_us : AP_HAL::micros64(), - gyro_x : _gyro_estimate.x, - gyro_y : _gyro_estimate.y, - gyro_z : _gyro_estimate.z, + gyro_x : state.gyro_estimate.x, + gyro_y : state.gyro_estimate.y, + gyro_z : state.gyro_estimate.z, accel_x : accel.x, accel_y : accel.y, accel_z : accel.z,