Closes #199: Use anchor position received from LPS

bitcraze · Feb 17, 2017 · b018879 · b018879
1 parent aa892ae
commit b018879
Show file tree

Hide file tree

Showing 5 changed files with 63 additions and 9 deletions.
diff --git a/src/deck/drivers/interface/locodeck.h b/src/deck/drivers/interface/locodeck.h
@@ -100,4 +100,18 @@ typedef struct {
 // Function to be used by the LPS algorithm
 bool lpsGetLppShort(lpsLppShortPacket_t* shortPacket);
 
+// Handle incoming short LPP packets from the UWB system
+void lpsHandleLppShortPacket(uint8_t srcId, uint8_t *data, int length);
+
+// LPP Packet types and format
+#define LPP_HEADER_SHORT_PACKET 0xF0
+
+#define LPP_SHORT_ANCHORPOS 0x01
+
+struct lppShortAnchorPos_s {
+  float x;
+  float y;
+  float z;
+} __attribute__((packed));
+
 #endif // __LOCODECK_H__
diff --git a/src/deck/drivers/interface/lpsTwrTag.h b/src/deck/drivers/interface/lpsTwrTag.h
@@ -15,8 +15,12 @@
 
 #define LPS_TWR_TYPE 0
 #define LPS_TWR_SEQ 1
+// LPP payload can be in the ANSWER packet
+#define LPS_TWR_LPP_HEADER 2
+#define LPS_TWR_LPP_TYPE 3
+#define LPS_TWR_LPP_PAYLOAD 4
 
-#define LPS_TWR_LPP_PAYLOAD 1
+#define LPS_TWR_SEND_LPP_PAYLOAD 1
 
 extern uwbAlgorithm_t uwbTwrTagAlgorithm;
 

diff --git a/src/deck/drivers/src/locodeck.c b/src/deck/drivers/src/locodeck.c
@@ -90,12 +90,12 @@ static lpsAlgoOptions_t algoOptions = {
   // To set a static anchor position from startup, uncomment and modify the
   // following code:
 //   .anchorPosition = {
-//     {timestamp: 4711, x: 0.99, y: 1.49, z: 1.80},
-//     {x: 0.99, y: 3.29, z: 1.80},
-//     {x: 4.67, y: 2.54, z: 1.80},
-//     {timestamp: 17, x: 0.59, y: 2.27, z: 0.20},
-//     {x: 4.70, y: 3.38, z: 0.20},
-//     {timestamp: 13, x: 4.70, y: 1.14, z: 0.20},
+//     {timestamp: 1, x: 0.99, y: 1.49, z: 1.80},
+//     {timestamp: 1, x: 0.99, y: 3.29, z: 1.80},
+//     {timestamp: 1, x: 4.67, y: 2.54, z: 1.80},
+//     {timestamp: 1, x: 0.59, y: 2.27, z: 0.20},
+//     {timestamp: 1, x: 4.70, y: 3.38, z: 0.20},
+//     {timestamp: 1, x: 4.70, y: 1.14, z: 0.20},
 //   },
 //
 //   .combinedAnchorPositionOk = true,
@@ -458,3 +458,21 @@ PARAM_ADD(PARAM_FLOAT, anchor7z, &algoOptions.anchorPosition[7].z)
 #endif
 PARAM_ADD(PARAM_UINT8, enable, &algoOptions.combinedAnchorPositionOk)
 PARAM_GROUP_STOP(anchorpos)
+
+
+// Loco Posisioning Protocol (LPP) handling
+
+void lpsHandleLppShortPacket(uint8_t srcId, uint8_t *data, int length)
+{
+  uint8_t type = data[0];
+
+  if (type == LPP_SHORT_ANCHORPOS) {
+    if (srcId < LOCODECK_NR_OF_ANCHORS) {
+      struct lppShortAnchorPos_s *newpos = (struct lppShortAnchorPos_s*)&data[1];
+      algoOptions.anchorPosition[srcId].timestamp = xTaskGetTickCount();
+      algoOptions.anchorPosition[srcId].x = newpos->x;
+      algoOptions.anchorPosition[srcId].y = newpos->y;
+      algoOptions.anchorPosition[srcId].z = newpos->z;
+    }
+  }
+}
diff --git a/src/deck/drivers/src/lpsTwrTag.c b/src/deck/drivers/src/lpsTwrTag.c
@@ -123,6 +123,24 @@ static uint32_t rxcallback(dwDevice_t *dev) {
         return 0;
       }
 
+      if (dataLength - MAC802154_HEADER_LENGTH > 3) {
+        if (rxPacket.payload[LPS_TWR_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_TWR_LPP_TYPE],
+                                    dataLength - MAC802154_HEADER_LENGTH - 3);
+          }
+        }
+      }
+
       txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_FINAL;
       txPacket.payload[LPS_TWR_SEQ] = rxPacket.payload[LPS_TWR_SEQ];
 
@@ -221,7 +239,7 @@ void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
   dwIdle(dev);
 
   txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_LPP_SHORT;
-  memcpy(&txPacket.payload[LPS_TWR_LPP_PAYLOAD], packet->data, packet->length);
+  memcpy(&txPacket.payload[LPS_TWR_SEND_LPP_PAYLOAD], packet->data, packet->length);
 
   txPacket.sourceAddress = options->tagAddress;
   txPacket.destAddress = options->anchorAddress[packet->dest];

diff --git a/test/deck/drivers/src/TestLpsTwrTag.c b/test/deck/drivers/src/TestLpsTwrTag.c
@@ -293,7 +293,7 @@ static void populateLppPacket(packet_t* packet, char *data, int length, locoAddr
 
   MAC80215_PACKET_INIT((*packet), MAC802154_TYPE_DATA);
   packet->pan = 0xbccf;
-  memcpy(&packet->payload[LPS_TWR_LPP_PAYLOAD], data, length);
+  memcpy(&packet->payload[LPS_TWR_SEND_LPP_PAYLOAD], data, length);
   packet->payload[LPS_TWR_TYPE] = LPS_TWR_LPP_SHORT;
   packet->sourceAddress = sourceAddress;
   packet->destAddress = destinationAddress;