diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index dcb227349e..2f2249312a 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -110,31 +110,6 @@ void Ekf::controlFusionModes() _gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); } - - if (_mag_buffer) { - _mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); - - if (_mag_data_ready) { - _mag_lpf.update(_mag_sample_delayed.mag); - - // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. - // this is useful if there is a lot of interference on the sensor measurement. - if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) - && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) - ) { - - const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); - _mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred); - _control_status.flags.synthetic_mag_z = true; - - } else { - _control_status.flags.synthetic_mag_z = false; - } - } - - - } - if (_range_buffer) { // Get range data from buffer and check validity bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); @@ -371,7 +346,14 @@ void Ekf::controlExternalVisionFusion() resetYawToEv(); } - fuseHeading(); + if (shouldUse321RotationSequence(_R_to_earth)) { + float measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat); + fuseYaw321(measured_hdg, _ev_sample_delayed.angVar); + + } else { + float measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat); + fuseYaw312(measured_hdg, _ev_sample_delayed.angVar); + } } // record observation and estimate for use next time diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 7e88e1ca3d..7984842ea4 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -143,16 +143,20 @@ bool Ekf::initialiseFilter() } // Sum the magnetometer measurements - if (_mag_buffer && _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - if (_mag_sample_delayed.time_us != 0) { - if (_mag_counter == 0) { - _mag_lpf.reset(_mag_sample_delayed.mag); + if (_mag_buffer) { + magSample mag_sample; - } else { - _mag_lpf.update(_mag_sample_delayed.mag); + if (_mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample)) { + if (mag_sample.time_us != 0) { + if (_mag_counter == 0) { + _mag_lpf.reset(mag_sample.mag); + + } else { + _mag_lpf.update(mag_sample.mag); + } + + _mag_counter++; } - - _mag_counter++; } } @@ -192,7 +196,7 @@ bool Ekf::initialiseFilter() // calculate the initial magnetic field and yaw alignment // but do not mark the yaw alignement complete as it needs to be // reset once the leveling phase is done - resetMagHeading(_mag_lpf.getState(), false, false); + resetMagHeading(false, false); // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index c9a009eadb..b14552ddf6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -364,7 +364,6 @@ private: // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused - bool _mag_data_ready{false}; ///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused bool _baro_data_ready{false}; ///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused @@ -577,22 +576,22 @@ private: void predictCovariance(); // ekf sequential fusion of magnetometer measurements - void fuseMag(); + void fuseMag(const Vector3f &mag); // fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer) - void fuseHeading(); + void fuseHeading(float measured_hdg = NAN, float obs_var = NAN); // fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence // yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad) // yaw_variance : variance of the yaw angle observation (rad^2) // zero_innovation : Fuse data with innovation set to zero - void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation); + void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false); // fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence // yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad) // yaw_variance : variance of the yaw angle observation (rad^2) // zero_innovation : Fuse data with innovation set to zero - void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation); + void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector // innovation : prediction - measurement @@ -700,7 +699,7 @@ private: // reset the heading and magnetic field states using the declination and magnetometer measurements // return true if successful - bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer = true); + bool resetMagHeading(bool increase_yaw_var = true, bool update_buffer = true); // reset the heading using the external vision measurements // return true if successful @@ -708,7 +707,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(); + bool realignYawGPS(const Vector3f &mag); // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); @@ -839,7 +838,7 @@ private: void runOnGroundYawReset(); bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; } bool canResetMagHeading() const; - void runInAirYawReset(); + void runInAirYawReset(const Vector3f &mag); bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; } void runVelPosReset(); @@ -854,11 +853,11 @@ private: void checkMagDeclRequired(); void checkMagInhibition(); bool shouldInhibitMag() const; - void checkMagFieldStrength(); + void checkMagFieldStrength(const Vector3f &mag); bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; } static bool isMeasuredMatchingExpected(float measured, float expected, float gate); - void runMagAndMagDeclFusions(); - void run3DMagAndDeclFusions(); + void runMagAndMagDeclFusions(const Vector3f &mag); + void run3DMagAndDeclFusions(const Vector3f &mag); // control fusion of range finder observations void controlRangeFinderFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 3f99570217..397ea3a44e 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -376,7 +376,7 @@ 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() +bool Ekf::realignYawGPS(const Vector3f &mag) { const float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1))); @@ -386,7 +386,7 @@ bool Ekf::realignYawGPS() if (!gps_yaw_alignment_possible) { // attempt a normal alignment using the magnetometer - return resetMagHeading(_mag_lpf.getState()); + return resetMagHeading(); } // check for excessive horizontal GPS velocity innovations @@ -455,7 +455,7 @@ bool Ekf::realignYawGPS() // Use the last magnetometer measurements to reset the field states _state.mag_B.zero(); _R_to_earth = Dcmf(_state.quat_nominal); - _state.mag_I = _R_to_earth * _mag_sample_delayed.mag; + _state.mag_I = _R_to_earth * mag; resetMagCov(); @@ -471,7 +471,7 @@ bool Ekf::realignYawGPS() // align mag states only // calculate initial earth magnetic field states - _state.mag_I = _R_to_earth * _mag_sample_delayed.mag; + _state.mag_I = _R_to_earth * mag; resetMagCov(); @@ -483,18 +483,28 @@ bool Ekf::realignYawGPS() } // Reset heading and magnetic field states -bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool update_buffer) +bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer) { // prevent a reset being performed more than once on the same frame if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) { return true; } + // low pass filtered mag required + if (_mag_counter == 0) { + return false; + } + + const Vector3f mag_init = _mag_lpf.getState(); + // calculate the observed yaw angle and yaw variance float yaw_new; float yaw_new_variance = 0.0f; - if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + const bool heading_required_for_navigation = _control_status.flags.gps || _control_status.flags.ev_pos; + + if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) && heading_required_for_navigation)) { + // rotate the magnetometer measurements into earth frame using a zero yaw angle const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 41b6379675..a7b3ae0e3a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -299,7 +299,6 @@ protected: imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon // measurement samples capturing measurements on the delayed time horizon - magSample _mag_sample_delayed{}; baroSample _baro_sample_delayed{}; gpsSample _gps_sample_delayed{}; sensor::SensorRangeFinder _range_sensor{}; diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index eade218a4e..eb5184021b 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -70,7 +70,7 @@ void Ekf::controlGpsFusion() fuseGpsVelPos(); - if (shouldResetGpsFusion()){ + if (shouldResetGpsFusion()) { const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000); /* A reset is not performed when getting GPS back after a significant period of no data @@ -88,7 +88,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 - _control_status.flags.mag_aligned_in_flight = realignYawGPS(); + _mag_yaw_reset_req = true; } _warning_events.flags.gps_fusion_timout = true; @@ -131,8 +131,7 @@ void Ekf::controlGpsFusion() startGpsFusion(); } - } else if(!_control_status.flags.yaw_align - && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) { + } else if (!_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) { // If no mag is used, align using the yaw estimator _do_ekfgsf_yaw_reset = true; } @@ -208,7 +207,7 @@ void Ekf::processYawEstimatorResetRequest() * to improve its estimate if the previous reset was not successful. */ if (_do_ekfgsf_yaw_reset - && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){ + && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) { if (resetYawToEKFGSF()) { _ekfgsf_yaw_reset_time = _time_last_imu; _time_last_hor_pos_fuse = _time_last_imu; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 6946c1ab4c..b4800a9ed7 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -41,7 +41,35 @@ void Ekf::controlMagFusion() { - checkMagFieldStrength(); + bool mag_data_ready = false; + + magSample mag_sample; + + if (_mag_buffer) { + mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample); + + if (mag_data_ready) { + _mag_lpf.update(mag_sample.mag); + _mag_counter++; + + // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. + // this is useful if there is a lot of interference on the sensor measurement. + if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) + && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) + ) { + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); + mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred); + _control_status.flags.synthetic_mag_z = true; + + } else { + _control_status.flags.synthetic_mag_z = false; + } + } + } + + if (mag_data_ready) { + checkMagFieldStrength(mag_sample.mag); + } // If we are on ground, reset the flight alignment flag so that the mag fields will be // re-initialised next time we achieve flight altitude @@ -76,7 +104,7 @@ void Ekf::controlMagFusion() _mag_yaw_reset_req |= !_control_status.flags.yaw_align; _mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req; - if (noOtherYawAidingThanMag() && _mag_data_ready) { + if (noOtherYawAidingThanMag() && mag_data_ready) { // Determine if we should use simple magnetic heading fusion which works better when // there are large external disturbances or the more accurate 3-axis fusion switch (_params.mag_fusion_type) { @@ -101,7 +129,7 @@ void Ekf::controlMagFusion() if (_control_status.flags.in_air) { checkHaglYawResetReq(); - runInAirYawReset(); + runInAirYawReset(mag_sample.mag); runVelPosReset(); // TODO: review this; a vel/pos reset can be requested from COG reset (for fixedwing) only } else { @@ -116,7 +144,7 @@ void Ekf::controlMagFusion() checkMagDeclRequired(); checkMagInhibition(); - runMagAndMagDeclFusions(); + runMagAndMagDeclFusions(mag_sample.mag); } } @@ -142,9 +170,7 @@ void Ekf::checkHaglYawResetReq() void Ekf::runOnGroundYawReset() { if (_mag_yaw_reset_req && isYawResetAuthorized()) { - const bool has_realigned_yaw = canResetMagHeading() - ? resetMagHeading(_mag_lpf.getState()) - : false; + const bool has_realigned_yaw = canResetMagHeading() ? resetMagHeading() : false; if (has_realigned_yaw) { _mag_yaw_reset_req = false; @@ -166,16 +192,16 @@ bool Ekf::canResetMagHeading() const return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE); } -void Ekf::runInAirYawReset() +void Ekf::runInAirYawReset(const Vector3f &mag_sample) { if (_mag_yaw_reset_req && isYawResetAuthorized()) { bool has_realigned_yaw = false; if (canRealignYawUsingGps()) { - has_realigned_yaw = realignYawGPS(); + has_realigned_yaw = realignYawGPS(mag_sample); } else if (canResetMagHeading()) { - has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); + has_realigned_yaw = resetMagHeading(); } if (has_realigned_yaw) { @@ -300,25 +326,23 @@ bool Ekf::shouldInhibitMag() const || isStrongMagneticDisturbance(); } -void Ekf::checkMagFieldStrength() +void Ekf::checkMagFieldStrength(const Vector3f &mag_sample) { - if (_mag_data_ready) { - if (_params.check_mag_strength - && ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) { + if (_params.check_mag_strength + && ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) { - if (PX4_ISFINITE(_mag_strength_gps)) { - constexpr float wmm_gate_size = 0.2f; // +/- Gauss - _control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size); - - } else { - constexpr float average_earth_mag_field_strength = 0.45f; // Gauss - constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss - _control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); - } + if (PX4_ISFINITE(_mag_strength_gps)) { + constexpr float wmm_gate_size = 0.2f; // +/- Gauss + _control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size); } else { - _control_status.flags.mag_field_disturbed = false; + constexpr float average_earth_mag_field_strength = 0.45f; // Gauss + constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss + _control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); } + + } else { + _control_status.flags.mag_field_disturbed = false; } } @@ -328,17 +352,25 @@ bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, && (measured <= expected + gate); } -void Ekf::runMagAndMagDeclFusions() +void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) { if (_control_status.flags.mag_3D) { - run3DMagAndDeclFusions(); + run3DMagAndDeclFusions(mag); } else if (_control_status.flags.mag_hdg) { - fuseHeading(); + // Rotate the measurements into earth frame using the zero yaw angle + Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth); + + Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B); + + // the angle of the projection onto the horizontal gives the yaw angle + float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + + fuseHeading(measured_hdg, sq(_params.mag_heading_noise)); } } -void Ekf::run3DMagAndDeclFusions() +void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) { if (!_mag_decl_cov_reset) { // After any magnetic field covariance reset event the earth field state @@ -347,13 +379,13 @@ void Ekf::run3DMagAndDeclFusions() // states for the first few observations. fuseDeclination(0.02f); _mag_decl_cov_reset = true; - fuseMag(); + fuseMag(mag); } else { // The normal sequence is to fuse the magnetometer data first before fusing // declination angle at a higher uncertainty to allow some learning of // declination angle over time. - fuseMag(); + fuseMag(mag); if (_control_status.flags.mag_dec) { fuseDeclination(0.5f); diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 34699bb8d3..f2ffd2b0d5 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -45,7 +45,7 @@ #include -void Ekf::fuseMag() +void Ekf::fuseMag(const Vector3f &mag) { // assign intermediate variables const float &q0 = _state.quat_nominal(0); @@ -161,7 +161,7 @@ void Ekf::fuseMag() const Vector3f mag_I_rot = R_to_body * _state.mag_I; // compute magnetometer innovations - _mag_innov = mag_I_rot + _state.mag_B - _mag_sample_delayed.mag; + _mag_innov = mag_I_rot + _state.mag_B - mag; // do not use the synthesized measurement for the magnetomter Z component for 3D fusion if (_control_status.flags.synthetic_mag_z) { @@ -714,149 +714,62 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f } } -void Ekf::fuseHeading() +void Ekf::fuseHeading(float measured_hdg, float obs_var) { - Vector3f mag_earth_pred; - float measured_hdg; - - // Calculate the observation variance - float R_YAW; - - if (_control_status.flags.mag_hdg) { - // using magnetic heading tuning parameter - R_YAW = sq(_params.mag_heading_noise); - - } else if (_control_status.flags.ev_yaw) { - // using error estimate from external vision data - R_YAW = _ev_sample_delayed.angVar; - - } else { - // default value - R_YAW = 0.01f; - } + // observation variance + float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f; // update transformation matrix from body to world frame using the current state estimate _R_to_earth = Dcmf(_state.quat_nominal); - if (shouldUse321RotationSequence(_R_to_earth)) { - const float predicted_hdg = getEuler321Yaw(_R_to_earth); + const bool use_321_rotation_seq = shouldUse321RotationSequence(_R_to_earth); - if (_control_status.flags.mag_hdg) { - // Rotate the measurements into earth frame using the zero yaw angle - const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth); - mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + const float predicted_hdg = use_321_rotation_seq ? getEuler321Yaw(_R_to_earth) : getEuler312Yaw(_R_to_earth); - // the angle of the projection onto the horizontal gives the yaw angle - measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + if (!PX4_ISFINITE(measured_hdg)) { + measured_hdg = predicted_hdg; + } - } else if (_control_status.flags.ev_yaw) { - measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat); + // handle special case where yaw measurement is unavailable + bool fuse_zero_innov = false; - } else { - measured_hdg = predicted_hdg; - } - - // handle special case where yaw measurement is unavailable - bool fuse_zero_innov = false; - - if (_is_yaw_fusion_inhibited) { - // The yaw measurement cannot be trusted but we need to fuse something to prevent a badly - // conditioned covariance matrix developing over time. - if (!_control_status.flags.vehicle_at_rest) { - // Vehicle is not at rest so fuse a zero innovation if necessary to prevent - // unconstrained quaternion variance growth and record the predicted heading - // to use as an observation when movement ceases. - // TODO a better way of determining when this is necessary - const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3); - - if (sumQuatVar > _params.quat_max_variance) { - fuse_zero_innov = true; - R_YAW = 0.25f; - - } - - _last_static_yaw = predicted_hdg; - - } else { - // Vehicle is at rest so use the last moving prediction as an observation - // to prevent the heading from drifting and to enable yaw gyro bias learning - // before takeoff. - - if (!PX4_ISFINITE(_last_static_yaw)) { - _last_static_yaw = predicted_hdg; - } - - measured_hdg = _last_static_yaw; + if (_is_yaw_fusion_inhibited) { + // The yaw measurement cannot be trusted but we need to fuse something to prevent a badly + // conditioned covariance matrix developing over time. + if (!_control_status.flags.vehicle_at_rest) { + // Vehicle is not at rest so fuse a zero innovation if necessary to prevent + // unconstrained quaternion variance growth and record the predicted heading + // to use as an observation when movement ceases. + // TODO a better way of determining when this is necessary + const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3); + if (sumQuatVar > _params.quat_max_variance) { + fuse_zero_innov = true; + R_YAW = 0.25f; } - } else { _last_static_yaw = predicted_hdg; + } else { + // Vehicle is at rest so use the last moving prediction as an observation + // to prevent the heading from drifting and to enable yaw gyro bias learning + // before takeoff. + if (!PX4_ISFINITE(_last_static_yaw)) { + _last_static_yaw = predicted_hdg; + } + + measured_hdg = _last_static_yaw; } + } else { + _last_static_yaw = predicted_hdg; + } + + if (use_321_rotation_seq) { fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov); } else { - const float predicted_hdg = getEuler312Yaw(_R_to_earth); - - if (_control_status.flags.mag_hdg) { - - // rotate the magnetometer measurements into earth frame using a zero yaw angle - const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth); - mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); - - // the angle of the projection onto the horizontal gives the yaw angle - measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - - } else if (_control_status.flags.ev_yaw) { - measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat); - - } else { - measured_hdg = predicted_hdg; - } - - // handle special case where yaw measurement is unavailable - bool fuse_zero_innov = false; - - if (_is_yaw_fusion_inhibited) { - // The yaw measurement cannot be trusted but we need to fuse something to prevent a badly - // conditioned covariance matrix developing over time. - if (!_control_status.flags.vehicle_at_rest) { - // Vehicle is not at rest so fuse a zero innovation if necessary to prevent - // unconstrained quaterniion variance growth and record the predicted heading - // to use as an observation when movement ceases. - // TODO a better way of determining when this is necessary - const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3); - - if (sumQuatVar > _params.quat_max_variance) { - fuse_zero_innov = true; - R_YAW = 0.25f; - - } - - _last_static_yaw = predicted_hdg; - - } else { - // Vehicle is at rest so use the last moving prediction as an observation - // to prevent the heading from drifting and to enable yaw gyro bias learning - // before takeoff. - - if (!PX4_ISFINITE(_last_static_yaw)) { - _last_static_yaw = predicted_hdg; - } - - measured_hdg = _last_static_yaw; - - } - - } else { - _last_static_yaw = predicted_hdg; - - } - fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov); - } }