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

navigator mission: add yaw offset to vehicle's yaw for ROI cmds and disabled gimbal yaw

Browse Source
This commit is contained in:
Beat Küng 2018-03-16 14:48:34 +01:00
parent 09dba29b6c
commit 76aa044105
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/navigator/mission.cpp
View File

@ -1058,6 +1058,14 @@ Mission::heading_sp_update()
_mission_item.yaw = _wrap_pi(yaw + M_PI_F);
pos_sp_triplet->current.yaw = _mission_item.yaw;
} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT
&& _navigator->get_vroi().mode == vehicle_roi_s::ROI_WPNEXT && !_param_mnt_yaw_ctl.get()) {
/* if yaw control for the mount is disabled and we have a valid ROI that points to the next
* waypoint, we add the gimbal's yaw offset to the vehicle's yaw */
yaw += _navigator->get_vroi().yaw_offset;
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
} else {
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;