99 lines
3.1 KiB
C++
99 lines
3.1 KiB
C++
#include "LowLevelMessages.h"
|
|
|
|
// #include <iostream>
|
|
#include "LinearAlgebra/float16.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
void LowLevelMessages::SendAngle8(char* buffer,
|
|
unsigned char* ix,
|
|
const float angle) {
|
|
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
|
buffer[(*ix)++] = packedAngle2.GetBinary();
|
|
}
|
|
Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer,
|
|
unsigned char* startIndex) {
|
|
unsigned char binary = buffer[(*startIndex)++];
|
|
|
|
Angle8 angle = Angle8::Binary(binary);
|
|
|
|
return angle;
|
|
}
|
|
|
|
void LowLevelMessages::SendFloat16(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 char* buffer,
|
|
unsigned char* startIndex) {
|
|
unsigned char ix = *startIndex;
|
|
unsigned char msb = buffer[ix++];
|
|
unsigned char lsb = buffer[ix++];
|
|
unsigned short value = msb << 8 | lsb;
|
|
float16 f = float16();
|
|
f.setBinary(value);
|
|
|
|
*startIndex = ix;
|
|
return (float)f.toFloat();
|
|
}
|
|
|
|
void LowLevelMessages::SendSpherical(char* buffer,
|
|
unsigned char* ix,
|
|
Spherical s) {
|
|
SendFloat16(buffer, ix, s.distance);
|
|
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
|
|
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
|
|
}
|
|
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
|
|
unsigned char* startIndex) {
|
|
float distance = ReceiveFloat16(buffer, startIndex);
|
|
|
|
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
|
Angle horizontal = Angle::Radians(horizontal8.InRadians());
|
|
|
|
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
|
Angle vertical = Angle::Radians(vertical8.InRadians());
|
|
|
|
Spherical s = Spherical(distance, horizontal, vertical);
|
|
return s;
|
|
}
|
|
|
|
void LowLevelMessages::SendQuat32(char* buffer,
|
|
unsigned char* ix,
|
|
SwingTwist 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;
|
|
}
|
|
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
|
|
// << "\n";
|
|
buffer[(*ix)++] = qx;
|
|
buffer[(*ix)++] = qy;
|
|
buffer[(*ix)++] = qz;
|
|
buffer[(*ix)++] = qw;
|
|
}
|
|
|
|
SwingTwist LowLevelMessages::ReceiveQuat32(const 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);
|
|
SwingTwist s = SwingTwist::FromQuaternion(q);
|
|
return s;
|
|
}
|
|
|
|
} // namespace RoboidControl
|