FD: add imbalanced propeller check

msg/vehicle_status.msg

@@ -17,6 +17,9 @@ 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)
 
 # HIL
 uint8 HIL_STATE_OFF = 0

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -66,6 +66,10 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		updateEscsStatus(vehicle_status);
 	}
 
+	if (_param_fd_imb_prop_thr.get() > 0) {
+		updateImbalancedPropStatus();
+	}
+
 	return _status != previous_status;
 }
 
@@ -155,3 +159,58 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
 		_status &= ~FAILURE_ARM_ESCS;
 	}
 }
+
+void FailureDetector::updateImbalancedPropStatus()
+{
+
+	if (_sensor_selection_sub.updated()) {
+		sensor_selection_s selection;
+
+		if (_sensor_selection_sub.copy(&selection)) {
+			_selected_accel_device_id = selection.accel_device_id;
+		}
+	}
+
+	const bool updated = _vehicle_imu_status_sub.updated(); // save before doing a copy
+
+	// Find the imu_status instance corresponding to the selected accelerometer
+	vehicle_imu_status_s imu_status{};
+	_vehicle_imu_status_sub.copy(&imu_status);
+
+	if (imu_status.accel_device_id != _selected_accel_device_id) {
+
+		for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			if (!_vehicle_imu_status_sub.ChangeInstance(i)) {
+				continue;
+			}
+
+			if (_vehicle_imu_status_sub.copy(&imu_status)
+			    && (imu_status.accel_device_id == _selected_accel_device_id)) {
+				// instance found
+				break;
+			}
+		}
+	}
+
+	if (updated) {
+
+		if (_vehicle_imu_status_sub.copy(&imu_status)) {
+
+			if ((imu_status.accel_device_id != 0)
+			    && (imu_status.accel_device_id == _selected_accel_device_id)) {
+				const float dt = math::constrain((float)(imu_status.timestamp - _imu_status_timestamp_prev), 0.01f, 1.f);
+				_imbalanced_prop_lpf.setParameters(dt, _imbalanced_prop_lpf_time_constant);
+
+				const float var_x = imu_status.var_accel[0];
+				const float var_y = imu_status.var_accel[1];
+				const float var_z = imu_status.var_accel[2];
+
+				const float metric = var_x + var_y - var_z;
+				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);
+			}
+		}
+	}
+}

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -43,6 +43,7 @@
 
 #pragma once
 
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <matrix/matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <px4_platform_common/module_params.h>
@@ -50,9 +51,11 @@
 
 // subscriptions
 #include <uORB/Subscription.hpp>
+#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/esc_status.h>
 #include <uORB/topics/pwm_input.h>
@@ -63,7 +66,10 @@ typedef enum {
 	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_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;
 
 using uORB::SubscriptionData;
@@ -75,11 +81,13 @@ public:
 
 	bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
 	uint8_t getStatus() const { return _status; }
+	float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
 
 private:
 	void updateAttitudeStatus();
 	void updateExternalAtsStatus();
 	void updateEscsStatus(const vehicle_status_s &vehicle_status);
+	void updateImbalancedPropStatus();
 
 	uint8_t _status{FAILURE_NONE};
 
@@ -88,9 +96,16 @@ private:
 	systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
 	systemlib::Hysteresis _esc_failure_hysteresis{false};
 
+	static constexpr float _imbalanced_prop_lpf_time_constant{5.f};
+	AlphaFilter<float> _imbalanced_prop_lpf{};
+	uint32_t _selected_accel_device_id{0};
+	hrt_abstime _imu_status_timestamp_prev{0};
+
 	uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
+	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
+	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@@ -99,6 +114,7 @@ private:
 		(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
 		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
-		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en
+		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
+		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
 	)
 };

src/modules/commander/failure_detector/failure_detector_params.c

@@ -141,3 +141,18 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
  * @group Failure Detector
  */
 PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
+
+/**
+ * Imbalanced propeller check threshold
+ *
+ * Value at which the imbalanced propeller metric (based on horizontal and
+ * vertical acceleration variance) triggers a failure
+ *
+ * Setting this value to 0 disables the feature.
+ *
+ * @min 0
+ * @max 1000
+ * @increment 1
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);