FilaureDetector: publish failure_detector_status in class and call that from commander

src/modules/commander/Commander.cpp

@@ -1956,21 +1956,7 @@ void Commander::run()
 			_vehicle_status.timestamp = hrt_absolute_time();
 			_vehicle_status_pub.publish(_vehicle_status);
 
-			// failure_detector_status publish
-			failure_detector_status_s fd_status{};
-			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.motor_stop_mask = _failure_detector.getMotorStopMask();
-			fd_status.timestamp = hrt_absolute_time();
-			_failure_detector_status_pub.publish(fd_status);
+			_failure_detector.publishStatus();
 		}
 
 		checkWorkerThread();

src/modules/commander/Commander.hpp

@@ -54,7 +54,6 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_test.h>
-#include <uORB/topics/failure_detector_status.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
@@ -313,7 +312,6 @@ private:
 	// Publications
 	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
 	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
-	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
 	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_command_s>			_vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -89,6 +89,24 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 	return _failure_detector_status.value != status_prev.value;
 }
 
+void FailureDetector::publishStatus()
+{
+	failure_detector_status_s failure_detector_status{};
+	failure_detector_status.fd_roll = _failure_detector_status.flags.roll;
+	failure_detector_status.fd_pitch = _failure_detector_status.flags.pitch;
+	failure_detector_status.fd_alt = _failure_detector_status.flags.alt;
+	failure_detector_status.fd_ext = _failure_detector_status.flags.ext;
+	failure_detector_status.fd_arm_escs = _failure_detector_status.flags.arm_escs;
+	failure_detector_status.fd_battery = _failure_detector_status.flags.battery;
+	failure_detector_status.fd_imbalanced_prop = _failure_detector_status.flags.imbalanced_prop;
+	failure_detector_status.fd_motor = _failure_detector_status.flags.motor;
+	failure_detector_status.imbalanced_prop_metric = _imbalanced_prop_lpf.getState();
+	failure_detector_status.motor_failure_mask = _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask;
+	failure_detector_status.motor_stop_mask = _failure_injector.getMotorStopMask();
+	failure_detector_status.timestamp = hrt_absolute_time();
+	_failure_detector_status_pub.publish(failure_detector_status);
+}
+
 void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status)
 {
 	vehicle_attitude_s attitude;

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -51,16 +51,18 @@
 #include <px4_platform_common/module_params.h>
 
 // subscriptions
-#include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/failure_detector_status.h>
+#include <uORB/topics/pwm_input.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu_status.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/pwm_input.h>
 
 union failure_detector_status_u {
 	struct {
@@ -86,10 +88,8 @@ 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 _failure_detector_status; }
-	const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _failure_detector_status.flags; }
-	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; }
-	uint16_t getMotorStopMask() { return _failure_injector.getMotorStopMask(); }
+
+	void publishStatus();
 
 private:
 	void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
@@ -124,6 +124,8 @@ private:
 	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 
+	uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
+
 	FailureInjector _failure_injector;
 
 	DEFINE_PARAMETERS(