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 )