diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 153a642bb4..4da165120c 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -14,6 +14,7 @@ bool flag_control_auto_enabled # true if onboard autopilot should act bool flag_control_offboard_enabled # true if offboard control should be used bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_rattitude_enabled # true if rate/attitude stabilization is enabled bool flag_control_force_enabled # true if force control is mixed in bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled bool flag_control_position_enabled # true if position is controlled diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e2e4f04c3e..26f39f64a3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3176,6 +3176,7 @@ set_control_mode() control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = stabilization_required(); control_mode.flag_control_attitude_enabled = stabilization_required(); + 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; @@ -3188,6 +3189,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_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; @@ -3202,6 +3204,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 = false; control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; @@ -3214,6 +3217,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 = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = false; @@ -3226,6 +3230,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 = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = !status.in_transition_mode; @@ -3249,6 +3254,7 @@ set_control_mode() control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = !status.in_transition_mode; @@ -3261,6 +3267,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 = false; control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = false; @@ -3273,6 +3280,7 @@ set_control_mode() control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; + 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; @@ -3286,6 +3294,7 @@ set_control_mode() control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_rattitude_enabled = false; /* in failsafe LAND mode position may be not available */ control_mode.flag_control_position_enabled = status_flags.condition_local_position_valid; control_mode.flag_control_velocity_enabled = status_flags.condition_local_position_valid; @@ -3300,6 +3309,7 @@ set_control_mode() control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_altitude_enabled = false; @@ -3313,6 +3323,7 @@ set_control_mode() control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_altitude_enabled = false; @@ -3340,6 +3351,8 @@ set_control_mode() !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; + control_mode.flag_control_rattitude_enabled = false; + control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position) && !status.in_transition_mode; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 50fbc9420b..eb665119d5 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -877,7 +877,7 @@ MulticopterAttitudeControl::task_main() /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ - if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) { + if (_v_control_mode.flag_control_rattitude_enabled) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || fabsf(_manual_control_sp.x) > _params.rattitude_thres) { _v_control_mode.flag_control_attitude_enabled = false;