Commander: trigger failsafe action if imbalanced propeller detected

src/modules/commander/Commander.cpp

@@ -2660,6 +2660,14 @@ Commander::run()
 						send_parachute_command();
 					}
 				}
+
+				if ((_status.failure_detector_status & vehicle_status_s::FAILURE_IMBALANCED_PROP)
+				    && !_imbalanced_propeller_check_triggered) {
+					_status_changed = true;
+					_imbalanced_propeller_check_triggered = true;
+					imbalanced_prop_failsafe(&_mavlink_log_pub, _status, _status_flags, &_internal_state,
+								 (imbalanced_propeller_action_t)_param_com_imb_prop_act.get());
+				}
 			}
 		}
 
@@ -2729,6 +2737,7 @@ Commander::run()
 		if (!_armed.armed) {
 			/* Reset the flag if disarmed. */
 			_have_taken_off_since_arming = false;
+			_imbalanced_propeller_check_triggered = false;
 		}
 
 		/* now set navigation state according to failsafe and main state */

src/modules/commander/Commander.hpp

@@ -211,6 +211,7 @@ private:
 		(ParamInt<px4::params::COM_POS_FS_GAIN>) _param_com_pos_fs_gain,
 
 		(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
+		(ParamInt<px4::params::COM_IMB_PROP_ACT>) _param_com_imb_prop_act,
 		(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
 		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
 
@@ -322,6 +323,7 @@ private:
 	FailureDetector	_failure_detector;
 	bool		_flight_termination_triggered{false};
 	bool		_lockdown_triggered{false};
+	bool            _imbalanced_propeller_check_triggered{false};
 
 
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};

src/modules/commander/commander_params.c

@@ -339,6 +339,23 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
  */
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 
+/**
+ * Imbalanced propeller failsafe mode
+ *
+ * Action the system takes when an imbalanced propeller is detected by the failure detector.
+ * See also FD_IMB_PROP_THR to set the failure threshold.
+ *
+ * @group Commander
+ *
+ * @value -1 Disabled
+ * @value 0 Warning
+ * @value 1 Return
+ * @value 2 Land
+ * @decimal 0
+ * @increment 1
+ */
+PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
+
 /**
  * Time-out to wait when offboard connection is lost before triggering offboard lost action.
  *

src/modules/commander/state_machine_helper.cpp

@@ -1231,3 +1231,52 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
 		break;
 	}
 }
+
+void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
+			      const vehicle_status_flags_s &status_flags, commander_state_s *internal_state,
+			      const imbalanced_propeller_action_t failsafe_action)
+{
+	static constexpr char failure_msg[] = "Imbalanced propeller detected";
+
+	switch (failsafe_action) {
+	case imbalanced_propeller_action_t::DISABLED:
+		break;
+
+	case imbalanced_propeller_action_t::WARNING:
+		mavlink_log_warning(mavlink_log_pub, "%s, landing advised", failure_msg);
+		break;
+
+	case imbalanced_propeller_action_t::RETURN:
+
+		if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
+			if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
+			      internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
+			      internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
+
+				internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
+				internal_state->timestamp = hrt_absolute_time();
+				mavlink_log_warning(mavlink_log_pub, "%s, executing RTL", failure_msg);
+			}
+
+		} else {
+			if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
+			      internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
+				internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
+				internal_state->timestamp = hrt_absolute_time();
+				mavlink_log_warning(mavlink_log_pub, "%s, can't execute RTL, landing instead", failure_msg);
+			}
+		}
+
+		break;
+
+	case imbalanced_propeller_action_t::LAND:
+		if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
+		      internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {
+			internal_state->main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
+			internal_state->timestamp = hrt_absolute_time();
+			mavlink_log_warning(mavlink_log_pub, "%s, landing", failure_msg);
+		}
+
+		break;
+	}
+}

src/modules/commander/state_machine_helper.h

@@ -145,4 +145,17 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
 		      const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, const uint8_t battery_warning,
 		      const low_battery_action_t low_bat_action);
 
+
+// COM_IMB_PROP_ACT parameter values
+enum class imbalanced_propeller_action_t {
+	DISABLED = -1,
+	WARNING = 0,
+	RETURN = 1,
+	LAND = 2
+};
+
+void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
+			      const vehicle_status_flags_s &status_flags, commander_state_s *internal_state,
+			      const imbalanced_propeller_action_t failsafe_action);
+
 #endif /* STATE_MACHINE_HELPER_H_ */