Reduce pre flight mag inconsistency test false positives

ROMFS/px4fmu_common/init.d/airframes/4250_teal

@@ -47,7 +47,8 @@ then
 	param set CAL_MAG0_ROT 0
 	param set CAL_MAG_SIDES 63
 	param set SENS_BOARD_ROT 0
-	param set COM_ARM_MAG 1.5
+	param set COM_ARM_MAG_H 4.0
+	param set COM_ARM_MAG_V 4.0
 	param set COM_ARM_EKF_AB 0.0032
 
 	# circuit breakers

msg/sensor_preflight.msg

@@ -5,4 +5,5 @@
 uint64 timestamp		   # time since system start (microseconds)
 float32 accel_inconsistency_m_s_s  # magnitude of maximum acceleration difference between IMU instances in (m/s/s).
 float32 gyro_inconsistency_rad_s   # magnitude of maximum angular rate difference between IMU instances in (rad/s).
-float32 mag_inconsistency_ga       # magnitude of maximum difference between Mag instances in (Gauss).
+float32 mag_inconsistency_h        # magnitude of maximum horizontal angular difference between Mag instances in (rad).
+float32 mag_inconsistency_v        # magnitude of maximum vertical (inclination) angular difference between Mag instances in (rad).

src/modules/commander/PreflightCheck.cpp

@@ -220,10 +220,13 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s
 
 	// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
 	// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
-	float test_limit;
-	param_get(param_find("COM_ARM_MAG"), &test_limit);
+	float test_limit_horiz;
+	param_get(param_find("COM_ARM_MAG_H"), &test_limit_horiz);
 
-	if (sensors.mag_inconsistency_ga > test_limit) {
+	float test_limit_vert;
+	param_get(param_find("COM_ARM_MAG_V"), &test_limit_vert);
+
+	if ((sensors.mag_inconsistency_h > test_limit_horiz) || (sensors.mag_inconsistency_v > test_limit_vert)) {
 		if (report_status) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensors inconsistent");
 			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status);

src/modules/commander/commander_params.c

@@ -599,16 +599,28 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_ACC, 0.7f);
 PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f);
 
 /**
- * Maximum magnetic field inconsistency between units that will allow arming
+ * Maximum magnetic field horizontal angle inconsistency between units that will allow arming. Values greater than Pi effectively disable this check.
  *
  * @group Commander
- * @unit Gauss
- * @min 0.05
- * @max 0.5
+ * @unit rad
+ * @min 0.1
+ * @max 4.0
+ * @decimal 2
+ * @increment 0.05
+ */
+PARAM_DEFINE_FLOAT(COM_ARM_MAG_H, 0.5f);
+
+/**
+ * Maximum magnetic field inclination angle inconsistency between units that will allow arming. Values greater than Pi effectively disable this check.
+ *
+ * @group Commander
+ * @unit rad
+ * @min 0.1
+ * @max 4.0
  * @decimal 2
  * @increment 0.05
  */
-PARAM_DEFINE_FLOAT(COM_ARM_MAG, 0.15f);
+PARAM_DEFINE_FLOAT(COM_ARM_MAG_V, 0.7f);
 
 /**
  * Enable RC stick override of auto modes

src/modules/sensors/voted_sensors_update.cpp

@@ -60,7 +60,7 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_en
 	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
 	memset(&_accel_diff, 0, sizeof(_accel_diff));
 	memset(&_gyro_diff, 0, sizeof(_gyro_diff));
-	memset(&_mag_diff, 0, sizeof(_mag_diff));
+	memset(&_mag_angle_diff, 0, sizeof(_mag_angle_diff));
 
 	// initialise the publication variables
 	memset(&_corrections, 0, sizeof(_corrections));
@@ -1209,7 +1209,8 @@ void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt)
 
 void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt)
 {
-	float mag_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
+	float mag_h_diff_max = 0.0f; // the maximum horizontal field angle difference (rad)
+	float mag_v_diff_max = 0.0f; // the maximum vertical field angle difference (rad)
 	unsigned check_index = 0; // the number of sensors the primary has been checked against
 
 	// Check each sensor against the primary
@@ -1218,21 +1219,50 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt)
 		// check that the sensor we are checking against is not the same as the primary
 		if ((_mag.priority[sensor_index] > 0) && (sensor_index != _mag.last_best_vote)) {
 
-			float mag_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary
+			// Get unit gravity vector in body frame (assume platform is not accelerating)
+			matrix::Vector3f gravity_vec(-_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[0],
+			-_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[1],
+			-_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[2]);
+			gravity_vec.normalize();
 
-			// calculate mag_diff_sum_sq for the specified sensor against the primary
-			for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-				_mag_diff[axis_index][check_index] = 0.95f * _mag_diff[axis_index][check_index] + 0.05f *
-								     (_last_magnetometer[_mag.last_best_vote].magnetometer_ga[axis_index] -
-								      _last_magnetometer[sensor_index].magnetometer_ga[axis_index]);
+			// calculate the cross product for each sensor with assumed gravity vector to produce a horizontal projection
+			// could use estimator body to earth rotation matrix to do this, but that would require subscription to
+			// a new topic and is unnecessary for quasi-static preflight checks.
+			matrix::Vector3f mag_vec_selected(_last_magnetometer[_mag.last_best_vote].magnetometer_ga[0],
+			_last_magnetometer[_mag.last_best_vote].magnetometer_ga[1],
+			_last_magnetometer[_mag.last_best_vote].magnetometer_ga[2]);
 
-				mag_diff_sum_sq += _mag_diff[axis_index][check_index] * _mag_diff[axis_index][check_index];
+			matrix::Vector3f mag_vec_alternate(_last_magnetometer[sensor_index].magnetometer_ga[0],
+			_last_magnetometer[sensor_index].magnetometer_ga[1],
+			_last_magnetometer[sensor_index].magnetometer_ga[2]);
 
-			}
+			matrix::Vector3f mag_hvec_selected = gravity_vec.cross(mag_vec_selected);
+			matrix::Vector3f mag_hvec_alternate = gravity_vec.cross(mag_vec_alternate);
+
+			// calculate the dot product for each sensor with the unit gravity vector to get the vertical component
+			float mag_down_selected = gravity_vec.dot(mag_vec_selected);
+			float mag_down_alternate = gravity_vec.dot(mag_vec_alternate);
+
+			// calculate the vertical angle between the two vectors
+			float mag_vang_selected = atan2f(mag_down_selected, mag_hvec_selected.norm());
+			float mag_vang_alternate = atan2f(mag_down_alternate, mag_hvec_alternate.norm());
+			float ang_v_delta = fabsf(mag_vang_selected - mag_vang_alternate);
+
+			// calculate the horizontal angle between the two vectors to estimate the difference in magnetic heading thee sensors would produce
+			matrix::Quatf q_delta(mag_hvec_selected, mag_hvec_alternate);
+			matrix::Vector3f ang_hvec_delta = q_delta.to_axis_angle();
+			float ang_h_delta = ang_hvec_delta.norm();
+
+			_mag_angle_diff[0][check_index] = 0.95f * _mag_angle_diff[0][check_index] + 0.05f * ang_h_delta;
+			_mag_angle_diff[1][check_index] = 0.95f * _mag_angle_diff[1][check_index] + 0.05f * ang_v_delta;
 
 			// capture the largest sum value
-			if (mag_diff_sum_sq > mag_diff_sum_max_sq) {
-				mag_diff_sum_max_sq = mag_diff_sum_sq;
+			if (fabsf(_mag_angle_diff[0][check_index]) > mag_h_diff_max) {
+				mag_h_diff_max = fabsf(_mag_angle_diff[0][check_index]);
+
+			}
+			if (fabsf(_mag_angle_diff[1][check_index]) > mag_v_diff_max) {
+				mag_v_diff_max = fabsf(_mag_angle_diff[1][check_index]);
 
 			}
 
@@ -1249,10 +1279,12 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt)
 
 	// skip check if less than 2 sensors
 	if (check_index < 1) {
-		preflt.mag_inconsistency_ga = 0.0f;
+		preflt.mag_inconsistency_h = 0.0f;
+		preflt.mag_inconsistency_v = 0.0f;
 
 	} else {
 		// get the vector length of the largest difference and write to the combined sensor struct
-		preflt.mag_inconsistency_ga = sqrtf(mag_diff_sum_max_sq);
+		preflt.mag_inconsistency_h = mag_h_diff_max;
+		preflt.mag_inconsistency_v = mag_v_diff_max;
 	}
 }

src/modules/sensors/voted_sensors_update.h

@@ -260,7 +260,7 @@ class VotedSensorsUpdate
 
 	float _accel_diff[3][2];	/**< filtered accel differences between IMU units (m/s/s) */
 	float _gyro_diff[3][2];		/**< filtered gyro differences between IMU uinits (rad/s) */
-	float _mag_diff[3][2];		/**< filtered mag differences between sensor instances (Ga) */
+	float _mag_angle_diff[2][2];	/**< filtered magnetic declination and inclination differences between sensor instances (rad) */
 
 	/* sensor thermal compensation */
 	TemperatureCompensation _temperature_compensation;