From 8d8695cb60a0485989a524c666bc6b3f01d677a7 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 8 May 2018 11:13:04 +0200
Subject: [PATCH] #27 Corrected packet length and added parsing of position
 data in the sniffer

---
 src/uwb_tdoa_anchor3.c         |  2 +-
 tools/sniffer/tdoa3_decoder.py | 32 ++++++++++++++++++++++++++++----
 2 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/src/uwb_tdoa_anchor3.c b/src/uwb_tdoa_anchor3.c
index 8cd621a..1630d18 100644
--- a/src/uwb_tdoa_anchor3.c
+++ b/src/uwb_tdoa_anchor3.c
@@ -112,7 +112,7 @@ static struct ctx_s {
 
 // Packet formats
 #define PACKET_TYPE_TDOA3 0x30
-#define TDOA_3_REMOTE_COUNT_MAX 8
+#define TDOA_3_REMOTE_COUNT_MAX 7
 
 typedef struct {
   uint8_t type;
diff --git a/tools/sniffer/tdoa3_decoder.py b/tools/sniffer/tdoa3_decoder.py
index 31177ba..fdaa863 100755
--- a/tools/sniffer/tdoa3_decoder.py
+++ b/tools/sniffer/tdoa3_decoder.py
@@ -49,7 +49,9 @@
         anchor_data_index = 7
 
         for i in range(remote_count):
-            decoded_anchor_data = struct.unpack('<BBL', packet["data"][anchor_data_index:(anchor_data_index + 6)])
+            decoded_anchor_data = struct.unpack(
+                '<BBL',
+                packet["data"][anchor_data_index:(anchor_data_index + 6)])
             seq = decoded_anchor_data[1] & 0x7f
             anchor_data = {
                 'id': decoded_anchor_data[0],
@@ -60,14 +62,36 @@
 
             has_distance = ((decoded_anchor_data[1] & 0x80) != 0)
             if (has_distance):
-                decoded_distance = struct.unpack('<H', packet["data"][anchor_data_index:(anchor_data_index + 2)])
+                decoded_distance = struct.unpack(
+                    '<H',
+                    packet["data"][anchor_data_index:(anchor_data_index + 2)])
                 anchor_data['distance'] = decoded_distance[0]
                 anchor_data_index += 2
 
             packet['remoteAnchorData'].append(anchor_data)
 
-        if len(packet["data"]) > 57:
-            packet["lpp_data"] = packet["data"][57:]
+        if len(packet["data"]) > anchor_data_index:
+            if packet["data"][anchor_data_index] == 0xf0 and \
+                    packet["data"][anchor_data_index + 1] == 0x01:
+                packet["lpp_data"] = {}
+                packet["lpp_data"]['header'] = packet["data"][
+                    anchor_data_index]
+                anchor_data_index += 1
+
+                packet["lpp_data"]['type'] = packet["data"][anchor_data_index]
+                anchor_data_index += 1
+
+                decoded = struct.unpack(
+                    "<fff",
+                    packet["data"][anchor_data_index:anchor_data_index + 3 * 6]
+                )
+                packet["lpp_data"]['position'] = {}
+                packet["lpp_data"]['position']['x'] = decoded[0]
+                packet["lpp_data"]['position']['y'] = decoded[1]
+                packet["lpp_data"]['position']['z'] = decoded[2]
+                anchor_data_index += 3 * 6
+            else:
+                packet["lpp_data"] = packet["data"][anchor_data_index:]
 
     print("---")
     print(yaml.dump(packet, Dumper=yaml.CDumper))