diff --git a/src/deck/drivers/src/lpsTdoaTag.c b/src/deck/drivers/src/lpsTdoaTag.c index c5dbf70133..7cfb623150 100644 --- a/src/deck/drivers/src/lpsTdoaTag.c +++ b/src/deck/drivers/src/lpsTdoaTag.c @@ -43,6 +43,7 @@ static float uwbTdoaDistDiff[LOCODECK_NR_OF_ANCHORS]; static uint8_t previousAnchor; static rangePacket_t rxPacketBuffer[LOCODECK_NR_OF_ANCHORS]; static dwTime_t arrivals[LOCODECK_NR_OF_ANCHORS]; +static uint8_t sequenceNrs[LOCODECK_NR_OF_ANCHORS]; static double clockCorrection_T_To_A[LOCODECK_NR_OF_ANCHORS]; @@ -73,6 +74,13 @@ static void enqueueTDOA(uint8_t anchor1, uint8_t anchor2, double distanceDiff) { estimatorKalmanEnqueueTDOA(&tdoa); } +// The default receive time in the anchors for messages from other anchors is 0 +// and is overwritten with the actual receive time when a packet arrives. +// That is, if no message was received the rx time will be 0. +static bool isValidTimeStamp(const int64_t anchorRxTime) { + return anchorRxTime != 0; +} + // A note on variable names. They might seem a bit verbose but express quite a lot of information // We have three actors: Reference anchor (Ar), Anchor n (An) and the deck on the CF called Tag (T) // rxAr_by_An_in_cl_An should be interpreted as "The time when packet was received from the Reference Anchor by Anchor N expressed in the clock of Anchor N" @@ -80,38 +88,49 @@ static void enqueueTDOA(uint8_t anchor1, uint8_t anchor2, double distanceDiff) { static double calcClockCorrection(const uint8_t anchor, const rangePacket_t* packet, const dwTime_t* arrival) { const int64_t rxAn_by_T_in_cl_T = arrival->full; const int64_t txAn_in_cl_An = packet->timestamps[anchor]; - const int64_t previous_rxAn_by_T_in_cl_T = arrivals[anchor].full; - const int64_t previous_txAn_in_cl_An = rxPacketBuffer[anchor].timestamps[anchor]; - const double frameTime_in_T = truncateToLocalTimeStamp(rxAn_by_T_in_cl_T - previous_rxAn_by_T_in_cl_T); - const double frameTime_in_cl_An = truncateToAnchorTimeStamp(txAn_in_cl_An - previous_txAn_in_cl_An); + const int64_t previous_rxAn_by_T_in_cl_T = arrivals[anchor].full; - double clockCorrection = frameTime_in_cl_An / frameTime_in_T; - return clockCorrection; + // The first time we get here, previous_txAn_in_cl_An will not be set + const int64_t previous_txAn_in_cl_An = rxPacketBuffer[anchor].timestamps[anchor]; + if (isValidTimeStamp(previous_txAn_in_cl_An)) { + const double frameTime_in_T = truncateToLocalTimeStamp(rxAn_by_T_in_cl_T - previous_rxAn_by_T_in_cl_T); + const double frameTime_in_cl_An = truncateToAnchorTimeStamp(txAn_in_cl_An - previous_txAn_in_cl_An); + + double clockCorrection = frameTime_in_cl_An / frameTime_in_T; + return clockCorrection; + } else { + return 0.0; + } } - -// The default receive time in the anchors for messages from other anchors is 0 -// and is overwritten with the actual receive time when a packet arrives. -// That is, if no message was received the rx time will be 0. -static bool isValidRxTime(const int64_t anchorRxTime) { - return anchorRxTime != 0; +static bool isSeqNrConsecutive(uint8_t prevSeqNr, uint8_t currentSeqNr) { + return (currentSeqNr == ((prevSeqNr + 1) & 0xff)); } static bool calcDistanceDiff(float* tdoaDistDiff, const uint8_t previousAnchor, const uint8_t anchor, const rangePacket_t* packet, const dwTime_t* arrival) { const int64_t rxAn_by_T_in_cl_T = arrival->full; const int64_t rxAr_by_An_in_cl_An = packet->timestamps[previousAnchor]; const int64_t tof_Ar_to_An_in_cl_An = packet->distances[previousAnchor]; + const double clockCorrection = clockCorrection_T_To_A[anchor]; + + const bool isSeqNrInTagOk = isSeqNrConsecutive(rxPacketBuffer[anchor].sequenceNrs[previousAnchor], packet->sequenceNrs[previousAnchor]); + const bool isSeqNrInAnchorOk = isSeqNrConsecutive(sequenceNrs[anchor], packet->sequenceNrs[anchor]); + const bool isAnchorDistanceOk = isValidTimeStamp(tof_Ar_to_An_in_cl_An); + const bool isRxTimeInTagOk = isValidTimeStamp(rxAr_by_An_in_cl_An); + const bool isClockCorrectionOk = (clockCorrection != 0.0); - if (! isValidRxTime(rxAr_by_An_in_cl_An)) { + if (! (isSeqNrInTagOk && isSeqNrInAnchorOk && isAnchorDistanceOk && isRxTimeInTagOk && isClockCorrectionOk)) { return false; } const int64_t txAn_in_cl_An = packet->timestamps[anchor]; - const int64_t rxAr_by_T_in_cl_T = arrivals[previousAnchor].full; + const int64_t rxAr_by_T_in_cl_T = arrivals[previousAnchor].full; + const int64_t delta_txAr_to_txAn_in_cl_An = (tof_Ar_to_An_in_cl_An + truncateToAnchorTimeStamp(txAn_in_cl_An - rxAr_by_An_in_cl_An)); - const int64_t timeDiffOfArrival_in_cl_An = truncateToAnchorTimeStamp(rxAn_by_T_in_cl_T - rxAr_by_T_in_cl_T) * clockCorrection_T_To_A[anchor] - delta_txAr_to_txAn_in_cl_An; + const int64_t timeDiffOfArrival_in_cl_An = truncateToAnchorTimeStamp(rxAn_by_T_in_cl_T - rxAr_by_T_in_cl_T) * clockCorrection - delta_txAr_to_txAn_in_cl_An; *tdoaDistDiff = SPEED_OF_LIGHT * timeDiffOfArrival_in_cl_An / LOCODECK_TS_FREQ; + return true; } @@ -149,6 +168,7 @@ static void rxcallback(dwDevice_t *dev) { arrivals[anchor].full = arrival.full; memcpy(&rxPacketBuffer[anchor], rxPacket.payload, sizeof(rangePacket_t)); + sequenceNrs[anchor] = packet->sequenceNrs[anchor]; previousAnchor = anchor; } @@ -187,6 +207,7 @@ static void Initialize(dwDevice_t *dev, lpsAlgoOptions_t* algoOptions) { // Reset module state. Needed by unit tests memset(rxPacketBuffer, 0, sizeof(rxPacketBuffer)); memset(arrivals, 0, sizeof(arrivals)); + memset(sequenceNrs, 0, sizeof(sequenceNrs)); memset(clockCorrection_T_To_A, 0, sizeof(clockCorrection_T_To_A)); diff --git a/test/deck/drivers/src/TestLpsTdoaTag.c b/test/deck/drivers/src/TestLpsTdoaTag.c index cd5112e401..4df7d7b47c 100644 --- a/test/deck/drivers/src/TestLpsTdoaTag.c +++ b/test/deck/drivers/src/TestLpsTdoaTag.c @@ -83,6 +83,12 @@ const uint64_t iTxTime2_3 = ANCHOR_TIME(2, 3); const uint64_t iTxTime2_4 = ANCHOR_TIME(2, 4); const uint64_t iTxTime2_5 = ANCHOR_TIME(2, 5); +const uint64_t iTxTime3_0 = ANCHOR_TIME(3, 0); +const uint64_t iTxTime3_1 = ANCHOR_TIME(3, 1); +const uint64_t iTxTime3_2 = ANCHOR_TIME(3, 2); +const uint64_t iTxTime3_3 = ANCHOR_TIME(3, 3); +const uint64_t iTxTime3_4 = ANCHOR_TIME(3, 4); +const uint64_t iTxTime3_5 = ANCHOR_TIME(3, 5); void setUp(void) { dwGetData_resetMock(); @@ -302,46 +308,48 @@ void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDrift3() { // Nothing here, verification in mocks } -// TODO krri Replace the following tests with tests that verify -// * missing packets -// * sequence numbering of anchor to CF packets -// * sequence numbering of anchor to anchor packets -// * wrap of sequence numbers, anchor to CF packets -// * wrap of sequence numbers, anchor to anchor packets - -/* -void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageA5toA0() { +void testMissingTimestamInhibitsClockDriftCalculationInFirstIteration() { // Fixture // Two anchors, separated by 1.0m // Distance from A0 to tag is 2.0m - // Distance from other anchor to tag is 2.5m + // Distance from A5 to tag is 2.5m float expectedDiff = 0.5; - const int anchor = 5; // Ideal times in universal clock - uint64_t timeA0ToAn = 1.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; - uint64_t timeAnToTag = 2.5 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; + uint64_t timeA0ToTag = time2m; + uint64_t timeA5ToTag = time2_5m; + uint64_t timeA0ToA5 = time1m; - // Clock start offset including any antenna delay - uint64_t tO = 17 * LOCODECK_TS_FREQ; - uint64_t a0O = 60 * LOCODECK_TS_FREQ; - uint64_t anO = 138 * LOCODECK_TS_FREQ; + mockMessageFromAnchor(5, iTxTime0_5 + timeA5ToTag, + (uint8_t[]) {10, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){NS, NS, NS, NS, NS, iTxTime0_5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS}); + + mockMessageFromAnchor(0, iTxTime1_0 + timeA0ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){iTxTime1_0, NS, NS, NS, NS, iTxTime0_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); + + mockMessageFromAnchor(5, iTxTime1_5 + timeA5ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 21, 0, 0}, + (uint64_t[]){iTxTime1_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime1_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); - // Clock drifts - float aD = 1.000010; - float tD = 0.999995; + mockMessageFromAnchor(0, iTxTime2_0 + timeA0ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 21, 0, 0}, + (uint64_t[]){iTxTime2_0, NS, NS, NS, NS, iTxTime1_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); - // Message from A5 -> A0 lost - // A Arrival Time A0 A1 A2 A3 A4 A5 A6 A7 - mockMessageFromAnchor(0 , drift(tD, iTxTime0_0 + timeA0ToTag + tO), iTxTime0_0 + a0O , NS, NS, NS, NS, NS , NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime0_5 + timeAnToTag + tO), drift(aD, iTxTime0_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime0_5 + anO) , NS, NS); - mockMessageFromAnchor(0 , drift(tD, iTxTime1_0 + timeA0ToTag + tO), iTxTime1_0 + a0O , NS, NS, NS, NS, iTxTime0_5 + timeA0ToAn + a0O, NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime1_5 + timeAnToTag + tO), drift(aD, iTxTime1_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime1_5 + anO) , NS, NS); - mockMessageFromAnchor(0 , drift(tD, iTxTime2_0 + timeA0ToTag + tO), iTxTime2_0 + a0O , NS, NS, NS, NS, 0 , NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime2_5 + timeAnToTag + tO), drift(aD, iTxTime2_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime2_5 + anO) , NS, NS); + mockMessageFromAnchor(5, iTxTime2_5 + timeA5ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 22, 0, 0}, + (uint64_t[]){iTxTime2_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime2_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); - // Only message 4 will lead to a call to the estimator - mockKalmanEstimator(0, anchor, expectedDiff); + + // Only the three last messages will create calls to the estimator. The two first are discarded due to missing data. + mockKalmanEstimator(0, 5, expectedDiff); + mockKalmanEstimator(5, 0, -expectedDiff); + mockKalmanEstimator(0, 5, expectedDiff); // Test uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); @@ -349,42 +357,57 @@ void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageA5 uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); - uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); // Assert // Nothing here, verification in mocks } -void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageA0toA5() { +void testMissingPacketAnchorToAnchorInhibitsDiffCalculation() { // Fixture // Two anchors, separated by 1.0m // Distance from A0 to tag is 2.0m - // Distance from other anchor to tag is 2.5m - const int anchor = 5; + // Distance from A5 to tag is 2.5m + float expectedDiff = 0.5; // Ideal times in universal clock - uint64_t timeA0ToAn = 1.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; - uint64_t timeAnToTag = 2.5 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; + uint64_t timeA0ToTag = time2m; + uint64_t timeA5ToTag = time2_5m; + uint64_t timeA0ToA5 = time1m; - // Clock start offset including any antenna delay - uint64_t tO = 17 * LOCODECK_TS_FREQ; - uint64_t a0O = 60 * LOCODECK_TS_FREQ; - uint64_t anO = 138 * LOCODECK_TS_FREQ; + uint64_t missing = 0; + + mockMessageFromAnchor(5, iTxTime0_5 + timeA5ToTag, + (uint8_t[]) {10, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){NS, NS, NS, NS, NS, iTxTime0_5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS}); - // Clock drifts - float aD = 1.000010; - float tD = 0.999995; + mockMessageFromAnchor(0, iTxTime1_0 + timeA0ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){iTxTime1_0, NS, NS, NS, NS, iTxTime0_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); - // Message from A5 -> A0 lost - // A Arrival Time A0 A1 A2 A3 A4 A5 A6 A7 - mockMessageFromAnchor(0 , drift(tD, iTxTime0_0 + timeA0ToTag + tO), iTxTime0_0 + a0O , NS, NS, NS, NS, NS , NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime0_5 + timeAnToTag + tO), drift(aD, iTxTime0_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime0_5 + anO) , NS, NS); - mockMessageFromAnchor(0 , drift(tD, iTxTime1_0 + timeA0ToTag + tO), iTxTime1_0 + a0O , NS, NS, NS, NS, iTxTime0_5 + timeA0ToAn + a0O, NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime1_5 + timeAnToTag + tO), 0 , NS, NS, NS, NS, drift(aD, iTxTime1_5 + anO) , NS, NS); - mockMessageFromAnchor(0 , drift(tD, iTxTime2_0 + timeA0ToTag + tO), iTxTime2_0 + a0O , NS, NS, NS, NS, iTxTime1_5 + timeA0ToAn + a0O, NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime2_5 + timeAnToTag + tO), drift(aD, iTxTime2_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime2_5 + anO) , NS, NS); + mockMessageFromAnchor(5, iTxTime1_5 + timeA5ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 21, 0, 0}, + (uint64_t[]){iTxTime1_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime1_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); - // There are no calls to the estimator + // The timestamp is missing for A5 + mockMessageFromAnchor(0, iTxTime2_0 + timeA0ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 21, 0, 0}, + (uint64_t[]){iTxTime2_0, NS, NS, NS, NS, missing , NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); + + mockMessageFromAnchor(5, iTxTime2_5 + timeA5ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 22, 0, 0}, + (uint64_t[]){iTxTime2_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime2_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); + + + // Only the three last messages will create calls to the estimator. The two first are discarded due to missing data. + mockKalmanEstimator(0, 5, expectedDiff); + // Not called + // mockKalmanEstimator(5, 0, -expectedDiff); + mockKalmanEstimator(0, 5, expectedDiff); // Test uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); @@ -392,46 +415,61 @@ void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageA0 uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); - uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); // Assert // Nothing here, verification in mocks } -void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageNr3() { +void testMissingAnchorToAnchorDistanceInhibitsDiffCalculation() { // Fixture // Two anchors, separated by 1.0m // Distance from A0 to tag is 2.0m - // Distance from other anchor to tag is 2.5m - const int anchor = 5; + // Distance from A5 to tag is 2.5m + float expectedDiff = 0.5; // Ideal times in universal clock - uint64_t timeA0ToAn = 1.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; - uint64_t timeAnToTag = 2.5 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; + uint64_t timeA0ToTag = time2m; + uint64_t timeA5ToTag = time2_5m; + uint64_t timeA0ToA5 = time1m; - // Clock start offset including any antenna delay - uint64_t tO = 17 * LOCODECK_TS_FREQ; - uint64_t a0O = 60 * LOCODECK_TS_FREQ; - uint64_t anO = 138 * LOCODECK_TS_FREQ; + uint64_t missing = 0; + + mockMessageFromAnchor(5, iTxTime0_5 + timeA5ToTag, + (uint8_t[]) {10, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){NS, NS, NS, NS, NS, iTxTime0_5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS}); - // Clock drifts - float aD = 1.000010; - float tD = 0.999995; + mockMessageFromAnchor(0, iTxTime1_0 + timeA0ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){iTxTime1_0, NS, NS, NS, NS, iTxTime0_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); - // A Arrival Time A0 A1 A2 A3 A4 A5 A6 A7 - mockMessageFromAnchor(0 , drift(tD, iTxTime0_0 + timeA0ToTag + tO), iTxTime0_0 + a0O , NS, NS, NS, NS, NS , NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime0_5 + timeAnToTag + tO), drift(aD, iTxTime0_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime0_5 + anO) , NS, NS); - //mockMessageFromAnchor(0 , drift(tD, iTxTime1_0 + timeA0ToTag + tO), iTxTime1_0 + a0O , NS, NS, NS, NS, iTxTime0_5 + timeA0ToAn + a0O, NS, NS); message lost - mockMessageFromAnchor(anchor, drift(tD, iTxTime1_5 + timeAnToTag + tO), drift(aD, iTxTime1_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime1_5 + anO) , NS, NS); - mockMessageFromAnchor(0 , drift(tD, iTxTime2_0 + timeA0ToTag + tO), iTxTime2_0 + a0O , NS, NS, NS, NS, iTxTime1_5 + timeA0ToAn + a0O, NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime2_5 + timeAnToTag + tO), drift(aD, iTxTime2_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime2_5 + anO) , NS, NS); + mockMessageFromAnchor(5, iTxTime1_5 + timeA5ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 21, 0, 0}, + (uint64_t[]){iTxTime1_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime1_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); - // The missing packet will lead to no calls to the estimator + // The distance is missing for A5 + mockMessageFromAnchor(0, iTxTime2_0 + timeA0ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 21, 0, 0}, + (uint64_t[]){iTxTime2_0, NS, NS, NS, NS, iTxTime1_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, missing, NS, NS}); + + mockMessageFromAnchor(5, iTxTime2_5 + timeA5ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 22, 0, 0}, + (uint64_t[]){iTxTime2_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime2_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); + + + // Only the three last messages will create calls to the estimator. The two first are discarded due to missing data. + mockKalmanEstimator(0, 5, expectedDiff); + // Not called + // mockKalmanEstimator(5, 0, -expectedDiff); + mockKalmanEstimator(0, 5, expectedDiff); // Test uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); - //uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); @@ -440,41 +478,56 @@ void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageNr // Nothing here, verification in mocks } -void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageNr4() { +void testMissingPacketPacketAnchorToAnchorInhibitsDiffCalculation() { // Fixture // Two anchors, separated by 1.0m // Distance from A0 to tag is 2.0m - // Distance from other anchor to tag is 2.5m - const int anchor = 5; + // Distance from A5 to tag is 2.5m + float expectedDiff = 0.5; // Ideal times in universal clock - uint64_t timeA0ToAn = 1.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; - uint64_t timeAnToTag = 2.5 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; + uint64_t timeA0ToTag = time2m; + uint64_t timeA5ToTag = time2_5m; + uint64_t timeA0ToA5 = time1m; - // Clock start offset including any antenna delay - uint64_t tO = 17 * LOCODECK_TS_FREQ; - uint64_t a0O = 60 * LOCODECK_TS_FREQ; - uint64_t anO = 138 * LOCODECK_TS_FREQ; + mockMessageFromAnchor(5, iTxTime0_5 + timeA5ToTag, + (uint8_t[]) {10, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){NS, NS, NS, NS, NS, iTxTime0_5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS}); + + mockMessageFromAnchor(0, iTxTime1_0 + timeA0ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){iTxTime1_0, NS, NS, NS, NS, iTxTime0_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); - // Clock drifts - float aD = 1.000010; - float tD = 0.999995; + mockMessageFromAnchor(5, iTxTime1_5 + timeA5ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 21, 0, 0}, + (uint64_t[]){iTxTime1_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime1_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); - // A Arrival Time A0 A1 A2 A3 A4 A5 A6 A7 - mockMessageFromAnchor(0 , drift(tD, iTxTime0_0 + timeA0ToTag + tO), iTxTime0_0 + a0O , NS, NS, NS, NS, NS , NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime0_5 + timeAnToTag + tO), drift(aD, iTxTime0_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime0_5 + anO) , NS, NS); - mockMessageFromAnchor(0 , drift(tD, iTxTime1_0 + timeA0ToTag + tO), iTxTime1_0 + a0O , NS, NS, NS, NS, iTxTime0_5 + timeA0ToAn + a0O, NS, NS); - //mockMessageFromAnchor(anchor, drift(tD, iTxTime1_5 + timeAnToTag + tO), drift(aD, iTxTime1_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime1_5 + anO) , NS, NS); message lost - mockMessageFromAnchor(0 , drift(tD, iTxTime2_0 + timeA0ToTag + tO), iTxTime2_0 + a0O , NS, NS, NS, NS, iTxTime1_5 + timeA0ToAn + a0O, NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime2_5 + timeAnToTag + tO), drift(aD, iTxTime2_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime2_5 + anO) , NS, NS); + // The packet from A5 to A0 is missing + // The sequence number, timestamp and distance is same as in previous packet from A0 + mockMessageFromAnchor(0, iTxTime2_0 + timeA0ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){iTxTime2_0, NS, NS, NS, NS, iTxTime0_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); - // The missing packet will lead to no calls to the estimator + mockMessageFromAnchor(5, iTxTime2_5 + timeA5ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 22, 0, 0}, + (uint64_t[]){iTxTime2_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime2_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); + + + // Only the three last messages will create calls to the estimator. The two first are discarded due to missing data. + mockKalmanEstimator(0, 5, expectedDiff); + // Not called + // mockKalmanEstimator(5, 0, -expectedDiff); + mockKalmanEstimator(0, 5, expectedDiff); // Test uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); - //uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); @@ -482,90 +535,130 @@ void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageNr // Nothing here, verification in mocks } -void testDifferenceOfDistanceWithTwoAnchors3FramesWithClockDriftAndLostMessageNr5() { +void testMissingPacketPacketAnchorToAnchorInhibitsDiffCalculationWhenSequenceNrWraps() { // Fixture // Two anchors, separated by 1.0m // Distance from A0 to tag is 2.0m - // Distance from other anchor to tag is 2.5m + // Distance from A5 to tag is 2.5m float expectedDiff = 0.5; - const int anchor = 5; // Ideal times in universal clock - uint64_t timeA0ToAn = 1.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; - uint64_t timeAnToTag = 2.5 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; + uint64_t timeA0ToTag = time2m; + uint64_t timeA5ToTag = time2_5m; + uint64_t timeA0ToA5 = time1m; - // Clock start offset including any antenna delay - uint64_t tO = 17 * LOCODECK_TS_FREQ; - uint64_t a0O = 60 * LOCODECK_TS_FREQ; - uint64_t anO = 138 * LOCODECK_TS_FREQ; + mockMessageFromAnchor(5, iTxTime0_5 + timeA5ToTag, + (uint8_t[]) {10, 0, 0, 0, 0, 255, 0, 0}, + (uint64_t[]){NS, NS, NS, NS, NS, iTxTime0_5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS}); + + mockMessageFromAnchor(0, iTxTime1_0 + timeA0ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 255, 0, 0}, + (uint64_t[]){iTxTime1_0, NS, NS, NS, NS, iTxTime0_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); - // Clock drifts - float aD = 1.000010; - float tD = 0.999995; + mockMessageFromAnchor(5, iTxTime1_5 + timeA5ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 0, 0, 0}, + (uint64_t[]){iTxTime1_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime1_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); - // A Arrival Time A0 A1 A2 A3 A4 A5 A6 A7 - mockMessageFromAnchor(0 , drift(tD, iTxTime0_0 + timeA0ToTag + tO), iTxTime0_0 + a0O , NS, NS, NS, NS, NS , NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime0_5 + timeAnToTag + tO), drift(aD, iTxTime0_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime0_5 + anO) , NS, NS); - mockMessageFromAnchor(0 , drift(tD, iTxTime1_0 + timeA0ToTag + tO), iTxTime1_0 + a0O , NS, NS, NS, NS, iTxTime0_5 + timeA0ToAn + a0O, NS, NS); - mockMessageFromAnchor(anchor, drift(tD, iTxTime1_5 + timeAnToTag + tO), drift(aD, iTxTime1_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime1_5 + anO) , NS, NS); - //mockMessageFromAnchor(0 , drift(tD, iTxTime2_0 + timeA0ToTag + tO), iTxTime2_0 + a0O , NS, NS, NS, NS, iTxTime1_5 + timeA0ToAn + a0O, NS, NS); message lost - mockMessageFromAnchor(anchor, drift(tD, iTxTime2_5 + timeAnToTag + tO), drift(aD, iTxTime2_0 + timeA0ToAn + anO), NS, NS, NS, NS, drift(aD, iTxTime2_5 + anO) , NS, NS); + // The packet from A5 to A0 is missing + // The sequence number, timestamp and distance is same as in previous packet from A0 + mockMessageFromAnchor(0, iTxTime2_0 + timeA0ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 255, 0, 0}, + (uint64_t[]){iTxTime2_0, NS, NS, NS, NS, iTxTime0_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); - // Only message 4 will lead to a call to the estimator - mockKalmanEstimator(0, anchor, expectedDiff); + mockMessageFromAnchor(5, iTxTime2_5 + timeA5ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 1, 0, 0}, + (uint64_t[]){iTxTime2_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime2_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); + + + // Only the three last messages will create calls to the estimator. The two first are discarded due to missing data. + mockKalmanEstimator(0, 5, expectedDiff); + // Not called + // mockKalmanEstimator(5, 0, -expectedDiff); + mockKalmanEstimator(0, 5, expectedDiff); // Test uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); - //uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); // Assert // Nothing here, verification in mocks } - -void testNotUsingAnchor0() { +void testMissingPacketAnchorToTagInhibitsDiffCalculation() { // Fixture - // Two anchors (A2 and A5), separated by 1.0m - // Distance from A2 to tag is 2.0m + // Two anchors, separated by 1.0m + // Distance from A0 to tag is 2.0m // Distance from A5 to tag is 2.5m + float expectedDiff = 0.5; - // Ideal times in universal clock (A0) - uint64_t timeA2ToA5 = 1.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; - uint64_t timeA2ToTag = 2.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; - uint64_t timeA5ToTag = 2.5 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT; + // Ideal times in universal clock + uint64_t timeA0ToTag = time2m; + uint64_t timeA5ToTag = time2_5m; + uint64_t timeA0ToA5 = time1m; - // Clock start offset including any antenna delay - // Intentionally less than 40 bit clock wrap around, approx 17s - uint64_t a2O = 1 * LOCODECK_TS_FREQ; - uint64_t a5O = 2 * LOCODECK_TS_FREQ; - uint64_t tO = 3 * LOCODECK_TS_FREQ; + mockMessageFromAnchor(5, iTxTime0_5 + timeA5ToTag, + (uint8_t[]) {10, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){NS, NS, NS, NS, NS, iTxTime0_5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS}); + + mockMessageFromAnchor(0, iTxTime1_0 + timeA0ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 20, 0, 0}, + (uint64_t[]){iTxTime1_0, NS, NS, NS, NS, iTxTime0_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); + + mockMessageFromAnchor(5, iTxTime1_5 + timeA5ToTag, + (uint8_t[]) {11, 0, 0, 0, 0, 21, 0, 0}, + (uint64_t[]){iTxTime1_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime1_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); - // A Arrival Time A0 A1 A2 A3 A4 A5 A6 A7 - mockMessageFromAnchor(2, iTxTime0_2 + timeA2ToTag + tO, NS, NS, iTxTime0_2 + a2O , NS, NS, NS , NS, NS); - mockMessageFromAnchor(5, iTxTime0_5 + timeA5ToTag + tO, NS, NS, iTxTime0_2 + timeA2ToA5 + a5O, NS, NS, iTxTime0_5 + a5O , NS, NS); - mockMessageFromAnchor(2, iTxTime1_2 + timeA2ToTag + tO, NS, NS, iTxTime1_2 + a2O , NS, NS, iTxTime0_5 + timeA2ToA5 + a2O , NS, NS); - mockMessageFromAnchor(5, iTxTime1_5 + timeA5ToTag + tO, NS, NS, iTxTime1_2 + timeA2ToA5 + a5O, NS, NS, iTxTime1_5 + a5O , NS, NS); + // Missing packet + // mockMessageFromAnchor(0, iTxTime2_0 + timeA0ToTag, + // (uint8_t[]) {12, 0, 0, 0, 0, 21, 0, 0}, + // (uint64_t[]){iTxTime2_0, NS, NS, NS, NS, iTxTime1_5 + timeA0ToA5, NS, NS}, + // (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); - // A1 A2 Exp diff Exp measurement diff - // mockKalmanEstimator(2, 5, 2.5 - 2.0); Not called as the packet is interpreted as packet loss and filtered out - mockKalmanEstimator(2, 5, 2.5 - 2.0); + mockMessageFromAnchor(5, iTxTime2_5 + timeA5ToTag, + (uint8_t[]) {12, 0, 0, 0, 0, 22, 0, 0}, + (uint64_t[]){iTxTime2_0 + timeA0ToA5, NS, NS, NS, NS, iTxTime2_5, NS, NS}, + (uint64_t[]){timeA0ToA5, NS, NS, NS, NS, NS, NS, NS}); + + mockMessageFromAnchor(0, iTxTime3_0 + timeA0ToTag, + (uint8_t[]) {13, 0, 0, 0, 0, 22, 0, 0}, + (uint64_t[]){iTxTime3_0, NS, NS, NS, NS, iTxTime2_5 + timeA0ToA5, NS, NS}, + (uint64_t[]){NS, NS, NS, NS, NS, timeA0ToA5, NS, NS}); + + + // Only the three last messages will create calls to the estimator. The two first are discarded due to missing data. + mockKalmanEstimator(0, 5, expectedDiff); + // Not called due to the packet loss + // mockKalmanEstimator(5, 0, -expectedDiff); + + // Not called since packet from previous anchor was lost + // mockKalmanEstimator(0, 5, expectedDiff); + + // Not called since previous packet from same anchor was lost + // mockKalmanEstimator(5, 0, -expectedDiff); // Test uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); + // uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); + uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); // Assert // Nothing here, verification in mocks } -*/ - void testPacketReceivedEventShouldSetTheRadioInReceiveMode() { // Fixture // mockRadioSetToReceiveMode() called as part of mockMessageFromAnchor() @@ -612,11 +705,6 @@ static dwTime_t ts(uint64_t time) { return a; } -static void setTimestampInPayload(uint64_t time, uint8_t* dest) { - dwTime_t timestamp = {.full = time}; - memcpy(dest, timestamp.raw, 5); -} - static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[]) { packet_t packet; MAC80215_PACKET_INIT(packet, MAC802154_TYPE_DATA); @@ -665,7 +753,7 @@ static bool estimatorKalmanEnqueueTDOAMockCallback(tdoaMeasurement_t* actual, in sprintf(message, "Failed in call %i to kalmanEstimatorEnqueueTDOA()", cmock_num_calls); tdoaMeasurement_t* expected = &stateEstimatorExpectations[cmock_num_calls]; - // TODO krri What is a reasonable accepted error here? 2 cm is needed to make the clock drift cases pass (expected: -0.500000 actual: -0.487943). + // What is a reasonable accepted error here? 2 cm is needed to make the clock drift cases pass (expected: -0.500000 actual: -0.487943). // Rounding error based on clock resolution is around 3 mm TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.02, expected->distanceDiff, actual->distanceDiff, message); @@ -740,7 +828,7 @@ void verifyDifferenceOfDistanceWithNoClockDriftButConfigurableClockOffset(uint64 mockMessageFromAnchor(0, iTxTime1_0 + timeA0ToTag + tO, (uint8_t[]) {11, 20, 0, 0, 0, 0, 0, 0}, - (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS}, + (uint64_t[]){iTxTime1_0 + a0O, NS, NS, NS, NS, NS, NS, NS}, (uint64_t[]){NS, NS, NS, NS, NS, NS, NS, NS}); mockMessageFromAnchor(1, iTxTime1_1 + timeA1ToTag + tO, @@ -748,6 +836,7 @@ void verifyDifferenceOfDistanceWithNoClockDriftButConfigurableClockOffset(uint64 (uint64_t[]){iTxTime1_0 + timeA0ToA1 + a1O, iTxTime1_1 + a1O, NS, NS, NS, NS, NS, NS}, (uint64_t[]){timeA0ToA1, NS, NS, NS, NS, NS, NS, NS}); + // Only the last message will create calls to the estimator. The two first are discarded due to missing data. mockKalmanEstimator(0, 1, expectedDiff); // Test