diff --git a/EKF/common.h b/EKF/common.h index 0bfcc9f4fe..f32e35da57 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -200,7 +200,7 @@ struct auxVelSample { #define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic #define MAG_FUSE_TYPE_HEADING 1 ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg #define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions -#define MAG_FUSE_TYPE_AUTOFW 3 ///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. +#define MAG_FUSE_TYPE_UNUSED 3 ///< Not implemented #define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates. #define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter. diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 6708d7ad06..ceb9d1fae9 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -261,7 +261,7 @@ bool Ekf::initialiseFilter() if (_control_status.flags.ev_yaw) { // using error estimate from external vision data TODO: this is never true increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // using magnetic heading tuning parameter increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); } diff --git a/EKF/ekf.h b/EKF/ekf.h index 66f3043287..a6afa174a4 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -637,7 +637,6 @@ class Ekf : public EstimatorInterface bool isMagBiasObservable() const; bool canUse3DMagFusion() const; - void controlMagStateOnlyFusion(); void checkMagDeclRequired(); void checkMagInhibition(); bool shouldInhibitMag() const; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8307dc4a14..60420ca3cb 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -589,7 +589,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // calculate the yaw angle for a 312 sequence euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle @@ -647,7 +647,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // calculate the yaw angle for a 312 sequence euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle @@ -723,7 +723,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool if (_control_status.flags.ev_yaw) { // using error estimate from external vision data increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // using magnetic heading tuning parameter increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); } diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp index 9608bb75dd..66008bd9af 100644 --- a/EKF/mag_control.cpp +++ b/EKF/mag_control.cpp @@ -76,11 +76,6 @@ void Ekf::controlMagFusion() selectMagAuto(); break; - case MAG_FUSE_TYPE_AUTOFW: - selectMagAuto(); - controlMagStateOnlyFusion(); - break; - case MAG_FUSE_TYPE_INDOOR: /* fallthrough */ case MAG_FUSE_TYPE_HEADING: @@ -92,7 +87,7 @@ void Ekf::controlMagFusion() break; default: - stopMagFusion(); + selectMagAuto(); break; } @@ -251,37 +246,6 @@ bool Ekf::canUse3DMagFusion() const && ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6); } -void Ekf::controlMagStateOnlyFusion() -{ - /* - Control switch-over between only updating the mag states to updating all states - When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates. - When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing, then magnetometer fusion is only allowed - to access the magnetic field states. - */ - _control_status.flags.update_mag_states_only = _control_status.flags.fixed_wing; - - // For the first 5 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise - // before they are used to constrain heading drift - _flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6); - - if (_control_status.flags.mag_3D - && _control_status_prev.flags.update_mag_states_only - && !_control_status.flags.update_mag_states_only) { - // When re-commencing use of magnetometer to correct vehicle states - // set the field state variance to the observation variance and zero - // the covariance terms to allow the field states re-learn rapidly - clearMagCov(); - - for (uint8_t index = 0; index <= 5; index ++) { - P[index + 16][index + 16] = sq(_params.mag_noise); - } - - // save covariance data for re-use when auto-switching between heading and 3-axis fusion - saveMagCovData(); - } -} - void Ekf::checkMagDeclRequired() { // if we are using 3-axis magnetometer fusion, but without external NE aiding,