diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 1b6e3818da..824f186025 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -12,10 +12,11 @@ uint8 ARMING_STATE_MAX = 6 # FailureDetector status uint8 FAILURE_NONE = 0 -uint8 FAILURE_ROLL = 1 # (1 << 0) -uint8 FAILURE_PITCH = 2 # (1 << 1) -uint8 FAILURE_ALT = 4 # (1 << 2) -uint8 FAILURE_EXT = 8 # (1 << 3) +uint8 FAILURE_ROLL = 1 # (1 << 0) +uint8 FAILURE_PITCH = 2 # (1 << 1) +uint8 FAILURE_ALT = 4 # (1 << 2) +uint8 FAILURE_EXT = 8 # (1 << 3) +uint8 FAILURE_ARM_ESC = 16 # (1 << 4) # HIL uint8 HIL_STATE_OFF = 0 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b815ac71e8..0bced55a30 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2145,15 +2145,26 @@ Commander::run() failure_detector_updated) { if (_failure_detector.isFailure()) { - if ((hrt_elapsed_time(&_time_at_takeoff) < 3_s) && - !_lockdown_triggered) { - // This handles the case where something fails during the early takeoff phase - armed.lockdown = true; - _lockdown_triggered = true; + if (!_have_taken_off_since_arming) { + arm_disarm(false, true, &mavlink_log_pub, "Failure detector"); _status_changed = true; - mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown"); + if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { + mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request"); + + } + + } else if (hrt_elapsed_time(&_time_at_takeoff) < 3_s) { + // This handles the case where something fails during the early takeoff phase + if (!_lockdown_triggered) { + + armed.lockdown = true; + _lockdown_triggered = true; + _status_changed = true; + + mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown"); + } } else if (!status_flags.circuit_breaker_flight_termination_disabled && !_flight_termination_triggered) { diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 16aae181c8..18f5483e33 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -47,10 +47,16 @@ FailureDetector::FailureDetector(ModuleParams *parent) : { } -bool FailureDetector::resetStatus() +bool FailureDetector::resetAttitudeStatus() { - bool status_changed = _status != FAILURE_NONE; - _status = FAILURE_NONE; + + int attitude_fields_bitmask = _status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT); + bool status_changed(false); + + if (attitude_fields_bitmask > FAILURE_NONE) { + _status &= ~attitude_fields_bitmask; + status_changed = true; + } return status_changed; } @@ -58,17 +64,26 @@ bool FailureDetector::resetStatus() bool FailureDetector::update(const vehicle_status_s &vehicle_status) { + bool updated(false); if (isAttitudeStabilized(vehicle_status)) { - updated = updateAttitudeStatus(); + updated |= updateAttitudeStatus(); if (_param_fd_ext_ats_en.get()) { updated |= updateExternalAtsStatus(); } } else { - updated = resetStatus(); + updated |= resetAttitudeStatus(); + } + + if (_sub_esc_status.updated()) { + + if (_param_escs_en.get()) { + updated |= updateEscsStatus(vehicle_status); + } + } return updated; @@ -143,9 +158,9 @@ bool FailureDetector::updateExternalAtsStatus() { pwm_input_s pwm_input; - bool updated = _sub_pwm_input.update(&pwm_input); + bool updated(false); - if (updated) { + if (_sub_pwm_input.update(&pwm_input)) { uint32_t pulse_width = pwm_input.pulse_width; bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms); @@ -161,7 +176,44 @@ FailureDetector::updateExternalAtsStatus() if (_ext_ats_failure_hysteresis.get_state()) { _status |= FAILURE_EXT; } + + updated = true; } return updated; -} \ No newline at end of file +} + +bool +FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status) +{ + hrt_abstime time_now = hrt_absolute_time(); + bool updated(false); + + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + esc_status_s esc_status{}; + _sub_esc_status.copy(&esc_status); + + int all_escs_armed = (1 << esc_status.esc_count) - 1; + + + _esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms); + _esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now); + + if (_esc_failure_hysteresis.get_state() && !(_status & FAILURE_ARM_ESCS)) { + _status |= FAILURE_ARM_ESCS; + updated = true; + } + + } else { + // reset ESC bitfield + _esc_failure_hysteresis.set_state_and_update(false, time_now); + + if (_status & FAILURE_ARM_ESCS) { + _status &= ~FAILURE_ARM_ESCS; + updated = true; + } + } + + return updated; +} diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 5463b5632a..e4911c8006 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include typedef enum { @@ -62,6 +63,7 @@ typedef enum { FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH, FAILURE_ALT = vehicle_status_s::FAILURE_ALT, FAILURE_EXT = vehicle_status_s::FAILURE_EXT, + FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC } failure_detector_bitmak; using uORB::SubscriptionData; @@ -84,11 +86,14 @@ private: (ParamFloat) _param_fd_fail_r_ttri, (ParamFloat) _param_fd_fail_p_ttri, (ParamBool) _param_fd_ext_ats_en, - (ParamInt) _param_fd_ext_ats_trig + (ParamInt) _param_fd_ext_ats_trig, + (ParamInt) _param_escs_en + ) // Subscriptions uORB::Subscription _sub_vehicule_attitude{ORB_ID(vehicle_attitude)}; + uORB::Subscription _sub_esc_status{ORB_ID(esc_status)}; uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)}; @@ -97,9 +102,11 @@ private: systemlib::Hysteresis _roll_failure_hysteresis{false}; systemlib::Hysteresis _pitch_failure_hysteresis{false}; systemlib::Hysteresis _ext_ats_failure_hysteresis{false}; + systemlib::Hysteresis _esc_failure_hysteresis{false}; - bool resetStatus(); + bool resetAttitudeStatus(); bool isAttitudeStabilized(const vehicle_status_s &vehicle_status); bool updateAttitudeStatus(); bool updateExternalAtsStatus(); + bool updateEscsStatus(const vehicle_status_s &vehicle_status); }; diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c index cfaadeb8cf..55ea9b3de4 100644 --- a/src/modules/commander/failure_detector/failure_detector_params.c +++ b/src/modules/commander/failure_detector/failure_detector_params.c @@ -129,3 +129,15 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0); * @group Failure Detector */ PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900); + +/** + * Enable checks on ESCs that report their arming state. + * If enabled, failure detector will verify that all the ESCs have successfully armed when the vehicle has transitioned to the armed state. + * Timeout for receiving an acknowledgement from the ESCs is 0.3s, if no feedback is received the failure detector will auto disarm the vehicle. + * + * @boolean + * @reboot_required true + * + * @group Failure Detector + */ +PARAM_DEFINE_INT32(FD_ESCS_EN, 1);