I am currently porting the TDoA2 algorithm to another platform and was wondering why you truncate the timestamps in the calcClockCorrection function in lpsTdoa2Tag.c?
The truncate functions are:
Code: Select all
static uint64_t truncateToLocalTimeStamp(uint64_t fullTimeStamp) {
return fullTimeStamp & 0x00FFFFFFFFul;
}
static uint64_t truncateToAnchorTimeStamp(uint64_t fullTimeStamp) {
return fullTimeStamp & 0x00FFFFFFFFul;
}
Code: Select all
static bool calcClockCorrection(double* clockCorrection, const uint8_t anchor, const rangePacket2_t* packet, const dwTime_t* arrival) {
if (! isSeqNrConsecutive(history[anchor].packet.sequenceNrs[anchor], packet->sequenceNrs[anchor])) {
return false;
}
const int64_t rxAn_by_T_in_cl_T = arrival->full;
const int64_t txAn_in_cl_An = packet->timestamps[anchor];
const int64_t latest_rxAn_by_T_in_cl_T = history[anchor].arrival.full;
const int64_t latest_txAn_in_cl_An = history[anchor].packet.timestamps[anchor];
const double frameTime_in_cl_An = truncateToAnchorTimeStamp(txAn_in_cl_An - latest_txAn_in_cl_An);
const double frameTime_in_T = truncateToLocalTimeStamp(rxAn_by_T_in_cl_T - latest_rxAn_by_T_in_cl_T);
*clockCorrection = frameTime_in_cl_An / frameTime_in_T;
return true;
}
Thomas