From 75aa11c9552a0d34bde959a2d9b3c3f0474bbc98 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 10 Mar 2022 17:35:18 +0100 Subject: [PATCH] PreFlightCheck: refactor sensors preflight checks --- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 46 +++++------ .../Arming/PreFlightCheck/PreFlightCheck.hpp | 20 +++-- .../checks/accelerometerCheck.cpp | 79 ++++++++++--------- .../PreFlightCheck/checks/baroCheck.cpp | 37 +++++---- .../checks/distanceSensorChecks.cpp | 29 +++---- .../PreFlightCheck/checks/gyroCheck.cpp | 58 +++++++------- .../checks/magnetometerCheck.cpp | 70 ++++++++-------- 7 files changed, 168 insertions(+), 171 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index b8a535843a..1da9f9abed 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -45,14 +45,10 @@ using namespace time_literals; -static constexpr unsigned max_mandatory_gyro_count = 1; -static constexpr unsigned max_optional_gyro_count = 4; -static constexpr unsigned max_mandatory_accel_count = 1; -static constexpr unsigned max_optional_accel_count = 4; static constexpr unsigned max_mandatory_mag_count = 1; -static constexpr unsigned max_optional_mag_count = 4; +static constexpr unsigned max_mandatory_gyro_count = 1; +static constexpr unsigned max_mandatory_accel_count = 1; static constexpr unsigned max_mandatory_baro_count = 1; -static constexpr unsigned max_optional_baro_count = 4; bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, @@ -72,8 +68,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu param_get(param_find("SYS_HAS_MAG"), &sys_has_mag); if (sys_has_mag == 1) { - failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, max_optional_mag_count, - mavlink_log_pub, status, magnetometerCheck, isMagRequired); + failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, + mavlink_log_pub, status, magnetometerCheck); /* mag consistency checks (need to be performed after the individual checks) */ if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) { @@ -84,16 +80,16 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- ACCEL ---- */ { - failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count, max_optional_accel_count, - mavlink_log_pub, status, accelerometerCheck, isAccelRequired); + failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count, + mavlink_log_pub, status, accelerometerCheck); // TODO: highest priority (from params) } /* ---- GYRO ---- */ { - failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, max_optional_gyro_count, - mavlink_log_pub, status, gyroCheck, isGyroRequired); + failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, + mavlink_log_pub, status, gyroCheck); // TODO: highest priority (from params) } @@ -104,8 +100,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu param_get(param_find("SYS_HAS_BARO"), &sys_has_baro); if (sys_has_baro == 1) { - static_cast(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, max_optional_baro_count, - mavlink_log_pub, status, baroCheck, isBaroRequired)); + static_cast(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, + mavlink_log_pub, status, baroCheck)); } } @@ -123,8 +119,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu param_get(param_find("SYS_HAS_NUM_DIST"), &sys_has_num_dist_sens); if (sys_has_num_dist_sens > 0) { - static_cast(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens, sys_has_num_dist_sens, - mavlink_log_pub, status, distSensCheck, isDistSensRequired)); + static_cast(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens, + mavlink_log_pub, status, distSensCheck)); } } @@ -236,23 +232,19 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu return !failed; } -bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count, - const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub, - vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired) +bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure, + const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub, + vehicle_status_s &status, sens_check_func_t sens_check) { bool pass_check = true; bool report_fail = report_failure; /* check all sensors, but fail only for mandatory ones */ - for (uint8_t i = 0u; i < max_optional_count; i++) { - const bool required = (i < max_mandatory_count) || isSensorRequired(i); + for (uint8_t i = 0u; i < ORB_MULTI_MAX_INSTANCES; i++) { + const bool is_mandatory = i < nb_mandatory_instances; - if (!sens_check(mavlink_log_pub, status, i, !required, report_fail)) { - if (required) { - pass_check = false; - } - - report_fail = false; // only report the first failure + if (!sens_check(mavlink_log_pub, status, i, is_mandatory, report_fail)) { + pass_check = false; } } diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 4e80807c90..fae89e005a 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -48,8 +48,7 @@ #include typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail); -typedef bool (*is_sens_req_func_t)(uint8_t instance); + const bool is_mandatory, bool &report_fail); class PreFlightCheck { @@ -100,25 +99,24 @@ public: bool report_fail = true); private: - static bool sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count, - const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub, - vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired); + static bool sensorAvailabilityCheck(const bool report_failure, + const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub, + vehicle_status_s &status, sens_check_func_t sens_check); static bool isMagRequired(uint8_t instance); static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail); + const bool is_mandatory, bool &report_fail); static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status); static bool isAccelRequired(uint8_t instance); static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail); + const bool is_mandatory, bool &report_fail); static bool isGyroRequired(uint8_t instance); static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail); + const bool is_mandatory, bool &report_fail); static bool isBaroRequired(uint8_t instance); static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail); - static bool isDistSensRequired(uint8_t instance); + const bool is_mandatory, bool &report_fail); static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail); + const bool is_mandatory, bool &report_fail); static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status); static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp index c15a537e0d..228d5671c9 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -50,70 +50,71 @@ bool PreFlightCheck::isAccelRequired(const uint8_t instance) uORB::SubscriptionData accel{ORB_ID(sensor_accel), instance}; const uint32_t device_id = static_cast(accel.get().device_id); + bool is_used_by_nav = false; + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { uORB::SubscriptionData estimator_status_sub{ORB_ID(estimator_status), i}; if (device_id > 0 && estimator_status_sub.get().accel_device_id == device_id) { - return true; + is_used_by_nav = true; + break; } } - return false; + return is_used_by_nav; } bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail) + const bool is_mandatory, bool &report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK); - bool calibration_valid = false; - bool valid = true; + const bool is_required = is_mandatory || isAccelRequired(instance); + + bool is_valid = false; + bool is_calibration_valid = false; + bool is_value_valid = false; if (exists) { - uORB::SubscriptionData accel{ORB_ID(sensor_accel), instance}; - valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0); - - if (!valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance); - } - } + is_valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0); if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - calibration_valid = true; + is_calibration_valid = true; } else { - calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0); + is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0); } - if (!calibration_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance); - } + const float accel_magnitude = sqrtf(accel.get().x * accel.get().x + + accel.get().y * accel.get().y + + accel.get().z * accel.get().z); - } else { - const float accel_magnitude = sqrtf(accel.get().x * accel.get().x - + accel.get().y * accel.get().y - + accel.get().z * accel.get().z); - - if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming"); - } - - // this is fatal - valid = false; - } - } - - } else { - if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance); + if (accel_magnitude > 4.0f && accel_magnitude < 15.0f /* m/s^2 */) { + is_value_valid = true; } } - const bool success = calibration_valid && valid; + if (report_fail && is_required) { + if (!exists) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance); + report_fail = false; - return success; + } else if (!is_valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance); + report_fail = false; + + } else if (!is_calibration_valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance); + report_fail = false; + + } else if (!is_value_valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming"); + report_fail = false; + } + } + + const bool is_sensor_ok = is_valid && is_calibration_valid && is_value_valid; + + return is_sensor_ok || !is_required; } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp index ac9e37fdfe..004fffdf93 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp @@ -48,43 +48,48 @@ bool PreFlightCheck::isBaroRequired(const uint8_t instance) uORB::SubscriptionData baro{ORB_ID(sensor_baro), instance}; const uint32_t device_id = static_cast(baro.get().device_id); + bool is_used_by_nav = false; + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { uORB::SubscriptionData estimator_status_sub{ORB_ID(estimator_status), i}; if (device_id > 0 && estimator_status_sub.get().baro_device_id == device_id) { - return true; + is_used_by_nav = true; + break; } } - return false; + return is_used_by_nav; } bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail) + const bool is_mandatory, bool &report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK); + const bool is_required = is_mandatory || isBaroRequired(instance); + bool valid = false; if (exists) { uORB::SubscriptionData baro{ORB_ID(sensor_baro), instance}; valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0); - - if (!valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro %u", instance); - } - } - - } else { - if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor %u missing", instance); - } } if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, valid, status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, is_required, valid, status); } - return valid; + if (report_fail && is_required) { + if (!exists) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor %u missing", instance); + report_fail = false; + + } else if (!valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro %u", instance); + report_fail = false; + } + } + + return valid || !is_required; } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp index 71c1406675..f02c3c0ebc 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp @@ -42,35 +42,30 @@ using namespace time_literals; -bool PreFlightCheck::isDistSensRequired(const uint8_t instance) -{ - return false; -} - bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail) + const bool is_mandatory, bool &report_fail) { const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK); - bool check_valid = false; + bool valid = false; if (exists) { uORB::SubscriptionData dist_sens_sub{ORB_ID(distance_sensor), instance}; dist_sens_sub.update(); const distance_sensor_s &dist_sens_data = dist_sens_sub.get(); - check_valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s); + valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s); + } - if (!check_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance); - } - } - - } else { - if (!optional && report_fail) { + if (report_fail && is_mandatory) { + if (!exists) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: distance sensor %u missing", instance); + report_fail = false; + + } else if (!valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance); + report_fail = false; } } - return check_valid; + return valid || !is_mandatory; } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp index 12e1242c0f..590445c0fa 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -49,54 +49,58 @@ bool PreFlightCheck::isGyroRequired(const uint8_t instance) uORB::SubscriptionData gyro{ORB_ID(sensor_gyro), instance}; const uint32_t device_id = static_cast(gyro.get().device_id); + bool is_used_by_nav = false; + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { uORB::SubscriptionData estimator_status_sub{ORB_ID(estimator_status), i}; if (device_id > 0 && estimator_status_sub.get().gyro_device_id == device_id) { - return true; + is_used_by_nav = true; + break; } } - return false; + return is_used_by_nav; } bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail) + const bool is_mandatory, bool &report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK); - bool calibration_valid = false; - bool valid = false; + const bool is_required = is_mandatory || isGyroRequired(instance); + + bool is_valid = false; + bool is_calibration_valid = false; if (exists) { - uORB::SubscriptionData gyro{ORB_ID(sensor_gyro), instance}; - valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0); - - if (!valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance); - } - } + is_valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0); if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - calibration_valid = true; + is_calibration_valid = true; } else { - calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0); - } - - if (!calibration_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u uncalibrated", instance); - } - } - - } else { - if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor %u missing", instance); + is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0); } } - return calibration_valid && valid; + if (report_fail && is_required) { + if (!exists) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor %u missing", instance); + report_fail = false; + + } else if (!is_valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance); + report_fail = false; + + } else if (!is_calibration_valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u uncalibrated", instance); + report_fail = false; + } + } + + const bool is_sensor_ok = is_calibration_valid && is_valid; + + return is_sensor_ok || !is_required; } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index 21cc051eba..338120b306 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -49,48 +49,40 @@ bool PreFlightCheck::isMagRequired(const uint8_t instance) uORB::SubscriptionData magnetometer{ORB_ID(sensor_mag), instance}; const uint32_t device_id = static_cast(magnetometer.get().device_id); + bool is_used_by_nav = false; + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { uORB::SubscriptionData estimator_status_sub{ORB_ID(estimator_status), i}; if (device_id > 0 && estimator_status_sub.get().mag_device_id == device_id) { - return true; + is_used_by_nav = true;; + break; } } - return false; + return is_used_by_nav; } bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool report_fail) + const bool is_mandatory, bool &report_fail) { const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK); - bool calibration_valid = false; - bool valid = false; + const bool is_required = is_mandatory || isMagRequired(instance); + + bool is_valid = false; + bool is_calibration_valid = false; bool is_mag_fault = false; if (exists) { - uORB::SubscriptionData magnetometer{ORB_ID(sensor_mag), instance}; - valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0); - - if (!valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass %u", instance); - } - } + is_valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0); if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - calibration_valid = true; + is_calibration_valid = true; } else { - calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", magnetometer.get().device_id) >= 0); - } - - if (!calibration_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance); - } + is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", magnetometer.get().device_id) >= 0); } for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { @@ -103,25 +95,35 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st } } } - - if (is_mag_fault && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance); - } - - } else { - if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance); - } } - const bool success = calibration_valid && valid && !is_mag_fault; + const bool is_sensor_ok = is_valid && is_calibration_valid && !is_mag_fault; if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, is_required, is_sensor_ok, status); } else if (instance == 1) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, is_required, is_sensor_ok, status); } - return success; + if (report_fail && is_required) { + if (!exists) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance); + report_fail = false; + + } else if (!is_valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass %u", instance); + report_fail = false; + + } else if (!is_calibration_valid) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance); + report_fail = false; + + } else if (is_mag_fault) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance); + report_fail = false; + } + } + + return is_sensor_ok || !is_required; }