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

mc pos control: offboard: set yaw and yawspeed depending on valid flags

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-25 13:15:30 +02:00
parent 55fde23233
commit 9bda573151
1 changed files with 5 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
+5 -1
View File
@@ -614,7 +614,6 @@ MulticopterPositionControl::control_offboard(float dt)
_pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y; _pos_sp(1) = _pos_sp_triplet.current.y;
_pos_sp(2) = _pos_sp_triplet.current.z; _pos_sp(2) = _pos_sp_triplet.current.z;
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* control velocity */ /* control velocity */
@@ -624,6 +623,11 @@ MulticopterPositionControl::control_offboard(float dt)
/* set position setpoint move rate */ /* set position setpoint move rate */
_sp_move_rate(0) = _pos_sp_triplet.current.vx; _sp_move_rate(0) = _pos_sp_triplet.current.vx;
_sp_move_rate(1) = _pos_sp_triplet.current.vy; _sp_move_rate(1) = _pos_sp_triplet.current.vy;
}
if (_pos_sp_triplet.current.yaw_valid) {
_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; _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
} }