From 8a203951594282a297b2af402a82b85f0f927619 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:29:51 +0100 Subject: [PATCH] mc_pos_control: fixed yaw setpoint in AUTO --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 3194534b9a..0d65b5b03c 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();