#251: Implement timeout for sending LPP packets

src/deck/drivers/interface/lpsTdoaTag.h

@@ -26,5 +26,7 @@ typedef struct rangePacket_s {
 #define LPS_TDOA2_TYPE 0
 #define LPS_TDOA2_SEND_LPP_PAYLOAD 1
 
+#define TDOA2_LPP_PACKET_SEND_TIMEOUT (LOCODECK_NR_OF_ANCHORS * 5)
+
 
 #endif // __LPS_TDOA_TAG_H__

src/deck/drivers/src/lpsTdoaTag.c

@@ -57,6 +57,7 @@ static uint32_t anchorStatusTimeout[LOCODECK_NR_OF_TDOA2_ANCHORS];
 // LPP packet handling
 lpsLppShortPacket_t lppPacket;
 bool lppPacketToSend;
+int lppPacketSendTryCounter;
 
 // Log data
 static float logUwbTdoaDistDiff[LOCODECK_NR_OF_TDOA2_ANCHORS];
@@ -288,11 +289,17 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
         lppPacketToSend = false;
       } else {
         setRadioInReceiveMode(dev);
+
+        // Discard lpp packet if we cannot send it for too long
+        if (++lppPacketSendTryCounter >= TDOA2_LPP_PACKET_SEND_TIMEOUT) {
+          lppPacketToSend = false;
+        }
       }
 
       if (!lppPacketToSend) {
         // Get next lpp packet
         lppPacketToSend = lpsGetLppShort(&lppPacket);
+        lppPacketSendTryCounter = 0;
       }
       break;
     case eventTimeout:
@@ -347,6 +354,8 @@ static void Initialize(dwDevice_t *dev, lpsAlgoOptions_t* algoOptions) {
 
   previousAnchor = 0;
 
+  lppPacketToSend = false;
+
   memset(logUwbTdoaDistDiff, 0, sizeof(logUwbTdoaDistDiff));
 
   options->rangingState = 0;

test/deck/drivers/src/TestLpsTdoaTag.c

@@ -83,6 +83,8 @@ static int lppShortPacketLength = 5;
 static int lppShortPacketDest = 3;
 static int lppShortPacketSource = 0xff;
 
+static int lpsGetLppShort_numberOfCall;
+
 // End stock test case
 
 #define SLOT_TIME 0.002
@@ -754,22 +756,31 @@ void testThatLppShortPacketIsNotSentToWrongAnchorWhenAvailable() {
     (uint8_t[]) {0, 0, 0,  0,  0,  0,  0,  0},
     (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS},
     (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS});
+  mockMessageFromAnchor(lppShortPacketDest+1, NS,
+    (uint8_t[]) {0, 0, 0,  0,  0,  0,  0,  0},
+    (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS},
+    (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS});
 
   ignoreKalmanEstimatorValidation();
 
   packet_t expectedTxPacket;
   populateLppPacket(&expectedTxPacket, lppShortPacketData, lppShortPacketLength, 0xbccf000000000000 | lppShortPacketDest, 0xbccf000000000000 | lppShortPacketSource);
 
   // Test
-  uint32_t actual = uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
+  uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
+  uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
 
   // Assert
-  TEST_ASSERT_EQUAL_UINT32(MAX_TIMEOUT, actual);
+  // Nothing here, verification in mocks
 }
 
 void testThatLppShortPacketIsSentToGoodAnchorWhenAvailable() {
   // Fixture
   // mockRadioSetToReceiveMode() called as part of mockMessageFromAnchor()
+  mockMessageFromAnchor(lppShortPacketDest, NS,
+    (uint8_t[]) {0, 0, 0,  0,  0,  0,  0,  0},
+    (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS},
+    (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS});
   mockMessageFromAnchorNotComingBackToReceive(lppShortPacketDest, NS,
     (uint8_t[]) {0, 0, 0,  0,  0,  0,  0,  0},
     (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS},
@@ -783,10 +794,37 @@ void testThatLppShortPacketIsSentToGoodAnchorWhenAvailable() {
   mockSendLppShortHandling(&expectedTxPacket, lppShortPacketLength);
 
   // Test
-  uint32_t actual = uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
+  uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
+  uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
 
   // Assert
-  TEST_ASSERT_EQUAL_UINT32(MAX_TIMEOUT, actual);
+  // Nothing here, verification in mocks
+}
+
+void testThatLppShortPacketIsDiscardedIfAnchorNotPresentForTooLong() {
+  // Fixture
+  // mockRadioSetToReceiveMode() called as part of mockMessageFromAnchor()
+  for (int i=0; i<TDOA2_LPP_PACKET_SEND_TIMEOUT+1; i++) {
+    mockMessageFromAnchor(lppShortPacketDest+1, NS,
+      (uint8_t[]) {0, 0, 0,  0,  0,  0,  0,  0},
+      (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS},
+      (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS});
+  }
+
+  ignoreKalmanEstimatorValidation();
+
+  packet_t expectedTxPacket;
+  populateLppPacket(&expectedTxPacket, lppShortPacketData, lppShortPacketLength, 0xbccf000000000000 | lppShortPacketDest, 0xbccf000000000000 | lppShortPacketSource);
+
+  lpsGetLppShort_numberOfCall =  0;
+
+  // Test
+  for (int i=0; i<TDOA2_LPP_PACKET_SEND_TIMEOUT+1; i++) {
+    uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
+  }
+
+  // Assert
+  TEST_ASSERT_EQUAL_INT(2, lpsGetLppShort_numberOfCall);
 }
 
 void testDifferenceOfDistancePushedInKalmanIfAnchorsPositionIsValid() {
@@ -826,8 +864,6 @@ void testDifferenceOfDistanceNotPushedInKalmanIfAnchorsPositionIsInValid() {
   // Distance from A0 to tag is 2.0m
   // Distance from A1 to tag is 2.5m
 
-  float expectedDiff = 0.5;
-
   // Ideal times in universal clock
   uint64_t timeA0ToTag = time2m;
   uint64_t timeA1ToTag = time2_5m;
@@ -1147,6 +1183,8 @@ static void populateLppPacket(packet_t* packet, char *data, int length, locoAddr
 }
 
 static bool lpsGetLppShortCallbackForLppShortPacketSent(lpsLppShortPacket_t* shortPacket, int cmock_num_calls) {
+  lpsGetLppShort_numberOfCall++;
+
   if (lpsGetLppShort_ignoreAndReturnFalse) {
     return false;
   } else {

test/testSupport/dw1000Mocks.c

@@ -7,7 +7,7 @@
 
 // dwGetData mock /////////////////////////////////////////////////////////////
 
-#define DW_GET_DATA_MAX_CALLS 10
+#define DW_GET_DATA_MAX_CALLS 100
 
 static struct {
   dwDevice_t* expectedDev;
@@ -43,7 +43,7 @@ void dwGetData_resetMock() {
 
 // dwGetTransmitTimestamp mock ////////////////////////////////////////////////
 
-#define DW_GET_TRANSMIT_TIMESTAMP_MAX_CALLS 10
+#define DW_GET_TRANSMIT_TIMESTAMP_MAX_CALLS 100
 
 static struct {
   dwDevice_t* expectedDev;
@@ -75,7 +75,7 @@ void dwGetTransmitTimestamp_resetMock() {
 
 // dwGetReceiveTimestamp mock /////////////////////////////////////////////////
 
-#define DW_GET_RECEIVE_TIMESTAMP_MAX_CALLS 10
+#define DW_GET_RECEIVE_TIMESTAMP_MAX_CALLS 100
 
 static struct {
   dwDevice_t* expectedDev;