diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-02-01 10:29:51 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-02-01 10:29:51 +0100 |
| commit | 8a203951594282a297b2af402a82b85f0f927619 (patch) | |
| tree | a52b1f72b09dbd6d2d7ee3bc67531472cad2f9c4 /src | |
| parent | bb8be966fcaa484c0f8209da41760d4bc24d6c5f (diff) | |
| download | px4-firmware-8a203951594282a297b2af402a82b85f0f927619.tar.gz px4-firmware-8a203951594282a297b2af402a82b85f0f927619.tar.bz2 px4-firmware-8a203951594282a297b2af402a82b85f0f927619.zip | |
mc_pos_control: fixed yaw setpoint in AUTO
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 |
1 files changed, 7 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 3194534b9..0d65b5b03 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -688,10 +688,16 @@ MulticopterPositionControl::task_main() _reset_lat_lon_sp = true; _reset_alt_sp = true; + /* update position setpoint */ _lat_sp = _pos_sp_triplet.current.lat; _lon_sp = _pos_sp_triplet.current.lon; _alt_sp = _pos_sp_triplet.current.alt; + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet.current.yaw)) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } + } else { /* no waypoint, loiter, reset position setpoint if needed */ reset_lat_lon_sp(); @@ -711,7 +717,7 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = 0.0f; + _att_sp.yaw_body = _att.yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); |