diff --git a/msg/event.msg b/msg/event.msg index 5f1fc7ef08..df1dd4a97d 100644 --- a/msg/event.msg +++ b/msg/event.msg @@ -7,4 +7,4 @@ uint8[25] arguments # (optional) arguments, depend on event id uint8 log_levels # Log levels: 4 bits MSB: internal, 4 bits LSB: external -uint8 ORB_QUEUE_LENGTH = 8 +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 38f5737eb5..0b1d61a985 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -113,3 +113,22 @@ uint64 onboard_control_sensors_health bool safety_button_available # Set to true if a safety button is connected bool safety_off # Set to true if safety is off +bool auto_mission_available + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool parachute_system_present +bool parachute_system_healthy + +bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter +bool avoidance_system_valid # Status of the obstacle avoidance system + +bool battery_healthy # set if battery is available and not low + +bool circuit_breaker_engaged_power_check +bool circuit_breaker_engaged_airspd_check +bool circuit_breaker_flight_termination_disabled +bool circuit_breaker_engaged_usb_check +bool circuit_breaker_vtol_fw_arming_check # set to true if for VTOLs arming in fixed-wing mode should be allowed + diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index f5a02c92b3..5d37b21e2b 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -1,7 +1,21 @@ -# This is a struct used by the commander internally. +# TODO: rename to failsafe_flags (will be input to failsafe state machine) +# uint64 timestamp # time since system start (microseconds) +# Per-mode requirements +uint32 mode_req_angular_velocity +uint32 mode_req_attitude +uint32 mode_req_local_position +uint32 mode_req_global_position +uint32 mode_req_local_alt +uint32 mode_req_mission +uint32 mode_req_offboard_signal +uint32 mode_req_home_position +uint32 mode_req_prevent_arming # if set, cannot arm while in this mode +uint32 mode_req_other # other requirements, not covered above (for external modes) + + bool calibration_enabled bool pre_flight_checks_pass # true if all checks necessary to arm pass bool auto_mission_available @@ -13,8 +27,6 @@ bool local_velocity_valid # set to true by the commander app if the quality of bool global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation bool gps_position_valid bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) -bool power_input_valid # set if input power is valid -bool battery_healthy # set if battery is available and not low bool escs_error # set to true if one or more ESCs reporting esc_status are offline bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure @@ -23,25 +35,10 @@ bool position_reliant_on_optical_flow bool position_reliant_on_vision_position bool dead_reckoning -bool flight_terminated - -bool circuit_breaker_engaged_power_check -bool circuit_breaker_engaged_airspd_check -bool circuit_breaker_flight_termination_disabled -bool circuit_breaker_engaged_usb_check -bool circuit_breaker_vtol_fw_arming_check # set to true if for VTOLs arming in fixed-wing mode should be allowed bool offboard_control_signal_lost bool rc_signal_found_once bool rc_calibration_in_progress -bool rc_calibration_valid # set if RC calibration is valid bool vtol_transition_failure # Set to true if vtol transition failed -bool usb_connected # status of the USB power supply -bool sd_card_detected_once # set to true if the SD card was detected -bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter -bool avoidance_system_valid # Status of the obstacle avoidance system - -bool parachute_system_present -bool parachute_system_healthy diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index a9d84c91ef..af86b8b4ee 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -39,10 +39,8 @@ constexpr bool ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]; transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status, - const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off, - const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks, - orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags, - arm_disarm_reason_t calling_reason) + const arming_state_t new_arming_state, actuator_armed_s &armed, HealthAndArmingChecks &checks, + const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason) { // Double check that our static arrays are still valid static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); @@ -67,10 +65,9 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s && !(status.hil_state == vehicle_status_s::HIL_STATE_ON) && (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) { - if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode, - true, // report_failures - safety_button_available, safety_off, - true)) { // is_arm_attempt + checks.update(); + + if (!checks.canArm(status.nav_state)) { feedback_provided = true; // Preflight checks report error messages valid_transition = false; } diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 4262091b0d..53eda6991a 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -33,7 +33,7 @@ #pragma once -#include "../PreFlightCheck/PreFlightCheck.hpp" +#include "../../HealthAndArmingChecks/HealthAndArmingChecks.hpp" #include #include #include @@ -54,11 +54,9 @@ public: void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; } transition_result_t - arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, - const bool safety_button_available, const bool safety_off, const arming_state_t new_arming_state, - actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, - vehicle_status_flags_s &status_flags, - arm_disarm_reason_t calling_reason); + arming_state_transition(vehicle_status_s &status, const arming_state_t new_arming_state, + actuator_armed_s &armed, HealthAndArmingChecks &checks, const bool fRunPreArmChecks, + orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason); // Getters uint8_t getArmState() const { return _arm_state; } diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index bc0751278c..2867efc149 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ #include -#include +#include "ArmStateMachine.hpp" TEST(ArmStateMachineTest, ArmingStateTransitionTest) { @@ -61,12 +61,12 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) } ArmingTransitionTest_t; // We use these defines so that our test cases are more readable -#define ATT_ARMED true -#define ATT_DISARMED false -#define ATT_SAFETY_AVAILABLE true -#define ATT_SAFETY_NOT_AVAILABLE true -#define ATT_SAFETY_OFF true -#define ATT_SAFETY_ON false + static constexpr bool ATT_ARMED = true; + static constexpr bool ATT_DISARMED = false; + static constexpr bool ATT_SAFETY_AVAILABLE = true; + static constexpr bool ATT_SAFETY_NOT_AVAILABLE = true; + static constexpr bool ATT_SAFETY_OFF = true; + static constexpr bool ATT_SAFETY_ON = false; // These are test cases for arming_state_transition static const ArmingTransitionTest_t rgArmingTransitionTests[] = { @@ -253,22 +253,17 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) // Setup initial machine state arm_state_machine.forceArmState(test->current_state.arming_state); status.hil_state = test->hil_state; - // The power status of the test unit is not relevant for the unit test - status_flags.circuit_breaker_engaged_power_check = true; - vehicle_control_mode_s control_mode{}; + HealthAndArmingChecks health_and_arming_checks(nullptr, status_flags, status); // Attempt transition transition_result_t result = arm_state_machine.arming_state_transition( status, - control_mode, - test->safety_button_available, - test->safety_off, test->requested_state, armed, + health_and_arming_checks, true /* enable pre-arm checks */, nullptr /* no mavlink_log_pub */, - status_flags, arm_disarm_reason_t::unit_test); // Validate result of transition diff --git a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt b/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt index 24f6be2c93..8a82b6de9e 100644 --- a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt +++ b/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt @@ -36,6 +36,9 @@ px4_add_library(ArmStateMachine ArmStateMachine.cpp ) target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(ArmStateMachine PUBLIC PreFlightCheck) +target_link_libraries(ArmStateMachine PUBLIC health_and_arming_checks) + +px4_add_functional_gtest(SRC ArmStateMachineTest.cpp + LINKLIBS ArmStateMachine health_and_arming_checks sensor_calibration ArmAuthorization mode_util + ) -px4_add_functional_gtest(SRC ArmStateMachineTest.cpp LINKLIBS ArmStateMachine) diff --git a/src/modules/commander/Arming/CMakeLists.txt b/src/modules/commander/Arming/CMakeLists.txt index ab21b1a479..b97cc35bc0 100644 --- a/src/modules/commander/Arming/CMakeLists.txt +++ b/src/modules/commander/Arming/CMakeLists.txt @@ -34,4 +34,3 @@ add_subdirectory(ArmAuthorization) add_subdirectory(ArmStateMachine) add_subdirectory(HealthFlags) -add_subdirectory(PreFlightCheck) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp deleted file mode 100644 index f3860b186d..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file PreFlightCheck.cpp - */ - -#include "PreFlightCheck.hpp" - -#include -#include -#include -#include -#include - -using namespace time_literals; - -static constexpr unsigned max_mandatory_mag_count = 1; -static constexpr unsigned max_mandatory_gyro_count = 1; -static constexpr unsigned max_mandatory_accel_count = 1; -static constexpr unsigned max_mandatory_baro_count = 1; - -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, - bool report_failures, - const bool safety_button_available, const bool safety_off, - const bool is_arm_attempt) -{ - report_failures = (report_failures && !status_flags.calibration_enabled); - - bool failed = false; - - failed = failed || !airframeCheck(mavlink_log_pub, status); - failed = failed || !sdcardCheck(mavlink_log_pub, status_flags.sd_card_detected_once, report_failures); - - /* ---- MAG ---- */ - { - int32_t sys_has_mag = 1; - param_get(param_find("SYS_HAS_MAG"), &sys_has_mag); - - if (sys_has_mag == 1) { - 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)) { - failed = true; - } - } - } - - /* ---- ACCEL ---- */ - { - 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, - mavlink_log_pub, status, gyroCheck); - - // TODO: highest priority (from params) - } - - /* ---- BARO ---- */ - { - int32_t sys_has_baro = 1; - param_get(param_find("SYS_HAS_BARO"), &sys_has_baro); - - if (sys_has_baro == 1) { - static_cast(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, - mavlink_log_pub, status, baroCheck)); - } - } - - /* ---- IMU CONSISTENCY ---- */ - // To be performed after the individual sensor checks have completed - { - if (!imuConsistencyCheck(mavlink_log_pub, status, report_failures)) { - failed = true; - } - } - - /* ---- Distance Sensor ---- */ - { - int32_t sys_has_num_dist_sens = 0; - 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, - mavlink_log_pub, status, distSensCheck)); - } - - } - - /* ---- AIRSPEED ---- */ - /* Perform airspeed check only if circuit breaker is not engaged and it's not a rotary wing */ - if (!status_flags.circuit_breaker_engaged_airspd_check && - (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) { - - int32_t airspeed_mode = 0; - param_get(param_find("FW_ARSP_MODE"), &airspeed_mode); - const bool optional = (airspeed_mode == 1); - - int32_t max_airspeed_check_en = 0; - param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en); - - float airspeed_trim = 10.0f; - param_get(param_find("FW_AIRSPD_TRIM"), &airspeed_trim); - - const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed - - if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, is_arm_attempt, (bool)max_airspeed_check_en, - arming_max_airspeed_allowed) - && !(bool)optional) { - failed = true; - } - } - - /* ---- RC CALIBRATION ---- */ - int32_t com_rc_in_mode{0}; - param_get(param_find("COM_RC_IN_MODE"), &com_rc_in_mode); - - if (com_rc_in_mode == 0) { - if (rcCalibrationCheck(mavlink_log_pub, report_failures) != OK) { - if (report_failures) { - mavlink_log_critical(mavlink_log_pub, "RC calibration check failed"); - } - - failed = true; - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status); - status_flags.rc_calibration_valid = false; - - } else { - // The calibration is fine, but only set the overall health state to true if the signal is not currently lost - status_flags.rc_calibration_valid = true; - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, - !status.rc_signal_lost, status); - } - } - - /* ---- SYSTEM POWER ---- */ - if (status_flags.power_input_valid && !status_flags.circuit_breaker_engaged_power_check) { - if (!powerCheck(mavlink_log_pub, status, report_failures)) { - failed = true; - } - } - - /* ---- Navigation EKF ---- */ - // only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled - int32_t estimator_type = -1; - - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) { - param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type); - - } else { - // EKF2 is currently the only supported option for FW & VTOL - estimator_type = 2; - } - - if (estimator_type == 2) { - const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, report_failures) && - ekf2CheckSensorBias(mavlink_log_pub, report_failures); - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status); - - if (control_mode.flag_control_attitude_enabled - || control_mode.flag_control_velocity_enabled - || control_mode.flag_control_position_enabled) { - // healthy estimator only required for dependent control modes - failed |= !ekf_healthy; - } - } - - /* ---- Failure Detector ---- */ - if (!failureDetectorCheck(mavlink_log_pub, status, report_failures)) { - failed = true; - } - - failed = failed || !manualControlCheck(mavlink_log_pub, report_failures); - failed = failed || !modeCheck(mavlink_log_pub, report_failures, status); - failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures); - failed = failed || !parachuteCheck(mavlink_log_pub, report_failures, status_flags); - failed = failed || !preArmCheck(mavlink_log_pub, status_flags, control_mode, - safety_button_available, safety_off, status, report_failures, is_arm_attempt); - - /* Report status */ - return !failed; -} - -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 < ORB_MULTI_MAX_INSTANCES; i++) { - const bool is_mandatory = i < nb_mandatory_instances; - - if (!sens_check(mavlink_log_pub, status, i, is_mandatory, report_fail)) { - pass_check = false; - } - } - - return pass_check; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp deleted file mode 100644 index e52a3ecbf5..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ /dev/null @@ -1,109 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 - 2022 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file PreFlightCheck.hpp - * - * Check if flight is safely possible - * if not prevent it and inform the user. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include "../../Safety.hpp" - -typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool is_mandatory, bool &report_fail); - -class PreFlightCheck -{ -public: - PreFlightCheck() = default; - ~PreFlightCheck() = default; - - /** - * Runs a preflight check to determine if the system is ready to be armed - * - * @param mavlink_log_pub - * Mavlink output orb handle reference for feedback when a sensor fails - **/ - static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, - vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, - bool reportFailures, - const bool safety_button_available, const bool safety_off, - const bool is_arm_attempt = false); - -private: - 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 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 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 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 is_mandatory, bool &report_fail); - static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - 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 is_arm_attempt, const bool max_airspeed_check_en, - const float arming_max_airspeed_allowed); - static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail); - static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail); - static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail); - static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail); - static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail); - static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail); - static bool modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_s &status); - static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status); - static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail); - static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail); - static bool parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, - const vehicle_status_flags_s &status_flags); - static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off, - vehicle_status_s &status, const bool report_fail, const bool is_arm_attempt); -}; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp deleted file mode 100644 index bc2cee9b43..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ /dev/null @@ -1,121 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#include -#include -#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); - - 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) { - is_used_by_nav = true; - break; - } - } - - return is_used_by_nav; -} - -bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool is_mandatory, bool &report_fail) -{ - const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK); - 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}; - - is_valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0) - && (hrt_elapsed_time(&accel.get().timestamp) < 1_s); - - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - is_calibration_valid = true; - - } else { - is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0); - } - - 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 */) { - is_value_valid = true; - } - } - - if (report_fail && is_required) { - if (!exists) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance); - report_fail = false; - - } 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/airspeedCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp deleted file mode 100644 index 994d502640..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, - const bool report_fail, const bool is_arm_attempt, const bool max_airspeed_check_en, - const float arming_max_airspeed_allowed) -{ - bool present = true; - bool success = true; - - uORB::SubscriptionData airspeed_validated_sub{ORB_ID(airspeed_validated)}; - airspeed_validated_sub.update(); - const airspeed_validated_s &airspeed_validated = airspeed_validated_sub.get(); - - /* - * Check if Airspeed Selector is up and running. - */ - if (hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) { - if (report_fail && !optional) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Selector module down."); - } - - present = false; - success = false; - goto out; - } - - /* - * Check if airspeed is declared valid or not by airspeed selector. - */ - if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed invalid."); - } - - present = true; - success = false; - goto out; - } - - /* - * Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying - * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover - * might have been removed. - */ - if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed - && !is_arm_attempt) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or pitot"); - } - - present = true; - success = false; - goto out; - } - -out: - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status); - - return success; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp deleted file mode 100644 index c30ddb9cd5..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#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); - - 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) { - is_used_by_nav = true; - break; - } - } - - return is_used_by_nav; -} - -bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - 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) && (hrt_elapsed_time(&baro.get().timestamp) < 1_s); - } - - if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, is_required, valid, status); - } - - 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/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp deleted file mode 100644 index 69b5ec912f..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ /dev/null @@ -1,297 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail) -{ - bool success = true; // start with a pass and change to a fail if any test fails - - int32_t mag_strength_check = 1; - param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check); - - float hgt_test_ratio_limit = 1.f; - param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit); - - float vel_test_ratio_limit = 1.f; - param_get(param_find("COM_ARM_EKF_VEL"), &vel_test_ratio_limit); - - float pos_test_ratio_limit = 1.f; - param_get(param_find("COM_ARM_EKF_POS"), &pos_test_ratio_limit); - - float mag_test_ratio_limit = 1.f; - param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit); - - int32_t arm_without_gps = 0; - param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps); - - int32_t sys_has_gps = 1; - param_get(param_find("SYS_HAS_GPS"), &sys_has_gps); - - bool gps_success = false; - bool gps_present = false; - - // Get estimator status data if available and exit with a fail recorded if not - uORB::SubscriptionData estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; - uORB::SubscriptionData status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance}; - const estimator_status_s &status = status_sub.get(); - - if (status.timestamp == 0) { - success = false; - goto out; - } - - // Check if preflight check performed by estimator has failed - if (status.pre_flt_fail_innov_heading || - status.pre_flt_fail_innov_vel_horiz || - status.pre_flt_fail_innov_vel_vert || - status.pre_flt_fail_innov_height) { - if (report_fail) { - if (status.pre_flt_fail_innov_heading) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable"); - - } else if (status.pre_flt_fail_innov_vel_horiz) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable"); - - } else if (status.pre_flt_fail_innov_vel_vert) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable"); - - } else if (status.pre_flt_fail_innov_height) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable"); - } - } - - success = false; - goto out; - } - - if ((mag_strength_check >= 1) && status.pre_flt_fail_mag_field_disturbed) { - const char *message = "Preflight%s: Strong magnetic interference detected"; - - if (mag_strength_check == 1) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, message, " Fail"); - } - - success = false; - goto out; - - } else if (report_fail) { - mavlink_log_warning(mavlink_log_pub, message, ""); - } - } - - // check vertical position innovation test ratio - if (status.hgt_test_ratio > hgt_test_ratio_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error"); - } - - success = false; - goto out; - } - - // check velocity innovation test ratio - if (status.vel_test_ratio > vel_test_ratio_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error"); - } - - success = false; - goto out; - } - - // check horizontal position innovation test ratio - if (status.pos_test_ratio > pos_test_ratio_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error"); - } - - success = false; - goto out; - } - - // check magnetometer innovation test ratio - if (status.mag_test_ratio > mag_test_ratio_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error"); - } - - success = false; - goto out; - } - - // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing - if (sys_has_gps == 1) { - const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS); - const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0; - - gps_present = true; - gps_success = ekf_gps_fusion; // default to success if gps data is fused - - if (ekf_gps_check_fail) { - if (report_fail) { - // Only report the first failure to avoid spamming - const char *message = nullptr; - - if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) { - message = "Preflight%s: GPS fix too low"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { - message = "Preflight%s: not enough GPS Satellites"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) { - message = "Preflight%s: GPS PDOP too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { - message = "Preflight%s: GPS Horizontal Pos Error too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) { - message = "Preflight%s: GPS Vertical Pos Error too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) { - message = "Preflight%s: GPS Speed Accuracy too low"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) { - message = "Preflight%s: GPS Horizontal Pos Drift too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) { - message = "Preflight%s: GPS Vertical Pos Drift too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) { - message = "Preflight%s: GPS Hor Speed Drift too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) { - message = "Preflight%s: GPS Vert Speed Drift too high"; - - } else { - if (!ekf_gps_fusion) { - // Likely cause unknown - message = "Preflight%s: Estimator not using GPS"; - gps_present = false; - - } else { - // if we land here there was a new flag added and the code not updated. Show a generic message. - message = "Preflight%s: Poor GPS Quality"; - } - } - - if (message) { - if (!arm_without_gps) { - mavlink_log_critical(mavlink_log_pub, message, " Fail"); - - } else { - mavlink_log_warning(mavlink_log_pub, message, ""); - } - } - } - - gps_success = false; - - if (!arm_without_gps) { - success = false; - goto out; - } - } - } - -out: - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, !arm_without_gps, gps_success, vehicle_status); - return success; -} - -bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail) -{ - // Get estimator states data if available and exit with a fail recorded if not - uORB::SubscriptionData estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; - uORB::SubscriptionData estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias), estimator_selector_status_sub.get().primary_instance}; - const estimator_sensor_bias_s &bias = estimator_sensor_bias_sub.get(); - - if (hrt_elapsed_time(&bias.timestamp) < 30_s) { - - // check accelerometer bias estimates - if (bias.accel_bias_valid) { - const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit; - - for (uint8_t axis_index = 0; axis_index < 3; axis_index++) { - // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives - // adjust test threshold by 3-sigma - const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f)); - - if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) { - if (report_fail) { - PX4_ERR("accel bias (axis %d): |%.8f| > %.8f + %.8f", axis_index, - (double)bias.accel_bias[axis_index], (double)ekf_ab_test_limit, (double)test_uncertainty); - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); - } - - return false; - } - } - } - - // check gyro bias estimates - if (bias.gyro_bias_valid) { - const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit; - - for (uint8_t axis_index = 0; axis_index < 3; axis_index++) { - // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives - // adjust test threshold by 3-sigma - const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f)); - - if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) { - if (report_fail) { - PX4_ERR("gyro bias (axis %d): |%.8f| > %.8f + %.8f", axis_index, - (double)bias.gyro_bias[axis_index], (double)ekf_gb_test_limit, (double)test_uncertainty); - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias"); - } - - return false; - } - } - } - } - - return true; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp deleted file mode 100644 index 20a4544f67..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include - -bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, - const bool report_fail) -{ - if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) { - if (report_fail) { - if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected"); - } - } - - return false; - } - - return true; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp deleted file mode 100644 index 0f330969fa..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#include -#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); - - 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) { - is_used_by_nav = true; - break; - } - } - - return is_used_by_nav; -} - -bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool is_mandatory, bool &report_fail) -{ - const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK); - 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}; - - is_valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0) - && (hrt_elapsed_time(&gyro.get().timestamp) < 1_s); - - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - is_calibration_valid = true; - - } else { - is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0); - } - } - - 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/imuConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp deleted file mode 100644 index cad290ca44..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include - -#include -#include -#include -#include - -bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, - const bool report_status) -{ - float accel_test_limit = 1.f; - param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit); - - float gyro_test_limit = 1.f; - param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit); - - // Get sensor_preflight data if available and exit with a fail recorded if not - uORB::SubscriptionData sensors_status_imu_sub{ORB_ID(sensors_status_imu)}; - const sensors_status_imu_s &imu = sensors_status_imu_sub.get(); - - // Use the difference between IMU's to detect a bad calibration. - // If a single IMU is fitted, the value being checked will be zero so this check will always pass. - for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) { - if (imu.accel_device_ids[i] != 0) { - if (imu.accel_device_ids[i] == imu.accel_device_id_primary) { - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, imu.accel_healthy[i], status); - - } else { - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, imu.accel_healthy[i], status); - } - - const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i]; - - if (accel_inconsistency_m_s_s > accel_test_limit) { - if (report_status) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u inconsistent - Check Cal", i); - - if (imu.accel_device_ids[i] == imu.accel_device_id_primary) { - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status); - - } else { - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status); - } - } - - return false; - - } else if (accel_inconsistency_m_s_s > accel_test_limit * 0.8f) { - if (report_status) { - mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accel %u inconsistent - Check Cal", i); - } - } - } - } - - // Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec - for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) { - if (imu.gyro_device_ids[i] != 0) { - if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) { - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, imu.accel_healthy[i], status); - - } else { - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, imu.accel_healthy[i], status); - } - - const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i]; - - if (gyro_inconsistency_rad_s > gyro_test_limit) { - if (report_status) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u inconsistent - Check Cal", i); - - if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) { - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status); - - } else { - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status); - } - } - - return false; - - } else if (gyro_inconsistency_rad_s > gyro_test_limit * 0.5f) { - if (report_status) { - mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyro %u inconsistent - Check Cal", i); - } - } - } - } - - return true; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp deleted file mode 100644 index 5c76650436..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include - -#include -#include -#include -#include -#include - -// return false if the magnetomer measurements are inconsistent -bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, - const bool report_status) -{ - bool pass = false; // flag for result of checks - - // get the sensor preflight data - uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight_mag)}; - sensors_sub.update(); - const sensor_preflight_mag_s &sensors = sensors_sub.get(); - - if (sensors.timestamp == 0) { - // can happen if not advertised (yet) - pass = true; - } - - // Use the difference between sensors to detect a bad calibration, orientation or magnetic interference. - // If a single sensor is fitted, the value being checked will be zero so this check will always pass. - int32_t angle_difference_limit_deg = 90; - param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg); - - pass = pass || angle_difference_limit_deg < 0; // disabled, pass check - pass = pass || sensors.mag_inconsistency_angle < math::radians(angle_difference_limit_deg); - - if (!pass && report_status) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent", - static_cast(math::degrees(sensors.mag_inconsistency_angle))); - mavlink_log_critical(mavlink_log_pub, "Please check orientations and recalibrate"); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status); - } - - return pass; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp deleted file mode 100644 index fe0f803a97..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -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); - - 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) { - is_used_by_nav = true;; - break; - } - } - - return is_used_by_nav; -} - -bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool is_mandatory, bool &report_fail) -{ - const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK); - 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}; - - const sensor_mag_s &mag_data = magnetometer.get(); - is_valid = (mag_data.device_id != 0) && (mag_data.timestamp != 0) && (hrt_elapsed_time(&mag_data.timestamp) < 1_s); - - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - is_calibration_valid = true; - - } else { - is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id) >= 0); - } - - for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - uORB::SubscriptionData estimator_status_sub{ORB_ID(estimator_status), i}; - - if (estimator_status_sub.get().mag_device_id == static_cast(mag_data.device_id)) { - if (estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)) { - is_mag_fault = true; - break; - } - } - } - } - - 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, is_required, is_sensor_ok, status); - - } else if (instance == 1) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, is_required, is_sensor_ok, status); - } - - 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; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp deleted file mode 100644 index 1a371d87a7..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail) -{ - bool success = true; - - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - // Ignore power check in HITL. - return true; - } - - uORB::SubscriptionData system_power_sub{ORB_ID(system_power)}; - system_power_sub.update(); - const system_power_s &system_power = system_power_sub.get(); - - if (system_power.timestamp != 0) { - int32_t required_power_module_count = 0; - param_get(param_find("COM_POWER_COUNT"), &required_power_module_count); - - // Check avionics rail voltages (if USB isn't connected) - if (!system_power.usb_connected) { - float avionics_power_rail_voltage = system_power.voltage5v_v; - - if (avionics_power_rail_voltage < 4.5f) { - success = false; - - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", - (double)avionics_power_rail_voltage); - } - - } else if (avionics_power_rail_voltage < 4.8f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); - } - - } else if (avionics_power_rail_voltage > 5.4f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage); - } - } - - - const int power_module_count = math::countSetBits(system_power.brick_valid); - - if (power_module_count < required_power_module_count) { - success = false; - - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Power redundancy not met: %d instead of %" PRId32, - power_module_count, required_power_module_count); - } - } - } - - } else { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "system power unavailable"); - } - - success = false; - } - - return success; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp deleted file mode 100644 index 22c3b0735f..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#include -#include -#include - -bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off, - vehicle_status_s &status, const bool report_fail, const bool is_arm_attempt) -{ - bool prearm_ok = true; - - // rate control mode require valid angular velocity - if (control_mode.flag_control_rates_enabled && !status_flags.angular_velocity_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Angular velocity invalid"); } - - prearm_ok = false; - } - - // attitude control mode require valid attitude - if (control_mode.flag_control_attitude_enabled && !status_flags.attitude_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Attitude invalid"); } - - prearm_ok = false; - } - - // velocity control mode require valid velocity - if (control_mode.flag_control_velocity_enabled && !status_flags.local_velocity_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Velocity invalid"); } - - prearm_ok = false; - } - - // position control mode require valid position - if (control_mode.flag_control_position_enabled && !status_flags.local_position_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Position invalid"); } - - prearm_ok = false; - } - - // manual control mode require valid manual control (rc) - if (control_mode.flag_control_manual_enabled && status.rc_signal_lost) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Manual control unavailable"); } - - prearm_ok = false; - } - - if (status_flags.flight_terminated) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Flight termination active"); } - - prearm_ok = false; - } - - // USB not connected - if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Flying with USB is not safe"); } - - prearm_ok = false; - } - - // battery and system power status - if (!status_flags.circuit_breaker_engaged_power_check) { - - // Fail transition if power is not good - if (!status_flags.power_input_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Connect power module"); } - - prearm_ok = false; - } - - // main battery level - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true, - status_flags.battery_healthy, status); - - // Only arm if healthy - if (!status_flags.battery_healthy) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Check battery"); } - } - - prearm_ok = false; - } - } - - // Arm Requirements: mission - int32_t _param_com_arm_mis_req = 0; - param_get(param_find("COM_ARM_MIS_REQ"), &_param_com_arm_mis_req); - const bool mission_required = (_param_com_arm_mis_req == 1); - - if (mission_required) { - if (!status_flags.auto_mission_available) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "No valid mission"); } - } - - prearm_ok = false; - } - - if (!status_flags.global_position_valid) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Missions require a global position"); } - } - - prearm_ok = false; - } - } - - int32_t _param_com_arm_wo_gps = 1; - param_get(param_find("COM_ARM_WO_GPS"), &_param_com_arm_wo_gps); - const bool global_position_required = (_param_com_arm_wo_gps == 0); - - if (global_position_required) { - if (!status_flags.global_position_valid) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Global position required"); } - } - - prearm_ok = false; - } - - if (!status_flags.home_position_valid) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Home position invalid"); } - } - - prearm_ok = false; - } - } - - // safety button - if (safety_button_available && !safety_off) { - // Fail transition if we need safety button press - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Press safety button first"); } - } - - prearm_ok = false; - } - - if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Avoidance system not ready"); } - } - - prearm_ok = false; - - } - - int32_t _param_com_arm_chk_escs = 1; - param_get(param_find("COM_ARM_CHK_ESCS"), &_param_com_arm_chk_escs); - const bool esc_checks_required = (_param_com_arm_chk_escs == 0); - - if (esc_checks_required && status_flags.escs_error) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "One or more ESCs are offline"); } - - prearm_ok = false; - } - } - - if (esc_checks_required && status_flags.escs_failure) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "One or more ESCs have a failure"); } - - prearm_ok = false; - } - } - - if (status.is_vtol) { - if (status.in_transition_mode) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Vehicle is in transition state"); } - - prearm_ok = false; - } - } - - if (!status_flags.circuit_breaker_vtol_fw_arming_check - && status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Vehicle is not in multicopter mode"); } - - prearm_ok = false; - } - } - } - - int32_t _param_gf_action = 0; - param_get(param_find("GF_ACTION"), &_param_gf_action); - const bool gefence_action_configured = (_param_gf_action != 0); - - if (gefence_action_configured && status.geofence_violated) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Vehicle outside geofence"); - } - - prearm_ok = false; - } - - int32_t _param_com_arm_auth_req = 0; - param_get(param_find("COM_ARM_AUTH_REQ"), &_param_com_arm_auth_req); - const bool arm_authorization_configured = (_param_com_arm_auth_req != 0); - - // Arm Requirements: authorization - // check last, and only if everything else has passed - // skip arm authorization check until actual arming attempt - if (arm_authorization_configured && prearm_ok && is_arm_attempt) { - if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) { - // feedback provided in arm_auth_check - prearm_ok = false; - } - } - - return prearm_ok; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp deleted file mode 100644 index 6ee1373c56..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "../PreFlightCheck.hpp" - -#include -#include -#include - -/** - * Maximum deadzone value - */ -#define RC_INPUT_MAX_DEADZONE_US 500 - -/** - * Minimum value - */ -#define RC_INPUT_LOWEST_MIN_US 0 - -/** - * Maximum value - */ -#define RC_INPUT_HIGHEST_MAX_US 2500 - -int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail) -{ - char nbuf[20]; - param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; - - unsigned map_fail_count = 0; - - const char *rc_map_mandatory[] = { /*"RC_MAP_MODE_SW",*/ - /* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/ - nullptr /* end marker */ - }; - - unsigned j = 0; - - /* first check channel mappings */ - while (rc_map_mandatory[j] != nullptr) { - - param_t map_parm = param_find(rc_map_mandatory[j]); - - if (map_parm == PARAM_INVALID) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: PARAM %s MISSING.", rc_map_mandatory[j]); } - - /* give system time to flush error message in case there are more */ - px4_usleep(100000); - map_fail_count++; - j++; - continue; - } - - int32_t mapping; - param_get(map_parm, &mapping); - - if (mapping > input_rc_s::RC_INPUT_MAX_CHANNELS) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: %s >= NUMBER OF CHANNELS.", rc_map_mandatory[j]); } - - /* give system time to flush error message in case there are more */ - px4_usleep(100000); - map_fail_count++; - } - - if (mapping == 0) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: mandatory %s is unmapped.", rc_map_mandatory[j]); } - - /* give system time to flush error message in case there are more */ - px4_usleep(100000); - map_fail_count++; - } - - j++; - } - - unsigned total_fail_count = 0; - unsigned channels_failed = 0; - - for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) { - /* should the channel be enabled? */ - uint8_t count = 0; - - /* initialize values to values failing the check */ - float param_min = 0.0f; - float param_max = 0.0f; - float param_trim = 0.0f; - float param_rev = 0.0f; - float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles_min = param_find(nbuf); - param_get(_parameter_handles_min, ¶m_min); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles_trim = param_find(nbuf); - param_get(_parameter_handles_trim, ¶m_trim); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles_max = param_find(nbuf); - param_get(_parameter_handles_max, ¶m_max); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles_rev = param_find(nbuf); - param_get(_parameter_handles_rev, ¶m_rev); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles_dz = param_find(nbuf); - param_get(_parameter_handles_dz, ¶m_dz); - - /* assert min..center..max ordering */ - if (param_min < RC_INPUT_LOWEST_MIN_US) { - count++; - - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MIN < %u.", i + 1, RC_INPUT_LOWEST_MIN_US); } - - /* give system time to flush error message in case there are more */ - px4_usleep(100000); - } - - if (param_max > RC_INPUT_HIGHEST_MAX_US) { - count++; - - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MAX > %u.", i + 1, RC_INPUT_HIGHEST_MAX_US); } - - /* give system time to flush error message in case there are more */ - px4_usleep(100000); - } - - if (param_trim < param_min) { - count++; - - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM < MIN (%d/%d).", i + 1, (int)param_trim, (int)param_min); } - - /* give system time to flush error message in case there are more */ - px4_usleep(100000); - } - - if (param_trim > param_max) { - count++; - - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM > MAX (%d/%d).", i + 1, (int)param_trim, (int)param_max); } - - /* give system time to flush error message in case there are more */ - px4_usleep(100000); - } - - /* assert deadzone is sane */ - if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_DZ > %u.", i + 1, RC_INPUT_MAX_DEADZONE_US); } - - /* give system time to flush error message in case there are more */ - px4_usleep(100000); - count++; - } - - total_fail_count += count; - - if (count) { - channels_failed++; - } - } - - if (channels_failed) { - px4_sleep(2); - - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "%d config error%s for %d RC channel%s.", - total_fail_count, - (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); - } - - px4_usleep(100000); - } - - return total_fail_count + map_fail_count; -} diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index bdc6943852..5bada2bf11 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -34,6 +34,7 @@ add_subdirectory(failure_detector) add_subdirectory(HealthAndArmingChecks) add_subdirectory(Arming) +add_subdirectory(ModeUtil) px4_add_module( MODULE modules__commander @@ -62,12 +63,12 @@ px4_add_module( geo health_and_arming_checks hysteresis - PreFlightCheck ArmAuthorization ArmStateMachine HealthFlags sensor_calibration world_magnetic_model + mode_util ) if(PX4_TESTING) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 40cf41740b..c48b7665b0 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -45,7 +45,6 @@ #include "Commander.hpp" /* commander module headers */ -#include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "Arming/ArmAuthorization/ArmAuthorization.h" #include "Arming/HealthFlags/HealthFlags.h" #include "commander_helper.h" @@ -300,26 +299,13 @@ int Commander::custom_command(int argc, char *argv[]) } if (!strcmp(argv[0], "check")) { - uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)}; - vehicle_status_s vehicle_status{}; - vehicle_status_sub.copy(&vehicle_status); + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS); - uORB::Subscription vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; - vehicle_status_flags_s vehicle_status_flags{}; - vehicle_status_flags_sub.copy(&vehicle_status_flags); + uORB::SubscriptionData vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + PX4_INFO("Preflight check: %s", vehicle_status_flags_sub.get().pre_flight_checks_pass ? "OK" : "FAILED"); - uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - vehicle_control_mode_s vehicle_control_mode{}; - vehicle_control_mode_sub.copy(&vehicle_control_mode); - - bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, - vehicle_control_mode, - true, // report_failures - false, // safety_buttton_available not known - false); // safety_off not known - PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); - - print_health_flags(vehicle_status); + print_health_flags(vehicle_status_sub.get()); return 0; } @@ -511,11 +497,9 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) bool Commander::shutdown_if_allowed() { - return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode, - _safety.isButtonAvailable(), _safety.isSafetyOff(), - vehicle_status_s::ARMING_STATE_SHUTDOWN, - _actuator_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags, - arm_disarm_reason_t::shutdown); + return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, + vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks, + false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown); } static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) @@ -764,11 +748,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ } } - transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode, - _safety.isButtonAvailable(), _safety.isSafetyOff(), - vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, run_preflight_checks, - &_mavlink_log_pub, _vehicle_status_flags, - calling_reason); + transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, + vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks, + &_mavlink_log_pub, calling_reason); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason)); @@ -808,11 +790,9 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f } } - transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode, - _safety.isButtonAvailable(), _safety.isSafetyOff(), - vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, false, - &_mavlink_log_pub, _vehicle_status_flags, - calling_reason); + transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, + vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false, + &_mavlink_log_pub, calling_reason); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason)); @@ -834,7 +814,8 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f Commander::Commander() : ModuleParams(nullptr), - _failure_detector(this) + _failure_detector(this), + _health_and_arming_checks(this, _vehicle_status_flags, _vehicle_status) { _vehicle_land_detected.landed = true; @@ -854,7 +835,7 @@ Commander::Commander() : _vehicle_status_flags.offboard_control_signal_lost = true; - _vehicle_status_flags.power_input_valid = true; + _vehicle_status.power_input_valid = true; // default for vtol is rotary wing _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; @@ -868,12 +849,6 @@ Commander::Commander() : _param_rc_map_fltmode = param_find("RC_MAP_FLTMODE"); updateParameters(); - - // run preflight immediately to find all relevant parameters, but don't report - PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode, - false, // report_failures - false, // safety_buttton_available not known - false); // safety_off not known } Commander::~Commander() @@ -1288,7 +1263,7 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; // check if current mission and first item are valid - if (_vehicle_status_flags.auto_mission_available) { + if (_vehicle_status.auto_mission_available) { // requested first mission item valid if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { @@ -1422,10 +1397,9 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode, - _safety.isButtonAvailable(), _safety.isSafetyOff(), - vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, - false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags, + if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status, + vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, _health_and_arming_checks, + false /* fRunPreArmChecks */, &_mavlink_log_pub, (cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal)) ) { @@ -2124,7 +2098,7 @@ void Commander::updateParameters() } - _vehicle_status_flags.avoidance_system_required = _param_com_obs_avoid.get(); + _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s); @@ -2149,19 +2123,6 @@ void Commander::updateParameters() _vehicle_status.is_vtol = is_vtol(_vehicle_status); _vehicle_status.is_vtol_tailsitter = is_vtol_tailsitter(_vehicle_status); - // CP_DIST: collision preventation enabled if CP_DIST > 0 - if (is_rotary_wing(_vehicle_status) || is_vtol(_vehicle_status)) { - if (_param_cp_dist == PARAM_INVALID) { - _param_cp_dist = param_find("CP_DIST"); - } - - float cp_dist = 0; - - if (_param_cp_dist != PARAM_INVALID && (param_get(_param_cp_dist, &cp_dist) == PX4_OK)) { - _collision_prevention_enabled = cp_dist > 0.f; - } - } - // _mode_switch_mapped = (RC_MAP_FLTMODE > 0) if (_param_rc_map_fltmode != PARAM_INVALID && (param_get(_param_rc_map_fltmode, &value_int32) == PX4_OK)) { _mode_switch_mapped = (value_int32 > 0); @@ -2230,7 +2191,7 @@ Commander::run() } /* Update OA parameter */ - _vehicle_status_flags.avoidance_system_required = _param_com_obs_avoid.get(); + _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); #if defined(BOARD_HAS_POWER_CONTROL) @@ -2260,10 +2221,10 @@ Commander::run() !system_power.brick_valid && !system_power.usb_connected) { /* flying only on servo rail, this is unsafe */ - _vehicle_status_flags.power_input_valid = false; + _vehicle_status.power_input_valid = false; } else { - _vehicle_status_flags.power_input_valid = true; + _vehicle_status.power_input_valid = true; } _system_power_usb_connected = system_power.usb_connected; @@ -2476,11 +2437,9 @@ Commander::run() /* If in INIT state, try to proceed to STANDBY state */ if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) { - _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode, - _safety.isButtonAvailable(), _safety.isSafetyOff(), - vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, - true /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags, - arm_disarm_reason_t::transition_to_standby); + _arm_state_machine.arming_state_transition(_vehicle_status, + vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, + true /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::transition_to_standby); } /* start mission result check */ @@ -2494,7 +2453,7 @@ Commander::run() const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp) && (mission_result.instance_count > 0); - _vehicle_status_flags.auto_mission_available = mission_result_ok && mission_result.valid; + _vehicle_status.auto_mission_available = mission_result_ok && mission_result.valid; if (mission_result_ok) { if (_vehicle_status.mission_failure != mission_result.failure) { @@ -2513,7 +2472,7 @@ Commander::run() if (_vehicle_status_flags.home_position_valid && (prev_mission_instance_count != mission_result.instance_count)) { - if (!_vehicle_status_flags.auto_mission_available) { + if (!_vehicle_status.auto_mission_available) { /* the mission is invalid */ tune_mission_fail(true); @@ -2535,7 +2494,7 @@ Commander::run() && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp) && mission_result.finished) { - if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status_flags.auto_mission_available) { + if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status.auto_mission_available) { main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _vehicle_status_flags, _commander_state); @@ -2658,7 +2617,7 @@ Commander::run() /* Check for mission flight termination */ if (_arm_state_machine.isArmed() && _mission_result_sub.get().flight_termination && - !_vehicle_status_flags.circuit_breaker_flight_termination_disabled) { + !_vehicle_status.circuit_breaker_flight_termination_disabled) { if (!_flight_termination_triggered && !_lockdown_triggered) { @@ -2770,7 +2729,7 @@ Commander::run() events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning}, "Critical failure detected: lockdown"); - } else if (!_vehicle_status_flags.circuit_breaker_flight_termination_disabled && + } else if (!_vehicle_status.circuit_breaker_flight_termination_disabled && !_flight_termination_triggered && !_lockdown_triggered) { _actuator_armed.force_failsafe = true; @@ -2868,8 +2827,6 @@ Commander::run() checkWindSpeedThresholds(); } - _vehicle_status_flags.flight_terminated = _actuator_armed.force_failsafe || _actuator_armed.manual_lockdown; - /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time(); @@ -3013,14 +2970,12 @@ Commander::run() if (hrt_elapsed_time(&_vehicle_status.timestamp) >= 500_ms || _status_changed || nav_state_changed || !(_actuator_armed == actuator_armed_prev)) { - // Evaluate current prearm status (skip during arm -> disarm transition) - if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) { + // Evaluate current prearm status (skip during arm <-> disarm transitions as checks are run there already) + if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status_flags.calibration_enabled) { perf_begin(_preflight_check_perf); - _vehicle_status_flags.pre_flight_checks_pass = PreFlightCheck::preflightCheck(nullptr, _vehicle_status, - _vehicle_status_flags, - _vehicle_control_mode, - false, // report_failures - _safety.isButtonAvailable(), _safety.isSafetyOff()); + _health_and_arming_checks.update(); + _vehicle_status_flags.pre_flight_checks_pass = _health_and_arming_checks.canArm( + _vehicle_status.nav_state); perf_end(_preflight_check_perf); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, @@ -3072,7 +3027,7 @@ Commander::run() set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); _arm_tune_played = true; - } else if (!_vehicle_status_flags.usb_connected && + } else if (!_vehicle_status.usb_connected && (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { /* play tune on battery critical */ @@ -3151,17 +3106,17 @@ Commander::run() void Commander::get_circuit_breaker_params() { - _vehicle_status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), + _vehicle_status.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY); - _vehicle_status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(), + _vehicle_status.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(), CBRK_USB_CHK_KEY); - _vehicle_status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val( + _vehicle_status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val( _param_cbrk_airspd_chk.get(), CBRK_AIRSPD_CHK_KEY); - _vehicle_status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val( + _vehicle_status.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val( _param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY); - _vehicle_status_flags.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val( + _vehicle_status.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val( _param_cbrk_vtolarming.get(), CBRK_VTOLARMING_KEY); } @@ -3604,7 +3559,7 @@ void Commander::data_link_check() switch (telemetry.type) { case telemetry_status_s::LINK_TYPE_USB: // set (but don't unset) telemetry via USB as active once a MAVLink connection is up - _vehicle_status_flags.usb_connected = true; + _vehicle_status.usb_connected = true; break; case telemetry_status_s::LINK_TYPE_IRIDIUM: { @@ -3641,13 +3596,6 @@ void Commander::data_link_check() mavlink_log_info(&_mavlink_log_pub, "Data link regained\t"); events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained"); } - - if (!_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) { - // make sure to report preflight check failures to a connecting GCS - PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode, - true, // report_failures - _safety.isButtonAvailable(), _safety.isSafetyOff()); - } } _datalink_last_heartbeat_gcs = telemetry.timestamp; @@ -3680,8 +3628,8 @@ void Commander::data_link_check() bool healthy = telemetry.parachute_system_healthy; _datalink_last_heartbeat_parachute_system = telemetry.timestamp; - _vehicle_status_flags.parachute_system_present = true; - _vehicle_status_flags.parachute_system_healthy = healthy; + _vehicle_status.parachute_system_present = true; + _vehicle_status.parachute_system_healthy = healthy; set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, true, true, healthy, _vehicle_status); } @@ -3692,7 +3640,7 @@ void Commander::data_link_check() } _datalink_last_heartbeat_avoidance_system = telemetry.timestamp; - _vehicle_status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy; + _vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy; } } } @@ -3729,21 +3677,21 @@ void Commander::data_link_check() if ((hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > 3_s) && !_parachute_system_lost) { mavlink_log_critical(&_mavlink_log_pub, "Parachute system lost"); - _vehicle_status_flags.parachute_system_present = false; - _vehicle_status_flags.parachute_system_healthy = false; + _vehicle_status.parachute_system_present = false; + _vehicle_status.parachute_system_healthy = false; _parachute_system_lost = true; _status_changed = true; set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, false, true, false, _vehicle_status); } // AVOIDANCE SYSTEM state check (only if it is enabled) - if (_vehicle_status_flags.avoidance_system_required && !_onboard_controller_lost) { + if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) { // if heartbeats stop if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0) && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) { _avoidance_system_lost = true; - _vehicle_status_flags.avoidance_system_valid = false; + _vehicle_status.avoidance_system_valid = false; } } @@ -3776,18 +3724,18 @@ void Commander::avoidance_check() } const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms; - const bool cp_healthy = _vehicle_status_flags.avoidance_system_valid || distance_sensor_valid; + const bool cp_healthy = _vehicle_status.avoidance_system_valid || distance_sensor_valid; - const bool sensor_oa_present = cp_healthy || _vehicle_status_flags.avoidance_system_required + const bool sensor_oa_present = cp_healthy || _vehicle_status.avoidance_system_required || _collision_prevention_enabled; const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled; const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled); - const bool sensor_oa_enabled = ((auto_mode && _vehicle_status_flags.avoidance_system_required) || (pos_ctl_mode + const bool sensor_oa_enabled = ((auto_mode && _vehicle_status.avoidance_system_required) || (pos_ctl_mode && _collision_prevention_enabled)); - const bool sensor_oa_healthy = ((auto_mode && _vehicle_status_flags.avoidance_system_valid) || (pos_ctl_mode + const bool sensor_oa_healthy = ((auto_mode && _vehicle_status.avoidance_system_valid) || (pos_ctl_mode && cp_healthy)); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled, @@ -3939,7 +3887,7 @@ void Commander::battery_status_check() _battery_warning = worst_warning; } - _vehicle_status_flags.battery_healthy = + _vehicle_status.battery_healthy = // All connected batteries are regularly being published (hrt_elapsed_time(&oldest_update) < 5_s) // There is at least one connected battery (in any slot) @@ -4323,7 +4271,7 @@ void Commander::manual_control_check() } else { // if not mavlink also report valid RC calibration for health - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _vehicle_status_flags.rc_calibration_valid, + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true, _vehicle_status); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index f9d04bf693..3f7d7b0cff 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -35,11 +35,11 @@ /* Helper classes */ #include "Arming/ArmStateMachine/ArmStateMachine.hpp" -#include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "failure_detector/FailureDetector.hpp" #include "Safety.hpp" #include "state_machine_helper.h" #include "worker_thread.hpp" +#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" #include #include @@ -264,7 +264,6 @@ private: ) // optional parameters - param_t _param_cp_dist{PARAM_INVALID}; param_t _param_mav_comp_id{PARAM_INVALID}; param_t _param_mav_sys_id{PARAM_INVALID}; param_t _param_mav_type{PARAM_INVALID}; @@ -317,7 +316,6 @@ private: bool _geofence_warning_action_on{false}; bool _geofence_violated_prev{false}; - bool _collision_prevention_enabled{false}; bool _rtl_time_actions_done{false}; @@ -425,7 +423,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; - uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; #if defined(BOARD_HAS_POWER_CONTROL) @@ -456,4 +453,5 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")}; + HealthAndArmingChecks _health_and_arming_checks; }; diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 474cf13e74..7b53acfbbe 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -34,7 +34,26 @@ px4_add_library(health_and_arming_checks Common.cpp HealthAndArmingChecks.cpp + checks/systemCheck.cpp + checks/magnetometerCheck.cpp + checks/accelerometerCheck.cpp + checks/gyroCheck.cpp + checks/baroCheck.cpp + checks/imuConsistencyCheck.cpp + checks/airspeedCheck.cpp + checks/distanceSensorChecks.cpp + checks/rcCalibrationCheck.cpp + checks/powerCheck.cpp + checks/estimatorCheck.cpp + checks/failureDetectorCheck.cpp + checks/manualControlCheck.cpp + checks/modeCheck.cpp + checks/cpuResourceCheck.cpp + checks/sdcardCheck.cpp + checks/parachuteCheck.cpp + checks/batteryCheck.cpp ) +add_dependencies(health_and_arming_checks mode_util) px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp LINKLIBS health_and_arming_checks mode_util diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index b10a377913..5a0aaceec5 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "Common.hpp" +#include "../ModeUtil/mode_requirements.hpp" void Report::getHealthReport(health_report_s &report) const { @@ -157,7 +158,12 @@ void Report::reset() _results[_current_result].reset(); _next_buffer_idx = 0; _buffer_overflowed = false; - _failing_checks_prevent_mode_switch = 0; +} + +void Report::prepare(uint8_t vehicle_type) +{ + // Get mode requirements before running any checks (in particular the mode checks require them) + getModeRequirements(vehicle_type, _status_flags); } NavModes Report::getModeGroup(uint8_t nav_state) const diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index e2b42c0580..ccaef35bef 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -316,6 +316,7 @@ private: * - report() (which can be called independently as well) */ void reset(); + void prepare(uint8_t vehicle_type); void finalize(); bool report(bool is_armed, bool force); diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index 229276d9a5..ada699a2e9 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -45,6 +45,8 @@ bool HealthAndArmingChecks::update(bool force_reporting) { _reporter.reset(); + _reporter.prepare(_context.status().vehicle_type); + for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) { if (!_checks[i]) { break; @@ -64,6 +66,8 @@ bool HealthAndArmingChecks::update(bool force_reporting) _reporter._mavlink_log_pub = &_mavlink_log_pub; _reporter.reset(); + _reporter.prepare(_context.status().vehicle_type); + for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) { if (!_checks[i]) { break; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index b001e03a45..c34e57b01e 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -39,6 +39,26 @@ #include #include +#include "checks/accelerometerCheck.hpp" +#include "checks/airspeedCheck.hpp" +#include "checks/baroCheck.hpp" +#include "checks/cpuResourceCheck.hpp" +#include "checks/distanceSensorChecks.hpp" +#include "checks/estimatorCheck.hpp" +#include "checks/failureDetectorCheck.hpp" +#include "checks/gyroCheck.hpp" +#include "checks/imuConsistencyCheck.hpp" +#include "checks/magnetometerCheck.hpp" +#include "checks/manualControlCheck.hpp" +#include "checks/modeCheck.hpp" +#include "checks/parachuteCheck.hpp" +#include "checks/powerCheck.hpp" +#include "checks/rcCalibrationCheck.hpp" +#include "checks/sdcardCheck.hpp" +#include "checks/systemCheck.hpp" +#include "checks/batteryCheck.hpp" + + class HealthAndArmingChecks : public ModuleParams { public: @@ -58,6 +78,11 @@ public: */ bool canArm(uint8_t nav_state) const { return _reporter.canArm(nav_state); } + /** + * Whether switching into a given navigation mode is possible + */ + bool canRun(uint8_t nav_state) const { return _reporter.canRun(nav_state); } + protected: void updateParams() override; private: @@ -68,7 +93,44 @@ private: uORB::Publication _health_report_pub{ORB_ID(health_report)}; // all checks - HealthAndArmingCheckBase *_checks[10] = { + AccelerometerChecks _accelerometer_checks; + AirspeedChecks _airspeed_checks; + BaroChecks _baro_checks; + CpuResourceChecks _cpu_resource_checks; + DistanceSensorChecks _distance_sensor_checks; + EstimatorChecks _estimator_checks; + FailureDetectorChecks _failure_detector_checks; + GyroChecks _gyro_checks; + ImuConsistencyChecks _imu_consistency_checks; + MagnetometerChecks _magnetometer_checks; + ManualControlChecks _manual_control_checks; + ModeChecks _mode_checks; + ParachuteChecks _parachute_checks; + PowerChecks _power_checks; + RcCalibrationChecks _rc_calibration_checks; + SdCardChecks _sd_card_checks; + SystemChecks _system_checks; + BatteryChecks _battery_checks; + + HealthAndArmingCheckBase *_checks[30] = { + &_accelerometer_checks, + &_airspeed_checks, + &_baro_checks, + &_cpu_resource_checks, + &_distance_sensor_checks, + &_estimator_checks, + &_failure_detector_checks, + &_gyro_checks, + &_imu_consistency_checks, + &_magnetometer_checks, + &_manual_control_checks, + &_mode_checks, + &_parachute_checks, + &_power_checks, + &_rc_calibration_checks, + &_sd_card_checks, + &_system_checks, + &_battery_checks, }; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp new file mode 100644 index 0000000000..c7e0684dd8 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "accelerometerCheck.hpp" + +#include + +using namespace time_literals; + +void AccelerometerChecks::checkAndReport(const Context &context, Report &reporter) +{ + for (int instance = 0; instance < _sensor_accel_sub.size(); instance++) { + const bool is_required = instance == 0 || isAccelRequired(instance); + + if (!is_required) { + continue; + } + + const bool exists = _sensor_accel_sub[instance].advertised(); + bool is_valid = false; + bool is_calibration_valid = false; + + if (exists) { + sensor_accel_s accel_data; + is_valid = _sensor_accel_sub[instance].copy(&accel_data) && (accel_data.device_id != 0) && (accel_data.timestamp != 0) + && (hrt_elapsed_time(&accel_data.timestamp) < 1_s); + + if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) { + is_calibration_valid = true; + + } else { + is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0); + } + + reporter.setIsPresent(health_component_t::gyro); + } + + const bool is_sensor_ok = is_valid && is_calibration_valid; + + if (!is_sensor_ok) { + if (!exists) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::accel, events::ID("check_accel_missing"), + events::Log::Error, "Accelerometer sensor {1} missing", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel Sensor %u missing", instance); + } + + } else if (!is_valid) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::accel, events::ID("check_accel_no_data"), + events::Log::Error, "No valid data from Accelerometer {1}", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Accel %u", instance); + } + + } else if (!is_calibration_valid) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::accel, events::ID("check_accel_not_calibrated"), + events::Log::Error, "Accelerometer {1} uncalibrated", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel %u uncalibrated", instance); + } + } + } + } +} + +bool AccelerometerChecks::isAccelRequired(int instance) +{ + sensor_accel_s sensor_accel; + + if (!_sensor_accel_sub[instance].copy(&sensor_accel)) { + return false; + } + + const uint32_t device_id = static_cast(sensor_accel.device_id); + + if (device_id == 0) { + return false; + } + + bool is_used_by_nav = false; + + for (int i = 0; i < _estimator_status_sub.size(); i++) { + estimator_status_s estimator_status; + + if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.accel_device_id == device_id) { + is_used_by_nav = true; + break; + } + } + + return is_used_by_nav; +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp new file mode 100644 index 0000000000..53f9b749b3 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include +#include +#include +#include + +class AccelerometerChecks : public HealthAndArmingCheckBase +{ +public: + AccelerometerChecks() = default; + ~AccelerometerChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + bool isAccelRequired(int instance); + + uORB::SubscriptionMultiArray _sensor_accel_sub{ORB_ID::sensor_accel}; + uORB::SubscriptionMultiArray _estimator_status_sub{ORB_ID::estimator_status}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp new file mode 100644 index 0000000000..0a54a3aca4 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "airspeedCheck.hpp" + +using namespace time_literals; + +AirspeedChecks::AirspeedChecks() + : _param_fw_arsp_mode_handle(param_find("FW_ARSP_MODE")), _param_fw_airspd_trim_handle(param_find("FW_AIRSPD_TRIM")) +{ +} + +void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (context.status().circuit_breaker_engaged_airspd_check || + (context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !context.status().is_vtol)) { + return; + } + + int32_t airspeed_mode = 0; + param_get(_param_fw_arsp_mode_handle, &airspeed_mode); + const bool optional = (airspeed_mode == 1); + + airspeed_validated_s airspeed_validated; + + if (_airspeed_validated_sub.copy(&airspeed_validated) && hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s) { + + reporter.setIsPresent(health_component_t::differential_pressure); + + float airspeed_trim = 10.0f; + param_get(_param_fw_airspd_trim_handle, &airspeed_trim); + + const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed + + /* + * Check if airspeed is declared valid or not by airspeed selector. + */ + if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) { + + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::differential_pressure, + events::ID("check_airspeed_invalid"), + events::Log::Error, "Airspeed invalid"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed invalid"); + } + } + + /* + * Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying + * Negative and positive offsets are considered. This check is optional, because the pitot cover + * might have been removed before arming. + */ + if (!context.isArmed() && _param_com_arm_arsp_en.get() + && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) { + /* EVENT + * @description + * Check the airspeed calibration and the pitot. + * + * + * Measured: {1:.1m/s}, limit: {2:.1m/s}. + * + * This check can be configured via COM_ARM_ARSP_EN parameter. + * + */ + reporter.armingCheckFailure(NavModes::None, health_component_t::differential_pressure, + events::ID("check_airspeed_too_high"), + events::Log::Error, "Airspeed too high", airspeed_validated.calibrated_airspeed_m_s, arming_max_airspeed_allowed); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed too high - check cal or pitot"); + } + } + + } else if (!optional) { + + /* EVENT + * @description + * + * Most likely the airspeed selector module is not running. + * This check can be configured via CBRK_AIRSPD_CHK parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::differential_pressure, + events::ID("check_airspeed_no_data"), + events::Log::Error, "No airspeed data"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed selector module down"); + } + } + +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp similarity index 64% rename from src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp rename to src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp index 4fd3b1094f..41549aee2e 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,38 +31,28 @@ * ****************************************************************************/ -#include "../PreFlightCheck.hpp" +#pragma once -#include -#include +#include "../Common.hpp" -using namespace time_literals; +#include +#include -bool PreFlightCheck::parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, - const vehicle_status_flags_s &status_flags) +class AirspeedChecks : public HealthAndArmingCheckBase { - bool success = true; +public: + AirspeedChecks(); + ~AirspeedChecks() = default; - int32_t param_com_parachute = false; - param_get(param_find("COM_PARACHUTE"), ¶m_com_parachute); - const bool parachute_required = param_com_parachute != 0; + void checkAndReport(const Context &context, Report &reporter) override; - if (parachute_required) { - if (!status_flags.parachute_system_present) { - success = false; +private: + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system missing"); - } + const param_t _param_fw_arsp_mode_handle; + const param_t _param_fw_airspd_trim_handle; - } else if (!status_flags.parachute_system_healthy) { - success = false; - - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system not ready"); - } - } - } - - return success; -} + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_com_arm_arsp_en + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.cpp new file mode 100644 index 0000000000..1f73758d9b --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "baroCheck.hpp" + +using namespace time_literals; + +void BaroChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (!_param_sys_has_baro.get()) { + return; + } + + for (int instance = 0; instance < _sensor_baro_sub.size(); instance++) { + const bool is_required = instance == 0 || isBaroRequired(instance); + + if (!is_required) { + continue; + } + + const bool exists = _sensor_baro_sub[instance].advertised(); + bool is_valid = false; + + if (exists) { + sensor_baro_s baro_data; + is_valid = _sensor_baro_sub[instance].copy(&baro_data) && (baro_data.device_id != 0) && (baro_data.timestamp != 0) + && (hrt_elapsed_time(&baro_data.timestamp) < 1_s); + reporter.setIsPresent(health_component_t::absolute_pressure); + } + + const bool is_sensor_ok = is_valid; + + if (!is_sensor_ok) { + if (!exists) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::absolute_pressure, + events::ID("check_baro_missing"), + events::Log::Error, "Barometer {1} missing", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: barometer %u missing", instance); + } + + } else if (!is_valid) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::absolute_pressure, + events::ID("check_baro_no_data"), + events::Log::Error, "No valid data from barometer {1}", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Baro %u", instance); + } + } + } + } +} + +bool BaroChecks::isBaroRequired(int instance) +{ + sensor_baro_s sensor_baro; + + if (!_sensor_baro_sub[instance].copy(&sensor_baro)) { + return false; + } + + const uint32_t device_id = static_cast(sensor_baro.device_id); + + if (device_id == 0) { + return false; + } + + bool is_used_by_nav = false; + + for (int i = 0; i < _estimator_status_sub.size(); i++) { + estimator_status_s estimator_status; + + if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.baro_device_id == device_id) { + is_used_by_nav = true; + break; + } + } + + return is_used_by_nav; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp similarity index 63% rename from src/modules/commander/Arming/PreFlightCheck/checks/modeCheck.cpp rename to src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp index fa69e77692..8689f9ff96 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/modeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp @@ -31,39 +31,31 @@ * ****************************************************************************/ -#include "../PreFlightCheck.hpp" +#pragma once -#include +#include "../Common.hpp" -using namespace time_literals; +#include +#include +#include +#include +#include -bool PreFlightCheck::modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, - const vehicle_status_s &vehicle_status) +class BaroChecks : public HealthAndArmingCheckBase { - bool success = true; +public: + BaroChecks() = default; + ~BaroChecks() = default; - switch (vehicle_status.nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: - case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - case vehicle_status_s::NAVIGATION_STATE_POSCTL: - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - case vehicle_status_s::NAVIGATION_STATE_ACRO: - case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: - case vehicle_status_s::NAVIGATION_STATE_STAB: - case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: - break; + void checkAndReport(const Context &context, Report &reporter) override; - default: - success = false; +private: + bool isBaroRequired(int instance); - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Mode not suitable for takeoff"); - } + uORB::SubscriptionMultiArray _sensor_baro_sub{ORB_ID::sensor_baro}; + uORB::SubscriptionMultiArray _estimator_status_sub{ORB_ID::estimator_status}; - break; - } - - return success; -} + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_sys_has_baro + ) +}; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp similarity index 72% rename from src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp rename to src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index e97c3ab310..66f8da9a6b 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,28 +31,24 @@ * ****************************************************************************/ -#include "../PreFlightCheck.hpp" - -#include -#include -#include +#include "batteryCheck.hpp" using namespace time_literals; -bool PreFlightCheck::airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status) +void BatteryChecks::checkAndReport(const Context &context, Report &reporter) { - bool success = true; + if (!context.status().battery_healthy) { -#ifdef CONFIG_ARCH_BOARD_PX4_FMU_V2 + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"), + events::Log::Error, "Battery unhealthy"); - // We no longer support VTOL on fmu-v2, so we need to warn existing users. - if (status.is_vtol) { - mavlink_log_critical(mavlink_log_pub, - "VTOL is not supported with fmu-v2, see docs.px4.io/main/en/config/firmware.html#bootloader"); - success = false; + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Battery unhealthy"); + } } -#endif + reporter.setIsPresent(health_component_t::battery); // assume it's present - return success; } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp new file mode 100644 index 0000000000..14b0924e41 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include + +class BatteryChecks : public HealthAndArmingCheckBase +{ +public: + BatteryChecks() = default; + ~BatteryChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp new file mode 100644 index 0000000000..2413bfeda5 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "cpuResourceCheck.hpp" + +using namespace time_literals; + +void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (_param_com_cpu_max.get() < FLT_EPSILON) { + return; + } + + cpuload_s cpuload; + + if (!_cpuload_sub.copy(&cpuload) || hrt_elapsed_time(&cpuload.timestamp) > 2_s) { + + /* EVENT + * @description + * + * If the system does not provide any CPU load information, use the parameter COM_CPU_MAX + * to disable the check. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_cpuload"), + events::Log::Error, "No CPU load information"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information"); + } + + } else { + const float cpuload_percent = cpuload.load * 100.f; + + if (cpuload_percent > _param_com_cpu_max.get()) { + + /* EVENT + * @description + * The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update + * rate via IMU_GYRO_RATEMAX. + * + * + * The threshold can be adjusted via COM_CPU_MAX parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_cpuload_too_high"), + events::Log::Error, "CPU load too high: {1:.1}%", cpuload_percent); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent); + } + } + } +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp similarity index 62% rename from src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp rename to src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp index bf84206a7c..c2582d171f 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,46 +31,25 @@ * ****************************************************************************/ -#include "../PreFlightCheck.hpp" +#pragma once + +#include "../Common.hpp" -#include -#include -#include #include #include -using namespace time_literals; - -bool PreFlightCheck::cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail) +class CpuResourceChecks : public HealthAndArmingCheckBase { - bool success = true; +public: + CpuResourceChecks() = default; + ~CpuResourceChecks() = default; - uORB::SubscriptionData cpuload_sub{ORB_ID(cpuload)}; - cpuload_sub.update(); + void checkAndReport(const Context &context, Report &reporter) override; - float cpuload_percent_max; - param_get(param_find("COM_CPU_MAX"), &cpuload_percent_max); +private: + uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; - if (cpuload_percent_max > 0.f) { - - if (hrt_elapsed_time(&cpuload_sub.get().timestamp) > 2_s) { - success = false; - - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Fail: No CPU load information"); - } - } - - const float cpuload_percent = cpuload_sub.get().load * 100.f; - - if (cpuload_percent > cpuload_percent_max) { - success = false; - - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Fail: CPU load too high: %3.1f%%", (double)cpuload_percent); - } - } - } - - return success; -} + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamFloat) _param_com_cpu_max + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp new file mode 100644 index 0000000000..b09cde4632 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "distanceSensorChecks.hpp" + +using namespace time_literals; + +void DistanceSensorChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (_param_sys_has_num_dist.get() <= 0) { + return; + } + + for (int instance = 0; instance < _distance_sensor_sub.size(); instance++) { + const bool exists = _distance_sensor_sub[instance].advertised(); + const bool is_mandatory = instance < _param_sys_has_num_dist.get(); + bool valid = false; + + if (exists) { + distance_sensor_s dist_sens; + valid = _distance_sensor_sub[instance].copy(&dist_sens) && hrt_elapsed_time(&dist_sens.timestamp) < 1_s; + reporter.setIsPresent(health_component_t::distance_sensor); + } + + if (is_mandatory) { + if (!exists) { + /* EVENT + * @description + * + * This check can be configured via SYS_HAS_NUM_DIST parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::distance_sensor, + events::ID("check_distance_sensor_missing"), + events::Log::Error, "Distance sensor {1} missing", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Distance Sensor %u missing", instance); + } + + } else if (!valid) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::distance_sensor, + events::ID("check_distance_sensor_invalid"), + events::Log::Error, "No valid data from distance sensor {1}", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: no valid data from distance sensor %u", instance); + } + } + } + } +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.hpp similarity index 62% rename from src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp rename to src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.hpp index f02c3c0ebc..90bb4dd6e5 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.hpp @@ -31,41 +31,26 @@ * ****************************************************************************/ -#include "../PreFlightCheck.hpp" +#pragma once + +#include "../Common.hpp" -#include -#include -#include -#include #include +#include #include -using namespace time_literals; - -bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool is_mandatory, bool &report_fail) +class DistanceSensorChecks : public HealthAndArmingCheckBase { - const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK); - bool valid = false; +public: + DistanceSensorChecks() = default; + ~DistanceSensorChecks() = default; - 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(); + void checkAndReport(const Context &context, Report &reporter) override; - valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s); - } +private: + uORB::SubscriptionMultiArray _distance_sensor_sub{ORB_ID::distance_sensor}; - 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 valid || !is_mandatory; -} + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_sys_has_num_dist + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp new file mode 100644 index 0000000000..d5939a6b2a --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -0,0 +1,473 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "estimatorCheck.hpp" + +using namespace time_literals; + +void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (_param_sys_mc_est_group.get() != 2) { + return; + } + + const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude; + bool missing_data = false; + + if (_param_sens_imu_mode.get() == 0) { // multi-ekf + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + bool instance_changed = _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance) + && _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + + if (!instance_changed) { + missing_data = true; + } + + } else { + missing_data = true; + } + } + + if (!missing_data) { + estimator_status_s estimator_status; + + if (_estimator_status_sub.copy(&estimator_status)) { + + checkEstimatorStatus(context, reporter, estimator_status, required_groups); + + } else { + missing_data = true; + } + } + + if (missing_data) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_missing_data"), + events::Log::Info, "Waiting for estimator to initialize"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ekf2 missing data"); + } + + } else { + reporter.setIsPresent(health_component_t::local_position_estimate); + checkSensorBias(context, reporter, required_groups); + } +} + +void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter, + const estimator_status_s &estimator_status, NavModes required_groups) +{ + if (estimator_status.pre_flt_fail_innov_heading) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_heading_not_stable"), + events::Log::Error, "Heading estimate not stable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable"); + } + + } else if (estimator_status.pre_flt_fail_innov_vel_horiz) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_hor_vel_not_stable"), + events::Log::Error, "Horizontal velocity estimate not stable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity estimate not stable"); + } + + } else if (estimator_status.pre_flt_fail_innov_vel_vert) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_vert_vel_not_stable"), + events::Log::Error, "Vertical velocity estimate not stable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity estimate not stable"); + } + + } else if (estimator_status.pre_flt_fail_innov_height) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_hgt_not_stable"), + events::Log::Error, "Height estimate not stable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable"); + } + } + + + if ((_param_com_arm_mag_str.get() >= 1) && estimator_status.pre_flt_fail_mag_field_disturbed) { + NavModes required_groups_mag = required_groups; + + if (_param_com_arm_mag_str.get() != 1) { + required_groups_mag = NavModes::None; // optional + } + + /* EVENT + * @description + * + * This check can be configured via COM_ARM_MAG_STR and EKF2_MAG_CHECK parameters. + * + */ + reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate, + events::ID("check_estimator_mag_interference"), + events::Log::Warning, "Strong magnetic interference detected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference detected"); + } + } + + // check vertical position innovation test ratio + if (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get()) { + /* EVENT + * + * Test ratio: {1:.3}, limit: {2:.3}. + * + * This check can be configured via COM_ARM_EKF_HGT parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_hgt_est_err"), + events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get()); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error"); + } + } + + // check velocity innovation test ratio + if (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get()) { + /* EVENT + * + * Test ratio: {1:.3}, limit: {2:.3}. + * + * This check can be configured via COM_ARM_EKF_VEL parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_vel_est_err"), + events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get()); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error"); + } + } + + // check horizontal position innovation test ratio + if (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get()) { + /* EVENT + * + * Test ratio: {1:.3}, limit: {2:.3}. + * + * This check can be configured via COM_ARM_EKF_POS parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_pos_est_err"), + events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get()); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error"); + } + } + + // check magnetometer innovation test ratio + if (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get()) { + /* EVENT + * + * Test ratio: {1:.3}, limit: {2:.3}. + * + * This check can be configured via COM_ARM_EKF_YAW parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_yaw_est_err"), + events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get()); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error"); + } + } + + // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing + if (_param_sys_has_gps.get()) { + const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); + const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0; + + if (ekf_gps_fusion) { + reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly + } + + if (ekf_gps_check_fail) { + NavModes required_groups_gps = required_groups; + events::Log log_level = events::Log::Error; + + if (_param_com_arm_wo_gps.get()) { + required_groups_gps = NavModes::None; // optional + log_level = events::Log::Warning; + } + + // Only report the first failure to avoid spamming + const char *message = nullptr; + + if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) { + message = "Preflight%s: GPS fix too low"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_fix_too_low"), + log_level, "GPS fix too low"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { + message = "Preflight%s: not enough GPS Satellites"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_num_sats_too_low"), + log_level, "Not enough GPS Satellites"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) { + message = "Preflight%s: GPS PDOP too high"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_pdop_too_high"), + log_level, "GPS PDOP too high"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { + message = "Preflight%s: GPS Horizontal Pos Error too high"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_hor_pos_err_too_high"), + log_level, "GPS Horizontal Position Error too high"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) { + message = "Preflight%s: GPS Vertical Pos Error too high"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_vert_pos_err_too_high"), + log_level, "GPS Vertical Position Error too high"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) { + message = "Preflight%s: GPS Speed Accuracy too low"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_speed_acc_too_low"), + log_level, "GPS Speed Accuracy too low"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) { + message = "Preflight%s: GPS Horizontal Pos Drift too high"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_hor_pos_drift_too_high"), + log_level, "GPS Horizontal Position Drift too high"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) { + message = "Preflight%s: GPS Vertical Pos Drift too high"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_vert_pos_drift_too_high"), + log_level, "GPS Vertical Position Drift too high"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) { + message = "Preflight%s: GPS Hor Speed Drift too high"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_hor_speed_drift_too_high"), + log_level, "GPS Horizontal Speed Drift too high"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) { + message = "Preflight%s: GPS Vert Speed Drift too high"; + /* EVENT + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_vert_speed_drift_too_high"), + log_level, "GPS Vertical Speed Drift too high"); + + } else { + if (!ekf_gps_fusion) { + // Likely cause unknown + message = "Preflight%s: Estimator not using GPS"; + /* EVENT + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_not_fusing"), + log_level, "Estimator not using GPS"); + + } else { + // if we land here there was a new flag added and the code not updated. Show a generic message. + message = "Preflight%s: Poor GPS Quality"; + /* EVENT + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_generic"), + log_level, "Poor GPS Quality"); + } + } + + if (message && reporter.mavlink_log_pub()) { + if (!_param_com_arm_wo_gps.get()) { + mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail"); + + } else { + mavlink_log_critical(reporter.mavlink_log_pub(), message, ""); + } + } + } + } + +} + +void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, NavModes required_groups) +{ + // _estimator_sensor_bias_sub instance got changed above already + estimator_sensor_bias_s bias; + + if (_estimator_sensor_bias_sub.copy(&bias) && hrt_elapsed_time(&bias.timestamp) < 30_s) { + + // check accelerometer bias estimates + if (bias.accel_bias_valid) { + const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit; + + for (int axis_index = 0; axis_index < 3; axis_index++) { + // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives + // adjust test threshold by 3-sigma + const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f)); + + if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) { + /* EVENT + * An accelerometer recalibration might help. + * + * + * Axis {1}: |{2:.8}| > {3:.8} + {4:.8} + * + * This check can be configured via EKF2_ABL_LIM parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_high_accel_bias"), + events::Log::Error, "High Accelerometer Bias", axis_index, + bias.accel_bias[axis_index], ekf_ab_test_limit, test_uncertainty); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Accelerometer Bias"); + } + + return; // avoid showing more than one error + } + } + } + + // check gyro bias estimates + if (bias.gyro_bias_valid) { + const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit; + + for (int axis_index = 0; axis_index < 3; axis_index++) { + // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives + // adjust test threshold by 3-sigma + const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f)); + + if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) { + /* EVENT + * A Gyro recalibration might help. + * + * + * Axis {1}: |{2:.8}| > {3:.8} + {4:.8} + * + * This check can be configured via EKF2_ABL_GYRLIM parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_high_gyro_bias"), + events::Log::Error, "High Gyro Bias", axis_index, + bias.gyro_bias[axis_index], ekf_gb_test_limit, test_uncertainty); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Gyro Bias"); + } + + return; // avoid showing more than one error + } + } + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp new file mode 100644 index 0000000000..002a2a720a --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include +#include +#include +#include + + +class EstimatorChecks : public HealthAndArmingCheckBase +{ +public: + EstimatorChecks() = default; + ~EstimatorChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status, + NavModes required_groups); + void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups); + + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_sys_mc_est_group, + (ParamInt) _param_sens_imu_mode, + (ParamInt) _param_com_arm_mag_str, + (ParamFloat) _param_com_arm_ekf_hgt, + (ParamFloat) _param_com_arm_ekf_vel, + (ParamFloat) _param_com_arm_ekf_pos, + (ParamFloat) _param_com_arm_ekf_yaw, + (ParamBool) _param_com_arm_wo_gps, + (ParamBool) _param_sys_has_gps + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp new file mode 100644 index 0000000000..26acd0e65b --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "failureDetectorCheck.hpp" + +void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (context.status().failure_detector_status != vehicle_status_s::FAILURE_NONE) { + if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) { + /* EVENT + * @description + * The vehicle exceeded the maximum configured roll angle. + * + * + * This check can be configured via FD_FAIL_R parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"), + events::Log::Critical, "Attitude failure detected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)"); + } + + } else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) { + /* EVENT + * @description + * The vehicle exceeded the maximum configured pitch angle. + * + * + * This check can be configured via FD_FAIL_P parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"), + events::Log::Critical, "Attitude failure detected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)"); + } + } + + if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"), + events::Log::Critical, "Altitude failure detected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure"); + } + } + + if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) { + /* EVENT + * @description + * + * This check can be configured via FD_EXT_ATS_EN parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"), + events::Log::Critical, "Failure triggered by external system"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system"); + } + } + + if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { + /* EVENT + * @description + * One or more ESCs failed to arm. + * + * + * This check can be configured via FD_ESCS_EN parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"), + events::Log::Critical, "ESC failure"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected"); + } + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.hpp new file mode 100644 index 0000000000..ebad706c37 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.hpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + + +class FailureDetectorChecks : public HealthAndArmingCheckBase +{ +public: + FailureDetectorChecks() = default; + ~FailureDetectorChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp new file mode 100644 index 0000000000..778f48dd75 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "gyroCheck.hpp" + +#include + +using namespace time_literals; + +void GyroChecks::checkAndReport(const Context &context, Report &reporter) +{ + for (int instance = 0; instance < _sensor_gyro_sub.size(); instance++) { + const bool is_required = instance == 0 || isGyroRequired(instance); + + if (!is_required) { + continue; + } + + const bool exists = _sensor_gyro_sub[instance].advertised(); + bool is_valid = false; + bool is_calibration_valid = false; + + if (exists) { + sensor_gyro_s gyro_data; + is_valid = _sensor_gyro_sub[instance].copy(&gyro_data) && (gyro_data.device_id != 0) && (gyro_data.timestamp != 0) + && (hrt_elapsed_time(&gyro_data.timestamp) < 1_s); + + if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) { + is_calibration_valid = true; + + } else { + is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0); + } + + reporter.setIsPresent(health_component_t::gyro); + } + + const bool is_sensor_ok = is_valid && is_calibration_valid; + + if (!is_sensor_ok) { + if (!exists) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::gyro, events::ID("check_gyro_missing"), + events::Log::Error, "Gyro sensor {1} missing", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro Sensor %u missing", instance); + } + + } else if (!is_valid) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::gyro, events::ID("check_gyro_no_data"), + events::Log::Error, "No valid data from Gyro {1}", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Gyro %u", instance); + } + + } else if (!is_calibration_valid) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::gyro, events::ID("check_gyro_not_calibrated"), + events::Log::Error, "Gyro {1} uncalibrated", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro %u uncalibrated", instance); + } + } + } + } +} + +bool GyroChecks::isGyroRequired(int instance) +{ + sensor_gyro_s sensor_gyro; + + if (!_sensor_gyro_sub[instance].copy(&sensor_gyro)) { + return false; + } + + const uint32_t device_id = static_cast(sensor_gyro.device_id); + + if (device_id == 0) { + return false; + } + + bool is_used_by_nav = false; + + for (int i = 0; i < _estimator_status_sub.size(); i++) { + estimator_status_s estimator_status; + + if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.gyro_device_id == device_id) { + is_used_by_nav = true; + break; + } + } + + return is_used_by_nav; +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp new file mode 100644 index 0000000000..705e27b0af --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include +#include +#include +#include + +class GyroChecks : public HealthAndArmingCheckBase +{ +public: + GyroChecks() = default; + ~GyroChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + bool isGyroRequired(int instance); + + uORB::SubscriptionMultiArray _sensor_gyro_sub{ORB_ID::sensor_gyro}; + uORB::SubscriptionMultiArray _estimator_status_sub{ORB_ID::estimator_status}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp new file mode 100644 index 0000000000..ca292d7b14 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "imuConsistencyCheck.hpp" + +void ImuConsistencyChecks::checkAndReport(const Context &context, Report &reporter) +{ + sensors_status_imu_s imu; + + if (!_sensors_status_imu_sub.copy(&imu) || context.isArmed()) { + return; + } + + // Use the difference between IMU's to detect a bad calibration. + // If a single IMU is fitted, the value being checked will be zero so this check will always pass. + for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) { + if (imu.accel_device_ids[i] != 0) { + + const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i]; + + NavModes required_groups = NavModes::None; + + if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get()) { + required_groups = NavModes::All; + } + + if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get() * 0.8f) { + /* EVENT + * @description + * Check the calibration. + * + * Inconsistency value: {2}. + * Configured Threshold: {3}. + * + * + * This check can be configured via COM_ARM_IMU_ACC parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::accel, + events::ID("check_imu_accel_inconsistent"), + events::Log::Warning, "Accel {1} inconsistent", i, accel_inconsistency_m_s_s, _param_com_arm_imu_acc.get()); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel %u inconsistent - check cal", i); + } + + break; + } + } + } + + // Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec + for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) { + if (imu.gyro_device_ids[i] != 0) { + + const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i]; + + NavModes required_groups = NavModes::None; + + if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get()) { + required_groups = NavModes::All; + } + + if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get() * 0.5f) { + /* EVENT + * @description + * Check the calibration. + * + * Inconsistency value: {2}. + * Configured Threshold: {3}. + * + * + * This check can be configured via COM_ARM_IMU_GYR parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::gyro, + events::ID("check_imu_gyro_inconsistent"), + events::Log::Warning, "Gyro {1} inconsistent", i, gyro_inconsistency_rad_s, _param_com_arm_imu_gyr.get()); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro %u inconsistent - check cal", i); + } + + break; + } + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.hpp new file mode 100644 index 0000000000..a6cce208a8 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.hpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class ImuConsistencyChecks : public HealthAndArmingCheckBase +{ +public: + ImuConsistencyChecks() = default; + ~ImuConsistencyChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _sensors_status_imu_sub{ORB_ID(sensors_status_imu)}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamFloat) _param_com_arm_imu_acc, + (ParamFloat) _param_com_arm_imu_gyr + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp new file mode 100644 index 0000000000..61ee9a81a0 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "magnetometerCheck.hpp" + +#include + +using namespace time_literals; + +void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (!_param_sys_has_mag.get()) { + return; + } + + bool had_failure = false; + + for (int instance = 0; instance < _sensor_mag_sub.size(); instance++) { + bool is_mag_fault = false; + const bool is_required = instance == 0 || isMagRequired(instance, is_mag_fault); + + if (!is_required) { + continue; + } + + const bool exists = _sensor_mag_sub[instance].advertised(); + bool is_valid = false; + bool is_calibration_valid = false; + + if (exists) { + sensor_mag_s mag_data; + is_valid = _sensor_mag_sub[instance].copy(&mag_data) && (mag_data.device_id != 0) && (mag_data.timestamp != 0) + && (hrt_elapsed_time(&mag_data.timestamp) < 1_s); + + if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) { + is_calibration_valid = true; + + } else { + is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id) >= 0); + } + + reporter.setIsPresent(health_component_t::magnetometer); + } + + const bool is_sensor_ok = is_valid && is_calibration_valid && !is_mag_fault; + + if (!is_sensor_ok) { + if (!exists) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::magnetometer, events::ID("check_mag_missing"), + events::Log::Error, "Compass sensor {1} missing", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Compass Sensor %u missing", instance); + } + + } else if (!is_valid) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::magnetometer, events::ID("check_mag_no_data"), + events::Log::Error, "No valid data from Compass {1}", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Compass %u", instance); + } + + } else if (!is_calibration_valid) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::magnetometer, + events::ID("check_mag_not_calibrated"), + events::Log::Error, "Compass {1} uncalibrated", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Compass %u uncalibrated", instance); + } + + } else if (is_mag_fault) { + /* EVENT + * @description + * Recalibrate the compass and check the orientation. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::magnetometer, events::ID("check_mag_fault"), + events::Log::Error, "Estimator fault due to Compass {1}", instance); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Compass %u fault", instance); + } + } + + had_failure = true; + } + } + + if (!had_failure && !context.isArmed()) { + consistencyCheck(context, reporter); + } +} + +bool MagnetometerChecks::isMagRequired(int instance, bool &mag_fault) +{ + sensor_mag_s sensor_mag; + + if (!_sensor_mag_sub[instance].copy(&sensor_mag)) { + return false; + } + + const uint32_t device_id = static_cast(sensor_mag.device_id); + + if (device_id == 0) { + return false; + } + + bool is_used_by_nav = false; + + for (int i = 0; i < _estimator_status_sub.size(); i++) { + estimator_status_s estimator_status; + + if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.mag_device_id == device_id) { + mag_fault = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT); + is_used_by_nav = true; + break; + } + } + + return is_used_by_nav; +} + + +void MagnetometerChecks::consistencyCheck(const Context &context, Report &reporter) +{ + if (_param_com_arm_mag_ang.get() < 0) { // Check disabled + return; + } + + sensor_preflight_mag_s sensors; + + if (!_sensor_preflight_mag_sub.copy(&sensors)) { + // can happen if not advertised (yet) + return; + } + + // Use the difference between sensors to detect a bad calibration, orientation or magnetic interference. + // If a single sensor is fitted, the value being checked will be zero so this check will always pass. + if (sensors.mag_inconsistency_angle > math::radians(_param_com_arm_mag_ang.get())) { + int inconsistency_angle_deg = static_cast(math::degrees(sensors.mag_inconsistency_angle)); + /* EVENT + * @description + * Check the compass orientations and recalibrate. + * + * + * This check can be configured via COM_ARM_MAG_ANG parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::magnetometer, + events::ID("check_mag_consistency"), + events::Log::Error, "Compass inconsistent by {1} degrees", inconsistency_angle_deg); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Compasses %d° inconsistent", + inconsistency_angle_deg); + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp new file mode 100644 index 0000000000..1be11d27b5 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include +#include +#include +#include +#include + +class MagnetometerChecks : public HealthAndArmingCheckBase +{ +public: + MagnetometerChecks() = default; + ~MagnetometerChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + bool isMagRequired(int instance, bool &mag_fault); + void consistencyCheck(const Context &context, Report &reporter); + + uORB::SubscriptionMultiArray _sensor_mag_sub{ORB_ID::sensor_mag}; + uORB::SubscriptionMultiArray _estimator_status_sub{ORB_ID::estimator_status}; + + uORB::Subscription _sensor_preflight_mag_sub{ORB_ID(sensor_preflight_mag)}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_sys_has_mag, + (ParamInt) _param_com_arm_mag_ang + ) +}; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/manualControlCheck.cpp similarity index 61% rename from src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp rename to src/modules/commander/HealthAndArmingChecks/checks/manualControlCheck.cpp index 661d8fbaba..fe22363d16 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/manualControlCheck.cpp @@ -31,49 +31,56 @@ * ****************************************************************************/ -#include "../PreFlightCheck.hpp" - -#include -#include -#include +#include "manualControlCheck.hpp" using namespace time_literals; -bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail) +void ManualControlChecks::checkAndReport(const Context &context, Report &reporter) { - bool success = true; + if (context.isArmed()) { + return; + } - uORB::SubscriptionData manual_control_switches_sub{ORB_ID(manual_control_switches)}; - const manual_control_switches_s &manual_control_switches = manual_control_switches_sub.get(); + manual_control_switches_s manual_control_switches; - if (manual_control_switches.timestamp != 0) { + if (_manual_control_switches_sub.copy(&manual_control_switches)) { // check action switches if (manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { - success = false; + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, + events::ID("check_man_control_rtl_engaged"), + events::Log::Error, "RTL switch engaged"); - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Failure: RTL switch engaged"); + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RTL switch engaged"); } } if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { - success = false; + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, + events::ID("check_man_control_kill_engaged"), + events::Log::Error, "Kill switch engaged"); - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Failure: Kill switch engaged"); + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Kill switch engaged"); } } if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { - success = false; + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, + events::ID("check_man_control_landing_gear_up"), + events::Log::Error, "Landing gear switch set in UP position"); - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Failure: Landing gear switch set in UP position"); + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Landing gear switch set in UP position"); } } } - - return success; } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/manualControlCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/manualControlCheck.hpp new file mode 100644 index 0000000000..6108d6b4bf --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/manualControlCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class ManualControlChecks : public HealthAndArmingCheckBase +{ +public: + ManualControlChecks() = default; + ~ManualControlChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp new file mode 100644 index 0000000000..b76cca5b3d --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "modeCheck.hpp" + +void ModeChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (!context.isArmed()) { + checkArmingRequirement(context, reporter); + } + + // Failing mode requirements generally also clear the can_run bits which prevents mode switching and + // might trigger a failsafe if already in that mode. + + if (!reporter.failsafeFlags().angular_velocity_valid && reporter.failsafeFlags().mode_req_angular_velocity != 0) { + /* EVENT + * @description + * Make sure the gyroscope is providing valid data. + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_angular_velocity, health_component_t::system, + events::ID("check_modes_angular_velocity"), + events::Log::Critical, "Angular velocity not valid"); + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_angular_velocity); + } + + if (!reporter.failsafeFlags().attitude_valid && reporter.failsafeFlags().mode_req_attitude != 0) { + /* EVENT + * @description + * Wait until the estimator initialized + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_attitude, health_component_t::system, + events::ID("check_modes_attitude"), + events::Log::Critical, "No valid attitude estimate"); + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_attitude); + } + + if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) { + /* EVENT + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_position, health_component_t::system, + events::ID("check_modes_local_pos"), + events::Log::Error, "No valid local position estimate"); + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_position); + } + + if (!reporter.failsafeFlags().global_position_valid && reporter.failsafeFlags().mode_req_global_position != 0) { + /* EVENT + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::system, + events::ID("check_modes_global_pos"), + events::Log::Error, "No valid global position estimate"); + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position); + } + + if (!reporter.failsafeFlags().local_altitude_valid && reporter.failsafeFlags().mode_req_local_alt != 0) { + /* EVENT + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_alt, health_component_t::system, + events::ID("check_modes_local_alt"), + events::Log::Critical, "No valid altitude estimate"); + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_alt); + } + + if (!reporter.failsafeFlags().auto_mission_available && reporter.failsafeFlags().mode_req_mission != 0) { + /* EVENT + * @description + * Upload a mission first. + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system, + events::ID("check_modes_mission"), + events::Log::Info, "No mission available"); + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission); + } + + if (reporter.failsafeFlags().offboard_control_signal_lost && reporter.failsafeFlags().mode_req_offboard_signal != 0) { + /* EVENT + * @description + * The offboard component is not sending setpoints. + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_offboard_signal, health_component_t::system, + events::ID("check_modes_offboard_signal"), + events::Log::Error, "No offboard signal"); + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_offboard_signal); + } + + if (!reporter.failsafeFlags().home_position_valid && reporter.failsafeFlags().mode_req_home_position != 0) { + /* EVENT + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_home_position, health_component_t::system, + events::ID("check_modes_home_position"), + events::Log::Info, "Home position not set"); + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_home_position); + } + + if (reporter.failsafeFlags().mode_req_other != 0) { + // Here we expect there is already an event reported for the failing check (this is for external modes) + reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_other); + } +} + +void ModeChecks::checkArmingRequirement(const Context &context, Report &reporter) +{ + if (reporter.failsafeFlags().mode_req_prevent_arming & (1u << context.status().nav_state)) { + /* EVENT + * @description + * Switch to another mode first. + */ + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_prevent_arming, health_component_t::system, + events::ID("check_modes_cannot_takeoff"), + events::Log::Info, "Mode not suitable for arming"); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp new file mode 100644 index 0000000000..dd7df427e8 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include + +class ModeChecks : public HealthAndArmingCheckBase +{ +public: + ModeChecks() = default; + ~ModeChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + void checkArmingRequirement(const Context &context, Report &reporter); + +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp new file mode 100644 index 0000000000..ce0c2f538f --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "parachuteCheck.hpp" + + +using namespace time_literals; + +void ParachuteChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (!_param_com_parachute.get()) { + return; + } + + if (!context.status().parachute_system_present) { + /* EVENT + * @description + * Parachute system failed to report. Make sure it it setup and installed properly. + * + * + * This check can be configured via COM_PARACHUTE parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::parachute, events::ID("check_parachute_missing"), + events::Log::Error, "Parachute system missing"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system missing"); + } + + } else if (!context.status().parachute_system_healthy) { + + /* EVENT + * @description + * Parachute system reported being unhealth. + * + * + * This check can be configured via COM_PARACHUTE parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::parachute, events::ID("check_parachute_unhealthy"), + events::Log::Error, "Parachute system not ready"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Parachute system not ready"); + } + } + + if (context.status().parachute_system_present) { + reporter.setIsPresent(health_component_t::parachute); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp new file mode 100644 index 0000000000..162c03277c --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/parachuteCheck.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +class ParachuteChecks : public HealthAndArmingCheckBase +{ +public: + ParachuteChecks() = default; + ~ParachuteChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_com_parachute + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp new file mode 100644 index 0000000000..77fd33c2bd --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "powerCheck.hpp" + +using namespace time_literals; + +void PowerChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (context.status().circuit_breaker_engaged_power_check) { + return; + } + + if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) { + // Ignore power check in HITL. + return; + } + + if (!context.status().power_input_valid) { + + /* EVENT + * @description + * Note that USB must be disconnected as well. + * + * + * This check can be configured via CBRK_SUPPLY_CHK parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_avionics_power_missing"), + events::Log::Error, "Power module not connected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Power module not connected"); + } + + return; + } + + system_power_s system_power; + + if (_system_power_sub.copy(&system_power)) { + // Check avionics rail voltages (if USB isn't connected) + if (!system_power.usb_connected) { + float avionics_power_rail_voltage = system_power.voltage5v_v; + + const float low_error_threshold = 4.5f; + const float low_warning_threshold = 4.8f; + const float high_warning_threshold = 5.4f; + + if (avionics_power_rail_voltage < low_warning_threshold) { + NavModes affected_groups = NavModes::None; + + if (avionics_power_rail_voltage < low_error_threshold) { + affected_groups = NavModes::All; + } + + /* EVENT + * @description + * Check the voltage supply to the FMU, it must be above {2:.2} Volt. + * + * + * This check can be configured via CBRK_SUPPLY_CHK parameter. + * + */ + reporter.healthFailure(affected_groups, health_component_t::system, + events::ID("check_avionics_power_low"), + events::Log::Error, "Avionics Power low: {1:.2} Volt", avionics_power_rail_voltage, low_warning_threshold); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power low: %6.2f Volt", + (double)avionics_power_rail_voltage); + } + + } else if (avionics_power_rail_voltage > high_warning_threshold) { + /* EVENT + * @description + * Check the voltage supply to the FMU, it must be below {2:.2} Volt. + * + * + * This check can be configured via CBRK_SUPPLY_CHK parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, + events::ID("check_avionics_power_high"), + events::Log::Error, "Avionics Power high: {1:.2} Volt", avionics_power_rail_voltage, high_warning_threshold); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power high: %6.2f Volt", + (double)avionics_power_rail_voltage); + } + } + + const int power_module_count = math::countSetBits(system_power.brick_valid); + + if (power_module_count < _param_com_power_count.get()) { + /* EVENT + * @description + * Available power modules: {1}. + * Required power modules: {2}. + * + * + * This check can be configured via COM_POWER_COUNT parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, + events::ID("check_avionics_power_redundancy"), + events::Log::Error, "Power redundancy not met", power_module_count, _param_com_power_count.get()); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Power redundancy not met: %d instead of %" PRId32 "", + power_module_count, _param_com_power_count.get()); + } + } + } + + } else { + /* EVENT + * @description + * + * Ensure the ADC is working and the system_power topic is published. + * + * This check can be configured via CBRK_SUPPLY_CHK parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_system_power"), + events::Log::Error, "System power unavailable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: system power unavailable"); + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp new file mode 100644 index 0000000000..94d753fd3a --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class PowerChecks : public HealthAndArmingCheckBase +{ +public: + PowerChecks() = default; + ~PowerChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _system_power_sub{ORB_ID(system_power)}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_com_power_count + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp new file mode 100644 index 0000000000..62a7921dd3 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "rcCalibrationCheck.hpp" + +/** + * Maximum deadzone value + */ +#define RC_INPUT_MAX_DEADZONE_US 500 + +/** + * Minimum value + */ +#define RC_INPUT_LOWEST_MIN_US 0 + +/** + * Maximum value + */ +#define RC_INPUT_HIGHEST_MAX_US 2500 + +RcCalibrationChecks::RcCalibrationChecks() +{ + char nbuf[20]; + + for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) { + sprintf(nbuf, "RC%d_MIN", i + 1); + _param_handles[i].min = param_find(nbuf); + + sprintf(nbuf, "RC%d_TRIM", i + 1); + _param_handles[i].trim = param_find(nbuf); + + sprintf(nbuf, "RC%d_MAX", i + 1); + _param_handles[i].max = param_find(nbuf); + + sprintf(nbuf, "RC%d_REV", i + 1); + _param_handles[i].rev = param_find(nbuf); + + sprintf(nbuf, "RC%d_DZ", i + 1); + _param_handles[i].dz = param_find(nbuf); + } + +} + +void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (context.isArmed()) { + return; + } + + if (_param_com_rc_in_mode.get() != 0 && _param_com_rc_in_mode.get() != 2 && _param_com_rc_in_mode.get() != 3) { + return; + } + + for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) { + /* initialize values to values failing the check */ + float param_min = 0.0f; + float param_max = 0.0f; + float param_trim = 0.0f; + float param_rev = 0.0f; + float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f; + + param_get(_param_handles[i].min, ¶m_min); + param_get(_param_handles[i].trim, ¶m_trim); + param_get(_param_handles[i].max, ¶m_max); + param_get(_param_handles[i].rev, ¶m_rev); + param_get(_param_handles[i].dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < RC_INPUT_LOWEST_MIN_US) { + /* EVENT + * @description + * Recalibrate the RC. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, + events::ID("check_rc_min_too_small"), + events::Log::Error, "RC calibration for channel {1} invalid: MIN less than {2}", i + 1, RC_INPUT_LOWEST_MIN_US); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_MIN < %u", i + 1, + RC_INPUT_LOWEST_MIN_US); + } + } + + if (param_max > RC_INPUT_HIGHEST_MAX_US) { + /* EVENT + * @description + * Recalibrate the RC. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, + events::ID("check_rc_max_too_high"), + events::Log::Error, "RC calibration for channel {1} invalid: MAX greater than {2}", i + 1, RC_INPUT_HIGHEST_MAX_US); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_MAX > %u", i + 1, + RC_INPUT_HIGHEST_MAX_US); + } + } + + if (param_trim < param_min) { + /* EVENT + * @description + * Recalibrate the RC. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, + events::ID("check_rc_trim_too_small"), + events::Log::Error, "RC calibration for channel {1} invalid: TRIM less than MIN ({2} less than {3})", i + 1, + (int16_t)param_trim, (int16_t)param_min); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_TRIM < MIN (%d/%d)", i + 1, + (int)param_trim, (int)param_min); + } + } + + if (param_trim > param_max) { + /* EVENT + * @description + * Recalibrate the RC. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, + events::ID("check_rc_trim_too_high"), + events::Log::Error, "RC calibration for channel {1} invalid: TRIM greater than MAX ({2} greater than {3})", i + 1, + (int16_t)param_trim, (int16_t)param_max); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_TRIM > MAX (%d/%d)", i + 1, + (int)param_trim, (int)param_max); + } + } + + /* assert deadzone is sane */ + if (param_dz > RC_INPUT_MAX_DEADZONE_US) { + /* EVENT + * @description + * Recalibrate the RC. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, + events::ID("check_rc_dz_too_high"), + events::Log::Error, "RC calibration for channel {1} invalid: DZ greater than {2}", i + 1, RC_INPUT_MAX_DEADZONE_US); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RC ERROR: RC%d_DZ > %u", i + 1, + RC_INPUT_MAX_DEADZONE_US); + } + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.hpp new file mode 100644 index 0000000000..fe9806246d --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.hpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class RcCalibrationChecks : public HealthAndArmingCheckBase +{ +public: + RcCalibrationChecks(); + ~RcCalibrationChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + struct ParamHandles { + param_t min; + param_t trim; + param_t max; + param_t rev; + param_t dz; + }; + ParamHandles _param_handles[input_rc_s::RC_INPUT_MAX_CHANNELS]; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_com_rc_in_mode + ) +}; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp similarity index 61% rename from src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp rename to src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp index bfcd4d5d34..e056b80061 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp @@ -31,9 +31,7 @@ * ****************************************************************************/ -#include "../PreFlightCheck.hpp" -#include -#include +#include "sdcardCheck.hpp" #ifdef __PX4_DARWIN #include @@ -42,33 +40,40 @@ #include #endif -bool PreFlightCheck::sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, - const bool report_fail) +void SdCardChecks::checkAndReport(const Context &context, Report &reporter) { - bool success = true; - - int32_t param_com_arm_sdcard{0}; - param_get(param_find("COM_ARM_SDCARD"), ¶m_com_arm_sdcard); - - if (param_com_arm_sdcard > 0) { - struct statfs statfs_buf; - - if (!sd_card_detected_once && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) { - // on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted - sd_card_detected_once = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0); - } - - if (!sd_card_detected_once) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Warning! Missing FMU SD Card."); - } - - if (param_com_arm_sdcard == 2) { - // disallow arming without sd card - success = false; - } - } + if (_param_com_arm_sdcard.get() <= 0) { + return; } - return success; + struct statfs statfs_buf; + + if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) { + // on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted + _sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0); + } + + if (!_sdcard_detected) { + NavModes affected_modes{NavModes::None}; + + if (_param_com_arm_sdcard.get() == 2) { + // disallow arming without sd card + affected_modes = NavModes::All; + } + + /* EVENT + * @description + * Insert an SD Card to the autopilot and reboot the system. + * + * + * This check can be configured via COM_ARM_SDCARD parameter. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::system, events::ID("check_missing_fmu_sdcard"), + events::Log::Error, "Missing FMU SD Card"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card"); + } + } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp new file mode 100644 index 0000000000..6870f648ad --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +class SdCardChecks : public HealthAndArmingCheckBase +{ +public: + SdCardChecks() = default; + ~SdCardChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + bool _sdcard_detected{false}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_com_arm_sdcard + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp new file mode 100644 index 0000000000..8da933093c --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -0,0 +1,254 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "systemCheck.hpp" + +#include "../../Arming/ArmAuthorization/ArmAuthorization.h" +#include + +void SystemChecks::checkAndReport(const Context &context, Report &reporter) +{ + actuator_armed_s actuator_armed; + + if (_actuator_armed_sub.copy(&actuator_armed)) { + if (actuator_armed.force_failsafe || actuator_armed.manual_lockdown) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_flight_term_active"), + events::Log::Critical, "Flight termination active"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Flight termination active"); + } + } + } + + // USB not connected + if (!context.status().circuit_breaker_engaged_usb_check && context.status().usb_connected) { + /* EVENT + * @description + * Flying with USB is not safe. Disconnect it and reboot the FMU. + * + * + * This check can be configured via CBRK_USB_CHK parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_usb_connected"), + events::Log::Error, "USB connected"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Flying with USB is not safe"); + } + } + + // Arm Requirements: mission + if (_param_com_arm_mis_req.get() && !context.isArmed()) { + if (!context.status().auto_mission_available) { + /* EVENT + * @description + * + * This check can be configured via COM_ARM_MIS_REQ parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_mission"), + events::Log::Error, "No valid mission"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid mission"); + } + } + } + + reporter.failsafeFlags().auto_mission_available = context.status().auto_mission_available; + + // Global position required + if (!_param_com_arm_wo_gps.get() && !context.isArmed()) { + if (!reporter.failsafeFlags().global_position_valid) { + /* EVENT + * @description + * + * This check can be configured via COM_ARM_WO_GPS parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_global_pos"), + events::Log::Error, "Global position required"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Global position required"); + } + + } else if (!reporter.failsafeFlags().home_position_valid) { + /* EVENT + * @description + * + * This check can be configured via COM_ARM_WO_GPS parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_home_pos"), + events::Log::Error, "Home position required"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Home position required"); + } + } + } + + // safety button + if (context.status().safety_button_available && !context.status().safety_off && !context.isArmed()) { + /* EVENT + * @description + * + * This check can be configured via CBRK_IO_SAFETY parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_safety_button"), + events::Log::Info, "Press safety button first"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Press safety button first"); + } + } + + // avoidance system + if (context.status().avoidance_system_required) { + if (context.status().avoidance_system_valid) { + reporter.setIsPresent(health_component_t::avoidance); + + } else { + /* EVENT + * @description + * + * This check can be configured via COM_OBS_AVOID parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_avoidance_not_ready"), + events::Log::Error, "Avoidance system not ready"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avoidance system not ready"); + } + } + } + + // ESC checks + if (_param_com_arm_chk_escs.get() && reporter.failsafeFlags().escs_error) { + /* EVENT + * @description + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_system_escs_offline"), + events::Log::Error, "One or more ESCs are offline"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: One or more ESCs are offline"); + } + } + + if (_param_com_arm_chk_escs.get() && reporter.failsafeFlags().escs_failure) { + /* EVENT + * @description + * + * This check can be configured via COM_ARM_CHK_ESCS parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_system_escs_failure"), + events::Log::Error, "One or more ESCs have a failure"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: One or more ESCs have a failure"); + } + } + + reporter.setIsPresent(health_component_t::motors_escs); // TODO: based on telemetry + + // VTOL in transition + if (context.status().is_vtol && !context.isArmed()) { + if (context.status().in_transition_mode) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_vtol_in_transition"), + events::Log::Warning, "Vehicle is in transition state"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle is in transition state"); + } + } + + if (!context.status().circuit_breaker_vtol_fw_arming_check + && context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + /* EVENT + * @description + * + * This check can be configured via CBRK_VTOLARMING parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_vtol_in_fw_mode"), + events::Log::Warning, "Vehicle is not in multicopter mode"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle is not in multicopter mode"); + } + } + } + + if (_param_gf_action.get() != 0 && context.status().geofence_violated) { + /* EVENT + * @description + * + * This check can be configured via GF_ACTION parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_gf_violation"), + events::Log::Error, "Vehicle outside geofence"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle outside geofence"); + } + } + + // Arm Requirements: authorization + if (_param_com_arm_auth_req.get() != 0 && !context.isArmed()) { + if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_arm_auth_failed"), + events::Log::Error, "Arm authorization denied"); + } + } + + if (reporter.failsafeFlags().rc_signal_found_once) { + reporter.setIsPresent(health_component_t::remote_control); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp new file mode 100644 index 0000000000..daf31fe5da --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class SystemChecks : public HealthAndArmingCheckBase +{ +public: + SystemChecks() = default; + ~SystemChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_com_arm_mis_req, + (ParamBool) _param_com_arm_wo_gps, + (ParamBool) _param_com_arm_chk_escs, + (ParamInt) _param_com_arm_auth_req, + (ParamInt) _param_gf_action + ) +}; diff --git a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/modules/commander/ModeUtil/CMakeLists.txt similarity index 67% rename from src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt rename to src/modules/commander/ModeUtil/CMakeLists.txt index edec715b23..82afff9f02 100644 --- a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt +++ b/src/modules/commander/ModeUtil/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,27 +31,6 @@ # ############################################################################ -px4_add_library(PreFlightCheck - PreFlightCheck.cpp - checks/preArmCheck.cpp - checks/magnetometerCheck.cpp - checks/magConsistencyCheck.cpp - checks/accelerometerCheck.cpp - checks/gyroCheck.cpp - checks/baroCheck.cpp - checks/imuConsistencyCheck.cpp - checks/airspeedCheck.cpp - checks/distanceSensorChecks.cpp - checks/rcCalibrationCheck.cpp - checks/powerCheck.cpp - checks/airframeCheck.cpp - checks/ekf2Check.cpp - checks/failureDetectorCheck.cpp - checks/manualControlCheck.cpp - checks/modeCheck.cpp - checks/cpuResourceCheck.cpp - checks/sdcardCheck.cpp - checks/parachuteCheck.cpp +px4_add_library(mode_util + mode_requirements.cpp ) -target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration) diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp new file mode 100644 index 0000000000..60d1efcd8b --- /dev/null +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "mode_requirements.hpp" +#include + +static inline void setRequirement(uint8_t nav_state, uint32_t &mode_requirement) +{ + mode_requirement |= 1u << nav_state; +} + + +void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags) +{ + flags.mode_req_angular_velocity = 0; + flags.mode_req_attitude = 0; + flags.mode_req_local_position = 0; + flags.mode_req_global_position = 0; + flags.mode_req_local_alt = 0; + flags.mode_req_mission = 0; + flags.mode_req_offboard_signal = 0; + flags.mode_req_home_position = 0; + flags.mode_req_prevent_arming = 0; + flags.mode_req_other = 0; + + // NAVIGATION_STATE_MANUAL + + // NAVIGATION_STATE_ALTCTL + setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_local_alt); + + // NAVIGATION_STATE_POSCTL + setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position); + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position); + } + + setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_alt); + + // NAVIGATION_STATE_AUTO_MISSION + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_mission); + + // NAVIGATION_STATE_AUTO_LOITER + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt); + + // NAVIGATION_STATE_AUTO_RTL + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_home_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_prevent_arming); + + // NAVIGATION_STATE_ACRO + setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_angular_velocity); + + // NAVIGATION_STATE_DESCEND + setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_prevent_arming); + + // NAVIGATION_STATE_TERMINATION + setRequirement(vehicle_status_s::NAVIGATION_STATE_TERMINATION, flags.mode_req_prevent_arming); + + // NAVIGATION_STATE_OFFBOARD + setRequirement(vehicle_status_s::NAVIGATION_STATE_OFFBOARD, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_OFFBOARD, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_OFFBOARD, flags.mode_req_offboard_signal); + + // NAVIGATION_STATE_STAB + setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_attitude); + + // NAVIGATION_STATE_AUTO_TAKEOFF + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_local_alt); + + // NAVIGATION_STATE_AUTO_LAND + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_prevent_arming); + + // NAVIGATION_STATE_AUTO_FOLLOW_TARGET + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET, flags.mode_req_prevent_arming); + + // NAVIGATION_STATE_AUTO_PRECLAND + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, flags.mode_req_prevent_arming); + + // NAVIGATION_STATE_ORBIT + setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_ORBIT, flags.mode_req_prevent_arming); + + // NAVIGATION_STATE_AUTO_VTOL_TAKEOFF + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_alt); + + + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "update mode requirements"); +} diff --git a/src/modules/commander/ModeUtil/mode_requirements.hpp b/src/modules/commander/ModeUtil/mode_requirements.hpp new file mode 100644 index 0000000000..64246e8697 --- /dev/null +++ b/src/modules/commander/ModeUtil/mode_requirements.hpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * Get per-mode requirements + * @param vehicle_type one of vehicle_status_s::VEHICLE_TYPE_* + * @param flags output flags, all mode_req_* entries are set + */ +void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags); + diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 4e76c256c9..648fcccd5c 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -43,7 +43,6 @@ #include "../state_machine_helper.h" #include #include "../Arming/ArmStateMachine/ArmStateMachine.hpp" -#include "../Arming/PreFlightCheck/PreFlightCheck.hpp" class StateMachineHelperTest : public UnitTest {