145 lines
5.0 KiB
C++
145 lines
5.0 KiB
C++
#include "Messages.h"
|
|
|
|
#include "float16/float16.h"
|
|
|
|
Angle8 Messages::ReceiveAngle8(unsigned char *buffer,
|
|
unsigned char *startIndex) {
|
|
unsigned char binary = buffer[(*startIndex)++];
|
|
|
|
Angle8 angle = Angle8::Binary(binary);
|
|
|
|
return angle;
|
|
}
|
|
|
|
float Messages::ReceiveFloat16(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 f.toDouble();
|
|
}
|
|
|
|
Spherical16 Messages::ReceiveSpherical16(unsigned char *buffer,
|
|
unsigned char *startIndex) {
|
|
float distance = ReceiveFloat16(buffer, startIndex);
|
|
|
|
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
|
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 255);
|
|
|
|
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
|
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 255);
|
|
|
|
Spherical16 s = Spherical16(distance, horizontal, vertical);
|
|
return s;
|
|
}
|
|
|
|
void Messages::SendVector3(unsigned char *buffer, unsigned char *startIndex,
|
|
const Vector3 v) {
|
|
SendSingle100(buffer, *startIndex, v.Right());
|
|
(*startIndex) += 4;
|
|
SendSingle100(buffer, *startIndex, v.Up());
|
|
(*startIndex) += 4;
|
|
SendSingle100(buffer, *startIndex, v.Forward());
|
|
(*startIndex) += 4;
|
|
}
|
|
|
|
void Messages::SendQuaternion(unsigned char *buffer, const int startIndex,
|
|
const Quaternion q) {
|
|
Vector3 angles = Quaternion::ToAngles(q);
|
|
int ix = startIndex;
|
|
SendAngle8(buffer, ix++, angles.Right());
|
|
SendAngle8(buffer, ix++, angles.Up());
|
|
SendAngle8(buffer, ix++, angles.Forward());
|
|
}
|
|
|
|
void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex,
|
|
Polar p) {
|
|
SendAngle8(buffer, *startIndex, (const float)p.angle.InDegrees());
|
|
SendSingle100(buffer, (*startIndex) + 1, p.distance);
|
|
}
|
|
|
|
void Messages::SendSpherical16(unsigned char *buffer, unsigned char *startIndex,
|
|
Spherical16 s) {
|
|
SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees());
|
|
SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees());
|
|
SendFloat16(buffer, startIndex, s.distance);
|
|
}
|
|
|
|
void Messages::SendSwingTwist(unsigned char *buffer, unsigned char *ix,
|
|
const SwingTwist16 r) {
|
|
Quaternion q = r.ToQuaternion();
|
|
SendQuat32(buffer, ix, q);
|
|
}
|
|
|
|
void Messages::SendQuat32(unsigned char *buffer, unsigned char *startIndex,
|
|
const Quaternion q) {
|
|
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[(*startIndex)++] = qx;
|
|
buffer[(*startIndex)++] = qy;
|
|
buffer[(*startIndex)++] = qz;
|
|
buffer[(*startIndex)++] = qw;
|
|
}
|
|
|
|
void Messages::SendAngle8(unsigned char *buffer, unsigned int startIndex,
|
|
const float angle) {
|
|
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
|
buffer[startIndex] = packedAngle2.GetBinary();
|
|
}
|
|
|
|
// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
|
|
// const float angle) {
|
|
// AngleUsing<signed short> packedAngle = AngleUsing<signed short>(angle);
|
|
// signed short value = packedAngle.GetValue();
|
|
// data[startIndex] = value >> 8;
|
|
// data[startIndex + 1] = value & 0xFF;
|
|
// }
|
|
|
|
// void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
|
|
// const float angle) {
|
|
// AngleUsing<signed long> packedAngle = AngleUsing<signed long>(angle);
|
|
// unsigned long value = packedAngle.GetValue();
|
|
// data[startIndex] = value >> 24 & 0xFF;
|
|
// data[startIndex + 1] = value >> 16 & 0xFF;
|
|
// data[startIndex + 2] = value >> 8 & 0xFF;
|
|
// data[startIndex + 3] = value & 0xFF;
|
|
// // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
|
|
// // data[startIndex + 1], data[startIndex + 2],
|
|
// // data[startIndex + 3]);
|
|
// }
|
|
|
|
// void Messages::SendSingle100(unsigned char *data, unsigned int startIndex,
|
|
// float value) {
|
|
// // Sends a float with truncated 2 decimal precision
|
|
// Int32 intValue = value * 100;
|
|
// SendInt32(data, startIndex, intValue);
|
|
// }
|
|
|
|
void Messages::SendFloat16(unsigned char *data, unsigned char *startIndex,
|
|
float value) {
|
|
float16 value16 = float16(value);
|
|
short binary = value16.getBinary();
|
|
|
|
data[(*startIndex)++] = (binary >> 8) & 0xFF;
|
|
data[(*startIndex)++] = binary & 0xFF;
|
|
}
|
|
|
|
// void Messages::SendInt32(unsigned char *data, unsigned int startIndex,
|
|
// Int32 value) {
|
|
// for (unsigned char ix = 0; ix < 4; ix++) {
|
|
// data[startIndex++] = ((unsigned char *)&value)[ix];
|
|
// }
|
|
// }
|