Closes #197: Implement LPS LPP short packet CRTP tunneling

src/deck/drivers/interface/locodeck.h

@@ -33,6 +33,9 @@
 #ifndef __LOCODECK_H__
 #define __LOCODECK_H__
 
+#include <stddef.h>
+#include <stdint.h>
+
 #include "libdw1000.h"
 #include "stabilizer_types.h"
 
@@ -81,4 +84,20 @@ typedef struct uwbAlgorithm_s {
 
 #define MAX_TIMEOUT portMAX_DELAY
 
+// Send a short configuration packet to the LPS system
+// Returns true if packet will be send, false instead
+bool lpsSendLppShort(uint8_t destId, void* data, size_t length);
+
+typedef struct {
+  uint8_t dest;
+  uint8_t length;
+  uint8_t data[30];
+} lpsLppShortPacket_t;
+
+// Poll if there is a LPS short configuration packet to send
+// Return true if the packet data has been filled in shortPacket
+// Return false if no packet to send
+// Function to be used by the LPS algorithm
+bool lpsGetLppShort(lpsLppShortPacket_t* shortPacket);
+
 #endif // __LOCODECK_H__

src/deck/drivers/interface/lpsTwrTag.h

@@ -11,9 +11,13 @@
 #define LPS_TWR_FINAL 0x03
 #define LPS_TWR_REPORT 0x04 // Report contains all measurement from the anchor
 
+#define LPS_TWR_LPP_SHORT 0xF0
+
 #define LPS_TWR_TYPE 0
 #define LPS_TWR_SEQ 1
 
+#define LPS_TWR_LPP_PAYLOAD 1
+
 extern uwbAlgorithm_t uwbTwrTagAlgorithm;
 
 typedef struct {
@@ -28,4 +32,4 @@ typedef struct {
 } __attribute__((packed)) lpsTwrTagReportPayload_t;
 
 
-#endif // __LPS_TWR_TAG_H__
+#endif // __LPS_TWR_TAG_H__

src/deck/drivers/src/locodeck.c

@@ -39,6 +39,7 @@
 #include "FreeRTOS.h"
 #include "semphr.h"
 #include "task.h"
+#include "queue.h"
 
 #include "deck.h"
 #include "system.h"
@@ -117,6 +118,8 @@ static SemaphoreHandle_t irqSemaphore;
 static dwDevice_t dwm_device;
 static dwDevice_t *dwm = &dwm_device;
 
+static QueueHandle_t lppShortQueue;
+
 static uint32_t timeout;
 
 static void txCallback(dwDevice_t *dev)
@@ -140,6 +143,8 @@ static void rxTimeoutCallback(dwDevice_t * dev) {
 
 static void uwbTask(void* parameters)
 {
+  lppShortQueue = xQueueCreate(10, sizeof(lpsLppShortPacket_t));
+
   systemWaitStart();
 
   algorithm->init(dwm, &algoOptions);
@@ -155,6 +160,21 @@ static void uwbTask(void* parameters)
   }
 }
 
+static lpsLppShortPacket_t lppShortPacket;
+
+bool lpsSendLppShort(uint8_t destId, void* data, size_t length)
+{
+  lppShortPacket.dest = destId;
+  lppShortPacket.length = length;
+  memcpy(lppShortPacket.data, data, length);
+  return xQueueSend(lppShortQueue, &lppShortPacket,0) == pdPASS;
+}
+
+bool lpsGetLppShort(lpsLppShortPacket_t* shortPacket)
+{
+  return xQueueReceive(lppShortQueue, shortPacket, 0) == pdPASS;
+}
+
 static uint8_t spiTxBuffer[196];
 static uint8_t spiRxBuffer[196];
 

src/deck/drivers/src/lpsTwrTag.c

@@ -72,6 +72,9 @@ static volatile uint8_t curr_seq = 0;
 static int current_anchor = 0;
 
 static bool ranging_complete = false;
+static bool lpp_transaction = false;
+
+static lpsLppShortPacket_t lppShortPacket;
 
 static lpsAlgoOptions_t* options;
 
@@ -213,6 +216,24 @@ void initiateRanging(dwDevice_t *dev)
   dwStartTransmit(dev);
 }
 
+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);
+
+  txPacket.sourceAddress = options->tagAddress;
+  txPacket.destAddress = options->anchorAddress[packet->dest];
+
+  dwNewTransmit(dev);
+  dwSetDefaults(dev);
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
+
+  dwWaitForResponse(dev, false);
+  dwStartTransmit(dev);
+}
+
 static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
 {
   static uint32_t statisticStartTick = 0;
@@ -227,10 +248,14 @@ static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
       break;
     case eventPacketSent:
       txcallback(dev);
+
+      if (lpp_transaction) {
+        return 0;
+      }
       return MAX_TIMEOUT;
       break;
     case eventTimeout:  // Comes back to timeout after each ranging attempt
-      if (!ranging_complete) {
+      if (!ranging_complete && !lpp_transaction) {
         options->rangingState &= ~(1<<current_anchor);
         if (options->failedRanging[current_anchor] < options->rangingFailedThreshold) {
           options->failedRanging[current_anchor] ++;
@@ -265,8 +290,14 @@ static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
       }
 
 
-      ranging_complete = false;
-      initiateRanging(dev);
+      if (lpsGetLppShort(&lppShortPacket)) {
+        lpp_transaction = true;
+        sendLppShort(dev, &lppShortPacket);
+      } else {
+        lpp_transaction = false;
+        ranging_complete = false;
+        initiateRanging(dev);
+      }
       return MAX_TIMEOUT;
       break;
     case eventReceiveTimeout:

src/modules/interface/crtp_localization_service.h

@@ -40,8 +40,9 @@ struct CrtpExtPosition
 
 typedef enum
 {
-  RANGE_STREAM_FLOAT = 0,
-  RANGE_STREAM_FP16  = 1,
+  RANGE_STREAM_FLOAT   = 0,
+  RANGE_STREAM_FP16    = 1,
+  LPS_SHORT_LPP_PACKET = 2,
 } locsrv_t;
 
 // Set up the callback for the CRTP_PORT_LOCALIZATION

src/modules/src/crtp_localization_service.c

@@ -33,6 +33,7 @@
 #include "crtp_localization_service.h"
 #include "log.h"
 #include "param.h"
+#include "locodeck.h"
 
 #ifdef ESTIMATOR_TYPE_kalman
 #include "estimator_kalman.h"
@@ -76,6 +77,7 @@ static bool isInit = false;
 
 static void locSrvCrtpCB(CRTPPacket* pk);
 static void extPositionHandler(CRTPPacket* pk);
+static void genericLocHandle(CRTPPacket* pk);
 
 void locSrvInit()
 {
@@ -95,7 +97,7 @@ static void locSrvCrtpCB(CRTPPacket* pk)
       extPositionHandler(pk);
       break;
     case GENERIC_TYPE:
-      // TODO: Implement
+      genericLocHandle(pk);
     default:
       break;
   }
@@ -108,6 +110,21 @@ static void extPositionHandler(CRTPPacket* pk)
   crtpExtPosCache.timestamp = xTaskGetTickCount();
 }
 
+static void genericLocHandle(CRTPPacket* pk)
+{
+  uint8_t type = pk->data[0];
+  if (pk->size < 1) return;
+
+  if (type == LPS_SHORT_LPP_PACKET && pk->size >= 2) {
+    bool success = lpsSendLppShort(pk->data[1], &pk->data[2], pk->size-2);
+
+    pk->port = CRTP_PORT_LOCALIZATION;
+    pk->channel = GENERIC_TYPE;
+    pk->size = 3;
+    pk->data[2] = success?1:0;
+    crtpSendPacket(pk);
+  }
+}
 
 bool getExtPosition(state_t *state)
 {