7781fcf2de
aebe4c0 Arduino Ant now works. d3cb4c1 Merge commit 'fbeed8e80922152c3404fbd5d2b243ae95792ec1' into V2 fbeed8e Used Client override for processing messages 394dc22 Merge commit '355dd5c1c519cf07cfb6b9f9200f7f7311e68f20' into V2 355dd5c Fixed ThingMsg format becb194 Merge commit 'f35d60369daf41a4fcd987ef8b31bd384b9536ba' into V2 9b53eee Merge commit 'a48ae12fc2f6d4a99119c128e78bf4b103e607c3' into V2 f35d603 Further improvements a48ae12 ControlCore mostly works (but I don't see a model on the site server yet) d8fc41f First step for ControlCore support git-subtree-dir: Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore git-subtree-split: aebe4c0f8e805259a5aea4a4cb6b72343d73257a
73 lines
2.2 KiB
C#
73 lines
2.2 KiB
C#
using Passer;
|
|
|
|
public class LowLevelMessages {
|
|
|
|
public static void SendSpherical(byte[] buffer, ref uint ix, Spherical v) {
|
|
SendAngle8(buffer, ref ix, v.horizontal);
|
|
SendAngle8(buffer, ref ix, v.vertical);
|
|
SendFloat16(buffer, ref ix, new float16(v.distance));
|
|
}
|
|
|
|
public static Spherical ReceiveSpherical(byte[] data, ref uint ix) {
|
|
float horizontal = ReceiveAngle8(data, ref ix);
|
|
float vertical = ReceiveAngle8(data, ref ix);
|
|
float distance = ReceiveFloat16(data, ref ix);
|
|
Spherical v = new(distance, horizontal, vertical);
|
|
return v;
|
|
}
|
|
|
|
public static void SendQuat32(byte[] buffer, ref uint ix, Quat32 q) {
|
|
int qx = (int)(q.x * 127 + 128);
|
|
int qy = (int)(q.y * 127 + 128);
|
|
int qz = (int)(q.z * 127 + 128);
|
|
int qw = (int)(q.w * 255);
|
|
if (q.w < 0) {
|
|
qx = -qx;
|
|
qy = -qy;
|
|
qz = -qz;
|
|
qw = -qw;
|
|
}
|
|
|
|
buffer[ix++] = (byte)qx;
|
|
buffer[ix++] = (byte)qy;
|
|
buffer[ix++] = (byte)qz;
|
|
buffer[ix++] = (byte)qw;
|
|
}
|
|
public static Quat32 ReceiveQuat32(byte[] data, ref uint ix) {
|
|
Quat32 q = new(
|
|
(data[ix++] - 128.0F) / 127.0F,
|
|
(data[ix++] - 128.0F) / 127.0F,
|
|
(data[ix++] - 128.0F) / 127.0F,
|
|
data[ix++] / 255.0F);
|
|
return q;
|
|
}
|
|
|
|
public static void SendAngle8(byte[] buffer, ref uint ix, float angle) {
|
|
// Normalize angle
|
|
while (angle >= 180)
|
|
angle -= 360;
|
|
while (angle < -180)
|
|
angle += 360;
|
|
buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
|
|
}
|
|
|
|
public static float ReceiveAngle8(byte[] data, ref uint ix) {
|
|
float value = (data[ix++] * 180) / 128.0F;
|
|
return value;
|
|
}
|
|
|
|
public static void SendFloat16(byte[] data, ref uint ix, float16 f) {
|
|
ushort binary = f.GetBinary();
|
|
data[ix++] = (byte)(binary >> 8);
|
|
data[ix++] = (byte)(binary & 255);
|
|
}
|
|
|
|
public static float ReceiveFloat16(byte[] data, ref uint ix) {
|
|
ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
|
|
float16 f16 = new();
|
|
f16.SetBinary(value);
|
|
float f = f16.toFloat();
|
|
return f;
|
|
}
|
|
}
|