Added logging of TDOA data

src/deck/drivers/src/locodeck.c

@@ -48,11 +48,22 @@
 #include "nvicconf.h"
 
 #include "locodeck.h"
-#include "lpsTwrTag.h"
+
+#if LPS_TDOA_ENABLE
+  #include "lpsTdoaTag.h"
+#else
+  #include "lpsTwrTag.h"
+#endif
+
 
 #define CS_PIN DECK_GPIO_IO1
 
-#define RX_TIMEOUT 1000
+#if LPS_TDOA_ENABLE
+  #define RX_TIMEOUT 10000
+#else
+  #define RX_TIMEOUT 1000
+#endif
+
 
 #define ANTENNA_OFFSET 154.6   // In meter
 
@@ -72,7 +83,7 @@ static lpsAlgoOptions_t algoOptions = {
   },
   .antennaDelay = (ANTENNA_OFFSET*499.2e6*128)/299792458.0, // In radio tick
   .rangingFailedThreshold = 6,
-
+ 
   .anchorPositionOk = false,
 
   // To set a static anchor position from startup, uncomment and modify the
@@ -90,7 +101,11 @@ static lpsAlgoOptions_t algoOptions = {
 */
 };
 
+#if LPS_TDOA_ENABLE
+static uwbAlgorithm_t *algorithm = &uwbTdoaTagAlgorithm;
+#else
 static uwbAlgorithm_t *algorithm = &uwbTwrTagAlgorithm;
+#endif
 
 static bool isInit = false;
 static xSemaphoreHandle spiSemaphore;

src/deck/drivers/src/lpsTdoaTag.c

@@ -26,6 +26,7 @@
 
 #include <string.h>
 
+#include "log.h"
 #include "lpsTdoaTag.h"
 
 #include "stabilizer_types.h"
@@ -50,7 +51,7 @@ static uint64_t dwTimestampToUint64(uint8_t *ts) {
   return timestamp.full;
 }
 
-static uint32_t rxcallback(dwDevice_t *dev) {
+static void rxcallback(dwDevice_t *dev) {
   int dataLength = dwGetDataLength(dev);
   packet_t rxPacket;
 
@@ -59,7 +60,7 @@ static uint32_t rxcallback(dwDevice_t *dev) {
   dwTime_t arrival = {.full = 0};
   dwGetReceiveTimestamp(dev, &arrival);
 
-  locoAddress_t anchor = rxPacket.sourceAddress;
+  uint8_t anchor = rxPacket.sourceAddress & 0xff;
 
   if (anchor < LOCODECK_NR_OF_ANCHORS) {
     rangePacket_t* packet = (rangePacket_t*)rxPacket.payload;
@@ -69,14 +70,14 @@ static uint32_t rxcallback(dwDevice_t *dev) {
       double frameTimeInLocalClock = arrival.full - arrivals[MASTER].full;
 
       localClockCorrection = 1.0;
-      if (frameTimeInLocalClock != 0) {
+      if (frameTimeInLocalClock != 0.0) {
         localClockCorrection = frameTimeInMasterClock / frameTimeInLocalClock;
       }
     } else {
       double frameTimeInAnchorClock = dwTimestampToUint64(packet->timestamps[MASTER]) - dwTimestampToUint64(rxPacketBuffer[anchor].timestamps[MASTER]);
 
       double anchorClockCorrection = 1.0;
-      if (frameTimeInAnchorClock != 0) {
+      if (frameTimeInAnchorClock != 0.0) {
         anchorClockCorrection = frameTimeInMasterClock / frameTimeInAnchorClock;
       }
 
@@ -99,16 +100,25 @@ static uint32_t rxcallback(dwDevice_t *dev) {
     arrivals[anchor].full = arrival.full;
     memcpy(&rxPacketBuffer[anchor], rxPacket.payload, sizeof(rangePacket_t));
   }
-
-
-  return 0;
 }
 
+static void setRadioInReceiveMode(dwDevice_t *dev) {
+  dwNewReceive(dev);
+  dwSetDefaults(dev);
+  dwStartReceive(dev);
+}
 
 static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
   switch(event) {
     case eventPacketReceived:
-      return rxcallback(dev);
+      rxcallback(dev);
+      setRadioInReceiveMode(dev);
+      break;
+    case eventTimeout:
+      setRadioInReceiveMode(dev);
+      break;
+    case eventReceiveTimeout:
+      setRadioInReceiveMode(dev);
       break;
     default:
       ASSERT_FAILED();
@@ -125,3 +135,14 @@ uwbAlgorithm_t uwbTdoaTagAlgorithm = {
   .init = Initialize,
   .onEvent = onEvent,
 };
+
+
+LOG_GROUP_START(tdoa)
+LOG_ADD(LOG_FLOAT, d01, &uwbTdoaDistDiff[1])
+LOG_ADD(LOG_FLOAT, d02, &uwbTdoaDistDiff[2])
+LOG_ADD(LOG_FLOAT, d03, &uwbTdoaDistDiff[3])
+LOG_ADD(LOG_FLOAT, d04, &uwbTdoaDistDiff[4])
+LOG_ADD(LOG_FLOAT, d05, &uwbTdoaDistDiff[5])
+LOG_ADD(LOG_FLOAT, d06, &uwbTdoaDistDiff[6])
+LOG_ADD(LOG_FLOAT, d07, &uwbTdoaDistDiff[7])
+LOG_GROUP_STOP(tdoa)

test/deck/drivers/src/TestLpsTdoaTag.c

@@ -13,7 +13,9 @@ static lpsAlgoOptions_t options;
 static const uint64_t NS = 0;
 static const int dataLength = sizeof(packet_t);
 
-static void mockMessageFromAnchor(int anchorIndex, uint64_t rxTime, uint64_t t0, uint64_t t1, uint64_t t2, uint64_t t3, uint64_t t4, uint64_t t5, uint64_t t6, uint64_t t7);
+static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint64_t t0, uint64_t t1, uint64_t t2, uint64_t t3, uint64_t t4, uint64_t t5, uint64_t t6, uint64_t t7);
+static void mockRadioSetToReceiveMode();
+
 static uint64_t drift(float factor, uint64_t time);
 
 void setUp(void) {
@@ -174,6 +176,41 @@ void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDrift() {
 }
 
 
+void testPacketReceivedEventShouldSetTheRadioInReceiveMode() {
+  // Fixture
+  // mockRadioSetToReceiveMode() called as part of mockMessageFromAnchor()
+  mockMessageFromAnchor(0, NS, NS, NS, NS, NS, NS, NS, NS, NS);
+
+  // Test
+  uint32_t actual = uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
+
+  // Assert
+  TEST_ASSERT_EQUAL_UINT32(MAX_TIMEOUT, actual);
+}
+
+
+void testEventTimeoutShouldSetTheRadioInReceiveMode() {
+  // Fixture
+  mockRadioSetToReceiveMode();
+
+  // Test
+  uint32_t actual = uwbTdoaTagAlgorithm.onEvent(&dev, eventTimeout);
+
+  // Assert
+  TEST_ASSERT_EQUAL_UINT32(MAX_TIMEOUT, actual);
+}
+
+void testEventReceiveTimeoutShouldSetTheRadioInReceiveMode() {
+  // Fixture
+  mockRadioSetToReceiveMode();
+
+  // Test
+  uint32_t actual = uwbTdoaTagAlgorithm.onEvent(&dev, eventReceiveTimeout);
+
+  // Assert
+  TEST_ASSERT_EQUAL_UINT32(MAX_TIMEOUT, actual);
+}
+
 ////////////////////////////////////////////
 
 static dwTime_t ts(uint64_t time) {
@@ -186,11 +223,11 @@ static void setTimestampInPayload(uint64_t time, uint8_t* dest) {
   memcpy(dest, timestamp.raw, 5);
 }
 
-static void mockMessageFromAnchor(int anchorIndex, uint64_t rxTime, uint64_t t0, uint64_t t1, uint64_t t2, uint64_t t3, uint64_t t4, uint64_t t5, uint64_t t6, uint64_t t7) {
+static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint64_t t0, uint64_t t1, uint64_t t2, uint64_t t3, uint64_t t4, uint64_t t5, uint64_t t6, uint64_t t7) {
   packet_t packet;
   MAC80215_PACKET_INIT(packet, MAC802154_TYPE_DATA);
 
-  packet.sourceAddress = anchorIndex;
+  packet.sourceAddress = 0xbccf000000000000 | anchorIndex;
 
   rangePacket_t* payload = (rangePacket_t*)&packet.payload;
   payload->type = 0x21;
@@ -208,8 +245,16 @@ static void mockMessageFromAnchor(int anchorIndex, uint64_t rxTime, uint64_t t0,
 
   dwTime_t rxTimeStr = ts(rxTime);
   dwGetReceiveTimestamp_ExpectAndCopyData(&dev, &rxTimeStr);
+
+  mockRadioSetToReceiveMode();
 }
 
 static uint64_t drift(float factor, uint64_t time) {
   return (uint64_t)((double)time * (double)factor);
+}
+
+static void mockRadioSetToReceiveMode() {
+  dwNewReceive_Expect(&dev);
+  dwSetDefaults_Expect(&dev);
+  dwStartReceive_Expect(&dev);
 }