fw_att_control add rattitude mode

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;

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:
-
+		ret = TRANSITION_CHANGED;
-		/* ACRO and RATTITUDE only implemented in MC */
-		if (status->is_rotary_wing) {
-			ret = TRANSITION_CHANGED;
-		}
-
 		break;
 
 	case commander_state_s::MAIN_STATE_ALTCTL:

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 */

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);