From 5f1b577b6db4296ef58bb493c6f701186f6fd820 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 10 Mar 2022 14:13:25 +0100 Subject: [PATCH] PreFlightCheck: mark sensor required if used by an EKF2 instance --- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 16 +++++++--------- .../Arming/PreFlightCheck/PreFlightCheck.hpp | 8 +++++++- .../checks/accelerometerCheck.cpp | 17 +++++++++++++++++ .../Arming/PreFlightCheck/checks/baroCheck.cpp | 17 +++++++++++++++++ .../checks/distanceSensorChecks.cpp | 5 +++++ .../Arming/PreFlightCheck/checks/gyroCheck.cpp | 17 +++++++++++++++++ .../PreFlightCheck/checks/magnetometerCheck.cpp | 16 ++++++++++++++++ 7 files changed, 86 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index a04b8e0b16..3684e50012 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -73,9 +73,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (sys_has_mag == 1) { failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, max_optional_mag_count, - mavlink_log_pub, status, magnetometerCheck); - - // TODO: highest priority mag + mavlink_log_pub, status, magnetometerCheck, isMagRequired); /* mag consistency checks (need to be performed after the individual checks) */ if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) { @@ -87,7 +85,7 @@ 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); + mavlink_log_pub, status, accelerometerCheck, isAccelRequired); // TODO: highest priority (from params) } @@ -95,7 +93,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- GYRO ---- */ { failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, max_optional_gyro_count, - mavlink_log_pub, status, gyroCheck); + mavlink_log_pub, status, gyroCheck, isGyroRequired); // TODO: highest priority (from params) } @@ -107,7 +105,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (sys_has_baro == 1) { static_cast(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, max_optional_baro_count, - mavlink_log_pub, status, baroCheck)); + mavlink_log_pub, status, baroCheck, isBaroRequired)); } } @@ -126,7 +124,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu 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)); + mavlink_log_pub, status, distSensCheck, isDistSensRequired)); } } @@ -240,14 +238,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu 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) + vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired) { 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); + const bool required = (i < max_mandatory_count) || isSensorRequired(i); int32_t device_id = -1; if (!sens_check(mavlink_log_pub, status, i, !required, device_id, report_fail)) { diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index cb7589a4c8..758520c289 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -49,6 +49,7 @@ typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail); +typedef bool (*is_sens_req_func_t)(uint8_t instance); class PreFlightCheck { @@ -101,16 +102,21 @@ public: 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); + vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired); + 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, int32_t &device_id, const 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, int32_t &device_id, const 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, int32_t &device_id, const 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, int32_t &device_id, const bool report_fail); + static bool isDistSensRequired(uint8_t instance); static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail); static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp index e7b1f57b99..7b2f3f2893 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -40,10 +40,27 @@ #include #include #include +#include #include using namespace time_literals; +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); + + 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; + } + } + + return false; +} + bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp index 00d48d7c72..ff53c36530 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp @@ -38,10 +38,27 @@ #include #include #include +#include #include using namespace time_literals; +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); + + 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; + } + } + + return false; +} + bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp index 387277b856..87fb0f2497 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp @@ -42,6 +42,11 @@ 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, int32_t &device_id, const bool report_fail) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp index 8587f39b50..e6f559acdb 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -39,10 +39,27 @@ #include #include #include +#include #include using namespace time_literals; +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); + + 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; + } + } + + return false; +} + bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index b0b1d80861..e49444c3d3 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -44,6 +44,22 @@ using namespace time_literals; +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); + + 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; + } + } + + return false; +} + bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail) {