100 lines
2.2 KiB
C++
100 lines
2.2 KiB
C++
#ifndef UWB_H
|
|
#define UWB_H
|
|
|
|
#include <cstdint>
|
|
#include <vector>
|
|
|
|
struct UwbPosition {
|
|
int x = 0;
|
|
int y = 0;
|
|
int z = 0;
|
|
uint8_t quali = 0;
|
|
};
|
|
|
|
struct UwbDistance {
|
|
uint16_t nodeID = 0;
|
|
int distance = 0;
|
|
uint8_t quali = 0;
|
|
};
|
|
|
|
struct UwbResult {
|
|
UwbPosition pos;
|
|
std::vector<UwbDistance> distances;
|
|
};
|
|
|
|
|
|
static void uwb_parse_pos(const unsigned char input[], UwbPosition& pos)
|
|
{
|
|
// X,Y,Z coordinates (each 4 bytes) and quality factor (1 byte), total size: 13 bytes
|
|
|
|
pos.x = input[0];
|
|
pos.x |= input[1] << 8;
|
|
pos.x |= input[2] << 16;
|
|
pos.x |= input[3] << 24;
|
|
|
|
pos.y = input[4];
|
|
pos.y |= input[5] << 8;
|
|
pos.y |= input[6] << 16;
|
|
pos.y |= input[7] << 24;
|
|
|
|
pos.z = input[8];
|
|
pos.z |= input[9] << 8;
|
|
pos.z |= input[10] << 16;
|
|
pos.z |= input[11] << 24;
|
|
|
|
pos.quali = input[12];
|
|
}
|
|
|
|
static void uwb_parse_dist(const unsigned char input[], std::vector<UwbDistance>& result)
|
|
{
|
|
// First byte is distance count(1 byte)
|
|
// Sequence of node ID(2 bytes), distance in mm(4 bytes) and quality factor(1 byte)
|
|
// Max value contains 15 elements, size: 8 - 106
|
|
|
|
size_t numOfAnchors = input[0];
|
|
|
|
for (size_t i = 0; i < numOfAnchors; i++)
|
|
{
|
|
size_t offset = 1 + i * 7;
|
|
|
|
UwbDistance dist;
|
|
|
|
dist.nodeID = input[offset + 1] << 8 | input[offset];
|
|
dist.distance = input[offset + 5] << 24
|
|
| input[offset + 4] << 16
|
|
| input[offset + 3] << 8
|
|
| input[offset + 2];
|
|
dist.quali = input[offset + 6];
|
|
|
|
result.push_back(dist);
|
|
}
|
|
}
|
|
|
|
static UwbResult uwb_parse(const unsigned char input[])
|
|
{
|
|
UwbResult result;
|
|
|
|
if (input[0] == 0)
|
|
{
|
|
// pos only
|
|
uwb_parse_pos(&input[1], result.pos);
|
|
}
|
|
else if (input[0] == 1)
|
|
{
|
|
// dist only
|
|
uwb_parse_dist(&input[1], result.distances);
|
|
}
|
|
else if (input[0] == 2)
|
|
{
|
|
// Encoded Position (as above, 13 bytes)
|
|
// Encoded Distances(as above, 8 - 29 bytes).Position and distances
|
|
// are sent by tag, with a maximum number of 4 ranging anchors
|
|
uwb_parse_pos(&input[1], result.pos);
|
|
uwb_parse_dist(&input[1 + 13], result.distances);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
#endif // UWB_H
|