diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 6a20d58211..65a2227604 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/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 diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 4830ed7c68..9247a8a1cd 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/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 diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index b72d16e8cb..b4d7e14e28 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/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; 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 6dfddfa9ad..61ee956b40 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/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 * diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index a6b2c0fa29..038f87b0cd 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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) {