From 981e05c6336e1eeb56c556dea865498528bc9586 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 30 Jul 2020 20:08:01 +0200 Subject: [PATCH] FailureDetector: move esc topic check into esc status update --- .../failure_detector/FailureDetector.cpp | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index e4f484b9c7..9b8fa5a49e 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -62,12 +62,8 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status) _status &= ~(FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT); } - if (_esc_status_sub.updated()) { - - if (_param_escs_en.get()) { - updateEscsStatus(vehicle_status); - } - + if (_param_escs_en.get()) { + updateEscsStatus(vehicle_status); } return _status != previous_status; @@ -159,17 +155,17 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status) hrt_abstime time_now = hrt_absolute_time(); if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + esc_status_s esc_status; - esc_status_s esc_status{}; - _esc_status_sub.copy(&esc_status); + if (_esc_status_sub.update(&esc_status)) { + int all_escs_armed = (1 << esc_status.esc_count) - 1; - 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); - _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; + if (_esc_failure_hysteresis.get_state()) { + _status |= FAILURE_ARM_ESCS; + } } } else {