fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw att+pos ctrl: add takeoff flaps setting

Browse Source
This commit is contained in:
Thomas Stastny 2018-07-06 11:14:32 +02:00 committed by Lorenz Meier
parent 3f0a398a25
commit 0c3399433d
5 changed files with 36 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_attitude_setpoint.msg
View File

@ -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

15
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -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

2
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

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

14
src/modules/fw_att_control/fw_att_control_params.c
View File

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

8
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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) {