From 9be0c97346960873892ff815b5c1e1e410a4a242 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 10 Aug 2018 13:31:57 +0200 Subject: [PATCH] Failure Detector - use bitmask field instead of boolean in vehicle_status msg (failure_detector_status) and instead of struct in class --- msg/vehicle_status.msg | 11 ++++++++-- src/modules/commander/Commander.cpp | 22 +++++++++---------- .../failure_detector/FailureDetector.cpp | 14 ++++++++++-- .../failure_detector/FailureDetector.hpp | 16 ++++++++------ 4 files changed, 41 insertions(+), 22 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index ea25211308..7d55b6e33e 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -10,6 +10,13 @@ uint8 ARMING_STATE_REBOOT = 4 uint8 ARMING_STATE_IN_AIR_RESTORE = 5 uint8 ARMING_STATE_MAX = 6 +# FailureDetector status +uint8 FAILURE_NONE = 0 +uint8 FAILURE_ROLL = 1 # (1 << 0) +uint8 FAILURE_PITCH = 2 # (1 << 1) +uint8 FAILURE_ALT = 4 # (1 << 2) + +# HIL uint8 HIL_STATE_OFF = 0 uint8 HIL_STATE_ON = 1 @@ -47,7 +54,7 @@ uint8 RC_IN_MODE_GENERATED = 2 uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state -bool failsafe # true if system is in failsafe state +bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) uint8 system_type # system type, contains mavlink MAV_TYPE uint8 system_id # system id, contains MAVLink's system ID field @@ -67,7 +74,7 @@ bool high_latency_data_link_active # all low latency datalinks to GCS lost uint8 data_link_lost_counter # counts unique data link lost events bool engine_failure # Set to true if an engine failure is detected bool mission_failure # Set to true if mission could not continue/finish -bool attitude_failure # Set to true if attitude failure has been detected +uint8 failure_detector_status # Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL] # see SYS_STATUS mavlink message for the following uint32 onboard_control_sensors_present diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b517ecb9f5..2835e722c3 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2289,22 +2289,22 @@ Commander::run() if (_failure_detector.update()) { - const failure_detector_status_s failure_status = _failure_detector.get_status(); + const uint8_t failure_status = _failure_detector.get_status(); - if (failure_status.roll || - failure_status.pitch) { - - status.attitude_failure = true; + if (failure_status != status.failure_detector_status) { + status.failure_detector_status = failure_status; status_changed = true; + } - // Only display an user message if the circuit-breaker is disabled - if (!status_flags.circuit_breaker_flight_termination_disabled) { + if (failure_status != 0 && !status_flags.circuit_breaker_flight_termination_disabled) { - if (!_failure_detector_termination_printed) { - mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe"); - _failure_detector_termination_printed = true; - } + // TODO: set force_failsafe flag + + if (!_failure_detector_termination_printed) { + mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe"); + _failure_detector_termination_printed = true; } + } } } diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index e5bf27593a..a52ab1f1ac 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -74,8 +74,18 @@ FailureDetector::update_attitude_status() const float max_roll(fabsf(math::radians(max_roll_deg))); const float max_pitch(fabsf(math::radians(max_pitch_deg))); - _status.roll = (max_roll > 0.0f) && (fabsf(roll) > max_roll); - _status.pitch = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch); + const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll); + const bool pitch_status = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch); + + // Update bitmask + _status &= ~(FAILURE_ROLL | FAILURE_PITCH); + + if (roll_status) { + _status |= FAILURE_ROLL; + } + if (pitch_status) { + _status |= FAILURE_PITCH; + } updated = true; diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 9120e25455..0baf47abf8 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -51,12 +51,14 @@ #include #include #include +#include -struct failure_detector_status_s { - bool roll; - bool pitch; - bool altitude; -}; +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_detector_bitmak; using uORB::Subscription; @@ -67,7 +69,7 @@ public: bool update(); - const failure_detector_status_s& get_status() const {return _status;} + uint8_t get_status() const {return _status;} private: @@ -80,7 +82,7 @@ private: Subscription _sub_vehicle_attitude_setpoint; Subscription _sub_vehicule_attitude; - struct failure_detector_status_s _status; + uint8_t _status{FAILURE_NONE}; bool update_attitude_status(); };