FD: add dedicated topic to log more internal states

log imbalanced propeller check metric

add failure_detector_status message

msg/CMakeLists.txt

@@ -75,6 +75,7 @@ set(msg_files
 	estimator_status_flags.msg
 	event.msg
 	follow_target.msg
+	failure_detector_status.msg
 	generator_status.msg
 	geofence_result.msg
 	gimbal_device_attitude_status.msg

msg/failure_detector_status.msg

@@ -0,0 +1,16 @@
+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
+
+float32 imbalanced_prop_metric      # Metric of the imbalanced propeller check (low-passed)

src/modules/commander/Commander.cpp

@@ -2776,7 +2776,7 @@ Commander::run()
 			_failsafe_old = _status.failsafe;
 		}
 
-		/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 2 Hz or immediately when changed */
+		/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed */
 		if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed) {
 
 			update_control_mode();
@@ -2839,6 +2839,13 @@ Commander::run()
 			/* publish vehicle_status_flags */
 			_status_flags.timestamp = hrt_absolute_time();
 			_vehicle_status_flags_pub.publish(_status_flags);
+
+			/* 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.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
+			_failure_detector_status_pub.publish(fd_status);
 		}
 
 		/* play arming and battery warning tunes */

src/modules/commander/Commander.hpp

@@ -49,6 +49,7 @@
 // publications
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/failure_detector_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/test_motor.h>
@@ -433,6 +434,7 @@ private:
 	// Publications
 	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};
 	uORB::Publication<commander_state_s>			_commander_state_pub{ORB_ID(commander_state)};
+	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
 	uORB::Publication<test_motor_s>				_test_motor_pub{ORB_ID(test_motor)};
 	uORB::Publication<vehicle_control_mode_s>		_control_mode_pub{ORB_ID(vehicle_control_mode)};
 	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};

src/modules/logger/logged_topics.cpp

@@ -60,6 +60,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("commander_state");
 	add_topic("cpuload");
 	add_topic("esc_status", 250);
+	add_topic("failure_detector_status", 100);
 	add_topic("follow_target", 500);
 	add_topic("generator_status");
 	add_topic("heater_status");