fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 09:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

implemented vtol weathervane yaw control for landing and loiter mission item

Browse Source
This commit is contained in:
Roman
2016-02-15 09:48:37 +01:00
parent be0f680863
commit 80f8fcbdf6
5 changed files with 74 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+19 -1
View File
@@ -201,6 +201,7 @@ private:
param_t roll_tc;
param_t pitch_tc;
param_t vtol_opt_recovery_enabled;
param_t vtol_wv_yaw_rate_scale;
} _params_handles; /**< handles for interesting parameters */
@@ -222,6 +223,7 @@ private:
float rattitude_thres;
int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
bool vtol_opt_recovery_enabled;
float vtol_wv_yaw_rate_scale; /**< Scale value [0, 1] for yaw rate setpoint */
} _params;
TailsitterRecovery *_ts_opt_recovery; /**< Computes optimal rates for tailsitter recovery */
@@ -353,6 +355,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_params.vtol_opt_recovery_enabled = false;
_params.vtol_wv_yaw_rate_scale = 1.0f;
_rates_prev.zero();
_rates_sp.zero();
@@ -390,7 +394,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.roll_tc = param_find("MC_ROLL_TC");
_params_handles.pitch_tc = param_find("MC_PITCH_TC");
_params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN");
_params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN");
_params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL");
/* fetch initial parameter values */
parameters_update();
@@ -511,6 +519,8 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.vtol_opt_recovery_enabled, &tmp);
_params.vtol_opt_recovery_enabled = (bool)tmp;
param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
@@ -724,6 +734,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
}
}
/* weather-vane mode, dampen yaw rate */
if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f;
}
/* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}