RoboidControl/RoboidControl-python
RoboidControl/RoboidControl-python
1
0
Fork 0
Code Issues Pull Requests Actions Packages Releases Activity

Merge branch 'main' of https://git.passer.life/RoboidControl/RoboidControl-python

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-14 16:52:00 +01:00
commit dca94c7194
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Messages/LowLevelMessages.py
View File

@ -2,6 +2,7 @@ import numpy as np
from LinearAlgebra.SwingTwist import SwingTwist
def SendAngle8(buffer, ix_ref, angle):
"""! Send an 8-bit angle value """
# Normalize angle
while angle >= 180:
angle -= 360
@ -13,6 +14,7 @@ def SendAngle8(buffer, ix_ref, angle):
ix_ref[0] += 1
def SendFloat16(buffer, ix_ref, value):
"""! Send a 16-bit floating point value """
ix = ix_ref[0]
value16 = np.float16(value)
binary = value16.view(np.uint16)
@ -23,6 +25,7 @@ def SendFloat16(buffer, ix_ref, value):
ix_ref[0] += 2
def ReceiveFloat16(buffer, ix_ref) -> float:
"""! Receive a 16-bit floating point value """
ix = ix_ref[0]
# if ix < len(buffer) - 1:
binary = (buffer[ix] << 8) + buffer[ix+1]
@ -33,11 +36,13 @@ def ReceiveFloat16(buffer, ix_ref) -> float:
return float(value16)
def SendSpherical(buffer, ix_ref, vector):
"""! Send a spherical vector """
SendFloat16(buffer, ix_ref, vector.distance)
SendAngle8(buffer, ix_ref, vector.direction.horizontal)
SendAngle8(buffer, ix_ref, vector.direction.vertical)
def SendQuat32(buffer, ix_ref, q):
"""! Send a 32-bit quaternion value """
if isinstance(q, SwingTwist):
q = q.ToQuaternion()