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: Tait-Bryan z-y-x angles for pitch, roll and yaw

Browse Source
This commit is contained in:
Oleg Kalachev
2018-05-10 22:31:56 +03:00
parent 6b74f75616
commit 6372ef8c22
2 changed files with 7 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
clever/src/simple_offboard.py
View File

@@ -368,11 +368,9 @@ def get_telemetry(req):
res['x'] = p.pose.position.x
res['y'] = p.pose.position.y
res['z'] = p.pose.position.z
# Get yaw in the request's frame_in
_, _, res['yaw'] = euler_from_orientation(p.pose.orientation)
# Calculate pitch and roll as angles between the pose and fcu_horiz
attitude_pose = tf_buffer.transform(pose, 'fcu_horiz', TRANSFORM_TIMEOUT)
res['roll'], res['pitch'], _ = euler_from_orientation(attitude_pose.pose.orientation)
# Calculate roll pitch and yaw as Tait-Bryan angles, order z-y-x
res['yaw'], res['pitch'], res['roll'] = euler_from_orientation(p.pose.orientation, axes='rzyx')
if velocity:
v = Vector3Stamped()

8
clever/src/util.py
View File

@@ -6,8 +6,8 @@ def orientation_from_quaternion(q):
return Quaternion(*q)
def orientation_from_euler(roll, pitch, yaw):
q = t.quaternion_from_euler(roll, pitch, yaw)
def orientation_from_euler(roll, pitch, yaw, axes='rzyx'):
q = t.quaternion_from_euler(roll, pitch, yaw, axes)
return orientation_from_quaternion(q)
@@ -15,9 +15,9 @@ def quaternion_from_orientation(o):
return o.x, o.y, o.z, o.w
def euler_from_orientation(o):
def euler_from_orientation(o, axes='rzyx'):
q = quaternion_from_orientation(o)
return t.euler_from_quaternion(q)
return t.euler_from_quaternion(q, axes)
def vector3_from_point(p):