From cdf9e6d35a23672b3b46a607e8377fe10d3dff77 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 21 Mar 2022 14:49:24 -0400 Subject: [PATCH] ekf2: merge runOnGroundYawReset() and runInAirYawReset() --- src/modules/ekf2/EKF/ekf.h | 8 ++-- src/modules/ekf2/EKF/ekf_helper.cpp | 32 +++++++------ src/modules/ekf2/EKF/gps_control.cpp | 2 +- src/modules/ekf2/EKF/mag_control.cpp | 69 +++++++++------------------- 4 files changed, 45 insertions(+), 66 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index de35f40c38..518ce537fa 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -724,7 +724,7 @@ private: // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle. - bool realignYawGPS(const Vector3f &mag); + bool realignYawGPS(bool force = false); // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); @@ -844,12 +844,10 @@ private: // control fusion of magnetometer observations void controlMagFusion(); - void checkHaglYawResetReq(); + bool haglYawResetReq() const; float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } - void runOnGroundYawReset(); - bool canResetMagHeading() const; - void runInAirYawReset(const Vector3f &mag); + void runYawReset(); void selectMagAuto(); void check3DMagFusionSuitability(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 88530272da..d14ba0a9e1 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -338,21 +338,25 @@ void Ekf::alignOutputFilter() // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only. -bool Ekf::realignYawGPS(const Vector3f &mag) +bool Ekf::realignYawGPS(bool force) { + if (!_control_status.flags.in_air || !_control_status.flags.fixed_wing) { + return false; + } + const float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1))); // Need at least 5 m/s of GPS horizontal speed and // ratio of velocity error to velocity < 0.15 for a reliable alignment - const bool gps_yaw_alignment_possible = (gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed)); + const bool gps_yaw_alignment_possible = (gpsSpeed > 5.f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed)); if (!gps_yaw_alignment_possible) { // attempt a normal alignment using the magnetometer - return resetMagHeading(); + return false; } // check for excessive horizontal GPS velocity innovations - const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps; + const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.f) && _control_status.flags.gps; // calculate GPS course over ground angle const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); @@ -364,25 +368,28 @@ bool Ekf::realignYawGPS(const Vector3f &mag) const float courseYawError = wrap_pi(gpsCOG - ekfCOG); // If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad - const bool badYawErr = fabsf(courseYawError) > 0.5f; + const bool badYawErr = fabsf(courseYawError) > math::radians(25.f); const bool badMagYaw = (badYawErr && badVelInnov); if (badMagYaw) { _num_bad_flight_yaw_events++; - } - // correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned - if (badMagYaw || !_control_status.flags.yaw_align) { _warning_events.flags.bad_yaw_using_gps_course = true; ECL_WARN("bad yaw, using GPS course"); // declare the magnetometer as failed if a bad yaw has occurred more than once - if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) + if (_control_status.flags.mag_aligned_in_flight + && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) { + _warning_events.flags.stopping_mag_use = true; ECL_WARN("stopping mag use"); _control_status.flags.mag_fault = true; } + } + + // correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned + if (badMagYaw || !_control_status.flags.yaw_align || force) { // calculate new yaw estimate float yaw_new; @@ -402,8 +409,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag) } else { // we don't have wind estimates, so align yaw to the GPS velocity vector - yaw_new = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); - + yaw_new = gpsCOG; } // use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment @@ -416,7 +422,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag) // Use the last magnetometer measurements to reset the field states _state.mag_B.zero(); - _state.mag_I = _R_to_earth * mag; + _state.mag_I = _R_to_earth * _mag_lpf.getState(); resetMagCov(); @@ -435,7 +441,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag) // align mag states only // calculate initial earth magnetic field states - _state.mag_I = _R_to_earth * mag; + _state.mag_I = _R_to_earth * _mag_lpf.getState(); resetMagCov(); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index c78eea0871..48a9f526fd 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -91,7 +91,7 @@ void Ekf::controlGpsFusion() // use GPS velocity data to check and correct yaw angle if a FW vehicle if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { // if flying a fixed wing aircraft, do a complete reset that includes yaw - _mag_yaw_reset_req = true; + realignYawGPS(); } _warning_events.flags.gps_fusion_timout = true; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 93d34d0714..bc0899e7b8 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -98,9 +98,6 @@ void Ekf::controlMagFusion() return; } - _mag_yaw_reset_req |= !_control_status.flags.yaw_align; - _mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req; - if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) { const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D; @@ -109,6 +106,7 @@ void Ekf::controlMagFusion() // there are large external disturbances or the more accurate 3-axis fusion switch (_params.mag_fusion_type) { default: + // FALLTHROUGH case MAG_FUSE_TYPE_AUTO: selectMagAuto(); @@ -128,16 +126,12 @@ void Ekf::controlMagFusion() const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D; - if (!mag_enabled_previously && mag_enabled) { - _mag_yaw_reset_req = true; - } + if (!_control_status.flags.yaw_align + || _mag_yaw_reset_req || _mag_inhibit_yaw_reset_req + || haglYawResetReq() + || (!mag_enabled_previously && mag_enabled)) { - if (_control_status.flags.in_air) { - checkHaglYawResetReq(); - runInAirYawReset(mag_sample.mag); - - } else { - runOnGroundYawReset(); + runYawReset(); } if (!_control_status.flags.yaw_align) { @@ -152,61 +146,43 @@ void Ekf::controlMagFusion() } } -void Ekf::checkHaglYawResetReq() +bool Ekf::haglYawResetReq() const { // We need to reset the yaw angle after climbing away from the ground to enable // recovery from ground level magnetic interference. - if (!_control_status.flags.mag_aligned_in_flight) { + if (_control_status.flags.in_air && !_control_status.flags.mag_aligned_in_flight) { // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. static constexpr float mag_anomalies_max_hagl = 1.5f; - const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; - _mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies; - } -} -void Ekf::runOnGroundYawReset() -{ - if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) { - const bool has_realigned_yaw = canResetMagHeading() ? resetMagHeading() : false; - - if (has_realigned_yaw) { - _mag_yaw_reset_req = false; - _control_status.flags.yaw_align = true; - - // Handle the special case where we have not been constraining yaw drift or learning yaw bias due - // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. - if (_mag_inhibit_yaw_reset_req) { - _mag_inhibit_yaw_reset_req = false; - // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty - P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg)); - } + if ((getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl) { + return true; } } + + return false; } -bool Ekf::canResetMagHeading() const +void Ekf::runYawReset() { - return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE); -} - -void Ekf::runInAirYawReset(const Vector3f &mag_sample) -{ - if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) { + if (!_is_yaw_fusion_inhibited) { bool has_realigned_yaw = false; if (_control_status.flags.gps && _control_status.flags.fixed_wing) { - has_realigned_yaw = realignYawGPS(mag_sample); + has_realigned_yaw = realignYawGPS(); } - if (!has_realigned_yaw && canResetMagHeading()) { + if (!has_realigned_yaw) { has_realigned_yaw = resetMagHeading(); } if (has_realigned_yaw) { _mag_yaw_reset_req = false; _control_status.flags.yaw_align = true; - _control_status.flags.mag_aligned_in_flight = true; + + if (_control_status.flags.in_air) { + _control_status.flags.mag_aligned_in_flight = true; + } // Handle the special case where we have not been constraining yaw drift or learning yaw bias due // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. @@ -258,7 +234,7 @@ void Ekf::checkMagBiasObservability() } else if (_mag_bias_observable) { // require sustained yaw motion of 50% the initial yaw rate threshold const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started); - const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt; + const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt; _mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req; } @@ -364,8 +340,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) { // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise // before they are used to constrain heading drift - const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6) - && !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed; + const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); if (!_mag_decl_cov_reset) { // After any magnetic field covariance reset event the earth field state