diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 6c9a2e180d..c28a329bc3 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2238,40 +2238,38 @@ Commander::run() status.failure_detector_status = _failure_detector.getStatus(); _status_changed = true; - if (armed.armed && _failure_detector.isFailure()) { - const hrt_abstime time_at_arm = armed.armed_time_ms * 1000; + if (armed.armed) { + if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { + const hrt_abstime time_at_arm = armed.armed_time_ms * 1000; - if (hrt_elapsed_time(&time_at_arm) < 500_ms) { // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs - - if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { + if (hrt_elapsed_time(&time_at_arm) < 500_ms) { arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::FAILURE_DETECTOR); _status_changed = true; mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request"); } - } - if (hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get())) { - // This handles the case where something fails during the early takeoff phase - if (!_lockdown_triggered) { + if (status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | + vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) { + const bool is_second_after_takeoff = hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get()); + if (is_second_after_takeoff && !_lockdown_triggered) { + // This handles the case where something fails during the early takeoff phase 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 && !_lockdown_triggered) { + } else if (!status_flags.circuit_breaker_flight_termination_disabled && + !_flight_termination_triggered && !_lockdown_triggered) { - armed.force_failsafe = true; - _flight_termination_triggered = true; - _status_changed = true; - - mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: terminate flight"); - set_tune_override(TONE_PARACHUTE_RELEASE_TUNE); + armed.force_failsafe = true; + _flight_termination_triggered = true; + _status_changed = true; + mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: terminate flight"); + set_tune_override(TONE_PARACHUTE_RELEASE_TUNE); + } } } } diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index fe338c1786..207f1e5686 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -74,9 +74,7 @@ public: FailureDetector(ModuleParams *parent); bool update(const vehicle_status_s &vehicle_status); - uint8_t getStatus() const { return _status; } - bool isFailure() const { return _status != FAILURE_NONE; } private: void resetAttitudeStatus();