TDoA2 Truncating Clock Correction Measurements

All discussions related to the Loco Positioning system
Post Reply
snyderthorst
Beginner
Posts: 23
Joined: Thu Jun 28, 2018 12:00 am

TDoA2 Truncating Clock Correction Measurements

Post by snyderthorst »

Hello,

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;
}
The calcClockCorrection function:

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;
}
Thank you,
Thomas
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: TDoA2 Truncating Clock Correction Measurements

Post by kristoffer »

Hi!

I must admit that I don't remember why we truncate them to 32 bits, I think the timestamps are 40 bits from the DW1000 chip (looks like that in the dwTime_t type at least). Since we clearly are indicating a 40 bit long bit mask (0x00FFFFFFFFul) in the code, I assume we had a good reason and it is probably not a mistake. I wrote the code, so I should know, but unfortunately I don't, sorry!

I do remember that working with differences between timestamps was the best way to handle wrapping of the timestamps though (they wrap pretty frequently).

You might want to look at the more generalized tdoaEngine implementation (https://github.com/bitcraze/crazyflie-f ... s/src/tdoa) instead of the TDoA2 flavour. I wrote it for TDoA3 but it should work equally well for TDoA2, and the code is probably more portable which should simplify your work. The plan was to use it in the TDoA2 implementation as well, but I never got around to do it.
Post Reply