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

Limit manual yaw command properly. Fixes #3600

Browse Source
This commit is contained in:
Lorenz Meier 2016-01-26 12:35:27 +01:00 committed by Julian Oes
parent c9b1fb154d
commit 5ea5ecf32b
1 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -185,6 +185,7 @@ private:
param_t man_roll_max;
param_t man_pitch_max;
param_t man_yaw_max;
param_t global_yaw_max;
param_t mc_att_yaw_p;
param_t hold_xy_dz;
param_t hold_z_dz;
@ -203,6 +204,7 @@ private:
float man_roll_max;
float man_pitch_max;
float man_yaw_max;
float global_yaw_max;
float mc_att_yaw_p;
float hold_xy_dz;
float hold_z_dz;
@ -432,6 +434,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.man_roll_max = param_find("MPC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX");
_params_handles.global_yaw_max = param_find("MC_YAWRATE_MAX");
_params_handles.mc_att_yaw_p = param_find("MC_YAW_P");
_params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ");
_params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ");
@ -547,9 +550,12 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
param_get(_params_handles.global_yaw_max, &_params.global_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
_params.global_yaw_max = math::radians(_params.global_yaw_max);
param_get(_params_handles.mc_att_yaw_p, &v);
_params.mc_att_yaw_p = v;
}
@ -1766,14 +1772,19 @@ MulticopterPositionControl::task_main()
/* do not move yaw while sitting on the ground */
else if (!_vehicle_status.condition_landed &&
!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
/* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max : _params.global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it
// XXX this needs inspection - probably requires a clamp, not an if
if (fabsf(yaw_offs) < yaw_offset_max) {
_att_sp.yaw_body = yaw_target;
}