Added Android UWB code

uwb.h

@@ -0,0 +1,99 @@
+#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