diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 0668f5a2ad..73595f1e6f 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -519,7 +519,6 @@ private: bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected - uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited float _last_static_yaw{NAN}; ///< last yaw angle recorded when on ground motion checks were passing (rad) @@ -776,10 +775,6 @@ private: // return true if successful bool resetYawToEv(); - // 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); - // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); @@ -899,7 +894,7 @@ private: void runOnGroundYawReset(); bool canResetMagHeading() const; - void runInAirYawReset(const Vector3f &mag); + void runInAirYawReset(); void selectMagAuto(); void check3DMagFusionSuitability(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 8eef82f6a4..c1187ba7cf 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -298,117 +298,6 @@ void Ekf::alignOutputFilter() _output_new = _output_buffer.get_newest(); } -// 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) -{ - 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)); - - if (!gps_yaw_alignment_possible) { - // attempt a normal alignment using the magnetometer - return resetMagHeading(); - } - - // check for excessive horizontal GPS velocity innovations - const float gps_vel_test_ratio = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]); - const bool badVelInnov = (gps_vel_test_ratio > 1.0f) && _control_status.flags.gps; - - // calculate GPS course over ground angle - const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); - - // calculate course yaw angle - const float ekfCOG = atan2f(_state.vel(1), _state.vel(0)); - - // Check the EKF and GPS course over ground for consistency - 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 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) - && !_control_status.flags.mag_fault) { - _warning_events.flags.stopping_mag_use = true; - ECL_WARN("stopping mag use"); - _control_status.flags.mag_fault = true; - } - - // calculate new yaw estimate - float yaw_new; - - if (!_control_status.flags.mag_aligned_in_flight) { - // This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a - // forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error - const float current_yaw = getEulerYaw(_R_to_earth); - yaw_new = current_yaw + courseYawError; - _control_status.flags.mag_aligned_in_flight = true; - - } else if (_control_status.flags.wind) { - // we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is - // aligned with the wind relative GPS velocity vector - yaw_new = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)), - (_gps_sample_delayed.vel(0) - _state.wind_vel(0))); - - } 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)); - - } - - // use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment - const float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P(4, 4) + P(5, 5); - const float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f); - const float yaw_variance_new = sq(asinf(sineYawError)); - - // Apply updated yaw and yaw variance to states and covariances - resetQuatStateYaw(yaw_new, yaw_variance_new); - - // Use the last magnetometer measurements to reset the field states - _state.mag_B.zero(); - _state.mag_I = _R_to_earth * mag; - - resetMagCov(); - - // record the start time for the magnetic field alignment - _flt_mag_align_start_time = _imu_sample_delayed.time_us; - - // If heading was bad, then we also need to reset the velocity and position states - if (badMagYaw) { - resetVelocityToGps(_gps_sample_delayed); - resetHorizontalPositionToGps(_gps_sample_delayed); - } - - return true; - - } else { - // align mag states only - - // calculate initial earth magnetic field states - _state.mag_I = _R_to_earth * mag; - - resetMagCov(); - - // record the start time for the magnetic field alignment - _flt_mag_align_start_time = _imu_sample_delayed.time_us; - - return true; - } -} - // Reset heading and magnetic field states bool Ekf::resetMagHeading() { diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index a0ad6b0bc5..bff29a7f74 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -127,18 +127,9 @@ void Ekf::controlGpsFusion() if (resetYawToEKFGSF()) { ECL_WARN("GPS emergency yaw reset"); } - - } else { - // 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; - } - - _warning_events.flags.gps_fusion_timout = true; - ECL_WARN("GPS fusion timeout - resetting"); } + ECL_WARN("GPS fusion timeout - resetting"); resetVelocityToGps(gps_sample); resetHorizontalPositionToGps(gps_sample); } diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 09af32b0d6..b12ce5db08 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -94,7 +94,6 @@ void Ekf::controlMagFusion() // re-initialised next time we achieve flight altitude if (!_control_status.flags.in_air) { _control_status.flags.mag_aligned_in_flight = false; - _num_bad_flight_yaw_events = 0; } if (_params.mag_fusion_type >= MagFuseType::NONE @@ -141,7 +140,7 @@ void Ekf::controlMagFusion() if (_control_status.flags.in_air) { checkHaglYawResetReq(); - runInAirYawReset(mag_sample.mag); + runInAirYawReset(); } else { runOnGroundYawReset(); @@ -197,13 +196,43 @@ bool Ekf::canResetMagHeading() const return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MagFuseType::NONE); } -void Ekf::runInAirYawReset(const Vector3f &mag_sample) +void Ekf::runInAirYawReset() { if (_mag_yaw_reset_req && !_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); + // use yaw estimator if available + if (_control_status.flags.gps && isYawEmergencyEstimateAvailable() && + (_mag_counter != 0) && isNewestSampleRecent(_time_last_mag_buffer_push, 500'000) // mag LPF available + ) { + + resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); + + _information_events.flags.yaw_aligned_to_imu_gps = true; + _ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us; + + // if world magnetic model (inclination, declination, strength) available then use it to reset mag states + if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) { + // use predicted earth field to reset states + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); + _state.mag_I = mag_earth_pred; + + const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); + _state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred); + + } else { + // Use the last magnetometer measurements to reset the field states + // calculate initial earth magnetic field states + _state.mag_I = _R_to_earth * _mag_lpf.getState(); + _state.mag_B.zero(); + } + + ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]", + (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2), + (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2) + ); + + resetMagCov(); } if (!has_realigned_yaw && canResetMagHeading()) { @@ -215,6 +244,9 @@ void Ekf::runInAirYawReset(const Vector3f &mag_sample) _control_status.flags.yaw_align = true; _control_status.flags.mag_aligned_in_flight = true; + // record the time for the magnetic field alignment event + _flt_mag_align_start_time = _imu_sample_delayed.time_us; + // 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) {