diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-25 13:15:30 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-25 13:15:30 +0200 |
| commit | 9bda573151ae1b5fa87686ee58596ea14e052941 (patch) | |
| tree | c8afbc9a7b226e86479a32810b3688e54d6a9c87 | |
| parent | 55fde23233de3d311d6c8d0b2cd368e45af3caac (diff) | |
| download | px4-firmware-9bda573151ae1b5fa87686ee58596ea14e052941.tar.gz px4-firmware-9bda573151ae1b5fa87686ee58596ea14e052941.tar.bz2 px4-firmware-9bda573151ae1b5fa87686ee58596ea14e052941.zip | |
mc pos control: offboard: set yaw and yawspeed depending on valid flags
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 |
1 files changed, 5 insertions, 1 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ecc40428d..4f5c62a59 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -614,7 +614,6 @@ MulticopterPositionControl::control_offboard(float dt) _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; _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) { /* control velocity */ @@ -624,6 +623,11 @@ MulticopterPositionControl::control_offboard(float dt) /* set position setpoint move rate */ _sp_move_rate(0) = _pos_sp_triplet.current.vx; _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; } |