From 42221483c4abc61b0a379e731fc9ec8f481a8a62 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Jul 2022 14:08:36 +0200 Subject: [PATCH] vehicle_status: Remove duplicate failure detector status --- msg/vehicle_status.msg | 12 ----- .../ArmStateMachine/ArmStateMachine.cpp | 4 +- .../ArmStateMachine/ArmStateMachine.hpp | 1 + .../ArmStateMachine/ArmStateMachineTest.cpp | 2 + .../Arming/PreFlightCheck/PreFlightCheck.cpp | 7 +-- .../Arming/PreFlightCheck/PreFlightCheck.hpp | 5 +- .../checks/failureDetectorCheck.cpp | 49 ++++++++++--------- src/modules/commander/Commander.cpp | 35 ++++++------- .../failure_detector/FailureDetector.cpp | 16 ++++++ .../failure_detector/FailureDetector.hpp | 2 + .../control_allocator/ControlAllocator.hpp | 2 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 22 +++++++-- 12 files changed, 92 insertions(+), 65 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 38f5737eb5..5b90bf3cfe 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -58,18 +58,6 @@ uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter uint8 NAVIGATION_STATE_MAX = 23 -# Bitmask of detected failures -uint16 failure_detector_status -uint16 FAILURE_NONE = 0 -uint16 FAILURE_ROLL = 1 # (1 << 0) -uint16 FAILURE_PITCH = 2 # (1 << 1) -uint16 FAILURE_ALT = 4 # (1 << 2) -uint16 FAILURE_EXT = 8 # (1 << 3) -uint16 FAILURE_ARM_ESC = 16 # (1 << 4) -uint16 FAILURE_BATTERY = 32 # (1 << 5) -uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) -uint16 FAILURE_MOTOR = 128 # (1 << 7) - uint8 hil_state uint8 HIL_STATE_OFF = 0 uint8 HIL_STATE_ON = 1 diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index a9d84c91ef..852522e350 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -39,7 +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 vehicle_control_mode_s &control_mode, const failure_detector_status_s failure_detector_status, + 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) @@ -68,6 +69,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s && (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) { if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode, + failure_detector_status, true, // report_failures safety_button_available, safety_off, true)) { // is_arm_attempt diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 4262091b0d..8122a49541 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -55,6 +55,7 @@ public: transition_result_t arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, + const failure_detector_status_s failure_detector_status, 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, diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index bc0751278c..76cdc773ae 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp @@ -257,11 +257,13 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) status_flags.circuit_breaker_engaged_power_check = true; vehicle_control_mode_s control_mode{}; + failure_detector_status_s failure_detector_status{}; // Attempt transition transition_result_t result = arm_state_machine.arming_state_transition( status, control_mode, + failure_detector_status, test->safety_button_available, test->safety_off, test->requested_state, diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index f3860b186d..4e492614ed 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -52,6 +52,7 @@ 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, + const failure_detector_status_s failure_detector_status, bool report_failures, const bool safety_button_available, const bool safety_off, const bool is_arm_attempt) @@ -206,11 +207,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } } - /* ---- Failure Detector ---- */ - if (!failureDetectorCheck(mavlink_log_pub, status, report_failures)) { - failed = true; - } - + failed = failed || !failureDetectorCheck(mavlink_log_pub, failure_detector_status, report_failures); 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); diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index e52a3ecbf5..dec987aa56 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -41,6 +41,7 @@ #pragma once #include +#include #include #include #include @@ -64,6 +65,7 @@ public: **/ 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, + const failure_detector_status_s failure_detector_status, bool reportFailures, const bool safety_button_available, const bool safety_off, const bool is_arm_attempt = false); @@ -95,7 +97,8 @@ private: 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 failureDetectorCheck(orb_advert_t *mavlink_log_pub, const failure_detector_status_s failure_detector_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); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp index 20a4544f67..b0c4514ad3 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp @@ -35,34 +35,35 @@ #include -bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, +bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, + const failure_detector_status_s failure_detector_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"); - } + if (report_fail) { + if (failure_detector_status.fd_roll) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected"); } - return false; + if (failure_detector_status.fd_pitch) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected"); + } + + if (failure_detector_status.fd_alt) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected"); + } + + if (failure_detector_status.fd_ext) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected"); + } + + if (failure_detector_status.fd_arm_escs) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected"); + } } - return true; + return !failure_detector_status.fd_roll + && !failure_detector_status.fd_pitch + && !failure_detector_status.fd_alt + && !failure_detector_status.fd_ext + && !failure_detector_status.fd_arm_escs; } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fa0b8c4f46..55a3340c31 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -294,8 +294,12 @@ int Commander::custom_command(int argc, char *argv[]) vehicle_control_mode_s vehicle_control_mode{}; vehicle_control_mode_sub.copy(&vehicle_control_mode); + uORB::Subscription failure_detector_status_sub{ORB_ID(failure_detector_status)}; + failure_detector_status_s failure_detector_status{}; + failure_detector_status_sub.copy(&failure_detector_status); + bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, - vehicle_control_mode, + vehicle_control_mode, failure_detector_status, true, // report_failures false, // safety_buttton_available not known false); // safety_off not known @@ -490,6 +494,7 @@ 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, + _failure_detector.getFailureDetectorStatus(), _safety.isButtonAvailable(), _safety.isSafetyOff(), vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags, @@ -743,6 +748,7 @@ 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, + _failure_detector.getFailureDetectorStatus(), _safety.isButtonAvailable(), _safety.isSafetyOff(), vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, run_preflight_checks, &_mavlink_log_pub, _vehicle_status_flags, @@ -787,6 +793,7 @@ 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, + _failure_detector.getFailureDetectorStatus(), _safety.isButtonAvailable(), _safety.isSafetyOff(), vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, false, &_mavlink_log_pub, _vehicle_status_flags, @@ -849,6 +856,7 @@ Commander::Commander() : // 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, + _failure_detector.getFailureDetectorStatus(), false, // report_failures false, // safety_buttton_available not known false); // safety_off not known @@ -1401,6 +1409,7 @@ Commander::handle_command(const vehicle_command_s &cmd) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode, + _failure_detector.getFailureDetectorStatus(), _safety.isButtonAvailable(), _safety.isSafetyOff(), vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags, @@ -2450,6 +2459,7 @@ Commander::run() if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) { _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode, + _failure_detector.getFailureDetectorStatus(), _safety.isButtonAvailable(), _safety.isSafetyOff(), vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, true /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags, @@ -2705,12 +2715,11 @@ Commander::run() } /* Check for failure detector status */ + const bool previous_motor_failure_flag = _failure_detector.getStatus().flags.motor; + if (_failure_detector.update(_vehicle_status, _vehicle_control_mode)) { - const bool motor_failure_changed = ((_vehicle_status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) > 0) != - _failure_detector.getStatus().flags.motor; - _vehicle_status.failure_detector_status = _failure_detector.getStatus().value; - auto fd_status_flags = _failure_detector.getStatusFlags(); _status_changed = true; + decltype(failure_detector_status_u::flags) fd_status_flags = _failure_detector.getStatusFlags(); if (_arm_state_machine.isArmed()) { if (fd_status_flags.arm_escs) { @@ -2773,7 +2782,7 @@ Commander::run() } // One-time actions based on motor failure - if (motor_failure_changed) { + if (fd_status_flags.motor != previous_motor_failure_flag) { if (fd_status_flags.motor) { mavlink_log_critical(&_mavlink_log_pub, "Motor failure detected\t"); events::send(events::ID("commander_motor_failure"), events::Log::Emergency, @@ -2992,6 +3001,7 @@ Commander::run() _vehicle_status_flags.pre_flight_checks_pass = PreFlightCheck::preflightCheck(nullptr, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode, + _failure_detector.getFailureDetectorStatus(), false, // report_failures _safety.isButtonAvailable(), _safety.isSafetyOff()); perf_end(_preflight_check_perf); @@ -3023,17 +3033,7 @@ Commander::run() _commander_state_pub.publish(_commander_state); // failure_detector_status publish - failure_detector_status_s fd_status{}; - fd_status.fd_roll = _failure_detector.getStatusFlags().roll; - fd_status.fd_pitch = _failure_detector.getStatusFlags().pitch; - fd_status.fd_alt = _failure_detector.getStatusFlags().alt; - fd_status.fd_ext = _failure_detector.getStatusFlags().ext; - fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs; - fd_status.fd_battery = _failure_detector.getStatusFlags().battery; - fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop; - fd_status.fd_motor = _failure_detector.getStatusFlags().motor; - fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric(); - fd_status.motor_failure_mask = _failure_detector.getMotorFailures(); + failure_detector_status_s fd_status = _failure_detector.getFailureDetectorStatus(); fd_status.timestamp = hrt_absolute_time(); _failure_detector_status_pub.publish(fd_status); } @@ -3621,6 +3621,7 @@ void Commander::data_link_check() 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, + _failure_detector.getFailureDetectorStatus(), true, // report_failures _safety.isButtonAvailable(), _safety.isSafetyOff()); } diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 5be44f9aff..581e893e48 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -164,6 +164,22 @@ FailureDetector::FailureDetector(ModuleParams *parent) : { } +const failure_detector_status_s FailureDetector::getFailureDetectorStatus() const +{ + failure_detector_status_s failure_detector_status{}; + failure_detector_status.fd_roll = getStatusFlags().roll; + failure_detector_status.fd_pitch = getStatusFlags().pitch; + failure_detector_status.fd_alt = getStatusFlags().alt; + failure_detector_status.fd_ext = getStatusFlags().ext; + failure_detector_status.fd_arm_escs = getStatusFlags().arm_escs; + failure_detector_status.fd_battery = getStatusFlags().battery; + failure_detector_status.fd_imbalanced_prop = getStatusFlags().imbalanced_prop; + failure_detector_status.fd_motor = getStatusFlags().motor; + failure_detector_status.imbalanced_prop_metric = getImbalancedPropMetric(); + failure_detector_status.motor_failure_mask = getMotorFailures(); + return failure_detector_status; +} + bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode) { _failure_injector.update(); diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index ba66c44e3a..bac48fc044 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -103,6 +104,7 @@ public: bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode); const failure_detector_status_u &getStatus() const { return _status; } const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; } + const failure_detector_status_s getFailureDetectorStatus() const; float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); } uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; } diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 1b016dd331..e59e603b64 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -70,11 +70,11 @@ #include #include #include +#include #include #include #include #include -#include class ControlAllocator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index ec3bf49923..4c2d2db9e7 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -126,6 +127,7 @@ private: updated |= write_attitude_sp(&msg); updated |= write_battery_status(&msg); updated |= write_estimator_status(&msg); + updated |= write_failure_detector_status(&msg); updated |= write_fw_ctrl_status(&msg); updated |= write_geofence_result(&msg); updated |= write_global_position(&msg); @@ -437,10 +439,6 @@ private: msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER; } - if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) { - msg->failure_flags |= HL_FAILURE_FLAG_ENGINE; - } - if (status.mission_failure) { msg->failure_flags |= HL_FAILURE_FLAG_MISSION; } @@ -474,6 +472,21 @@ private: return false; } + bool write_failure_detector_status(mavlink_high_latency2_t *msg) + { + failure_detector_status_s failure_detector_status; + + if (_failure_detector_status_sub.update(&failure_detector_status)) { + if (failure_detector_status.fd_motor) { + msg->failure_flags |= HL_FAILURE_FLAG_ENGINE; + } + + return true; + } + + return false; + } + bool write_wind(mavlink_high_latency2_t *msg) { wind_s wind; @@ -630,6 +643,7 @@ private: uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)}; uORB::Subscription _geofence_sub{ORB_ID(geofence_result)}; uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};