48 lines
1.6 KiB
C++
48 lines
1.6 KiB
C++
#include "LowLevelMessages.h"
|
|
|
|
#include "../float16/float16.h"
|
|
|
|
void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
|
|
const float angle) {
|
|
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
|
buffer[(*ix)++] = packedAngle2.GetBinary();
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void Passer::Control::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());
|
|
}
|
|
|
|
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;
|
|
}
|