From 000c1e364cd121e57db993e1d4272abcc4f01d97 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 7 Aug 2019 11:35:05 +0200 Subject: [PATCH] FailureDetector - Ignore attitude check for MC in acro and rattude, and FW in manual, acro and rattitude modes --- .../failure_detector/FailureDetector.cpp | 41 ++++++++++++++++++- .../failure_detector/FailureDetector.hpp | 4 ++ 2 files changed, 43 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 49e3482b7d..07736770bd 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -43,20 +43,57 @@ FailureDetector::FailureDetector(ModuleParams *parent) : ModuleParams(parent), _sub_vehicle_attitude_setpoint(ORB_ID(vehicle_attitude_setpoint)), - _sub_vehicule_attitude(ORB_ID(vehicle_attitude)) + _sub_vehicule_attitude(ORB_ID(vehicle_attitude)), + _sub_vehicle_status(ORB_ID(vehicle_status)) { } +bool FailureDetector::resetStatus() +{ + bool status_changed = _status != FAILURE_NONE; + _status = FAILURE_NONE; + + return status_changed; +} + bool FailureDetector::update() { bool updated(false); - updated = updateAttitudeStatus(); + if (isAttitudeStabilized()) { + updated = updateAttitudeStatus(); + + } else { + updated = resetStatus(); + } return updated; } +bool +FailureDetector::isAttitudeStabilized() +{ + if (_sub_vehicle_status.update()) { + const vehicle_status_s &vehicle_status = _sub_vehicle_status.get(); + const uint8_t vehicle_type = vehicle_status.vehicle_type; + const uint8_t nav_state = vehicle_status.nav_state; + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _attitude_is_stabilized = nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO && + nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE; + + } else if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _attitude_is_stabilized = nav_state != vehicle_status_s::NAVIGATION_STATE_MANUAL && + nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO && + nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE; + } + } + + // Note that if we do not have an update from the vehicle_status topic, the previous value is sent + return _attitude_is_stabilized; +} + bool FailureDetector::updateAttitudeStatus() { diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index f9cdd4ce12..5492d7cb97 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -85,11 +85,15 @@ private: // Subscriptions SubscriptionData _sub_vehicle_attitude_setpoint; SubscriptionData _sub_vehicule_attitude; + SubscriptionData _sub_vehicle_status; uint8_t _status{FAILURE_NONE}; + bool _attitude_is_stabilized{false}; systemlib::Hysteresis _roll_failure_hysteresis{false}; systemlib::Hysteresis _pitch_failure_hysteresis{false}; + bool resetStatus(); + bool isAttitudeStabilized(); bool updateAttitudeStatus(); };