diff --git a/EKF/common.h b/EKF/common.h index 643ccecdb5..70cb67cf49 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -179,6 +179,7 @@ struct dragSample { #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. // Maximum sensor intervals in usec #define GPS_MAX_INTERVAL 5e5 ///< Maximum allowable time interval between GPS measurements (uSec) diff --git a/EKF/control.cpp b/EKF/control.cpp index e8c1673d1f..d4690753e3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1105,7 +1105,7 @@ void Ekf::controlMagFusion() // do no magnetometer fusion at all _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = false; - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) { // Check if height has increased sufficiently to be away from ground magnetic anomalies bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f; @@ -1188,7 +1188,15 @@ void Ekf::controlMagFusion() _control_status.flags.mag_hdg = true; } - // perform switch-over from only updating the mag states to updating all states + /* + 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 = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) + && _control_status.flags.fixed_wing; + if (!_control_status.flags.update_mag_states_only && _control_status_prev.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 diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 835ed6a44b..8277ba0fab 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -506,7 +506,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // 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_3D) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; // the angle of the projection onto the horizontal gives the yaw angle @@ -579,7 +579,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // 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_3D) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; // the angle of the projection onto the horizontal gives the yaw angle @@ -616,7 +616,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // using error estimate from external vision data angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // using magnetic heading tuning parameter angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); }