#251 Added LPP packet handling

bitcraze · Dec 4, 2017 · 0a45dad · 0a45dad
1 parent 1e05b9e
commit 0a45dad
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 2 deletions.
diff --git a/src/deck/drivers/interface/locodeck.h b/src/deck/drivers/interface/locodeck.h
@@ -118,7 +118,7 @@ typedef struct {
 bool lpsGetLppShort(lpsLppShortPacket_t* shortPacket);
 
 // Handle incoming short LPP packets from the UWB system
-void lpsHandleLppShortPacket(uint8_t srcId, uint8_t *data, int length);
+void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data, const int length);
 
 // LPP Packet types and format
 #define LPP_HEADER_SHORT_PACKET 0xF0

diff --git a/src/deck/drivers/interface/lpsTdoaTag.h b/src/deck/drivers/interface/lpsTdoaTag.h
@@ -17,4 +17,9 @@ typedef struct rangePacket_s {
   uint16_t distances[LOCODECK_NR_OF_TDOA2_ANCHORS];
 } __attribute__((packed)) rangePacket_t;
 
+#define LPS_TDOA_LPP_HEADER (sizeof(rangePacket_t))
+#define LPS_TDOA_LPP_TYPE (sizeof(rangePacket_t) + 1)
+#define LPS_TDOA_LPP_PAYLOAD (sizeof(rangePacket_t) + 2)
+
+
 #endif // __LPS_TDOA_TAG_H__
diff --git a/src/deck/drivers/src/locodeck.c b/src/deck/drivers/src/locodeck.c
@@ -524,7 +524,7 @@ PARAM_GROUP_STOP(deck)
 
 // Loco Posisioning Protocol (LPP) handling
 
-void lpsHandleLppShortPacket(uint8_t srcId, uint8_t *data, int length)
+void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data, const int length)
 {
   uint8_t type = data[0];
 

diff --git a/src/deck/drivers/src/lpsTdoaTag.c b/src/deck/drivers/src/lpsTdoaTag.c
@@ -171,6 +171,26 @@ static void addToLog(const uint8_t anchor, const uint8_t previousAnchor, const f
   }
 }
 
+static void handleLppPacket(const int dataLength, const packet_t* rxPacket) {
+  if (dataLength - MAC802154_HEADER_LENGTH > (int)LPS_TDOA_LPP_HEADER) {
+    if (rxPacket->payload[LPS_TDOA_LPP_HEADER] == LPP_HEADER_SHORT_PACKET) {
+      int srcId = -1;
+
+      for (int i=0; i<LOCODECK_NR_OF_ANCHORS; i++) {
+        if (rxPacket->sourceAddress == options->anchorAddress[i]) {
+          srcId = i;
+          break;
+        }
+      }
+
+      if (srcId >= 0) {
+        lpsHandleLppShortPacket(srcId, &rxPacket->payload[LPS_TDOA_LPP_TYPE],
+          dataLength - MAC802154_HEADER_LENGTH - LPS_TDOA_LPP_HEADER);
+      }
+    }
+  }
+}
+
 static void rxcallback(dwDevice_t *dev) {
   statsPacketsReceived++;
 
@@ -205,6 +225,8 @@ static void rxcallback(dwDevice_t *dev) {
     anchorStatusTimeout[anchor] = xTaskGetTickCount() + ANCHOR_OK_TIMEOUT;
 
     previousAnchor = anchor;
+
+    handleLppPacket(dataLength, &rxPacket);
   }
 }