Skip to content

Commit

Permalink
#251 Added unit test for incoming LPP packets
Browse files Browse the repository at this point in the history
  • Loading branch information
@krichardsson
krichardsson committed Dec 6, 2017
1 parent 826ec46 commit b22133b
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 14 deletions.
14 changes: 10 additions & 4 deletions src/deck/drivers/src/lpsTdoaTag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,20 +176,26 @@ 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; i<LOCODECK_NR_OF_ANCHORS; i++) {
for (int i=0; i < LOCODECK_NR_OF_TDOA2_ANCHORS; i++) {
if (rxPacket->sourceAddress == options->anchorAddress[i]) {
srcId = i;
break;
}
}

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);
}
}
}
Expand Down
64 changes: 54 additions & 10 deletions test/deck/drivers/src/TestLpsTdoaTag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#include "lpsTdoaTag.h"

#include <string.h>
#include <stdlib.h>
#include "unity.h"

#include "mock_libdw1000.h"
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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});
Expand All @@ -798,18 +802,41 @@ 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) {
dwTime_t a = {.full = time & 0x00FFFFFFFFFFul};
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;
Expand All @@ -819,16 +846,33 @@ 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);

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);

Expand All @@ -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);
Expand Down

0 comments on commit b22133b

Please sign in to comment.