35 lines
1018 B
C++
35 lines
1018 B
C++
#include "Messages.h"
|
|
|
|
#include "float16/float16.h"
|
|
|
|
Angle8 Messages::ReceiveAngle8(unsigned char *data, unsigned char *startIndex) {
|
|
unsigned char binary = data[(*startIndex)++];
|
|
|
|
Angle8 angle = Angle8::Binary(binary);
|
|
|
|
return angle;
|
|
}
|
|
|
|
float Messages::ReceiveFloat16(unsigned char *data, unsigned char *startIndex) {
|
|
unsigned char ix = *startIndex;
|
|
unsigned short value = data[ix++] << 8 | data[ix];
|
|
float16 f = float16();
|
|
f.setBinary(value);
|
|
|
|
*startIndex = ix;
|
|
return f.toDouble();
|
|
}
|
|
|
|
Spherical16 Messages::ReceiveSpherical16(unsigned char *data,
|
|
unsigned char *startIndex) {
|
|
float distance = ReceiveFloat16(data, startIndex);
|
|
|
|
Angle8 horizontal8 = ReceiveAngle8(data, startIndex);
|
|
Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 255);
|
|
|
|
Angle8 vertical8 = ReceiveAngle8(data, startIndex);
|
|
Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 255);
|
|
|
|
Spherical16 s = Spherical16(distance, horizontal, vertical);
|
|
return s;
|
|
} |