diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 12e8e4b82c..3701cb28df 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3701,7 +3701,7 @@ set_control_mode() control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = true; + control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0462a1c894..ee7d08b646 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -377,16 +377,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_MANUAL: case commander_state_s::MAIN_STATE_STAB: case commander_state_s::MAIN_STATE_ACRO: - ret = TRANSITION_CHANGED; - break; - case commander_state_s::MAIN_STATE_RATTITUDE: - - /* ACRO and RATTITUDE only implemented in MC */ - if (status->is_rotary_wing) { - ret = TRANSITION_CHANGED; - } - + ret = TRANSITION_CHANGED; break; case commander_state_s::MAIN_STATE_ALTCTL: diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 793fd14248..b3ca530d08 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -207,6 +207,8 @@ private: float flaps_scale; /**< Scale factor for flaps */ float flaperon_scale; /**< Scale factor for flaperons */ + float rattitude_thres; + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ int bat_scale_en; /**< Battery scaling enabled */ @@ -264,6 +266,8 @@ private: param_t flaps_scale; param_t flaperon_scale; + param_t rattitude_thres; + param_t vtol_type; param_t bat_scale_en; @@ -457,6 +461,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); + _parameter_handles.rattitude_thres = param_find("FW_RATT_TH"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); _parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN"); @@ -558,6 +564,8 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); + param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en); @@ -930,6 +938,14 @@ FixedwingAttitudeControl::task_main() _flaperons_applied = flaperon_control; } + // Check if we are in rattitude mode and the pilot is above the threshold on pitch + if (_vcontrol_mode.flag_control_rattitude_enabled) { + if (fabsf(_manual.y) > _parameters.rattitude_thres || + fabsf(_manual.x) > _parameters.rattitude_thres) { + _vcontrol_mode.flag_control_attitude_enabled = false; + } + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_rates_enabled) { /* scale around tuning airspeed */ diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 4151b309c0..7540d26007 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -629,3 +629,17 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); + +/** + * Threshold for Rattitude mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_RATT_TH, 0.8f);