diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py index 11d06d24..806f29ec 100755 --- a/clever/src/simple_offboard.py +++ b/clever/src/simple_offboard.py @@ -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() diff --git a/clever/src/util.py b/clever/src/util.py index eafa72fd..275fe009 100644 --- a/clever/src/util.py +++ b/clever/src/util.py @@ -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):