diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b72ad45f6b..117e1e8024 100644 --- a/src/modules/commander/Commander.cpp +++ b/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 */ diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 34724b5100..4ea3c3ab64 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -211,6 +211,7 @@ private: (ParamInt) _param_com_pos_fs_gain, (ParamInt) _param_com_low_bat_act, + (ParamInt) _param_com_imb_prop_act, (ParamFloat) _param_com_disarm_land, (ParamFloat) _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}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 16f3cdea93..0df0589228 100644 --- a/src/modules/commander/commander_params.c +++ b/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. * diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d44d2fbff0..5ee30b98d5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/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; + } +} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f810625ce1..a7c5e2d709 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/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_ */