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

multiplat mc att ctrl: move yaw sp only if xy is not controlled

Browse Source 
This is a work-around until #1741 makes it to the multiplatform version
This commit is contained in:
Thomas Gubler
2015-02-05 21:43:48 +01:00
parent a58d73b5d0
commit 9532b680dd
1 changed files with 25 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+25 -26
View File
@@ -112,32 +112,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
_reset_yaw_sp = true;
}
/* move yaw setpoint in all modes */
if (_v_att_sp_mod.data().thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
_v_att_sp_mod.data().yaw_body = _wrap_pi(
_v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
if (yaw_offs < -yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
}
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}
/* reset yaw setpoint to current position if needed */
if (_reset_yaw_sp) {
_reset_yaw_sp = false;
@@ -147,6 +121,31 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
}
if (!_v_control_mode->data().flag_control_velocity_enabled) {
if (_v_att_sp_mod.data().thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
_v_att_sp_mod.data().yaw_body = _wrap_pi(
_v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
if (yaw_offs < -yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
}
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}
/* update attitude setpoint if not in position control mode */
_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x