diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 952ed5d275..e4f484b9c7 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -47,15 +47,6 @@ FailureDetector::FailureDetector(ModuleParams *parent) : { } -void FailureDetector::resetAttitudeStatus() -{ - int attitude_fields_bitmask = _status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT); - - if (attitude_fields_bitmask > FAILURE_NONE) { - _status &= ~attitude_fields_bitmask; - } -} - bool FailureDetector::update(const vehicle_status_s &vehicle_status) { uint8_t previous_status = _status; @@ -68,7 +59,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status) } } else { - resetAttitudeStatus(); + _status &= ~(FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT); } if (_esc_status_sub.updated()) { diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 207f1e5686..959b6c145d 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -77,7 +77,6 @@ public: uint8_t getStatus() const { return _status; } private: - void resetAttitudeStatus(); bool isAttitudeStabilized(const vehicle_status_s &vehicle_status); void updateAttitudeStatus(); void updateExternalAtsStatus();