From b22133bcef23e3de6861e52b1016709c964d6d6f Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Wed, 6 Dec 2017 14:48:46 +0100 Subject: [PATCH] #251 Added unit test for incoming LPP packets --- src/deck/drivers/src/lpsTdoaTag.c | 14 ++++-- test/deck/drivers/src/TestLpsTdoaTag.c | 64 ++++++++++++++++++++++---- 2 files changed, 64 insertions(+), 14 deletions(-) diff --git a/src/deck/drivers/src/lpsTdoaTag.c b/src/deck/drivers/src/lpsTdoaTag.c index 5c3e9f544c..7802ed92da 100644 --- a/src/deck/drivers/src/lpsTdoaTag.c +++ b/src/deck/drivers/src/lpsTdoaTag.c @@ -176,11 +176,16 @@ static void addToLog(const uint8_t anchor, const uint8_t previousAnchor, const f } static void handleLppPacket(const int dataLength, const packet_t* rxPacket) { - if (dataLength - MAC802154_HEADER_LENGTH > (int)LPS_TDOA_LPP_HEADER) { - if (rxPacket->payload[LPS_TDOA_LPP_HEADER] == LPP_HEADER_SHORT_PACKET) { + const int32_t payloadLength = dataLength - MAC802154_HEADER_LENGTH; + const int32_t startOfLppDataInPayload = LPS_TDOA_LPP_HEADER; + const int32_t lppDataLength = payloadLength - startOfLppDataInPayload; + + if (lppDataLength > 0) { + const uint8_t lppPacketHeader = rxPacket->payload[LPS_TDOA_LPP_HEADER]; + if (lppPacketHeader == LPP_HEADER_SHORT_PACKET) { int srcId = -1; - for (int i=0; isourceAddress == options->anchorAddress[i]) { srcId = i; break; @@ -188,8 +193,9 @@ static void handleLppPacket(const int dataLength, const packet_t* rxPacket) { } if (srcId >= 0) { + const int32_t lppTypeAndPayloadLength = lppDataLength - 1; lpsHandleLppShortPacket(srcId, &rxPacket->payload[LPS_TDOA_LPP_TYPE], - dataLength - MAC802154_HEADER_LENGTH - LPS_TDOA_LPP_HEADER); + lppTypeAndPayloadLength); } } } diff --git a/test/deck/drivers/src/TestLpsTdoaTag.c b/test/deck/drivers/src/TestLpsTdoaTag.c index b2ac68cf25..3e151f7cc7 100644 --- a/test/deck/drivers/src/TestLpsTdoaTag.c +++ b/test/deck/drivers/src/TestLpsTdoaTag.c @@ -2,6 +2,7 @@ #include "lpsTdoaTag.h" #include +#include #include "unity.h" #include "mock_libdw1000.h" @@ -37,14 +38,17 @@ static lpsAlgoOptions_t options = { {.x = 0.59, .y = 2.27, .z = 0.20}, {.x = 4.70, .y = 3.38, .z = 0.20}, {.x = 4.70, .y = 1.14, .z = 0.20}, + {.x = 4.70, .y = 3.38, .z = 0.20}, + {.x = 4.70, .y = 1.14, .z = 0.20}, }, }; static const uint64_t NS = 0; -static const int dataLength = sizeof(packet_t); +static const uint32_t dataLengthNoLpp = MAC802154_HEADER_LENGTH + sizeof(rangePacket_t); static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[]); -static void mockMessageFromAnchorNotCommingBackToReceive(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[]); +static void mockMessageFromAnchorNotComingBackToReceive(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[]); +static void mockMessageFromAnchorWithLppData(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[], uint32_t lppDataSize, uint8_t* lppData); static void mockRadioSetToReceiveMode(); static void ignoreKalmanEstimatorValidation(); @@ -778,7 +782,7 @@ void testThatLppShortPacketIsNotSentToWrongAnchorWhenAvailable() { void testThatLppShortPacketIsSentToGoodAnchorWhenAvailable() { // Fixture // mockRadioSetToReceiveMode() called as part of mockMessageFromAnchor() - mockMessageFromAnchorNotCommingBackToReceive(lppShortPacketDest, NS, + mockMessageFromAnchorNotComingBackToReceive(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}); @@ -798,6 +802,29 @@ void testThatLppShortPacketIsSentToGoodAnchorWhenAvailable() { TEST_ASSERT_EQUAL_UINT32(MAX_TIMEOUT, actual); } +void testLppPacketIsHandled() { + // Simplified test, not verifying the actual LPP data + // Fixture + uint8_t lppData[] = {1, 2, 3, 4}; + uint8_t lppDataSize = sizeof(lppData); + uint8_t expectedId = 4; + + mockMessageFromAnchorWithLppData(expectedId, 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}, + lppDataSize, lppData); + + lpsGetLppShort_IgnoreAndReturn(false); + ignoreKalmanEstimatorValidation(); + + // Test + uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived); + + // Assert + // Verified in mock +} + //////////////////////////////////////////// static dwTime_t ts(uint64_t time) { @@ -805,11 +832,11 @@ static dwTime_t ts(uint64_t time) { return a; } -static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[]) { +static void mockMessageFromAnchorWithLppData(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[], uint32_t lppDataSize, uint8_t* lppData) { packet_t packet; MAC80215_PACKET_INIT(packet, MAC802154_TYPE_DATA); - packet.sourceAddress = 0xbccf000000000000 | anchorIndex; + packet.sourceAddress = options.anchorAddress[anchorIndex]; rangePacket_t* payload = (rangePacket_t*)&packet.payload; payload->type = 0x22; @@ -819,8 +846,21 @@ static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint8_t payload->distances[i] = (uint16_t)(distances[i] & 0xffff); } - dwGetDataLength_ExpectAndReturn(&dev, dataLength); - dwGetData_ExpectAndCopyData(&dev, &packet, dataLength); + uint32_t lppTotalSize = 0; + if (lppDataSize > 0) { + packet.payload[LPS_TDOA_LPP_HEADER] = LPP_HEADER_SHORT_PACKET; + packet.payload[LPS_TDOA_LPP_TYPE] = LPP_SHORT_ANCHORPOS; + memcpy(&packet.payload[LPS_TDOA_LPP_PAYLOAD], lppData, lppDataSize); + lppTotalSize = lppDataSize + 2; + + uint8_t* ignore = 0; + lpsHandleLppShortPacket_Expect(anchorIndex, ignore, lppTotalSize - 1); // Header byte not included + lpsHandleLppShortPacket_IgnoreArg_data(); + } + + uint32_t totalDataLength = dataLengthNoLpp + lppTotalSize; + dwGetDataLength_ExpectAndReturn(&dev, totalDataLength); + dwGetData_ExpectAndCopyData(&dev, &packet, totalDataLength); dwTime_t rxTimeStr = ts(rxTime); dwGetReceiveTimestamp_ExpectAndCopyData(&dev, &rxTimeStr); @@ -828,7 +868,11 @@ static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint8_t mockRadioSetToReceiveMode(); } -static void mockMessageFromAnchorNotCommingBackToReceive(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[]) { +static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint8_t sequenceNrs[], uint64_t timestamps[], uint64_t distances[]) { + mockMessageFromAnchorWithLppData(anchorIndex, rxTime, sequenceNrs, timestamps, distances, 0, 0); +} + +static void mockMessageFromAnchorNotComingBackToReceive(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); @@ -842,8 +886,8 @@ static void mockMessageFromAnchorNotCommingBackToReceive(uint8_t anchorIndex, ui payload->distances[i] = (uint16_t)(distances[i] & 0xffff); } - dwGetDataLength_ExpectAndReturn(&dev, dataLength); - dwGetData_ExpectAndCopyData(&dev, &packet, dataLength); + dwGetDataLength_ExpectAndReturn(&dev, dataLengthNoLpp); + dwGetData_ExpectAndCopyData(&dev, &packet, dataLengthNoLpp); dwTime_t rxTimeStr = ts(rxTime); dwGetReceiveTimestamp_ExpectAndCopyData(&dev, &rxTimeStr);