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

@@ -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

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;
 					}
+
 				}
 			}
 		}

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;
 

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -51,12 +51,14 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
 
-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<vehicle_attitude_s> _sub_vehicle_attitude_setpoint;
 	Subscription<vehicle_attitude_s> _sub_vehicule_attitude;
 
-	struct failure_detector_status_s _status;
+	uint8_t _status{FAILURE_NONE};
 
 	bool update_attitude_status();
 };