vehicle_status: Remove duplicate failure detector status

msg/vehicle_status.msg

@@ -58,18 +58,6 @@ uint8 NAVIGATION_STATE_ORBIT = 21               # Orbit in a circle
 uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
 uint8 NAVIGATION_STATE_MAX = 23
 
-# Bitmask of detected failures
-uint16 failure_detector_status
-uint16 FAILURE_NONE = 0
-uint16 FAILURE_ROLL = 1              # (1 << 0)
-uint16 FAILURE_PITCH = 2             # (1 << 1)
-uint16 FAILURE_ALT = 4               # (1 << 2)
-uint16 FAILURE_EXT = 8               # (1 << 3)
-uint16 FAILURE_ARM_ESC = 16          # (1 << 4)
-uint16 FAILURE_BATTERY = 32          # (1 << 5)
-uint16 FAILURE_IMBALANCED_PROP = 64  # (1 << 6)
-uint16 FAILURE_MOTOR = 128           # (1 << 7)
-
 uint8 hil_state
 uint8 HIL_STATE_OFF = 0
 uint8 HIL_STATE_ON = 1

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp

@@ -39,7 +39,8 @@ constexpr bool
 ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX];
 
 transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
-		const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
+		const vehicle_control_mode_s &control_mode, const failure_detector_status_s failure_detector_status,
+		const bool safety_button_available, const bool safety_off,
 		const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
 		orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
 		arm_disarm_reason_t calling_reason)
@@ -68,6 +69,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		    && (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) {
 
 			if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
+							    failure_detector_status,
 							    true, // report_failures
 							    safety_button_available, safety_off,
 							    true)) { // is_arm_attempt

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp

@@ -55,6 +55,7 @@ public:
 
 	transition_result_t
 	arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode,
+				const failure_detector_status_s failure_detector_status,
 				const bool safety_button_available, const bool safety_off, const arming_state_t new_arming_state,
 				actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
 				vehicle_status_flags_s &status_flags,

src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp

@@ -257,11 +257,13 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		status_flags.circuit_breaker_engaged_power_check = true;
 
 		vehicle_control_mode_s control_mode{};
+		failure_detector_status_s failure_detector_status{};
 
 		// Attempt transition
 		transition_result_t result = arm_state_machine.arming_state_transition(
 						     status,
 						     control_mode,
+						     failure_detector_status,
 						     test->safety_button_available,
 						     test->safety_off,
 						     test->requested_state,

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -52,6 +52,7 @@ static constexpr unsigned max_mandatory_baro_count = 1;
 
 bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
 				    vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
+				    const failure_detector_status_s failure_detector_status,
 				    bool report_failures,
 				    const bool safety_button_available, const bool safety_off,
 				    const bool is_arm_attempt)
@@ -206,11 +207,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		}
 	}
 
-	/* ---- Failure Detector ---- */
+	failed = failed || !failureDetectorCheck(mavlink_log_pub, failure_detector_status, report_failures);
-	if (!failureDetectorCheck(mavlink_log_pub, status, report_failures)) {
-		failed = true;
-	}
-
 	failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
 	failed = failed || !modeCheck(mavlink_log_pub, report_failures, status);
 	failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp

@@ -41,6 +41,7 @@
 #pragma once
 
 #include <uORB/uORB.h>
+#include <uORB/topics/failure_detector_status.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status_flags.h>
 #include <uORB/topics/vehicle_status.h>
@@ -64,6 +65,7 @@ public:
 	**/
 	static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
 				   vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
+				   const failure_detector_status_s failure_detector_status,
 				   bool reportFailures,
 				   const bool safety_button_available, const bool safety_off,
 				   const bool is_arm_attempt = false);
@@ -95,7 +97,8 @@ private:
 	static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
 	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail);
 	static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
-	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
+	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const failure_detector_status_s failure_detector_status,
+					 const bool report_fail);
 	static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
 	static bool modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_s &status);
 	static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);

src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp

@@ -35,34 +35,35 @@
 
 #include <systemlib/mavlink_log.h>
 
-bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
+bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub,
+		const failure_detector_status_s failure_detector_status,
 		const bool report_fail)
 {
-	if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
+	if (report_fail) {
-		if (report_fail) {
+		if (failure_detector_status.fd_roll) {
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected");
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected");
-			}
 		}
 
-		return false;
+		if (failure_detector_status.fd_pitch) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected");
+		}
+
+		if (failure_detector_status.fd_alt) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected");
+		}
+
+		if (failure_detector_status.fd_ext) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected");
+		}
+
+		if (failure_detector_status.fd_arm_escs) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected");
+		}
 	}
 
-	return true;
+	return !failure_detector_status.fd_roll
+	       && !failure_detector_status.fd_pitch
+	       && !failure_detector_status.fd_alt
+	       && !failure_detector_status.fd_ext
+	       && !failure_detector_status.fd_arm_escs;
 }

src/modules/commander/Commander.cpp

@@ -294,8 +294,12 @@ int Commander::custom_command(int argc, char *argv[])
 		vehicle_control_mode_s vehicle_control_mode{};
 		vehicle_control_mode_sub.copy(&vehicle_control_mode);
 
+		uORB::Subscription failure_detector_status_sub{ORB_ID(failure_detector_status)};
+		failure_detector_status_s failure_detector_status{};
+		failure_detector_status_sub.copy(&failure_detector_status);
+
 		bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
-					   vehicle_control_mode,
+					   vehicle_control_mode, failure_detector_status,
 					   true, // report_failures
 					   false, // safety_buttton_available not known
 					   false); // safety_off not known
@@ -490,6 +494,7 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
 bool Commander::shutdown_if_allowed()
 {
 	return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+			_failure_detector.getFailureDetectorStatus(),
 			_safety.isButtonAvailable(), _safety.isSafetyOff(),
 			vehicle_status_s::ARMING_STATE_SHUTDOWN,
 			_actuator_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
@@ -743,6 +748,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 	}
 
 	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+					 _failure_detector.getFailureDetectorStatus(),
 					 _safety.isButtonAvailable(), _safety.isSafetyOff(),
 					 vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, run_preflight_checks,
 					 &_mavlink_log_pub, _vehicle_status_flags,
@@ -787,6 +793,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 	}
 
 	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+					 _failure_detector.getFailureDetectorStatus(),
 					 _safety.isButtonAvailable(), _safety.isSafetyOff(),
 					 vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, false,
 					 &_mavlink_log_pub, _vehicle_status_flags,
@@ -849,6 +856,7 @@ Commander::Commander() :
 
 	// run preflight immediately to find all relevant parameters, but don't report
 	PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
+				       _failure_detector.getFailureDetectorStatus(),
 				       false, // report_failures
 				       false, // safety_buttton_available not known
 				       false); // safety_off not known
@@ -1401,6 +1409,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				/* try to go to INIT/PREFLIGHT arming state */
 				if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+						_failure_detector.getFailureDetectorStatus(),
 						_safety.isButtonAvailable(), _safety.isSafetyOff(),
 						vehicle_status_s::ARMING_STATE_INIT, _actuator_armed,
 						false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
@@ -2450,6 +2459,7 @@ Commander::run()
 		if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
 
 			_arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
+					_failure_detector.getFailureDetectorStatus(),
 					_safety.isButtonAvailable(), _safety.isSafetyOff(),
 					vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed,
 					true /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
@@ -2705,12 +2715,11 @@ Commander::run()
 		}
 
 		/* Check for failure detector status */
+		const bool previous_motor_failure_flag = _failure_detector.getStatus().flags.motor;
+
 		if (_failure_detector.update(_vehicle_status, _vehicle_control_mode)) {
-			const bool motor_failure_changed = ((_vehicle_status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) > 0) !=
-							   _failure_detector.getStatus().flags.motor;
-			_vehicle_status.failure_detector_status = _failure_detector.getStatus().value;
-			auto fd_status_flags = _failure_detector.getStatusFlags();
 			_status_changed = true;
+			decltype(failure_detector_status_u::flags) fd_status_flags = _failure_detector.getStatusFlags();
 
 			if (_arm_state_machine.isArmed()) {
 				if (fd_status_flags.arm_escs) {
@@ -2773,7 +2782,7 @@ Commander::run()
 			}
 
 			// One-time actions based on motor failure
-			if (motor_failure_changed) {
+			if (fd_status_flags.motor != previous_motor_failure_flag) {
 				if (fd_status_flags.motor) {
 					mavlink_log_critical(&_mavlink_log_pub, "Motor failure detected\t");
 					events::send(events::ID("commander_motor_failure"), events::Log::Emergency,
@@ -2992,6 +3001,7 @@ Commander::run()
 				_vehicle_status_flags.pre_flight_checks_pass = PreFlightCheck::preflightCheck(nullptr, _vehicle_status,
 						_vehicle_status_flags,
 						_vehicle_control_mode,
+						_failure_detector.getFailureDetectorStatus(),
 						false, // report_failures
 						_safety.isButtonAvailable(), _safety.isSafetyOff());
 				perf_end(_preflight_check_perf);
@@ -3023,17 +3033,7 @@ Commander::run()
 			_commander_state_pub.publish(_commander_state);
 
 			// failure_detector_status publish
-			failure_detector_status_s fd_status{};
+			failure_detector_status_s fd_status = _failure_detector.getFailureDetectorStatus();
-			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_battery = _failure_detector.getStatusFlags().battery;
-			fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
-			fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
-			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
-			fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
 			fd_status.timestamp = hrt_absolute_time();
 			_failure_detector_status_pub.publish(fd_status);
 		}
@@ -3621,6 +3621,7 @@ void Commander::data_link_check()
 					if (!_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) {
 						// make sure to report preflight check failures to a connecting GCS
 						PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
+									       _failure_detector.getFailureDetectorStatus(),
 									       true, // report_failures
 									       _safety.isButtonAvailable(), _safety.isSafetyOff());
 					}

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -164,6 +164,22 @@ FailureDetector::FailureDetector(ModuleParams *parent) :
 {
 }
 
+const failure_detector_status_s FailureDetector::getFailureDetectorStatus() const
+{
+	failure_detector_status_s failure_detector_status{};
+	failure_detector_status.fd_roll = getStatusFlags().roll;
+	failure_detector_status.fd_pitch = getStatusFlags().pitch;
+	failure_detector_status.fd_alt = getStatusFlags().alt;
+	failure_detector_status.fd_ext = getStatusFlags().ext;
+	failure_detector_status.fd_arm_escs = getStatusFlags().arm_escs;
+	failure_detector_status.fd_battery = getStatusFlags().battery;
+	failure_detector_status.fd_imbalanced_prop = getStatusFlags().imbalanced_prop;
+	failure_detector_status.fd_motor = getStatusFlags().motor;
+	failure_detector_status.imbalanced_prop_metric = getImbalancedPropMetric();
+	failure_detector_status.motor_failure_mask = getMotorFailures();
+	return failure_detector_status;
+}
+
 bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode)
 {
 	_failure_injector.update();

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -53,6 +53,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/failure_detector_status.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -103,6 +104,7 @@ public:
 	bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
 	const failure_detector_status_u &getStatus() const { return _status; }
 	const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; }
+	const failure_detector_status_s getFailureDetectorStatus() const;
 	float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
 	uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; }
 

src/modules/control_allocator/ControlAllocator.hpp

@@ -70,11 +70,11 @@
 #include <uORB/topics/actuator_servos.h>
 #include <uORB/topics/actuator_servos_trim.h>
 #include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/failure_detector_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/failure_detector_status.h>
 
 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
 {

src/modules/mavlink/streams/HIGH_LATENCY2.hpp

@@ -44,6 +44,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/failure_detector_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/position_controller_status.h>
@@ -126,6 +127,7 @@ private:
 			updated |= write_attitude_sp(&msg);
 			updated |= write_battery_status(&msg);
 			updated |= write_estimator_status(&msg);
+			updated |= write_failure_detector_status(&msg);
 			updated |= write_fw_ctrl_status(&msg);
 			updated |= write_geofence_result(&msg);
 			updated |= write_global_position(&msg);
@@ -437,10 +439,6 @@ private:
 				msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
 			}
 
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) {
-				msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
-			}
-
 			if (status.mission_failure) {
 				msg->failure_flags |= HL_FAILURE_FLAG_MISSION;
 			}
@@ -474,6 +472,21 @@ private:
 		return false;
 	}
 
+	bool write_failure_detector_status(mavlink_high_latency2_t *msg)
+	{
+		failure_detector_status_s failure_detector_status;
+
+		if (_failure_detector_status_sub.update(&failure_detector_status)) {
+			if (failure_detector_status.fd_motor) {
+				msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
+			}
+
+			return true;
+		}
+
+		return false;
+	}
+
 	bool write_wind(mavlink_high_latency2_t *msg)
 	{
 		wind_s wind;
@@ -630,6 +643,7 @@ private:
 	uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
+	uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
 	uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)};
 	uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};
 	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};