07f78105aa
git-subtree-dir: ControlCore git-subtree-mainline: 2b5f5a58ac608aca3aad452a87f6cb27f428cbde git-subtree-split: 0a57e6d99abadc3257c6b1fdf5880b993e0d0fcb
88 lines
3.0 KiB
C++
88 lines
3.0 KiB
C++
#include "LowLevelMessages.h"
|
|
|
|
#include "float16.h"
|
|
|
|
void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
|
|
const float angle) {
|
|
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
|
buffer[(*ix)++] = packedAngle2.GetBinary();
|
|
}
|
|
Angle8 LowLevelMessages::ReceiveAngle8(const unsigned char *buffer,
|
|
unsigned char *startIndex) {
|
|
unsigned char binary = buffer[(*startIndex)++];
|
|
|
|
Angle8 angle = Angle8::Binary(binary);
|
|
|
|
return angle;
|
|
}
|
|
|
|
void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
|
|
float value) {
|
|
float16 value16 = float16(value);
|
|
short binary = value16.getBinary();
|
|
|
|
buffer[(*ix)++] = (binary >> 8) & 0xFF;
|
|
buffer[(*ix)++] = binary & 0xFF;
|
|
}
|
|
float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
|
|
unsigned char *startIndex) {
|
|
unsigned char ix = *startIndex;
|
|
unsigned short value = buffer[ix++] << 8 | buffer[ix++];
|
|
float16 f = float16();
|
|
f.setBinary(value);
|
|
|
|
*startIndex = ix;
|
|
return (float)f.toFloat();
|
|
}
|
|
|
|
void LowLevelMessages::SendSpherical16(unsigned char *buffer, unsigned char *ix,
|
|
Spherical16 s) {
|
|
SendFloat16(buffer, ix, s.distance);
|
|
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
|
|
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
|
|
}
|
|
Spherical16 LowLevelMessages::ReceiveSpherical16(const unsigned char *buffer,
|
|
unsigned char *startIndex) {
|
|
float distance = ReceiveFloat16(buffer, startIndex);
|
|
|
|
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
|
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
|
|
|
|
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
|
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
|
|
|
|
Spherical16 s = Spherical16(distance, horizontal, vertical);
|
|
return s;
|
|
}
|
|
|
|
void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
|
|
unsigned char *ix,
|
|
SwingTwist16 rotation) {
|
|
Quaternion q = rotation.ToQuaternion();
|
|
unsigned char qx = (char)(q.x * 127 + 128);
|
|
unsigned char qy = (char)(q.y * 127 + 128);
|
|
unsigned char qz = (char)(q.z * 127 + 128);
|
|
unsigned char qw = (char)(q.w * 255);
|
|
if (q.w < 0) {
|
|
qx = -qx;
|
|
qy = -qy;
|
|
qz = -qz;
|
|
qw = -qw;
|
|
}
|
|
// Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
|
|
buffer[(*ix)++] = qx;
|
|
buffer[(*ix)++] = qy;
|
|
buffer[(*ix)++] = qz;
|
|
buffer[(*ix)++] = qw;
|
|
}
|
|
|
|
SwingTwist16 LowLevelMessages::ReceiveQuat32(const unsigned char *buffer,
|
|
unsigned char *ix) {
|
|
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
|
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
|
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
|
float qw = buffer[(*ix)++] / 255.0F;
|
|
Quaternion q = Quaternion(qx, qy, qz, qw);
|
|
SwingTwist16 s = SwingTwist16::FromQuaternion(q);
|
|
return s;
|
|
} |