commander: make SYS_HAS_MAG a count param and ensure system has N calibrated + enabled mags

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -142,7 +142,9 @@ then
 	param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
 
 	param set CAL_MAG0_ID 197388
+	param set CAL_MAG0_PRIO 50
 	param set CAL_MAG1_ID 197644
+	param set CAL_MAG1_PRIO 50
 
 	param set SENS_BOARD_X_OFF 0.000001
 	param set SENS_DPRES_OFF 0.001

src/lib/systemlib/system_params.c

@@ -192,13 +192,11 @@ PARAM_DEFINE_INT32(SYS_HAS_GPS, 1);
 /**
  * Control if the vehicle has a magnetometer
  *
- * Disable this if the board has no magnetometer, such as the Omnibus F4 SD.
- * If disabled, the preflight checks will not check for the presence of a
- * magnetometer.
+ * Set this to 0 if the board has no magnetometer.
+ * If set to 0, the preflight checks will not check for the presence of a
+ * magnetometer, otherwise N sensors are required.
  *
- * @boolean
  * @reboot_required true
- *
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_HAS_MAG, 1);

src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp

@@ -39,11 +39,12 @@ using namespace time_literals;
 
 void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (!_param_sys_has_mag.get()) {
+	if (_param_sys_has_mag.get() == 0) {
 		return;
 	}
 
 	bool had_failure = false;
+	int num_enabled_and_valid_calibration = 0;
 
 	for (int instance = 0; instance < _sensor_mag_sub.size(); instance++) {
 		bool is_mag_fault = false;
@@ -66,7 +67,16 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter
 				is_calibration_valid = true;
 
 			} else {
-				is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id) >= 0);
+				int calibration_index = calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id);
+				is_calibration_valid = (calibration_index >= 0);
+
+				if (is_calibration_valid) {
+					int priority = calibration::GetCalibrationParamInt32("MAG", "PRIO", calibration_index);
+
+					if (priority > 0) {
+						++num_enabled_and_valid_calibration;
+					}
+				}
 			}
 
 			reporter.setIsPresent(health_component_t::magnetometer);
@@ -125,6 +135,25 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter
 
 	if (!had_failure && !context.isArmed()) {
 		consistencyCheck(context, reporter);
+
+		if (num_enabled_and_valid_calibration < _param_sys_has_mag.get()) {
+			/* EVENT
+			 * @description
+			 * Make sure all required sensors are working, enabled and calibrated.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>SYS_HAS_MAG</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<uint8_t, uint8_t>(NavModes::All, health_component_t::magnetometer,
+					events::ID("check_mag_sys_has_mag_missing"),
+					events::Log::Error, "Found {1} compass (required: {2})", num_enabled_and_valid_calibration, _param_sys_has_mag.get());
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Found %i compass (required: %" PRId32 ")",
+						     num_enabled_and_valid_calibration, _param_sys_has_mag.get());
+			}
+		}
 	}
 }
 

src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp

@@ -60,7 +60,7 @@ private:
 	uORB::Subscription _sensor_preflight_mag_sub{ORB_ID(sensor_preflight_mag)};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
+					(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
 					(ParamInt<px4::params::COM_ARM_MAG_ANG>) _param_com_arm_mag_ang
 				       )
 };

src/modules/sensors/sensors.hpp

@@ -259,7 +259,7 @@ private:
 		(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
-		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
+		(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)