diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index 1bcdf974fe..dc07543c46 100644
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/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
diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c
index 7e74a71d96..f019b6f2d0 100644
--- a/src/lib/systemlib/system_params.c
+++ b/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);
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp
index 61ee9a81a0..e28a8bd134 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp
+++ b/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.
+ *
+ *
+ * This check can be configured via SYS_HAS_MAG parameter.
+ *
+ */
+ reporter.armingCheckFailure(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());
+ }
+ }
}
}
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp
index 1be11d27b5..510dd305ab 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp
+++ b/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) _param_sys_has_mag,
+ (ParamInt) _param_sys_has_mag,
(ParamInt) _param_com_arm_mag_ang
)
};
diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp
index c5ecae266c..102be087b1 100644
--- a/src/modules/sensors/sensors.hpp
+++ b/src/modules/sensors/sensors.hpp
@@ -259,7 +259,7 @@ private:
(ParamBool) _param_sys_has_gps,
#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
- (ParamBool) _param_sys_has_mag,
+ (ParamInt) _param_sys_has_mag,
#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
(ParamBool) _param_sens_imu_mode
)