From 1e94512719d46bbd5fd79a9652ee25ce9b7e7321 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 3 Nov 2021 11:13:37 +0100 Subject: [PATCH] FD: use flags union instead of bitmask --- msg/failure_detector_status.msg | 19 +++++------ src/modules/commander/Commander.cpp | 19 +++++++---- .../failure_detector/FailureDetector.cpp | 34 +++++++------------ .../failure_detector/FailureDetector.hpp | 29 +++++++++------- 4 files changed, 50 insertions(+), 51 deletions(-) diff --git a/msg/failure_detector_status.msg b/msg/failure_detector_status.msg index a7f23b89ff..5f7dbc1b31 100644 --- a/msg/failure_detector_status.msg +++ b/msg/failure_detector_status.msg @@ -1,16 +1,13 @@ uint64 timestamp # time since system start (microseconds) # 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_ARM_ESC = 16 # (1 << 4) -uint8 FAILURE_HIGH_WIND = 32 # (1 << 5) -uint8 FAILURE_BATTERY = 64 # (1 << 6) -uint8 FAILURE_IMBALANCED_PROP = 128 # (1 << 7) - -uint8 failure_status # Bitmask containing FailureDetector status +bool fd_roll +bool fd_pitch +bool fd_alt +bool fd_ext +bool fd_arm_escs +bool fd_high_wind +bool fd_battery +bool fd_imbalanced_prop float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index cdb158854f..548bdecd5f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2607,11 +2607,12 @@ Commander::run() /* Check for failure detector status */ if (_failure_detector.update(_status, _vehicle_control_mode)) { - _status.failure_detector_status = _failure_detector.getStatus(); + _status.failure_detector_status = _failure_detector.getStatus().value; + auto fd_status_flags = _failure_detector.getStatusFlags(); _status_changed = true; if (_armed.armed) { - if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { + if (fd_status_flags.arm_escs) { // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { disarm(arm_disarm_reason_t::failure_detector); @@ -2620,8 +2621,7 @@ Commander::run() } } - 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)) { + if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) { const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get()); if (is_right_after_takeoff && !_lockdown_triggered) { @@ -2661,7 +2661,7 @@ Commander::run() } } - if ((_status.failure_detector_status & vehicle_status_s::FAILURE_IMBALANCED_PROP) + if (fd_status_flags.imbalanced_prop && !_imbalanced_propeller_check_triggered) { _status_changed = true; _imbalanced_propeller_check_triggered = true; @@ -2843,7 +2843,14 @@ Commander::run() /* publish failure_detector data */ failure_detector_status_s fd_status{}; fd_status.timestamp = hrt_absolute_time(); - fd_status.failure_status = _failure_detector.getStatus(); + 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_high_wind = _failure_detector.getStatusFlags().high_wind; + fd_status.fd_battery = _failure_detector.getStatusFlags().battery; + fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop; fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric(); _failure_detector_status_pub.publish(fd_status); } diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 39c6de0496..ed02808e6e 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -49,7 +49,7 @@ FailureDetector::FailureDetector(ModuleParams *parent) : bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode) { - uint8_t previous_status = _status; + failure_detector_status_u status_prev = _status; if (vehicle_control_mode.flag_control_attitude_enabled) { updateAttitudeStatus(); @@ -59,7 +59,10 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic } } else { - _status &= ~(FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT); + _status.flags.roll = false; + _status.flags.pitch = false; + _status.flags.alt = false; + _status.flags.ext = false; } if (_param_escs_en.get()) { @@ -70,7 +73,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic updateImbalancedPropStatus(); } - return _status != previous_status; + return _status.value != status_prev.value; } void FailureDetector::updateAttitudeStatus() @@ -99,16 +102,9 @@ void FailureDetector::updateAttitudeStatus() _roll_failure_hysteresis.set_state_and_update(roll_status, time_now); _pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now); - // Update bitmask - _status &= ~(FAILURE_ROLL | FAILURE_PITCH); - - if (_roll_failure_hysteresis.get_state()) { - _status |= FAILURE_ROLL; - } - - if (_pitch_failure_hysteresis.get_state()) { - _status |= FAILURE_PITCH; - } + // Update status + _status.flags.roll = _roll_failure_hysteresis.get_state(); + _status.flags.pitch = _pitch_failure_hysteresis.get_state(); } } @@ -127,11 +123,7 @@ void FailureDetector::updateExternalAtsStatus() _ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz _ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now); - _status &= ~FAILURE_EXT; - - if (_ext_ats_failure_hysteresis.get_state()) { - _status |= FAILURE_EXT; - } + _status.flags.ext = _ext_ats_failure_hysteresis.get_state(); } } @@ -149,14 +141,14 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status) _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.flags.arm_escs = true; } } } else { // reset ESC bitfield _esc_failure_hysteresis.set_state_and_update(false, time_now); - _status &= ~FAILURE_ARM_ESCS; + _status.flags.arm_escs = false; } } @@ -209,7 +201,7 @@ void FailureDetector::updateImbalancedPropStatus() const float metric_lpf = _imbalanced_prop_lpf.update(metric); const bool is_imbalanced = metric_lpf > _param_fd_imb_prop_thr.get(); - _status = (_status & ~FAILURE_IMBALANCED_PROP) | (is_imbalanced * FAILURE_IMBALANCED_PROP); + _status.flags.imbalanced_prop = is_imbalanced; } } } diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index eeed0726a1..812ed726e2 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -60,17 +60,19 @@ #include #include -typedef enum { - FAILURE_NONE = vehicle_status_s::FAILURE_NONE, - FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL, - 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_HIGH_WIND = vehicle_status_s::FAILURE_HIGH_WIND, - FAILURE_BATTERY = vehicle_status_s::FAILURE_BATTERY, - FAILURE_IMBALANCED_PROP = vehicle_status_s::FAILURE_IMBALANCED_PROP -} failure_detector_bitmak; +union failure_detector_status_u { + struct { + uint16_t roll : 1; + uint16_t pitch : 1; + uint16_t alt : 1; + uint16_t ext : 1; + uint16_t arm_escs : 1; + uint16_t high_wind : 1; + uint16_t battery : 1; + uint16_t imbalanced_prop : 1; + } flags; + uint16_t value; +}; using uORB::SubscriptionData; @@ -80,7 +82,8 @@ public: FailureDetector(ModuleParams *parent); bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode); - uint8_t getStatus() const { return _status; } + const failure_detector_status_u &getStatus() const { return _status; } + const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; } float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); } private: @@ -89,7 +92,7 @@ private: void updateEscsStatus(const vehicle_status_s &vehicle_status); void updateImbalancedPropStatus(); - uint8_t _status{FAILURE_NONE}; + failure_detector_status_u _status{}; systemlib::Hysteresis _roll_failure_hysteresis{false}; systemlib::Hysteresis _pitch_failure_hysteresis{false};