Skip to content

Commit

Permalink
#315 Simplified the tdoa engine API
Browse files Browse the repository at this point in the history
  • Loading branch information
@krichardsson
krichardsson committed May 22, 2018
1 parent a5dd1d5 commit 37c9d35
Show file tree
Hide file tree
Showing 3 changed files with 47 additions and 42 deletions.
8 changes: 5 additions & 3 deletions src/deck/drivers/interface/lpsTdoaTagEngine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,15 @@ typedef struct {
} anchorInfo_t;


typedef int (*updateRemoteDataFromPacketFkn_t)(anchorInfo_t* anchorCtx, const void* payload);

void tdoaEngineInit();

void tdoaEngineSetRemoteRxTime(anchorInfo_t* anchorCtx, const uint8_t remoteAnchor, const int64_t remoteRxTime, const uint8_t remoteSeqNr);
void tdoaEngineSetTimeOfFlight(anchorInfo_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof);
uint8_t tdoaEngineGetId(const anchorInfo_t* anchorCtx);
void tdoaEngineSetAnchorPosition(anchorInfo_t* anchorCtx, const float x, const float y, const float z);
anchorInfo_t* tdoaEngineProcessPacket(uint8_t anchorId, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const uint8_t seqNr, updateRemoteDataFromPacketFkn_t updateRemoteData, const void* packet, int* rangeDataLength);
void tdoaEngineSetRxTxData(anchorInfo_t* anchorCtx, int64_t rxTime, int64_t txTime, uint8_t seqNr);

anchorInfo_t* getAnchorCtxForPacketProcessing(const uint8_t anchor);
void tdoaEngineProcessPacket(anchorInfo_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T);

#endif // __LPS_TDOA_TAG_ENGINE_H__
10 changes: 7 additions & 3 deletions src/deck/drivers/src/lpsTdoa3Tag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,9 +196,13 @@ static void rxcallback(dwDevice_t *dev) {
const int64_t txAn_in_cl_An = packet->header.txTimeStamp;;
const uint8_t seqNr = packet->header.seq;

int rangeDataLength = 0;
anchorInfo_t* anchorCtx = tdoaEngineProcessPacket(anchorId, txAn_in_cl_An, rxAn_by_T_in_cl_T, seqNr, updateRemoteData, packet, &rangeDataLength);
handleLppPacket(dataLength, rangeDataLength, &rxPacket, anchorCtx);
anchorInfo_t* anchorCtx = getAnchorCtxForPacketProcessing(anchorId);
if (anchorCtx) {
int rangeDataLength = updateRemoteData(anchorCtx, packet);
tdoaEngineProcessPacket(anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
tdoaEngineSetRxTxData(anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
handleLppPacket(dataLength, rangeDataLength, &rxPacket, anchorCtx);
}

rangingOk = true;
}
Expand Down
71 changes: 35 additions & 36 deletions src/deck/drivers/src/lpsTdoaTagEngine.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ void tdoaEngineInit() {
memset(anchorStorage, 0, sizeof(anchorStorage));
}

anchorInfo_t* tdoaEngineGetAnchorCtx(const uint8_t anchor) {
static anchorInfo_t* getAnchorCtx(const uint8_t anchor) {
// TODO krri add lookup table to avoid linear search
for (int i = 0; i < ANCHOR_STORAGE_COUNT; i++) {
if (anchor == anchorStorage[i].id && anchorStorage[i].isInitialized) {
Expand All @@ -84,7 +84,7 @@ anchorInfo_t* tdoaEngineGetAnchorCtx(const uint8_t anchor) {
return 0;
}

static anchorInfo_t* initializeNewAchorContext(const uint8_t anchor) {
static anchorInfo_t* initializeNewAnchorContext(const uint8_t anchor) {
int indexToInitialize = 0;
uint32_t now = xTaskGetTickCount();
uint32_t oldestUpdateTime = now;
Expand Down Expand Up @@ -286,10 +286,7 @@ static void enqueueTDOA(const anchorInfo_t* anchorACtx, const anchorInfo_t* anch
}
}

static double calcClockCorrection(const anchorInfo_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T) {
const int64_t latest_rxAn_by_T_in_cl_T = getRxTime(anchorCtx);
const int64_t latest_txAn_in_cl_An = getTxTime(anchorCtx);

static double calcClockCorrection(const int64_t latest_rxAn_by_T_in_cl_T, const int64_t latest_txAn_in_cl_An, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T) {
const double tickCount_in_cl_An = truncateToAnchorTimeStamp(txAn_in_cl_An - latest_txAn_in_cl_An);
const double tickCount_in_T = truncateToLocalTimeStamp(rxAn_by_T_in_cl_T - latest_rxAn_by_T_in_cl_T);

Expand All @@ -301,26 +298,33 @@ static double calcClockCorrection(const anchorInfo_t* anchorCtx, const int64_t t
}

static bool updateClockCorrection(anchorInfo_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T) {
double clockCorrectionCandidate = calcClockCorrection(anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
bool result = false;

const int64_t latest_rxAn_by_T_in_cl_T = getRxTime(anchorCtx);
const int64_t latest_txAn_in_cl_An = getTxTime(anchorCtx);

const double MAX_CLOCK_CORRECTION_ERR = 0.00001;
if ((1.0 - MAX_CLOCK_CORRECTION_ERR) < clockCorrectionCandidate && clockCorrectionCandidate < (1.0 + MAX_CLOCK_CORRECTION_ERR)) {
if (latest_rxAn_by_T_in_cl_T != 0 && latest_txAn_in_cl_An != 0) {
double clockCorrectionCandidate = calcClockCorrection(latest_rxAn_by_T_in_cl_T, latest_txAn_in_cl_An, txAn_in_cl_An, rxAn_by_T_in_cl_T);

// TODO krri Add sanity checks
// * reject if missing seq nrs?
// * If too long between samples, clocks may have wrapped several times, problem?
// TODO krri reject outliers?
// TODO krri LP filter
const double MAX_CLOCK_CORRECTION_ERR = 0.00001;
if ((1.0 - MAX_CLOCK_CORRECTION_ERR) < clockCorrectionCandidate && clockCorrectionCandidate < (1.0 + MAX_CLOCK_CORRECTION_ERR)) {

setClockCorrection(anchorCtx, clockCorrectionCandidate);
if (tdoaEngineGetId(anchorCtx) == lpsTdoaStats.anchorId) {
lpsTdoaStats.clockCorrection = clockCorrectionCandidate;
lpsTdoaStats.clockCorrectionCount++;
// TODO krri Add sanity checks
// * reject if missing seq nrs?
// * If too long between samples, clocks may have wrapped several times, problem?
// TODO krri reject outliers?
// TODO krri LP filter

setClockCorrection(anchorCtx, clockCorrectionCandidate);
if (tdoaEngineGetId(anchorCtx) == lpsTdoaStats.anchorId) {
lpsTdoaStats.clockCorrection = clockCorrectionCandidate;
lpsTdoaStats.clockCorrectionCount++;
}
result = true;
}
return true;
}

return false;
return result;
}

static int64_t calcTDoA(const anchorInfo_t* otherAnchorCtx, const anchorInfo_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T) {
Expand Down Expand Up @@ -358,7 +362,7 @@ static bool findSuitableAnchor(anchorInfo_t** otherAnchorCtx, const anchorInfo_t
// TODO krri Add randomization of which anchor to pick. Current implementation will favour anchors early in the list.
for (int i = 0; i < remoteCount; i++) {
const uint8_t candidateAnchorId = id[i];
anchorInfo_t* candidateAnchorCtx = tdoaEngineGetAnchorCtx(candidateAnchorId);
anchorInfo_t* candidateAnchorCtx = getAnchorCtx(candidateAnchorId);
if (candidateAnchorCtx) {
if (seqNr[i] == getSeqNr(candidateAnchorCtx) && getTimeOfFlight(anchorCtx, candidateAnchorId)) {
*otherAnchorCtx = candidateAnchorCtx;
Expand All @@ -370,15 +374,19 @@ static bool findSuitableAnchor(anchorInfo_t** otherAnchorCtx, const anchorInfo_t
return false;
}


anchorInfo_t* tdoaEngineProcessPacket(uint8_t anchorId, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const uint8_t seqNr, updateRemoteDataFromPacketFkn_t updateRemoteData, const void* packet, int* rangeDataLength) {
*rangeDataLength = 0;

anchorInfo_t* anchorCtx = tdoaEngineGetAnchorCtx(anchorId);
anchorInfo_t* getAnchorCtxForPacketProcessing(const uint8_t anchorId) {
anchorInfo_t* anchorCtx = getAnchorCtx(anchorId);
if (anchorCtx) {
lpsTdoaStats.contextHitCount++;
*rangeDataLength = updateRemoteData(anchorCtx, packet);
} else {
lpsTdoaStats.contextMissCount++;
anchorCtx = initializeNewAnchorContext(anchorId);
}

return anchorCtx;
}

void tdoaEngineProcessPacket(anchorInfo_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T) {
bool timeIsGood = updateClockCorrection(anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
if (timeIsGood) {
lpsTdoaStats.timeIsGood++;
Expand All @@ -390,13 +398,4 @@ anchorInfo_t* tdoaEngineProcessPacket(uint8_t anchorId, const int64_t txAn_in_cl
enqueueTDOA(otherAnchorCtx, anchorCtx, tdoaDistDiff);
}
}
} else {
lpsTdoaStats.contextMissCount++;
anchorCtx = initializeNewAchorContext(anchorId);
*rangeDataLength = updateRemoteData(anchorCtx, packet);
}

tdoaEngineSetRxTxData(anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);

return anchorCtx;
}

0 comments on commit 37c9d35

Please sign in to comment.