FD: use flags union instead of bitmask

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)

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

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

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -60,17 +60,19 @@
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/pwm_input.h>
 
-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};