fw att+pos ctrl: add takeoff flaps setting

msg/vehicle_attitude_setpoint.msg

@@ -20,7 +20,7 @@ bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)
 bool fw_control_yaw					# control heading with rudder (used for auto takeoff on runway)
 bool disable_mc_yaw_control			# control yaw for mc (used for vtol weather-vane mode)
 
-bool apply_flaps
+int8 apply_flaps      # 0 = no flaps, 1 = landing flaps setting, 2 = take-off flaps setting
 
 float32 landing_gear
 

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -106,6 +106,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.acro_max_z_rate = param_find("FW_ACRO_Z_MAX");
 
 	_parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL");
+	_parameter_handles.flaps_takeoff_scale = param_find("FW_FLAPS_TO_SCL");
 	_parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL");
 
 	_parameter_handles.rattitude_thres = param_find("FW_RATT_TH");
@@ -201,6 +202,7 @@ FixedwingAttitudeControl::parameters_update()
 	_parameters.acro_max_z_rate_rad = math::radians(_parameters.acro_max_z_rate_rad);
 
 	param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
+	param_get(_parameter_handles.flaps_takeoff_scale, &_parameters.flaps_takeoff_scale);
 	param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
 
 	param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres);
@@ -875,7 +877,16 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
 
 	} else if (_vcontrol_mode.flag_control_auto_enabled
 		   && fabsf(_parameters.flaps_scale) > 0.01f) {
-		flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
+		switch (_att_sp.apply_flaps) {
+		case 0 : flap_control = 0.0f; // no flaps
+			break;
+
+		case 1 : flap_control = 1.0f * _parameters.flaps_scale; // landing flaps
+			break;
+
+		case 2 : flap_control = 1.0f * _parameters.flaps_scale * _parameters.flaps_takeoff_scale; // take-off flaps
+			break;
+		}
 	}
 
 	// move the actual control value continuous with time, full flap travel in 1sec
@@ -896,7 +907,7 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
 
 	} else if (_vcontrol_mode.flag_control_auto_enabled
 		   && fabsf(_parameters.flaperon_scale) > 0.01f) {
-		flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
+		flaperon_control = (_att_sp.apply_flaps == 1) ? 1.0f * _parameters.flaperon_scale : 0.0f;
 	}
 
 	// move the actual control value continuous with time, full flap travel in 1sec

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -196,6 +196,7 @@ private:
 		float acro_max_z_rate_rad;
 
 		float flaps_scale;				/**< Scale factor for flaps */
+		float flaps_takeoff_scale; /**< Scale factor for flaps on take-off */
 		float flaperon_scale;			/**< Scale factor for flaperons */
 
 		float rattitude_thres;
@@ -264,6 +265,7 @@ private:
 		param_t acro_max_z_rate;
 
 		param_t flaps_scale;
+		param_t flaps_takeoff_scale;
 		param_t flaperon_scale;
 
 		param_t rattitude_thres;

src/modules/fw_att_control/fw_att_control_params.c

@@ -485,6 +485,20 @@ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
  */
 PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f);
 
+/**
+ * Flaps setting during take-off
+ *
+ * Sets a fraction of full flaps (FW_FLAPS_SCL) during take-off
+ *
+ * @unit norm
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
+
 /**
  * Scale factor for flaperons
  *

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -704,7 +704,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 	bool setpoint = true;
 
 	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
-	_att_sp.apply_flaps = false;		// by default we don't use flaps
+	_att_sp.apply_flaps = 0;		// by default we don't use flaps
 
 	calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
 
@@ -1137,6 +1137,10 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 		prev_wp(1) = (float)pos_sp_curr.lon;
 	}
 
+	// apply flaps for takeoff according to the corresponding scale factor set
+	// via FW_FLAPS_TO_SCL
+	_att_sp.apply_flaps = 2;
+
 	// continuously reset launch detection and runway takeoff until armed
 	if (!_control_mode.flag_armed) {
 		_launchDetector.reset();
@@ -1305,7 +1309,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 
 	// apply full flaps for landings. this flag will also trigger the use of flaperons
 	// if they have been enabled using the corresponding parameter
-	_att_sp.apply_flaps = true;
+	_att_sp.apply_flaps = 1;
 
 	// save time at which we started landing and reset abort_landing
 	if (_time_started_landing == 0) {