CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

simple_offboard: correct axes for calculating yaw for setpoints

Browse Source
This commit is contained in:
Oleg Kalachev
2018-05-10 22:32:45 +03:00
parent 6372ef8c22
commit 8f53301b79
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
clever/src/simple_offboard.py
View File

@@ -188,7 +188,7 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
position=setpoint,
yaw=euler_from_orientation(current_nav_finish.pose.orientation)[2] - math.pi / 2,
yaw=euler_from_orientation(current_nav_finish.pose.orientation, 'szyx')[2] - math.pi / 2,
yaw_rate=req.yaw_rate)
return position_pub, msg
@@ -207,7 +207,7 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
position=pose_local.pose.position,
yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2,
yaw=euler_from_orientation(pose_local.pose.orientation, 'szyx')[2] - math.pi / 2,
yaw_rate=req.yaw_rate)
return position_pub, msg
@@ -225,7 +225,7 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
velocity=vector_local.vector,
yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2,
yaw=euler_from_orientation(pose_local.pose.orientation, 'szyx')[2] - math.pi / 2,
yaw_rate=req.yaw_rate)
return position_pub, msg