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

mc_pos_control_main: wrap yaw attitude setpoint for offboard mode

Browse Source
This commit is contained in:
Martina
2017-11-24 09:10:31 +01:00
committed by Lorenz Meier
parent ec27865184
commit a61974709b
1 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+13 -1
View File
@@ -1626,7 +1626,19 @@ MulticopterPositionControl::control_offboard(float dt)
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_pos_sp_triplet.current.yawspeed_valid) {
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
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;
// If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again.
if (fabsf(yaw_offs) < yaw_offset_max ||
(_pos_sp_triplet.current.yawspeed > 0 && yaw_offs < 0) ||
(_pos_sp_triplet.current.yawspeed < 0 && yaw_offs > 0)) {
_att_sp.yaw_body = yaw_target;
}
}
} else {