TDoA2 #17: Add LPP anchor position to packets

inc/lpp.h

@@ -30,6 +30,8 @@
 
 void lppHandleShortPacket(char *data, size_t length);
 
+#define SHORT_LPP 0xF0
+
 #define LPP_SHORT_ANCHOR_POSITION 0x01
 #define LPP_SHORT_REBOOT 0x02
 

inc/mac.h

@@ -5,7 +5,7 @@
 
 
 // Packet format with compressed PAN and 64Bit addresses
-// Maximum 64 bytes payload
+// Maximum 128s bytes payload
 typedef struct packet_s {
     union {
       uint16_t fcf;
@@ -27,7 +27,7 @@ typedef struct packet_s {
     uint8_t destAddress[8];
     uint8_t sourceAddress[8];
 
-    uint8_t payload[64];
+    uint8_t payload[128];
 } __attribute__((packed)) packet_t;
 
 #define MAC80215_PACKET_INIT(packet, TYPE) packet.fcf_s.type = (TYPE); \

src/uwb_tdoa_anchor2.c

@@ -55,6 +55,9 @@
 #include "libdw1000.h"
 #include "mac.h"
 
+#include "cfg.h"
+#include "lpp.h"
+
 // Still using modulo 2 calculation for slots
 // TODO: If A0 is the TDMA master it could transmit slots parameters and frame
 //       start so that we would not be limited to modulo 2 anymore
@@ -77,7 +80,7 @@
 #define TDMA_GUARD_LENGTH (uint64_t)( TDMA_GUARD_LENGTH_S * 499.2e6 * 128 )
 
 // Timeout for receiving a packet in a timeslot
-#define RECEIVE_TIMEOUT 250
+#define RECEIVE_TIMEOUT 300
 
 #define TS_TX_SIZE 4
 
@@ -130,6 +133,10 @@ typedef struct rangePacket_s {
   uint16_t distances[NSLOTS];
 } __attribute__((packed)) rangePacket_t;
 
+#define LPP_HEADER (sizeof(rangePacket_t))
+#define LPP_TYPE (sizeof(rangePacket_t)+1)
+#define LPP_PAYLOAD (sizeof(rangePacket_t)+2)
+
 /* Adjust time for schedule transfer by DW1000 radio. Set 9 LSB to 0 */
 static uint32_t adjustTxRxTime(dwTime_t *time)
 {
@@ -249,6 +256,7 @@ static void setTxData(dwDevice_t *dev)
 {
   static packet_t txPacket;
   static bool firstEntry = true;
+  static int lppLength = 0;
 
   if (firstEntry) {
 
@@ -259,6 +267,19 @@ static void setTxData(dwDevice_t *dev)
 
     txPacket.payload[0] = PACKET_TYPE_TDOA2;
 
+    uwbConfig_t *uwbConfig = uwbGetConfig();
+
+    // LPP anchor position is currently sent in all packets
+    if (uwbConfig->positionEnabled) {
+      txPacket.payload[LPP_HEADER] = SHORT_LPP;
+      txPacket.payload[LPP_TYPE] = LPP_SHORT_ANCHOR_POSITION;
+
+      struct lppShortAnchorPosition_s *pos = (struct lppShortAnchorPosition_s*) &txPacket.payload[LPP_PAYLOAD];
+      memcpy(pos->position, uwbConfig->position, 3*sizeof(float));
+
+      lppLength = 2 + sizeof(struct lppShortAnchorPosition_s);
+    }
+
     firstEntry = false;
   }
 
@@ -271,7 +292,7 @@ static void setTxData(dwDevice_t *dev)
   memcpy(rangePacket->timestamps[ctx.anchorId], &ctx.txTimestamps[ctx.anchorId], TS_TX_SIZE);
   memcpy(rangePacket->distances, ctx.distances, sizeof(ctx.distances));
 
-  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH + sizeof(rangePacket_t));
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH + sizeof(rangePacket_t) + lppLength);
 }
 
 // Setup the radio to send a packet in the next timeslot

src/uwb_twr_anchor.c

@@ -50,9 +50,6 @@ static struct uwbConfig_s config;
 #define FINAL 0x03
 #define REPORT 0x04 // Report contains all measurement from the anchor
 
-// LPP packets
-#define SHORT_LPP 0xF0
-
 typedef struct {
   uint8_t pollRx[5];
   uint8_t answerTx[5];

tools/sniffer/tdoa2_decoder.py

@@ -25,12 +25,13 @@
     packetType = packet["data"][0]
 
     if packetType == 34:
-        decoded = struct.unpack("<BBBBBBBBBLLLLLLLLHHHHHHHH", packet["data"])
+        decoded = struct.unpack("<BBBBBBBBBLLLLLLLLHHHHHHHH", packet["data"][:57])
         packet["type"] = decoded[0]
-        if packet["type"] == 34:
-            packet["seqs"] = list(decoded[1:9])
-            packet["timestamps"] = list(decoded[9:17])
-            packet["distances"] = list(decoded[17:25])
+        packet["seqs"] = list(decoded[1:9])
+        packet["timestamps"] = list(decoded[9:17])
+        packet["distances"] = list(decoded[17:25])
+        if len(packet["data"]) > 57:
+            packet["lpp_data"] = packet["data"][57:]
 
     print("---")
     print(yaml.dump(packet, Dumper=yaml.CDumper))