Skip to content

Commit

Permalink
Refactoring of TDOA code. Updated variable names
Browse files Browse the repository at this point in the history
  • Loading branch information
@krichardsson
krichardsson committed Nov 16, 2016
1 parent 001d351 commit 157fb23
Showing 1 changed file with 37 additions and 29 deletions.
66 changes: 37 additions & 29 deletions src/deck/drivers/src/lpsTdoaTag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,8 @@ static lpsAlgoOptions_t* options;
static rangePacket_t rxPacketBuffer[LOCODECK_NR_OF_ANCHORS];
static dwTime_t arrivals[LOCODECK_NR_OF_ANCHORS];

static double frameTimeInMasterClock = 0.0;
static double localClockCorrection = 1.0;
static double frameTime_in_cl_M = 0.0;
static double clockCorrection_T_To_M = 1.0;

#define MASTER 0

Expand Down Expand Up @@ -106,6 +106,9 @@ static void enqueueTDOA(uint8_t senderId, int64_t rxT, int64_t txAn_A0time) {
#endif
}

// A note on variable names. They might seem a bit verbose but express quite a lot of information
// We have three actors: Master (M), Anchor n (An) and the deck on the CF called Tag (T)
// rxM_by_An_in_cl_An should be interpreted as "The time when packet was received from the Master Anchor by Anchor N expressed in the clock of Anchor N"
static void rxcallback(dwDevice_t *dev) {
int dataLength = dwGetDataLength(dev);
packet_t rxPacket;
Expand All @@ -121,47 +124,49 @@ static void rxcallback(dwDevice_t *dev) {
rangePacket_t* packet = (rangePacket_t*)rxPacket.payload;

if (anchor == MASTER) {
frameTimeInMasterClock = truncateToTimeStamp(timestampToUint64(packet->timestamps[MASTER]) - timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]));
double frameTimeInLocalClock = truncateToTimeStamp(arrival.full - arrivals[MASTER].full);
int64_t txM_in_cl_M = timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]);
int64_t rxM_by_T_in_cl_T = arrivals[MASTER].full;
int64_t rxAn_by_T_in_cl_T = arrival.full;

localClockCorrection = 1.0;
if (frameTimeInLocalClock != 0.0) {
localClockCorrection = frameTimeInMasterClock / frameTimeInLocalClock;
frameTime_in_cl_M = truncateToTimeStamp(timestampToUint64(packet->timestamps[MASTER]) - timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]));
double frameTime_in_T = truncateToTimeStamp(rxAn_by_T_in_cl_T - rxM_by_T_in_cl_T);

clockCorrection_T_To_M = 1.0;
if (frameTime_in_T != 0.0) {
clockCorrection_T_To_M = frameTime_in_cl_M / frameTime_in_T;
}

int64_t txA0_X = timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]);
int64_t rxT_0 = arrivals[MASTER].full;
enqueueTDOA(MASTER, rxT_0, txA0_X);
enqueueTDOA(MASTER, rxM_by_T_in_cl_T, txM_in_cl_M);
} else {
double frameTimeInAnchorClock = truncateToTimeStamp(timestampToUint64(packet->timestamps[MASTER]) - timestampToUint64(rxPacketBuffer[anchor].timestamps[MASTER]));

double anchorClockCorrection = 1.0;
if (frameTimeInAnchorClock != 0.0) {
anchorClockCorrection = frameTimeInMasterClock / frameTimeInAnchorClock;
}
int64_t previous_txAn_in_cl_An = timestampToUint64(rxPacketBuffer[anchor].timestamps[anchor]);
int64_t txM_in_cl_M = timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]);
int64_t rxAn_by_M_in_cl_M = timestampToUint64(rxPacketBuffer[MASTER].timestamps[anchor]);
int64_t rxM_by_An_in_cl_An = timestampToUint64(packet->timestamps[MASTER]);

float tdoaDistDiff;
int64_t previuos_rxM_by_An_in_cl_An = timestampToUint64(rxPacketBuffer[anchor].timestamps[MASTER]);

int64_t txAn_X = timestampToUint64(rxPacketBuffer[anchor].timestamps[anchor]);
int64_t txA0_X = timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]);
int64_t rxAn_0 = timestampToUint64(rxPacketBuffer[MASTER].timestamps[anchor]);
int64_t rxA0_n = timestampToUint64(packet->timestamps[MASTER]);
int64_t rxM_by_T_in_cl_T = arrivals[MASTER].full;
int64_t rxAn_by_T_in_cl_T = arrival.full;
int64_t txAn_in_cl_An = timestampToUint64(packet->timestamps[anchor]);

int64_t rxT_0 = arrivals[MASTER].full;
int64_t rxT_n = arrival.full;
int64_t txAn_X2 = timestampToUint64(packet->timestamps[anchor]);
double frameTime_in_cl_An = truncateToTimeStamp(rxM_by_An_in_cl_An) - previuos_rxM_by_An_in_cl_An;

double clockCorrection_An_To_M = 1.0;
if (frameTime_in_cl_An != 0.0) {
clockCorrection_An_To_M = frameTime_in_cl_M / frameTime_in_cl_An;
}

int64_t tA0_n = (((truncateToTimeStamp(rxA0_n - txAn_X) * anchorClockCorrection) - truncateToTimeStamp(txA0_X - rxAn_0))) / 2.0;
int64_t txAn_A0time = (tA0_n + truncateToTimeStamp(txAn_X2 - rxA0_n) * anchorClockCorrection);
int64_t tT = truncateToTimeStamp(rxT_n - rxT_0) * localClockCorrection - txAn_A0time;
int64_t tof_M_to_An_in_cl_M = (((truncateToTimeStamp(rxM_by_An_in_cl_An - previous_txAn_in_cl_An) * clockCorrection_An_To_M) - truncateToTimeStamp(txM_in_cl_M - rxAn_by_M_in_cl_M))) / 2.0;
int64_t delta_txM_to_txAn_in_cl_M = (tof_M_to_An_in_cl_M + truncateToTimeStamp(txAn_in_cl_An - rxM_by_An_in_cl_An) * clockCorrection_An_To_M);
int64_t timeDiffOfArrival_in_cl_M = truncateToTimeStamp(rxAn_by_T_in_cl_T - rxM_by_T_in_cl_T) * clockCorrection_T_To_M - delta_txM_to_txAn_in_cl_M;
int64_t txAn_in_cl_M = txM_in_cl_M + delta_txM_to_txAn_in_cl_M;

tdoaDistDiff = SPEED_OF_LIGHT * tT / LOCODECK_TS_FREQ;
float tdoaDistDiff = SPEED_OF_LIGHT * timeDiffOfArrival_in_cl_M / LOCODECK_TS_FREQ;

// Sanity check distances in case of missed packages
if (tdoaDistDiff > -MAX_DISTANCE_DIFF && tdoaDistDiff < MAX_DISTANCE_DIFF) {
uwbTdoaDistDiff[anchor] = tdoaDistDiff;
enqueueTDOA(anchor, rxT_n, txAn_A0time);
enqueueTDOA(anchor, rxAn_by_T_in_cl_T, txAn_in_cl_M);
}
}

Expand Down Expand Up @@ -195,9 +200,12 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
return MAX_TIMEOUT;
}

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
static void Initialize(dwDevice_t *dev, lpsAlgoOptions_t* algoOptions) {
options = algoOptions;
}
#pragma GCC diagnostic pop

uwbAlgorithm_t uwbTdoaTagAlgorithm = {
.init = Initialize,
Expand Down

0 comments on commit 157fb23

Please sign in to comment.