commander: add pre-flight check and parameter for magnetic field strength

msg/estimator_status.msg

@@ -110,6 +110,7 @@ bool pre_flt_fail_innov_heading
 bool pre_flt_fail_innov_vel_horiz
 bool pre_flt_fail_innov_vel_vert
 bool pre_flt_fail_innov_height
+bool pre_flt_fail_mag_field_disturbed
 
 # legacy local position estimator (LPE) flags
 uint8 health_flags		# Bitmask to indicate sensor health states (vel, pos, hgt)

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -65,7 +65,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	if (status.pre_flt_fail_innov_heading ||
 	    status.pre_flt_fail_innov_vel_horiz ||
 	    status.pre_flt_fail_innov_vel_vert ||
-	    status.pre_flt_fail_innov_height) {
+	    status.pre_flt_fail_innov_height ||
+	    status.pre_flt_fail_mag_field_disturbed) {
 		if (report_fail) {
 			if (status.pre_flt_fail_innov_heading) {
 				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
@@ -78,6 +79,9 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 
 			} else if (status.pre_flt_fail_innov_height) {
 				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
+
+			} else if (status.pre_flt_fail_mag_field_disturbed) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
 			}
 		}
 

src/modules/ekf2/ekf2_main.cpp

@@ -522,7 +522,8 @@ private:
 		(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
 		_param_ekf2_move_test,	///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
 
-		(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h	///< Required GPS health time
+		(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
+		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check ///< Mag field strength check
 
 	)
 
@@ -627,7 +628,8 @@ Ekf2::Ekf2(bool replay_mode):
 	_param_ekf2_drag_noise(_params->drag_noise),
 	_param_ekf2_bcoef_x(_params->bcoef_x),
 	_param_ekf2_bcoef_y(_params->bcoef_y),
-	_param_ekf2_move_test(_params->is_moving_scaler)
+	_param_ekf2_move_test(_params->is_moving_scaler),
+	_param_ekf2_mag_check(_params->check_mag_strength)
 {
 	// initialise parameter cache
 	updateParams();
@@ -1541,6 +1543,7 @@ void Ekf2::Run()
 			status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
 			status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
 			status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
+			status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed;
 
 			_estimator_status_pub.publish(status);
 

src/modules/ekf2/ekf2_params.c

@@ -1403,3 +1403,17 @@ PARAM_DEFINE_FLOAT(EKF2_MOVE_TEST, 1.0f);
  * @reboot_required true
  */
 PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f);
+
+/**
+ * Magnetic field strength test selection
+ *
+ * When set, the EKF checks the strength of the magnetic field
+ * to decide whether the magnetometer data is valid.
+ * If GPS data is received, the magnetic field is compared to a World
+ * Magnetic Model (WMM), otherwise an average value is used.
+ * This check is useful to reject occasional hard iron disturbance.
+ *
+ * @group EKF2
+ * @boolean
+ */
+PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 0);