From 2b9a5e834ca68bff09227d619f6da0a2e89f73dc Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 27 Oct 2023 13:09:38 +0200
Subject: [PATCH 01/13] From the gist

---
 src/deck/drivers/src/lpsTdoa3Tag.c | 375 +++++++++++++++++++++++++----
 1 file changed, 326 insertions(+), 49 deletions(-)

diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 18e488a1de..a1e9f3ec00 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -7,7 +7,7 @@
  *
  * Crazyflie firmware.
  *
- * Copyright 2018, Bitcraze AB
+ * Copyright 2018-2021, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU Lesser General Public License as published by
@@ -40,6 +40,14 @@ The implementation must handle
 3. Dynamically changing visibility of anchors over time
 4. Random TX times from anchors with possible packet collisions and packet loss
 
+
+Hybrid Mode
+
+In Hybrid Mode, the tag is not only passively listening for packets, but is
+also transmitting, which enabled Two Way Ranging with peers in the network.
+The default behaviour is to send the aquired distance data to the estimator
+for improved position estimation.
+
 */
 
 #include <string.h>
@@ -55,7 +63,11 @@ The implementation must handle
 
 #include "libdw1000.h"
 #include "mac.h"
+#include "physicalConstants.h"
 
+#include "log.h"
+#include "statsCnt.h"
+#include "log.h"
 #include "param.h"
 
 #define DEBUG_MODULE "TDOA3"
@@ -68,7 +80,30 @@ The implementation must handle
 
 #define PACKET_TYPE_TDOA3 0x30
 
-#define TDOA3_RECEIVE_TIMEOUT 10000
+#define TDOA3_RECEIVE_TIMEOUT 50000
+
+// The delay required for the radio to be ready to transmit
+#define TX_DELAY_TIME_S ( 500e-6 )
+#define TX_DELAY_TIME (uint64_t)( TX_DELAY_TIME_S * LOCODECK_TS_FREQ )
+
+#define LPP_HEADER 0
+#define LPP_TYPE (LPP_HEADER + 1)
+#define LPP_PAYLOAD (LPP_HEADER + 2)
+
+#define MAX_NR_OF_ANCHORS_IN_TX 8
+
+#define STATS_INTERVAL 500
+
+#define SYSTEM_TX_FREQ 400.0f
+#define ANCHOR_MAX_TX_FREQ 50.0f
+// We need a lower limit of minimum tx rate. The TX timestamp in the protocol is
+// only 32 bits (equal to 67 ms) and we want to avoid double wraps of the TX counter.
+// To have some margin set the lowest tx frequency to 20 Hz (= 50 ms)
+#define ANCHOR_MIN_TX_FREQ 20.0f
+#define ID_COUNT 256
+
+static const locoAddress_t base_address = 0xcfbc;
+static const float hybridModeTwrStd = 0.25;
 
 typedef struct {
   uint8_t type;
@@ -96,11 +131,30 @@ typedef struct {
 } __attribute__((packed)) rangePacket3_t;
 
 
-// Outgoing LPP packet
-static lpsLppShortPacket_t lppPacket;
+static struct {
+  bool isTdoaActive;
+  bool isReceivingPackets;
+
+  // Hybrid mode transmission information
+  int anchorId;
+  uint8_t txSeqNr;
+  uint32_t txT_in_cl_T; // In UWB clock ticks
+  uint32_t latestTransmissionTime_ms; // System clock
+  uint32_t nextTxTick;  // in system clock ticks
+
+  uint32_t averageTxDelay;
+  uint32_t nextTxDelayEvaluationTime_ms;
+  uint32_t rxCount[ID_COUNT];
+
+  bool isTwrActive;
 
-static bool rangingOk;
-static float stdDev = TDOA_ENGINE_MEASUREMENT_NOISE_STD;
+  // Outgoing LPP packet
+  lpsLppShortPacket_t lppPacket;
+
+  statsCntRateLogger_t cntPacketsTransmited;
+  statsCntRateLogger_t cntTwrSeqNrOk;
+  statsCntRateLogger_t cntTwrToEstimator;
+} ctx;
 
 static bool isValidTimeStamp(const int64_t anchorRxTime) {
   return anchorRxTime != 0;
@@ -166,6 +220,45 @@ static void handleLppPacket(const int dataLength, int rangePacketLength, const p
   }
 }
 
+
+static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T) {
+  // We assume updateRemoteData() has been called before this function
+  // and that the remote data from the current packet is in the storage, as we read data from the storage.
+
+  const uint32_t MAX_TIME_SINCE_TRANSMISSION_MS = 100;
+  const bool isLatestTransmisionTimeClose = ((now_ms - ctx.latestTransmissionTime_ms) < MAX_TIME_SINCE_TRANSMISSION_MS);
+  if (isLatestTransmisionTimeClose) {
+    uint8_t latestSeqReceivedByRemote = 255;
+    int64_t rxT_by_An_in_cl_An = 0;
+    if (tdoaStorageGetRemoteRxTimeSeqNr(anchorCtx, ctx.anchorId, &rxT_by_An_in_cl_An, &latestSeqReceivedByRemote)) {
+      STATS_CNT_RATE_EVENT(&ctx.cntTwrSeqNrOk);
+      const bool isLatestTxPacketReturnedFromRemote = (ctx.txSeqNr == latestSeqReceivedByRemote);
+      if (isLatestTxPacketReturnedFromRemote) {
+        const double clockCorrection = tdoaStorageGetClockCorrection(anchorCtx);
+        int64_t t_in_anchor_T = (int64_t)(clockCorrection * tdoaEngineTruncateToAnchorTimeStamp(txAn_in_cl_An - rxT_by_An_in_cl_An));
+        int64_t t_since_tx_T = tdoaEngineTruncateToAnchorTimeStamp(rxAn_by_T_in_cl_T - ctx.txT_in_cl_T);
+        int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
+
+        // TODO krri add outlier filter
+
+        point_t position;
+        if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
+          distanceMeasurement_t measurement = {
+            .distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ,
+            .stdDev = hybridModeTwrStd,
+            .x = position.x,
+            .y = position.y,
+            .z = position.z,
+          };
+
+          estimatorEnqueueDistance(&measurement);
+          STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
+        }
+      }
+    }
+  }
+}
+
 static void rxcallback(dwDevice_t *dev) {
   tdoaStats_t* stats = &tdoaEngineState.stats;
   STATS_CNT_RATE_EVENT(&stats->packetsReceived);
@@ -175,6 +268,7 @@ static void rxcallback(dwDevice_t *dev) {
 
   dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
   const uint8_t anchorId = rxPacket.sourceAddress & 0xff;
+  ctx.rxCount[anchorId]++;
 
   dwTime_t arrival = {.full = 0};
   dwGetReceiveTimestamp(dev, &arrival);
@@ -182,19 +276,26 @@ static void rxcallback(dwDevice_t *dev) {
 
   const rangePacket3_t* packet = (rangePacket3_t*)rxPacket.payload;
   if (packet->header.type == PACKET_TYPE_TDOA3) {
-    const int64_t txAn_in_cl_An = packet->header.txTimeStamp;;
-    const uint8_t seqNr = packet->header.seq & 0x7f;;
+    const int64_t txAn_in_cl_An = packet->header.txTimeStamp;
+    const uint8_t seqNr = packet->header.seq & 0x7f;
 
-    tdoaAnchorContext_t anchorCtx;
     uint32_t now_ms = T2M(xTaskGetTickCount());
-
+    tdoaAnchorContext_t anchorCtx;
     tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchorId, now_ms, &anchorCtx);
     int rangeDataLength = updateRemoteData(&anchorCtx, packet);
-    tdoaEngineProcessPacket(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+
+    const bool doExcludeId = ctx.isTwrActive;
+    const uint8_t excludedId = ctx.anchorId;
+    tdoaEngineProcessPacketFiltered(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, doExcludeId, excludedId);
+
     tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
     handleLppPacket(dataLength, rangeDataLength, &rxPacket, &anchorCtx);
 
-    rangingOk = true;
+    if (ctx.isTwrActive) {
+      processTwoWayRanging(&anchorCtx, now_ms, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+    }
+
+    ctx.isReceivingPackets = true;
   }
 }
 
@@ -204,8 +305,63 @@ static void setRadioInReceiveMode(dwDevice_t *dev) {
   dwStartReceive(dev);
 }
 
-static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
-{
+static int countSeenAnchorsAndClearCounters() {
+  int anchorsCount = 0;
+
+  for (int i = 0; i < ID_COUNT; i++) {
+    if (ctx.rxCount[i] != 0) {
+      anchorsCount++;
+      ctx.rxCount[i] = 0;
+    }
+  }
+
+  return anchorsCount;
+}
+
+static uint32_t updateAverageTxDelay(const uint32_t now_ms) {
+  if (now_ms > ctx.nextTxDelayEvaluationTime_ms) {
+    const uint32_t evalutationPeriod_ms = 500;
+    int anchorsCount = countSeenAnchorsAndClearCounters();
+
+    // Set the TX rate based on the number of transmitting anchors around us
+    // Aim for 400 messages/s. Up to 8 anchors: 50 Hz / anchor
+    float freq = SYSTEM_TX_FREQ / (anchorsCount + 1);
+    if (freq > ANCHOR_MAX_TX_FREQ) {
+      freq = ANCHOR_MAX_TX_FREQ;
+    }
+    if (freq < ANCHOR_MIN_TX_FREQ) {
+      freq = ANCHOR_MIN_TX_FREQ;
+    }
+    ctx.averageTxDelay = 1000.0f / freq;
+
+    ctx.nextTxDelayEvaluationTime_ms = now_ms + evalutationPeriod_ms;
+  }
+
+  return ctx.averageTxDelay;
+}
+
+static uint32_t randomizeDelayToNextTx(const uint32_t now_ms) {
+  const uint32_t interval = 10;
+
+  uint32_t averageTxDelay = updateAverageTxDelay(now_ms);
+
+  uint32_t r = rand();
+  uint32_t delay = averageTxDelay + r % interval - interval / 2;
+
+  return delay;
+}
+
+static dwTime_t findTransmitTimeAsSoonAsPossible(dwDevice_t *dev) {
+  dwTime_t transmitTime = { .full = 0 };
+  dwGetSystemTimestamp(dev, &transmitTime);
+
+  // Add some extra time to make sure the radio is ready to transmit, taking into account that this task may
+  // be halted for a while. This time also includes time for the preamble (128 * 1017.63e-9 s).
+  transmitTime.full += TX_DELAY_TIME;
+  return transmitTime;
+}
+
+static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet) {
   static packet_t txPacket;
   dwIdle(dev);
 
@@ -226,15 +382,123 @@ static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
 }
 
 static bool sendLpp(dwDevice_t *dev) {
-  bool lppPacketToSend = lpsGetLppShort(&lppPacket);
+  bool lppPacketToSend = lpsGetLppShort(&ctx.lppPacket);
   if (lppPacketToSend) {
-    sendLppShort(dev, &lppPacket);
+    sendLppShort(dev, &ctx.lppPacket);
     return true;
   }
 
   return false;
 }
 
+static int populateTxData(rangePacket3_t *rangePacket) {
+  // rangePacket->header.type already populated
+  rangePacket->header.seq = ctx.txSeqNr;
+  rangePacket->header.txTimeStamp = ctx.txT_in_cl_T;
+
+  uint8_t remoteAnchorCount = 0;
+  uint8_t* anchorDataPtr = &rangePacket->remoteAnchorData;
+
+  // Consider a more clever selection of which anchors to include as remote data.
+  // This implementation will give a somewhat randomized set but can probably be improved
+  uint8_t ids[MAX_NR_OF_ANCHORS_IN_TX];
+  uint8_t anchorCount = tdoaStorageGetListOfAnchorIds(tdoaEngineState.anchorInfoArray, ids, MAX_NR_OF_ANCHORS_IN_TX);
+
+  for (uint8_t i = 0; i < anchorCount; i++) {
+    remoteAnchorDataFull_t* anchorData = (remoteAnchorDataFull_t*) anchorDataPtr;
+
+    uint32_t now_ms = T2M(xTaskGetTickCount());
+
+    uint8_t id = ids[i];
+    tdoaAnchorContext_t anchorCtx;
+    tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, id, now_ms, &anchorCtx);
+
+    anchorData->id = id;
+    anchorData->seq = anchorCtx.anchorInfo->seqNr;
+    anchorData->rxTimeStamp = anchorCtx.anchorInfo->rxTime;
+
+    // Not adding distance to other anchors, only support simple ranging in hybrid mode (TWR) to anchors for now.
+    anchorDataPtr += sizeof(remoteAnchorDataShort_t);
+
+    remoteAnchorCount++;
+  }
+  rangePacket->header.remoteCount = remoteAnchorCount;
+
+  return (uint8_t*)anchorDataPtr - (uint8_t*)rangePacket;
+}
+
+// Set TX data in the radio TX buffer
+static void setTxData(dwDevice_t *dev)
+{
+  static packet_t txPacket;
+  static bool firstEntry = true;
+  static int lppLength = 0;
+
+  if (firstEntry) {
+    MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+
+    txPacket.sourceAddress = (base_address & 0xffffffffffffff00) | ctx.anchorId;
+    txPacket.destAddress = (base_address & 0xffffffffffffff00) | 0xff;
+
+    txPacket.payload[0] = PACKET_TYPE_TDOA3;
+
+    firstEntry = false;
+  }
+
+  int rangePacketSize = populateTxData((rangePacket3_t *)txPacket.payload);
+  // This where to add LPP data, for instance the position to enable the
+  // CF to act as an anchor in hybrid mode (TWR)
+
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH + rangePacketSize + lppLength);
+}
+
+
+// Setup the radio to send a ranging packet
+static void setupTx(dwDevice_t *dev) {
+  dwTime_t txTime = findTransmitTimeAsSoonAsPossible(dev);
+  ctx.txT_in_cl_T = txTime.low32;
+  ctx.txSeqNr = (ctx.txSeqNr + 1) & 0x7f;
+
+  dwIdle(dev);
+
+  setTxData(dev);
+
+  dwNewTransmit(dev);
+  dwSetDefaults(dev);
+  dwSetTxRxTime(dev, txTime);
+
+  dwStartTransmit(dev);
+}
+
+static uint32_t startNextEvent(dwDevice_t *dev, const uint32_t now) {
+  uint32_t timeout = 500;
+
+  bool isTxPending = sendLpp(dev);
+
+  if (ctx.isTwrActive) {
+    if (!isTxPending) {
+      if (ctx.nextTxTick < now) {
+        const uint32_t now_ms = T2M(now);
+        setupTx(dev);
+        isTxPending = true;
+        ctx.latestTransmissionTime_ms = now_ms;
+        STATS_CNT_RATE_EVENT(&ctx.cntPacketsTransmited);
+
+        uint32_t newDelay = randomizeDelayToNextTx(now_ms);
+        ctx.nextTxTick = now + newDelay;
+      }
+    }
+
+    timeout = ctx.nextTxTick - now;
+  }
+
+  if (!isTxPending) {
+    setRadioInReceiveMode(dev);
+  }
+
+  return timeout;
+}
+
 static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
   switch(event) {
     case eventPacketReceived:
@@ -244,38 +508,33 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
       break;
     case eventReceiveTimeout:
       break;
-    case eventReceiveFailed:
-      break;
     case eventPacketSent:
-      // Service packet sent, the radio is back to receive automatically
       break;
     default:
       ASSERT_FAILED();
   }
 
-  if(!sendLpp(dev)) {
-    setRadioInReceiveMode(dev);
-  }
+  uint32_t now = xTaskGetTickCount();
+  tdoaStatsUpdate(&tdoaEngineState.stats, T2M(now));
 
-  uint32_t now_ms = T2M(xTaskGetTickCount());
-  tdoaStatsUpdate(&tdoaEngineState.stats, now_ms);
-
-  return MAX_TIMEOUT;
+  uint32_t timeout = startNextEvent(dev, now);
+  return timeout;
 }
 
 static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
-  // Override the default standard deviation set by the TDoA engine.
-  tdoaMeasurement->stdDev = stdDev;
-
-  estimatorEnqueueTDOA(tdoaMeasurement);
-
-  #ifdef CONFIG_DECK_LOCO_2D_POSITION
-  heightMeasurement_t heightData;
-  heightData.timestamp = xTaskGetTickCount();
-  heightData.height = DECK_LOCO_2D_POSITION_HEIGHT;
-  heightData.stdDev = 0.0001;
-  estimatorEnqueueAbsoluteHeight(&heightData);
-  #endif
+  if (ctx.isTdoaActive) {
+    estimatorEnqueueTDOA(tdoaMeasurement);
+
+    #ifdef LPS_2D_POSITION_HEIGHT
+    // If LPS_2D_POSITION_HEIGHT is defined we assume that we are doing 2D positioning.
+    // LPS_2D_POSITION_HEIGHT contains the height (Z) that the tag will be located at
+    heightMeasurement_t heightData;
+    heightData.timestamp = xTaskGetTickCount();
+    heightData.height = LPS_2D_POSITION_HEIGHT;
+    heightData.stdDev = 0.0001;
+    estimatorEnqueueAbsoluteHeight(&heightData);
+    #endif
+  }
 }
 
 static bool getAnchorPosition(const uint8_t anchorId, point_t* position) {
@@ -304,20 +563,33 @@ static void Initialize(dwDevice_t *dev) {
   uint32_t now_ms = T2M(xTaskGetTickCount());
   tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmRandom);
 
-  #ifdef CONFIG_DECK_LOCO_2D_POSITION
-  DEBUG_PRINT("2D positioning enabled at %f m height\n", DECK_LOCO_2D_POSITION_HEIGHT);
+  #ifdef LPS_2D_POSITION_HEIGHT
+  DEBUG_PRINT("2D positioning enabled at %f m height\n", LPS_2D_POSITION_HEIGHT);
   #endif
 
   dwSetReceiveWaitTimeout(dev, TDOA3_RECEIVE_TIMEOUT);
 
   dwCommitConfiguration(dev);
 
-  rangingOk = false;
+  ctx.anchorId = 255;
+  ctx.txSeqNr = 0;
+  ctx.latestTransmissionTime_ms = 0;
+  ctx.nextTxTick = 0;
+  ctx.isTdoaActive = true;
+  ctx.isTwrActive = false;
+
+  ctx.averageTxDelay = 1000.0f / ANCHOR_MAX_TX_FREQ;
+  ctx.nextTxDelayEvaluationTime_ms = 0;
+
+  ctx.isReceivingPackets = false;
+
+  STATS_CNT_RATE_INIT(&ctx.cntPacketsTransmited, STATS_INTERVAL);
+  STATS_CNT_RATE_INIT(&ctx.cntTwrSeqNrOk, STATS_INTERVAL);
+  STATS_CNT_RATE_INIT(&ctx.cntTwrToEstimator, STATS_INTERVAL);
 }
 
-static bool isRangingOk()
-{
-  return rangingOk;
+static bool isRangingOk() {
+  return ctx.isReceivingPackets;
 }
 
 uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
@@ -329,10 +601,15 @@ uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
   .getActiveAnchorIdList = getActiveAnchorIdList,
 };
 
-PARAM_GROUP_START(tdoa3)
-/**
- * @brief The measurement noise to use when sending TDoA measurements to the estimator.
- */
-PARAM_ADD(PARAM_FLOAT, stddev, &stdDev)
 
+LOG_GROUP_START(tdoa3)
+  STATS_CNT_RATE_LOG_ADD(hmTx, &ctx.cntPacketsTransmited)
+  STATS_CNT_RATE_LOG_ADD(hmSeqOk, &ctx.cntTwrSeqNrOk)
+  STATS_CNT_RATE_LOG_ADD(hmEst, &ctx.cntTwrToEstimator)
+LOG_GROUP_STOP(tdoa3)
+
+PARAM_GROUP_START(tdoa3)
+  PARAM_ADD(PARAM_UINT8, hmId, &ctx.anchorId)
+  PARAM_ADD(PARAM_UINT8, hmTdoa, &ctx.isTdoaActive)
+  PARAM_ADD(PARAM_UINT8, hmTwr, &ctx.isTwrActive)
 PARAM_GROUP_STOP(tdoa3)

From 5121e008b47379312ab5c7f9d59f9ef4bda0bdcc Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 27 Oct 2023 13:50:25 +0200
Subject: [PATCH 02/13] Flyable with TWR

---
 src/deck/drivers/src/lpsTdoa3Tag.c | 44 +++++++++++++++++++-----------
 1 file changed, 28 insertions(+), 16 deletions(-)

diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index a1e9f3ec00..79c66a538d 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -104,6 +104,8 @@ for improved position estimation.
 
 static const locoAddress_t base_address = 0xcfbc;
 static const float hybridModeTwrStd = 0.25;
+static float logDistance;
+static uint8_t logDistAnchorId;
 
 typedef struct {
   uint8_t type;
@@ -221,7 +223,7 @@ static void handleLppPacket(const int dataLength, int rangePacketLength, const p
 }
 
 
-static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T) {
+static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T, const uint8_t anchorId) {
   // We assume updateRemoteData() has been called before this function
   // and that the remote data from the current packet is in the storage, as we read data from the storage.
 
@@ -239,20 +241,26 @@ static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uin
         int64_t t_since_tx_T = tdoaEngineTruncateToAnchorTimeStamp(rxAn_by_T_in_cl_T - ctx.txT_in_cl_T);
         int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
 
-        // TODO krri add outlier filter
-
-        point_t position;
-        if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
-          distanceMeasurement_t measurement = {
-            .distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ,
-            .stdDev = hybridModeTwrStd,
-            .x = position.x,
-            .y = position.y,
-            .z = position.z,
-          };
-
-          estimatorEnqueueDistance(&measurement);
-          STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
+        float distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ;
+        // TODO krri add real outlier filter. Simple implementation for our flight lab
+        if (distance > 0.0f && distance < 10.0f) {
+          point_t position;
+          if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
+            distanceMeasurement_t measurement = {
+              .distance = distance,
+              .stdDev = hybridModeTwrStd,
+              .x = position.x,
+              .y = position.y,
+              .z = position.z,
+            };
+
+            if (anchorId == logDistAnchorId) {
+              logDistance = measurement.distance;
+            }
+
+            estimatorEnqueueDistance(&measurement);
+            STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
+          }
         }
       }
     }
@@ -292,7 +300,7 @@ static void rxcallback(dwDevice_t *dev) {
     handleLppPacket(dataLength, rangeDataLength, &rxPacket, &anchorCtx);
 
     if (ctx.isTwrActive) {
-      processTwoWayRanging(&anchorCtx, now_ms, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+      processTwoWayRanging(&anchorCtx, now_ms, txAn_in_cl_An, rxAn_by_T_in_cl_T, anchorId);
     }
 
     ctx.isReceivingPackets = true;
@@ -506,6 +514,8 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
       break;
     case eventTimeout:
       break;
+    case eventReceiveFailed:
+      break;
     case eventReceiveTimeout:
       break;
     case eventPacketSent:
@@ -606,10 +616,12 @@ LOG_GROUP_START(tdoa3)
   STATS_CNT_RATE_LOG_ADD(hmTx, &ctx.cntPacketsTransmited)
   STATS_CNT_RATE_LOG_ADD(hmSeqOk, &ctx.cntTwrSeqNrOk)
   STATS_CNT_RATE_LOG_ADD(hmEst, &ctx.cntTwrToEstimator)
+  LOG_ADD(LOG_FLOAT, hmDist, &logDistance)
 LOG_GROUP_STOP(tdoa3)
 
 PARAM_GROUP_START(tdoa3)
   PARAM_ADD(PARAM_UINT8, hmId, &ctx.anchorId)
   PARAM_ADD(PARAM_UINT8, hmTdoa, &ctx.isTdoaActive)
   PARAM_ADD(PARAM_UINT8, hmTwr, &ctx.isTwrActive)
+  PARAM_ADD(PARAM_UINT8, hmAnchLog, &logDistAnchorId)
 PARAM_GROUP_STOP(tdoa3)

From 4c43bda42dcc5e67375a8b998280dcc8abdbeed6 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 27 Oct 2023 14:59:26 +0200
Subject: [PATCH 03/13] Fully working hybrid mode

---
 src/deck/drivers/src/lpsTdoa3Tag.c | 52 ++++++++++++++++++++++++++----
 1 file changed, 45 insertions(+), 7 deletions(-)

diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 79c66a538d..007696e4bd 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -102,6 +102,15 @@ for improved position estimation.
 #define ANCHOR_MIN_TX_FREQ 20.0f
 #define ID_COUNT 256
 
+// Short LPP packets for position
+#define SHORT_LPP 0xF0
+#define LPP_SHORT_ANCHOR_POSITION 0x01
+
+// Log ids for estimated position
+static logVarId_t estimatedPosLogX;
+static logVarId_t estimatedPosLogY;
+static logVarId_t estimatedPosLogZ;
+
 static const locoAddress_t base_address = 0xcfbc;
 static const float hybridModeTwrStd = 0.25;
 static float logDistance;
@@ -148,8 +157,17 @@ static struct {
   uint32_t nextTxDelayEvaluationTime_ms;
   uint32_t rxCount[ID_COUNT];
 
+  // Transmit packets if true
   bool isTwrActive;
 
+  // Indicates if the current estimated position should be transmitted when sending packets.
+  // If false, the CF can use TWR for positioning itself only
+  // If true, other CFs can use the this CF as an anchor
+  bool twrSendEstimatedPosition;
+
+  // If TWR data should be used for position estimation
+  bool useTwrForPositionEstimation;
+
   // Outgoing LPP packet
   lpsLppShortPacket_t lppPacket;
 
@@ -258,8 +276,10 @@ static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uin
               logDistance = measurement.distance;
             }
 
-            estimatorEnqueueDistance(&measurement);
-            STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
+            if (ctx.useTwrForPositionEstimation) {
+              estimatorEnqueueDistance(&measurement);
+              STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
+            }
           }
         }
       }
@@ -444,18 +464,28 @@ static void setTxData(dwDevice_t *dev)
 
   if (firstEntry) {
     MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
-
-    txPacket.sourceAddress = (base_address & 0xffffffffffffff00) | ctx.anchorId;
     txPacket.destAddress = (base_address & 0xffffffffffffff00) | 0xff;
-
     txPacket.payload[0] = PACKET_TYPE_TDOA3;
 
     firstEntry = false;
   }
 
+  txPacket.sourceAddress = (base_address & 0xffffffffffffff00) | ctx.anchorId;
+
   int rangePacketSize = populateTxData((rangePacket3_t *)txPacket.payload);
-  // This where to add LPP data, for instance the position to enable the
-  // CF to act as an anchor in hybrid mode (TWR)
+
+  if (ctx.twrSendEstimatedPosition) {
+    txPacket.payload[rangePacketSize + LPP_HEADER] = SHORT_LPP;
+    txPacket.payload[rangePacketSize + LPP_TYPE] = LPP_SHORT_ANCHOR_POSITION;
+
+    struct lppShortAnchorPos_s *pos = (struct lppShortAnchorPos_s*) &txPacket.payload[rangePacketSize + LPP_PAYLOAD];
+    pos->x = logGetFloat(estimatedPosLogX);
+    pos->y = logGetFloat(estimatedPosLogY);
+    pos->z = logGetFloat(estimatedPosLogZ);
+
+    lppLength = 2 + sizeof(struct lppShortAnchorPos_s);
+  }
+
 
   dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH + rangePacketSize + lppLength);
 }
@@ -587,12 +617,18 @@ static void Initialize(dwDevice_t *dev) {
   ctx.nextTxTick = 0;
   ctx.isTdoaActive = true;
   ctx.isTwrActive = false;
+  ctx.twrSendEstimatedPosition = true;
 
   ctx.averageTxDelay = 1000.0f / ANCHOR_MAX_TX_FREQ;
   ctx.nextTxDelayEvaluationTime_ms = 0;
 
   ctx.isReceivingPackets = false;
 
+  // Get log ids to aquire the current estimated position
+  estimatedPosLogX = logGetVarId("stateEstimate", "x");
+  estimatedPosLogY = logGetVarId("stateEstimate", "y");
+  estimatedPosLogZ = logGetVarId("stateEstimate", "z");
+
   STATS_CNT_RATE_INIT(&ctx.cntPacketsTransmited, STATS_INTERVAL);
   STATS_CNT_RATE_INIT(&ctx.cntTwrSeqNrOk, STATS_INTERVAL);
   STATS_CNT_RATE_INIT(&ctx.cntTwrToEstimator, STATS_INTERVAL);
@@ -623,5 +659,7 @@ PARAM_GROUP_START(tdoa3)
   PARAM_ADD(PARAM_UINT8, hmId, &ctx.anchorId)
   PARAM_ADD(PARAM_UINT8, hmTdoa, &ctx.isTdoaActive)
   PARAM_ADD(PARAM_UINT8, hmTwr, &ctx.isTwrActive)
+  PARAM_ADD(PARAM_UINT8, hmTwrTXPos, &ctx.twrSendEstimatedPosition)
+  PARAM_ADD(PARAM_UINT8, hmTwrEstPos, &ctx.useTwrForPositionEstimation)
   PARAM_ADD(PARAM_UINT8, hmAnchLog, &logDistAnchorId)
 PARAM_GROUP_STOP(tdoa3)

From 0a4700c2dc34935761c773ca373c5a509529d797 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 27 Oct 2023 15:53:11 +0200
Subject: [PATCH 04/13] Added kconfig for hybrid mode

---
 src/deck/drivers/src/Kconfig       |   9 ++
 src/deck/drivers/src/lpsTdoa3Tag.c | 251 +++++++++++++++++------------
 2 files changed, 154 insertions(+), 106 deletions(-)

diff --git a/src/deck/drivers/src/Kconfig b/src/deck/drivers/src/Kconfig
index 7a40ee3565..1db2aa01b4 100644
--- a/src/deck/drivers/src/Kconfig
+++ b/src/deck/drivers/src/Kconfig
@@ -337,6 +337,15 @@ config DECK_LOCO_LONGER_RANGE
     help
         Note that anchors need to be built with support for this as well
 
+config DECK_LOCO_TDOA3_HYBRID_MODE
+    bool "Support TDoA3 hybrid mode"
+    depends on DECK_LOCO
+    default n
+    help
+        In the standard TDoA3 mode anchors are transmitting while the Crazyflies only receive. In hybrid mode the
+        Crazyflies can also transmit and thus do TWR, either for positioning them selfs or to act as an anchor for
+        other Crazyflies.
+
 config DECK_LOCO_TDMA
     bool "Use Time Division Multiple Access"
     depends on DECK_LOCO_ALGORITHM_TWR
diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 007696e4bd..8b4f07e82c 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -7,7 +7,7 @@
  *
  * Crazyflie firmware.
  *
- * Copyright 2018-2021, Bitcraze AB
+ * Copyright 2018-2023, Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU Lesser General Public License as published by
@@ -44,8 +44,8 @@ The implementation must handle
 Hybrid Mode
 
 In Hybrid Mode, the tag is not only passively listening for packets, but is
-also transmitting, which enabled Two Way Ranging with peers in the network.
-The default behaviour is to send the aquired distance data to the estimator
+also transmitting, which enables Two Way Ranging with peers in the network.
+The default behaviour is to send the acquired distance data to the estimator
 for improved position estimation.
 
 */
@@ -63,12 +63,13 @@ for improved position estimation.
 
 #include "libdw1000.h"
 #include "mac.h"
-#include "physicalConstants.h"
 
-#include "log.h"
+#include "param.h"
+#include "autoconf.h"
+
+#include "physicalConstants.h"
 #include "statsCnt.h"
 #include "log.h"
-#include "param.h"
 
 #define DEBUG_MODULE "TDOA3"
 #include "debug.h"
@@ -82,6 +83,7 @@ for improved position estimation.
 
 #define TDOA3_RECEIVE_TIMEOUT 50000
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
 // The delay required for the radio to be ready to transmit
 #define TX_DELAY_TIME_S ( 500e-6 )
 #define TX_DELAY_TIME (uint64_t)( TX_DELAY_TIME_S * LOCODECK_TS_FREQ )
@@ -112,9 +114,9 @@ static logVarId_t estimatedPosLogY;
 static logVarId_t estimatedPosLogZ;
 
 static const locoAddress_t base_address = 0xcfbc;
-static const float hybridModeTwrStd = 0.25;
-static float logDistance;
-static uint8_t logDistAnchorId;
+
+static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T, const uint8_t anchorId);
+#endif
 
 typedef struct {
   uint8_t type;
@@ -141,11 +143,17 @@ typedef struct {
   uint8_t remoteAnchorData;
 } __attribute__((packed)) rangePacket3_t;
 
-
 static struct {
-  bool isTdoaActive;
+  float tdoaStdDev;
   bool isReceivingPackets;
 
+  // Outgoing LPP packet
+  lpsLppShortPacket_t lppPacket;
+
+  bool isTdoaActive;
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+
   // Hybrid mode transmission information
   int anchorId;
   uint8_t txSeqNr;
@@ -165,15 +173,19 @@ static struct {
   // If true, other CFs can use the this CF as an anchor
   bool twrSendEstimatedPosition;
 
+  // Std dev for TWR samples sent to the estimator
+  float twrStdDev;
+
   // If TWR data should be used for position estimation
   bool useTwrForPositionEstimation;
 
-  // Outgoing LPP packet
-  lpsLppShortPacket_t lppPacket;
-
   statsCntRateLogger_t cntPacketsTransmited;
   statsCntRateLogger_t cntTwrSeqNrOk;
   statsCntRateLogger_t cntTwrToEstimator;
+
+  uint8_t logDistAnchorId;
+  float logDistance;
+#endif
 } ctx;
 
 static bool isValidTimeStamp(const int64_t anchorRxTime) {
@@ -240,53 +252,6 @@ static void handleLppPacket(const int dataLength, int rangePacketLength, const p
   }
 }
 
-
-static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T, const uint8_t anchorId) {
-  // We assume updateRemoteData() has been called before this function
-  // and that the remote data from the current packet is in the storage, as we read data from the storage.
-
-  const uint32_t MAX_TIME_SINCE_TRANSMISSION_MS = 100;
-  const bool isLatestTransmisionTimeClose = ((now_ms - ctx.latestTransmissionTime_ms) < MAX_TIME_SINCE_TRANSMISSION_MS);
-  if (isLatestTransmisionTimeClose) {
-    uint8_t latestSeqReceivedByRemote = 255;
-    int64_t rxT_by_An_in_cl_An = 0;
-    if (tdoaStorageGetRemoteRxTimeSeqNr(anchorCtx, ctx.anchorId, &rxT_by_An_in_cl_An, &latestSeqReceivedByRemote)) {
-      STATS_CNT_RATE_EVENT(&ctx.cntTwrSeqNrOk);
-      const bool isLatestTxPacketReturnedFromRemote = (ctx.txSeqNr == latestSeqReceivedByRemote);
-      if (isLatestTxPacketReturnedFromRemote) {
-        const double clockCorrection = tdoaStorageGetClockCorrection(anchorCtx);
-        int64_t t_in_anchor_T = (int64_t)(clockCorrection * tdoaEngineTruncateToAnchorTimeStamp(txAn_in_cl_An - rxT_by_An_in_cl_An));
-        int64_t t_since_tx_T = tdoaEngineTruncateToAnchorTimeStamp(rxAn_by_T_in_cl_T - ctx.txT_in_cl_T);
-        int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
-
-        float distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ;
-        // TODO krri add real outlier filter. Simple implementation for our flight lab
-        if (distance > 0.0f && distance < 10.0f) {
-          point_t position;
-          if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
-            distanceMeasurement_t measurement = {
-              .distance = distance,
-              .stdDev = hybridModeTwrStd,
-              .x = position.x,
-              .y = position.y,
-              .z = position.z,
-            };
-
-            if (anchorId == logDistAnchorId) {
-              logDistance = measurement.distance;
-            }
-
-            if (ctx.useTwrForPositionEstimation) {
-              estimatorEnqueueDistance(&measurement);
-              STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
-            }
-          }
-        }
-      }
-    }
-  }
-}
-
 static void rxcallback(dwDevice_t *dev) {
   tdoaStats_t* stats = &tdoaEngineState.stats;
   STATS_CNT_RATE_EVENT(&stats->packetsReceived);
@@ -296,7 +261,10 @@ static void rxcallback(dwDevice_t *dev) {
 
   dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
   const uint8_t anchorId = rxPacket.sourceAddress & 0xff;
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
   ctx.rxCount[anchorId]++;
+#endif
 
   dwTime_t arrival = {.full = 0};
   dwGetReceiveTimestamp(dev, &arrival);
@@ -312,16 +280,23 @@ static void rxcallback(dwDevice_t *dev) {
     tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchorId, now_ms, &anchorCtx);
     int rangeDataLength = updateRemoteData(&anchorCtx, packet);
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
     const bool doExcludeId = ctx.isTwrActive;
     const uint8_t excludedId = ctx.anchorId;
+#else
+    const bool doExcludeId = false;
+    const uint8_t excludedId = 0;
+#endif
     tdoaEngineProcessPacketFiltered(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, doExcludeId, excludedId);
 
     tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
     handleLppPacket(dataLength, rangeDataLength, &rxPacket, &anchorCtx);
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
     if (ctx.isTwrActive) {
       processTwoWayRanging(&anchorCtx, now_ms, txAn_in_cl_An, rxAn_by_T_in_cl_T, anchorId);
     }
+#endif
 
     ctx.isReceivingPackets = true;
   }
@@ -333,6 +308,53 @@ static void setRadioInReceiveMode(dwDevice_t *dev) {
   dwStartReceive(dev);
 }
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T, const uint8_t anchorId) {
+  // We assume updateRemoteData() has been called before this function
+  // and that the remote data from the current packet is in the storage, as we read data from the storage.
+
+  const uint32_t MAX_TIME_SINCE_TRANSMISSION_MS = 100;
+  const bool isLatestTransmisionTimeClose = ((now_ms - ctx.latestTransmissionTime_ms) < MAX_TIME_SINCE_TRANSMISSION_MS);
+  if (isLatestTransmisionTimeClose) {
+    uint8_t latestSeqReceivedByRemote = 255;
+    int64_t rxT_by_An_in_cl_An = 0;
+    if (tdoaStorageGetRemoteRxTimeSeqNr(anchorCtx, ctx.anchorId, &rxT_by_An_in_cl_An, &latestSeqReceivedByRemote)) {
+      STATS_CNT_RATE_EVENT(&ctx.cntTwrSeqNrOk);
+      const bool isLatestTxPacketReturnedFromRemote = (ctx.txSeqNr == latestSeqReceivedByRemote);
+      if (isLatestTxPacketReturnedFromRemote) {
+        const double clockCorrection = tdoaStorageGetClockCorrection(anchorCtx);
+        int64_t t_in_anchor_T = (int64_t)(clockCorrection * tdoaEngineTruncateToAnchorTimeStamp(txAn_in_cl_An - rxT_by_An_in_cl_An));
+        int64_t t_since_tx_T = tdoaEngineTruncateToAnchorTimeStamp(rxAn_by_T_in_cl_T - ctx.txT_in_cl_T);
+        int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
+
+        float distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ;
+        // TODO krri add real outlier filter. Simple implementation for our flight lab
+        if (distance > 0.0f && distance < 10.0f) {
+          point_t position;
+          if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
+            distanceMeasurement_t measurement = {
+              .distance = distance,
+              .stdDev = ctx.twrStdDev,
+              .x = position.x,
+              .y = position.y,
+              .z = position.z,
+            };
+
+            if (anchorId == ctx.logDistAnchorId) {
+              ctx.logDistance = measurement.distance;
+            }
+
+            if (ctx.useTwrForPositionEstimation) {
+              estimatorEnqueueDistance(&measurement);
+              STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
 static int countSeenAnchorsAndClearCounters() {
   int anchorsCount = 0;
 
@@ -389,36 +411,6 @@ static dwTime_t findTransmitTimeAsSoonAsPossible(dwDevice_t *dev) {
   return transmitTime;
 }
 
-static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet) {
-  static packet_t txPacket;
-  dwIdle(dev);
-
-  MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
-
-  txPacket.payload[LPS_TDOA3_TYPE] = LPP_HEADER_SHORT_PACKET;
-  memcpy(&txPacket.payload[LPS_TDOA3_SEND_LPP_PAYLOAD], packet->data, packet->length);
-
-  txPacket.pan = 0xbccf;
-  txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
-  txPacket.destAddress = 0xbccf000000000000 | packet->dest;
-
-  dwNewTransmit(dev);
-  dwSetDefaults(dev);
-  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
-
-  dwStartTransmit(dev);
-}
-
-static bool sendLpp(dwDevice_t *dev) {
-  bool lppPacketToSend = lpsGetLppShort(&ctx.lppPacket);
-  if (lppPacketToSend) {
-    sendLppShort(dev, &ctx.lppPacket);
-    return true;
-  }
-
-  return false;
-}
-
 static int populateTxData(rangePacket3_t *rangePacket) {
   // rangePacket->header.type already populated
   rangePacket->header.seq = ctx.txSeqNr;
@@ -507,12 +499,44 @@ static void setupTx(dwDevice_t *dev) {
 
   dwStartTransmit(dev);
 }
+#endif
+
+static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet) {
+  static packet_t txPacket;
+  dwIdle(dev);
+
+  MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+
+  txPacket.payload[LPS_TDOA3_TYPE] = LPP_HEADER_SHORT_PACKET;
+  memcpy(&txPacket.payload[LPS_TDOA3_SEND_LPP_PAYLOAD], packet->data, packet->length);
+
+  txPacket.pan = 0xbccf;
+  txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
+  txPacket.destAddress = 0xbccf000000000000 | packet->dest;
+
+  dwNewTransmit(dev);
+  dwSetDefaults(dev);
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
+
+  dwStartTransmit(dev);
+}
+
+static bool sendLpp(dwDevice_t *dev) {
+  bool lppPacketToSend = lpsGetLppShort(&ctx.lppPacket);
+  if (lppPacketToSend) {
+    sendLppShort(dev, &ctx.lppPacket);
+    return true;
+  }
+
+  return false;
+}
 
 static uint32_t startNextEvent(dwDevice_t *dev, const uint32_t now) {
   uint32_t timeout = 500;
 
   bool isTxPending = sendLpp(dev);
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
   if (ctx.isTwrActive) {
     if (!isTxPending) {
       if (ctx.nextTxTick < now) {
@@ -529,6 +553,7 @@ static uint32_t startNextEvent(dwDevice_t *dev, const uint32_t now) {
 
     timeout = ctx.nextTxTick - now;
   }
+#endif
 
   if (!isTxPending) {
     setRadioInReceiveMode(dev);
@@ -544,10 +569,10 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
       break;
     case eventTimeout:
       break;
-    case eventReceiveFailed:
-      break;
     case eventReceiveTimeout:
       break;
+    case eventReceiveFailed:
+      break;
     case eventPacketSent:
       break;
     default:
@@ -563,14 +588,15 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
 
 static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
   if (ctx.isTdoaActive) {
+    // Override the default standard deviation set by the TDoA engine.
+    tdoaMeasurement->stdDev = ctx.tdoaStdDev;
+
     estimatorEnqueueTDOA(tdoaMeasurement);
 
-    #ifdef LPS_2D_POSITION_HEIGHT
-    // If LPS_2D_POSITION_HEIGHT is defined we assume that we are doing 2D positioning.
-    // LPS_2D_POSITION_HEIGHT contains the height (Z) that the tag will be located at
+    #ifdef CONFIG_DECK_LOCO_2D_POSITION
     heightMeasurement_t heightData;
     heightData.timestamp = xTaskGetTickCount();
-    heightData.height = LPS_2D_POSITION_HEIGHT;
+    heightData.height = DECK_LOCO_2D_POSITION_HEIGHT;
     heightData.stdDev = 0.0001;
     estimatorEnqueueAbsoluteHeight(&heightData);
     #endif
@@ -603,28 +629,31 @@ static void Initialize(dwDevice_t *dev) {
   uint32_t now_ms = T2M(xTaskGetTickCount());
   tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmRandom);
 
-  #ifdef LPS_2D_POSITION_HEIGHT
-  DEBUG_PRINT("2D positioning enabled at %f m height\n", LPS_2D_POSITION_HEIGHT);
+  #ifdef CONFIG_DECK_LOCO_2D_POSITION
+  DEBUG_PRINT("2D positioning enabled at %f m height\n", DECK_LOCO_2D_POSITION_HEIGHT);
   #endif
 
   dwSetReceiveWaitTimeout(dev, TDOA3_RECEIVE_TIMEOUT);
 
   dwCommitConfiguration(dev);
 
+  ctx.tdoaStdDev = TDOA_ENGINE_MEASUREMENT_NOISE_STD;
+  ctx.isTdoaActive = true;
+  ctx.isReceivingPackets = false;
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
   ctx.anchorId = 255;
   ctx.txSeqNr = 0;
   ctx.latestTransmissionTime_ms = 0;
   ctx.nextTxTick = 0;
-  ctx.isTdoaActive = true;
   ctx.isTwrActive = false;
   ctx.twrSendEstimatedPosition = true;
+  ctx.twrStdDev = 0.25;
 
   ctx.averageTxDelay = 1000.0f / ANCHOR_MAX_TX_FREQ;
   ctx.nextTxDelayEvaluationTime_ms = 0;
 
-  ctx.isReceivingPackets = false;
-
-  // Get log ids to aquire the current estimated position
+  // Get log ids to acquire the current estimated position
   estimatedPosLogX = logGetVarId("stateEstimate", "x");
   estimatedPosLogY = logGetVarId("stateEstimate", "y");
   estimatedPosLogZ = logGetVarId("stateEstimate", "z");
@@ -632,6 +661,7 @@ static void Initialize(dwDevice_t *dev) {
   STATS_CNT_RATE_INIT(&ctx.cntPacketsTransmited, STATS_INTERVAL);
   STATS_CNT_RATE_INIT(&ctx.cntTwrSeqNrOk, STATS_INTERVAL);
   STATS_CNT_RATE_INIT(&ctx.cntTwrToEstimator, STATS_INTERVAL);
+#endif
 }
 
 static bool isRangingOk() {
@@ -647,19 +677,28 @@ uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
   .getActiveAnchorIdList = getActiveAnchorIdList,
 };
 
-
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
 LOG_GROUP_START(tdoa3)
   STATS_CNT_RATE_LOG_ADD(hmTx, &ctx.cntPacketsTransmited)
   STATS_CNT_RATE_LOG_ADD(hmSeqOk, &ctx.cntTwrSeqNrOk)
   STATS_CNT_RATE_LOG_ADD(hmEst, &ctx.cntTwrToEstimator)
-  LOG_ADD(LOG_FLOAT, hmDist, &logDistance)
+  LOG_ADD(LOG_FLOAT, hmDist, &ctx.logDistance)
 LOG_GROUP_STOP(tdoa3)
+#endif
 
 PARAM_GROUP_START(tdoa3)
+/**
+ * @brief The measurement noise to use when sending TDoA measurements to the estimator.
+ */
+PARAM_ADD(PARAM_FLOAT, stddev, &ctx.tdoaStdDev)
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
   PARAM_ADD(PARAM_UINT8, hmId, &ctx.anchorId)
   PARAM_ADD(PARAM_UINT8, hmTdoa, &ctx.isTdoaActive)
   PARAM_ADD(PARAM_UINT8, hmTwr, &ctx.isTwrActive)
   PARAM_ADD(PARAM_UINT8, hmTwrTXPos, &ctx.twrSendEstimatedPosition)
   PARAM_ADD(PARAM_UINT8, hmTwrEstPos, &ctx.useTwrForPositionEstimation)
-  PARAM_ADD(PARAM_UINT8, hmAnchLog, &logDistAnchorId)
+  PARAM_ADD(PARAM_UINT8, hmAnchLog, &ctx.logDistAnchorId)
+  PARAM_ADD(PARAM_FLOAT, twrStd, &ctx.twrStdDev)
+#endif
 PARAM_GROUP_STOP(tdoa3)

From aaa998f23977df8aeb588fc2d3441df5c4b40ca1 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Mon, 6 Nov 2023 14:21:34 +0100
Subject: [PATCH 05/13] Added outlier filter from TWR

---
 src/deck/drivers/src/lpsTdoa3Tag.c     | 114 +++++++++++++++++++++----
 src/utils/interface/tdoa/tdoaStorage.h |  11 +++
 2 files changed, 109 insertions(+), 16 deletions(-)

diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 8b4f07e82c..972aa8e0ac 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -66,6 +66,7 @@ for improved position estimation.
 
 #include "param.h"
 #include "autoconf.h"
+#include "cf_math.h"
 
 #include "physicalConstants.h"
 #include "statsCnt.h"
@@ -116,6 +117,12 @@ static logVarId_t estimatedPosLogZ;
 static const locoAddress_t base_address = 0xcfbc;
 
 static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T, const uint8_t anchorId);
+
+typedef enum {
+  txOwnPosNot = 0,
+  txOwnPosEstimated,
+  txOwnPosLocked
+} TxOwnPosition;
 #endif
 
 typedef struct {
@@ -168,10 +175,12 @@ static struct {
   // Transmit packets if true
   bool isTwrActive;
 
-  // Indicates if the current estimated position should be transmitted when sending packets.
-  // If false, the CF can use TWR for positioning itself only
-  // If true, other CFs can use the this CF as an anchor
-  bool twrSendEstimatedPosition;
+  // Indicates if position information should be transmitted when sending packets.
+  // If no position information is transmitted, the CF can use TWR for positioning itself only while other CFs can not
+  // use the transmitted packets for positioning.
+  // If position is transmitted other CFs can use the packets for positioning.
+  TxOwnPosition twrSendPosition;
+  struct lppShortAnchorPos_s twrTxPos;
 
   // Std dev for TWR samples sent to the estimator
   float twrStdDev;
@@ -179,10 +188,11 @@ static struct {
   // If TWR data should be used for position estimation
   bool useTwrForPositionEstimation;
 
-  statsCntRateLogger_t cntPacketsTransmited;
+  statsCntRateLogger_t cntPacketsTransmitted;
   statsCntRateLogger_t cntTwrSeqNrOk;
   statsCntRateLogger_t cntTwrToEstimator;
 
+  // Logging of hybrid mode distances
   uint8_t logDistAnchorId;
   float logDistance;
 #endif
@@ -309,6 +319,26 @@ static void setRadioInReceiveMode(dwDevice_t *dev) {
 }
 
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+
+static bool twrOutlierFilter(const tdoaAnchorContext_t* anchorCtx, const float distance) {
+  // Simplistic outlier filter for TWR samples in hybrid mode
+
+  tdoaAnchorInfo_t* info = anchorCtx->anchorInfo;
+  info->twrHistoryIndex = (info->twrHistoryIndex + 1) % TWR_HISTORY_LENGTH;
+
+  float32_t stddev;
+  arm_std_f32(info->twrHistory, TWR_HISTORY_LENGTH, &stddev);
+
+  float32_t mean;
+  arm_mean_f32(info->twrHistory, TWR_HISTORY_LENGTH, &mean);
+
+  float32_t diff = fabsf(mean - distance);
+  info->twrHistory[info->twrHistoryIndex] = distance;
+
+  bool sampleAccepted = diff < (TWR_OUTLIER_TH * stddev);
+  return sampleAccepted;
+}
+
 static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T, const uint8_t anchorId) {
   // We assume updateRemoteData() has been called before this function
   // and that the remote data from the current packet is in the storage, as we read data from the storage.
@@ -328,8 +358,9 @@ static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uin
         int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
 
         float distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ;
-        // TODO krri add real outlier filter. Simple implementation for our flight lab
-        if (distance > 0.0f && distance < 10.0f) {
+
+        bool sampleAccepted = twrOutlierFilter(anchorCtx, distance);
+        if (sampleAccepted) {
           point_t position;
           if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
             distanceMeasurement_t measurement = {
@@ -466,14 +497,19 @@ static void setTxData(dwDevice_t *dev)
 
   int rangePacketSize = populateTxData((rangePacket3_t *)txPacket.payload);
 
-  if (ctx.twrSendEstimatedPosition) {
+  if (ctx.twrSendPosition != txOwnPosLocked) {
+      // Store current estimated position
+      ctx.twrTxPos.x = logGetFloat(estimatedPosLogX);
+      ctx.twrTxPos.y = logGetFloat(estimatedPosLogY);
+      ctx.twrTxPos.z = logGetFloat(estimatedPosLogZ);
+  }
+
+  if (ctx.twrSendPosition != txOwnPosNot) {
     txPacket.payload[rangePacketSize + LPP_HEADER] = SHORT_LPP;
     txPacket.payload[rangePacketSize + LPP_TYPE] = LPP_SHORT_ANCHOR_POSITION;
 
     struct lppShortAnchorPos_s *pos = (struct lppShortAnchorPos_s*) &txPacket.payload[rangePacketSize + LPP_PAYLOAD];
-    pos->x = logGetFloat(estimatedPosLogX);
-    pos->y = logGetFloat(estimatedPosLogY);
-    pos->z = logGetFloat(estimatedPosLogZ);
+    *pos = ctx.twrTxPos;
 
     lppLength = 2 + sizeof(struct lppShortAnchorPos_s);
   }
@@ -544,7 +580,7 @@ static uint32_t startNextEvent(dwDevice_t *dev, const uint32_t now) {
         setupTx(dev);
         isTxPending = true;
         ctx.latestTransmissionTime_ms = now_ms;
-        STATS_CNT_RATE_EVENT(&ctx.cntPacketsTransmited);
+        STATS_CNT_RATE_EVENT(&ctx.cntPacketsTransmitted);
 
         uint32_t newDelay = randomizeDelayToNextTx(now_ms);
         ctx.nextTxTick = now + newDelay;
@@ -647,7 +683,7 @@ static void Initialize(dwDevice_t *dev) {
   ctx.latestTransmissionTime_ms = 0;
   ctx.nextTxTick = 0;
   ctx.isTwrActive = false;
-  ctx.twrSendEstimatedPosition = true;
+  ctx.twrSendPosition = txOwnPosNot;
   ctx.twrStdDev = 0.25;
 
   ctx.averageTxDelay = 1000.0f / ANCHOR_MAX_TX_FREQ;
@@ -658,7 +694,7 @@ static void Initialize(dwDevice_t *dev) {
   estimatedPosLogY = logGetVarId("stateEstimate", "y");
   estimatedPosLogZ = logGetVarId("stateEstimate", "z");
 
-  STATS_CNT_RATE_INIT(&ctx.cntPacketsTransmited, STATS_INTERVAL);
+  STATS_CNT_RATE_INIT(&ctx.cntPacketsTransmitted, STATS_INTERVAL);
   STATS_CNT_RATE_INIT(&ctx.cntTwrSeqNrOk, STATS_INTERVAL);
   STATS_CNT_RATE_INIT(&ctx.cntTwrToEstimator, STATS_INTERVAL);
 #endif
@@ -679,9 +715,24 @@ uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
 
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
 LOG_GROUP_START(tdoa3)
-  STATS_CNT_RATE_LOG_ADD(hmTx, &ctx.cntPacketsTransmited)
+  /**
+   * @brief Transmission rate of TWR packets in hybrid mode [packets/s]
+   */
+  STATS_CNT_RATE_LOG_ADD(hmTx, &ctx.cntPacketsTransmitted)
+
+  /**
+   * @brief Rate of received TWR packets with matching seq nr in hybrid mode [packets/s]
+   */
   STATS_CNT_RATE_LOG_ADD(hmSeqOk, &ctx.cntTwrSeqNrOk)
+
+  /**
+   * @brief Rate of TWR measurements that are sent to the estimator in hybrid mode [samples/s]
+   */
   STATS_CNT_RATE_LOG_ADD(hmEst, &ctx.cntTwrToEstimator)
+
+  /**
+   * @brief Measured distance to the anchor selected by the tdoa3.hmAnchLog parameter in hybrid mode [m]
+   */
   LOG_ADD(LOG_FLOAT, hmDist, &ctx.logDistance)
 LOG_GROUP_STOP(tdoa3)
 #endif
@@ -693,12 +744,43 @@ PARAM_GROUP_START(tdoa3)
 PARAM_ADD(PARAM_FLOAT, stddev, &ctx.tdoaStdDev)
 
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+  /**
+   * @brief Anchor id used when transmitting packets in hybrid mode [m]
+   */
   PARAM_ADD(PARAM_UINT8, hmId, &ctx.anchorId)
+
+  /**
+   * @brief If non-zero use TDoA for position estimation in hybrid mode
+   */
   PARAM_ADD(PARAM_UINT8, hmTdoa, &ctx.isTdoaActive)
+
+  /**
+   * @brief If non-zero transmit TWR packets in hybrid mode
+   */
   PARAM_ADD(PARAM_UINT8, hmTwr, &ctx.isTwrActive)
-  PARAM_ADD(PARAM_UINT8, hmTwrTXPos, &ctx.twrSendEstimatedPosition)
+
+  /**
+   * @brief Include position information in transmitted TWR packets for other CFs to use for positioning
+   * 0 = do not send TWR packets
+   * 1 = use the current estimated position
+   * 2 = use the estimated position that was sampled when this parameter was set to 2
+   */
+  PARAM_ADD(PARAM_UINT8, hmTwrTXPos, &ctx.twrSendPosition)
+
+  /**
+   * @brief If non-zero use data from the TWR process for position estimation in hybrid mode. Requires hmTwr to be enabled.
+   */
   PARAM_ADD(PARAM_UINT8, hmTwrEstPos, &ctx.useTwrForPositionEstimation)
+
+  /**
+   * @brief Select an anchor id to use for logging distance to a specific anchor. The distance is available in the
+   * tdoa3.hmDist log variable. Used in hybrid mode.
+   */
   PARAM_ADD(PARAM_UINT8, hmAnchLog, &ctx.logDistAnchorId)
+
+  /**
+   * @brief The measurement noise to use when sending TWR measurements to the estimator in hybrid mode
+   */
   PARAM_ADD(PARAM_FLOAT, twrStd, &ctx.twrStdDev)
 #endif
 PARAM_GROUP_STOP(tdoa3)
diff --git a/src/utils/interface/tdoa/tdoaStorage.h b/src/utils/interface/tdoa/tdoaStorage.h
index 5f5f0c26fc..ea2a0c4529 100644
--- a/src/utils/interface/tdoa/tdoaStorage.h
+++ b/src/utils/interface/tdoa/tdoaStorage.h
@@ -3,11 +3,16 @@
 
 #include "stabilizer_types.h"
 #include "clockCorrectionEngine.h"
+#include "autoconf.h"
 
 #define ANCHOR_STORAGE_COUNT 16
 #define REMOTE_ANCHOR_DATA_COUNT 16
 #define TOF_PER_ANCHOR_COUNT 16
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+#define TWR_HISTORY_LENGTH 32
+#define TWR_OUTLIER_TH 4
+#endif
 
 typedef struct {
   uint8_t id; // Id of remote remote anchor
@@ -37,6 +42,12 @@ typedef struct {
 
   tdoaTimeOfFlight_t tof[TOF_PER_ANCHOR_COUNT];
   tdoaRemoteAnchorData_t remoteAnchorData[REMOTE_ANCHOR_DATA_COUNT];
+
+  #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+  // Outlier filter for TWR in hybrid mode
+  float twrHistory[TWR_HISTORY_LENGTH];
+  uint8_t twrHistoryIndex;
+  #endif
 } tdoaAnchorInfo_t;
 
 typedef tdoaAnchorInfo_t tdaoAnchorInfoArray_t[ANCHOR_STORAGE_COUNT];

From f0959c80dde2d84ca04923809c493425622fc5bd Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 7 Nov 2023 15:10:46 +0100
Subject: [PATCH 06/13] Added documentation

---
 .../loco-positioning-system/index.md          |  69 +---------
 .../kalman_measurement_models.md              |  71 ++++++++++
 .../tdoa3_hybrid_mode.md                      | 121 ++++++++++++++++++
 3 files changed, 193 insertions(+), 68 deletions(-)
 create mode 100644 docs/functional-areas/loco-positioning-system/kalman_measurement_models.md
 create mode 100644 docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md

diff --git a/docs/functional-areas/loco-positioning-system/index.md b/docs/functional-areas/loco-positioning-system/index.md
index f7ffdeeaea..68c11d78fd 100644
--- a/docs/functional-areas/loco-positioning-system/index.md
+++ b/docs/functional-areas/loco-positioning-system/index.md
@@ -9,71 +9,4 @@ Most of the documentation for the Loco Positioning System can be found in the [l
 
 This section contains details specific to the implementation in the Crazyflie, mainly estimator related information.
 
-## Position estimation
-
-When using the Loco Positioning System the [Kalman estimator](/docs/functional-areas/sensor-to-control/state_estimators.md#extended-kalman-filter) is used for position estimation. The two main ranging schemes used for UWB localization are (i) two-way ranging (TWR) and (ii) time-difference-of-arrival (TDoA). In this page, we elaborate the measurement model, Kalman filter update and robust estimation for both TWR and TDoA.
-
-### Two-way ranging (TWR)
-
-In TWR, the UWB tag mounted on the Crazyflie communicates with fixed UWB anchors and acquires range measurements through two-way communication. The measurement model is as follows:
-
-$$d_i = \sqrt{(x-x_i)^2 +(y-y_i)^2 + (z-z_i)^2}$$,
-
-where $$(x, y, z)$$ is the position of the Crazyflie and $$(x_i, y_i, z_i)$$ is the position of the fixed anchor i. For the conventional extended Kalman filter, we have
-
-$$g_x = (x-x_i) / d_i$$
-
-$$g_y = (y-y_i) / d_i$$
-
-$$g_z = (z-z_i) / d_i$$.
-
-The H vector is
-
-$$H = (g_x, g_y, g_z, 0, 0, 0, 0, 0, 0)$$.
-
-Then, we call the function `kalmanCoreScalarUpdate()` in `kalman_core.c` to update the states and covariance matrix.
-
-### Time-difference-of-arrival (TDoA)
-In TDoA, UWB tags receive signals from anchors passively and compute the difference in distance beween two anchors as TDoA measurements. Since in TDoA scheme, UWB tags only listen to the messages from anchors, a TDoA-based localization system allows a theoretically unlimited number of robots to localize themselves with a small number of fixed anchors. However, TDoA measurements are more noisy than TWR measurements, leading to a less accurate localization performance. Two types of TDoA protocols ([TDoA2](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa2_protocol/) and [TDoA3](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa3_protocol/)) are implemented in LPS system. The main difference between the two TDoA protocols is that TDoA3 protocol achieves the scalability at the cost of localization accuracy. The readers are refer to [TDoA2 VS TDoA3](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/functional-areas/tdoa2-vs-tdoa3/) for detailed information.
-
-The TDoA measurement model is as follows:
-
-$$d_{ij} = d_j - d_i = \sqrt{(x-x_j)^2 +(y-y_j)^2 + (z-z_j)^2} - \sqrt{(x-x_i)^2 +(y-y_i)^2 + (z-z_i)^2}$$,
-
-where $$(x, y, z)$$ is the position of the Crazyflie and $$(x_i, y_i, z_i)$$ and $$(x_j, y_j, z_j)$$ are the positions of fixed anchor i and j, respectively. For the conventional extended Kalman filter, we have
-
-$$g_x = (x-x_j) / d_j - (x-x_i) / d_i$$
-
-$$g_y = (y-y_j) / d_j - (y-y_i) / d_i$$
-
-$$g_z = (z-z_j) / d_j - (z-z_i) / d_i$$.
-
-The H vector is
-
-$$H = (g_x, g_y, g_z, 0, 0, 0, 0, 0, 0)$$.
-
-Then, we call the function `kalmanCoreScalarUpdate()` in `kalman_core.c` to update the states and covariance matrix.
-
-### M-estimation based robust Kalman filter
-UWB radio signal suffers from outlier measurements caused by radio multi-path reflection and non-line-of-sight propagation. The large erroneous measurements often deteriorate the accuracy of UWB localization. The conventional Kalman filter is sensitive to measurement outliers due to its intrinsic minimum mean-square-error (MMSE) criterion. Here, we provide a robust estiamtion approach based on M-estimation robust cost function. We will explain the general idea of the robust Kalman filter and readers are encouraged to look into the firmware `mm_distance_robust.c` and `mm_tdoa_robust.c`. The implementation is based on paper [1] and please read the paper for implementation details.
-
-From the Bayesian maximum a posteriori perspective, the Kalman filter state estimation framework can be derived by solving the following minimization problem:
-
-![equation](/docs/images/rkf-eq1.png)
-
-Therein, $$x_k$$ and $$y_k$$ are the system state and measurements at timestep k, $$P_k$$ and $$R_k$$ denote the prior covariance and measurement covariance, respectively. Through Cholesky factorization of $$P_k$$ and $$R_k$    $, the original optimization problem is equivalent to:
-
-![equation](/docs/images/rkf-eq2.png)
-
-where $$e_{x,k,i}$$ and $$e_{y,k,i}$$ are the elements of $$e_{x,k}$$ and $$e_{y,k}$$. To reduce the influence of outliers, we incorporate a robust cost function into the Kalman filter framework as follows:
-
-![equation](/docs/images/rkf-eq3.png)
-
-where $$\rho()$$ could be any robust function (e.g., G-M, SC-DCS, Huber, Cauchy, etc.)
-
-By introducing a weight function for the process and measurement uncertainties---with e as input---we can translate the optimization problem into an Iterative Reweight Least-Square (IRLS) problem. Then, the optimal posterior estimate can be computed through iteratively solving the least-square problem using the robust weights computed from the previous solution. In our implementation, we use the G-M robust cost function and the maximum iteration is set to be two for computational frugality. Then, we call the function `kalmanCoreUpdateWithPKE()` in `kalman_core.c` with the weighted covariance matrix $$P_{w_m}$$, kalman gain Km, and innovation error to update the states and covariance matrix.
-
-This functionality can be turned on through setting a parameter (kalman.robustTwr or kalman.robustTdoa).
-
-## References
-[1] Zhao, Wenda, Jacopo Panerati, and Angela P. Schoellig. "Learning-based Bias Correction for Time Difference of Arrival Ultra-wideband Localization of Resource-constrained Mobile Robots." IEEE Robotics and Automation Letters 6, no. 2 (2021): 3639-3646.
+{% sub_page_menu %}
diff --git a/docs/functional-areas/loco-positioning-system/kalman_measurement_models.md b/docs/functional-areas/loco-positioning-system/kalman_measurement_models.md
new file mode 100644
index 0000000000..df81af9668
--- /dev/null
+++ b/docs/functional-areas/loco-positioning-system/kalman_measurement_models.md
@@ -0,0 +1,71 @@
+---
+title: Loco kalman measurment model
+page_id: loco_measurement_models
+---
+
+When using the Loco Positioning System the [Kalman estimator](/docs/functional-areas/sensor-to-control/state_estimators.md#extended-kalman-filter) is used for position estimation. The two main ranging schemes used for UWB localization are (i) two-way ranging (TWR) and (ii) time-difference-of-arrival (TDoA). In this page, we elaborate the measurement model, Kalman filter update and robust estimation for both TWR and TDoA.
+
+### Two-way ranging (TWR)
+
+In TWR, the UWB tag mounted on the Crazyflie communicates with fixed UWB anchors and acquires range measurements through two-way communication. The measurement model is as follows:
+
+$$d_i = \sqrt{(x-x_i)^2 +(y-y_i)^2 + (z-z_i)^2}$$,
+
+where $$(x, y, z)$$ is the position of the Crazyflie and $$(x_i, y_i, z_i)$$ is the position of the fixed anchor i. For the conventional extended Kalman filter, we have
+
+$$g_x = (x-x_i) / d_i$$
+
+$$g_y = (y-y_i) / d_i$$
+
+$$g_z = (z-z_i) / d_i$$.
+
+The H vector is
+
+$$H = (g_x, g_y, g_z, 0, 0, 0, 0, 0, 0)$$.
+
+Then, we call the function `kalmanCoreScalarUpdate()` in `kalman_core.c` to update the states and covariance matrix.
+
+### Time-difference-of-arrival (TDoA)
+In TDoA, UWB tags receive signals from anchors passively and compute the difference in distance beween two anchors as TDoA measurements. Since in TDoA scheme, UWB tags only listen to the messages from anchors, a TDoA-based localization system allows a theoretically unlimited number of robots to localize themselves with a small number of fixed anchors. However, TDoA measurements are more noisy than TWR measurements, leading to a less accurate localization performance. Two types of TDoA protocols ([TDoA2](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa2_protocol/) and [TDoA3](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa3_protocol/)) are implemented in LPS system. The main difference between the two TDoA protocols is that TDoA3 protocol achieves the scalability at the cost of localization accuracy. The readers are refer to [TDoA2 VS TDoA3](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/functional-areas/tdoa2-vs-tdoa3/) for detailed information.
+
+The TDoA measurement model is as follows:
+
+$$d_{ij} = d_j - d_i = \sqrt{(x-x_j)^2 +(y-y_j)^2 + (z-z_j)^2} - \sqrt{(x-x_i)^2 +(y-y_i)^2 + (z-z_i)^2}$$,
+
+where $$(x, y, z)$$ is the position of the Crazyflie and $$(x_i, y_i, z_i)$$ and $$(x_j, y_j, z_j)$$ are the positions of fixed anchor i and j, respectively. For the conventional extended Kalman filter, we have
+
+$$g_x = (x-x_j) / d_j - (x-x_i) / d_i$$
+
+$$g_y = (y-y_j) / d_j - (y-y_i) / d_i$$
+
+$$g_z = (z-z_j) / d_j - (z-z_i) / d_i$$.
+
+The H vector is
+
+$$H = (g_x, g_y, g_z, 0, 0, 0, 0, 0, 0)$$.
+
+Then, we call the function `kalmanCoreScalarUpdate()` in `kalman_core.c` to update the states and covariance matrix.
+
+### M-estimation based robust Kalman filter
+UWB radio signal suffers from outlier measurements caused by radio multi-path reflection and non-line-of-sight propagation. The large erroneous measurements often deteriorate the accuracy of UWB localization. The conventional Kalman filter is sensitive to measurement outliers due to its intrinsic minimum mean-square-error (MMSE) criterion. Here, we provide a robust estiamtion approach based on M-estimation robust cost function. We will explain the general idea of the robust Kalman filter and readers are encouraged to look into the firmware `mm_distance_robust.c` and `mm_tdoa_robust.c`. The implementation is based on paper [1] and please read the paper for implementation details.
+
+From the Bayesian maximum a posteriori perspective, the Kalman filter state estimation framework can be derived by solving the following minimization problem:
+
+![equation](/docs/images/rkf-eq1.png)
+
+Therein, $$x_k$$ and $$y_k$$ are the system state and measurements at timestep k, $$P_k$$ and $$R_k$$ denote the prior covariance and measurement covariance, respectively. Through Cholesky factorization of $$P_k$$ and $$R_k$    $, the original optimization problem is equivalent to:
+
+![equation](/docs/images/rkf-eq2.png)
+
+where $$e_{x,k,i}$$ and $$e_{y,k,i}$$ are the elements of $$e_{x,k}$$ and $$e_{y,k}$$. To reduce the influence of outliers, we incorporate a robust cost function into the Kalman filter framework as follows:
+
+![equation](/docs/images/rkf-eq3.png)
+
+where $$\rho()$$ could be any robust function (e.g., G-M, SC-DCS, Huber, Cauchy, etc.)
+
+By introducing a weight function for the process and measurement uncertainties---with e as input---we can translate the optimization problem into an Iterative Reweight Least-Square (IRLS) problem. Then, the optimal posterior estimate can be computed through iteratively solving the least-square problem using the robust weights computed from the previous solution. In our implementation, we use the G-M robust cost function and the maximum iteration is set to be two for computational frugality. Then, we call the function `kalmanCoreUpdateWithPKE()` in `kalman_core.c` with the weighted covariance matrix $$P_{w_m}$$, kalman gain Km, and innovation error to update the states and covariance matrix.
+
+This functionality can be turned on through setting a parameter (kalman.robustTwr or kalman.robustTdoa).
+
+## References
+[1] Zhao, Wenda, Jacopo Panerati, and Angela P. Schoellig. "Learning-based Bias Correction for Time Difference of Arrival Ultra-wideband Localization of Resource-constrained Mobile Robots." IEEE Robotics and Automation Letters 6, no. 2 (2021): 3639-3646.
diff --git a/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md b/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md
new file mode 100644
index 0000000000..2672b0284e
--- /dev/null
+++ b/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md
@@ -0,0 +1,121 @@
+---
+title: Loco TDoA3 hybrid mode
+page_id: loco_tdoa3_hybrid_mode
+---
+
+TDoA3 Hybrid mode is an experimental extension to the standard TDoA3 positioning mode. In standard TDoA3 the Crazyflies
+are passively listening to the UWB traffic between anchors for positioning, while the hybrid mode also enables the
+Crayzflies to transmit UWB packets. This makes it possible to use TWR for positioning the Crazyflie and use a Crazyflie
+as an anchor.
+
+## Functionality
+
+In a TDoA3 system the anchors are doing TWR with each other and the TDoA positioning that is used by the Crazyflies
+is almost a "side effect". For positioning to work the anchors are also including their own position in the transmitted
+packets, see [the TDoA3 protocol](https://www.bitcraze.io/documentation/repository/lps-node-firmware/master/protocols/tdoa3_protocol/).
+
+The TDoa3 implementation is designed to be dynamic in terms of availability of anchors and if an anchor is added or
+removed the system will adopt to the new environment, similarly if a Crazyflie passes through a large system it will
+adopt to the availability of anchors that are reachable in the current location. As long as each anchor uses a unique
+id all parts of the system will be able to handle dynamic situations.
+This property makes it possible to convert a passively listening Crazyflie to an actively transmitting node, the other
+members of the network will simply see it as another anchor and respond accordingly.
+
+## Use cases
+
+The most basic use case is to let a Crazyflie temporarily do TWR to measure the absolute distance to anchors instead of
+the relative distance measurements used in TDoA. TWR is more stable than TDoA when flying outside the convex hull of the
+anchors and it enables better positioning.
+
+If two Crazyflies turns on TWR it is possible to measure the distance between the two Crazyflies which could be used
+for collision avoidance or relative positioning for instance.
+
+Note that it is possible to use TWR and TDoA at the same time for estimating the position, the Crazyflie receives all
+packets and will use them for TDoA and/or TWR depending on if the remote party has received a packet from the Crazyflie
+that can be used for TWR.
+
+If a Crazyflie includes its position in the transmitted packets, other Crazyflies can also use the UWB traffic for TDoA
+or TWR positioning, just like another anchor. This makes it possible for a Crazyflie to fly to a new position, land and act
+as an anchor for other Crazyflies to use for positioning and thus extend the reach of the positioning system dynamically.
+There is an option to samples the current estimated position when the Crazyflie starts to transmit its position, the
+sampled position will be used in all transmissions to avoid a dynamically changing position.
+
+It is also possible to use the currently estimated position in the transmissions to use a Crazyflie as moving anchor.
+It might work for one moving anchor, but most likely the system will be instable if many Crazyflies do this at the
+same time without extended error handling as they will use each others position estimates (including errors) to
+estimate their own position. An extended error handling is not included in the implementation but hopefully it can be
+used as a base if someone is interested in experimenting in this area.
+
+## Implementation
+
+The hybrid mode is mainly implemented in the `lpsTdoa3Tag.c` file and is normally not compiled into the firmware, to
+enable it use the `CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE` kbuild flag (`Expansion deck configuration/Support TDoA3 hybrid mode`
+in `menuconfig`).
+
+There are no changes or extensions to the UWB protocol and the standard TDoA3 protocol is used. There are also no special
+handling in relation to position estimation, the same measurement models in the kalman estimator are used as in standard
+TDoA and TWR.
+
+## Params and logs
+
+There are a few parameters that must be configured to use the hybrid mode functionality, they are all part of the
+`tdoa3` parameter group.
+
+| Parameter     | Functionality                                                                                        |
+|---------------|------------------------------------------------------------------------------------------------------|
+| `hmId`        | The anchor id used in transmissions, this id must be unique in the system                            |
+| `hmTdoa`      | Indicates if received packets should be used for TDoA positioning                                    |
+| `hmTwr`       | Indicates if TWR packets should be transmitted                                                       |
+| `hmTwrTXPos`  | Indicates if a position should be included in transmitted packets, that is enable other Crazyflies to use the transmissions for TDoA positioning |
+| `hmTwrEstPos` | Indicates if received packets should be used for TWR positioning, requires hmTwr to be set           |
+| `twrStd`      | The measurement noise to use when sending TWR measurements to the estimator                          |
+| `hmAnchLog`   | Select an anchor id to use for logging distance to a specific anchor. The distance is available in the `hmDist` log variable |
+
+Similarly all logs are in the `tdoa3` log group.
+
+| Log       | Functionality                                                                      |
+|-----------|------------------------------------------------------------------------------------|
+| `hmDist`  | Measured distance to the anchor selected by the `hmAnchLog`` parameter [m]         |
+| `hmTx`    | Transmission rate of TWR packets in hybrid mode [packets/s]                        |
+| `hmSeqOk` | Rate of received TWR packets with matching seq nr in hybrid mode [packets/s]       |
+| `hmEst`   | Rate of TWR measurements that are sent to the estimator in hybrid mode [samples/s] |
+
+## Example configurations
+
+### Use TDoA and TWR for positioning
+
+| Parameter     | Value | Comment                                                    |
+|---------------|-------|------------------------------------------------------------|
+| `hmId`        | X     | Unique ids for each Crazyflie, it is set to 255 by default |
+| `hmTwr`       | 1     | to start transmission                                      |
+| `hmTwrEstPos` | 1     | to use TWR in position estimation                          |
+
+### Measure distance between two Crazyflies
+
+Settings for Crazyflie 1
+
+| Parameter     | Value | Comment                |
+|---------------|--------------------------------|
+| `hmId`        | 255   | the anchor id to use   |
+| `hmTwr`       | 1     | to start transmission  |
+| `hmAnchLog`   | 254   | the id of the other CF |
+
+Settings for Crazyflie 2
+
+| Parameter     | Value | Comment                |
+|---------------|--------------------------------|
+| `hmId`        | 254   | the anchor id to use   |
+| `hmTwr`       | 1     | to start transmission  |
+| `hmAnchLog`   | 255   | the id of the other CF |
+
+The distance to the other Crazyflie is available in the `tdoa3.hmDist` log variable.
+
+### Use the Crazyflie as a new anchor
+
+Assuming the Crazyflie is landed in a position where the estimated position is OK
+
+| Parameter     | Value                                                                  |
+|---------------|------------------------------------------------------------------------|
+| `hmId`        | Unique anchor id                                                       |
+| `hmTwr`       | 1 to start transmission                                                |
+| `hmTwrTXPos`  | 2 to sample the current estimated position and use it in transmissions |

From 0949762d490cb87e6d8cae48c799a28db109f01e Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 8 Nov 2023 14:32:17 +0100
Subject: [PATCH 07/13] Use quality info from the tdoa engine Various
 corrections

---
 .../tdoa3_hybrid_mode.md                      | 12 +--
 src/deck/drivers/src/lpsTdoa3Tag.c            | 74 ++++++++-----------
 src/utils/interface/tdoa/tdoaEngine.h         |  2 +-
 src/utils/src/tdoa/tdoaEngine.c               |  5 +-
 4 files changed, 39 insertions(+), 54 deletions(-)

diff --git a/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md b/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md
index 2672b0284e..0fc533e3a7 100644
--- a/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md
+++ b/docs/functional-areas/loco-positioning-system/tdoa3_hybrid_mode.md
@@ -63,7 +63,7 @@ There are a few parameters that must be configured to use the hybrid mode functi
 
 | Parameter     | Functionality                                                                                        |
 |---------------|------------------------------------------------------------------------------------------------------|
-| `hmId`        | The anchor id used in transmissions, this id must be unique in the system                            |
+| `hmId`        | The anchor id used in transmissions, this id must be unique in the system. Don't use 255 as it means "broadcast" and is received by all nodes |
 | `hmTdoa`      | Indicates if received packets should be used for TDoA positioning                                    |
 | `hmTwr`       | Indicates if TWR packets should be transmitted                                                       |
 | `hmTwrTXPos`  | Indicates if a position should be included in transmitted packets, that is enable other Crazyflies to use the transmissions for TDoA positioning |
@@ -86,7 +86,7 @@ Similarly all logs are in the `tdoa3` log group.
 
 | Parameter     | Value | Comment                                                    |
 |---------------|-------|------------------------------------------------------------|
-| `hmId`        | X     | Unique ids for each Crazyflie, it is set to 255 by default |
+| `hmId`        | X     | Unique ids for each Crazyflie, it is set to 254 by default |
 | `hmTwr`       | 1     | to start transmission                                      |
 | `hmTwrEstPos` | 1     | to use TWR in position estimation                          |
 
@@ -96,17 +96,17 @@ Settings for Crazyflie 1
 
 | Parameter     | Value | Comment                |
 |---------------|--------------------------------|
-| `hmId`        | 255   | the anchor id to use   |
+| `hmId`        | 254   | the anchor id to use   |
 | `hmTwr`       | 1     | to start transmission  |
-| `hmAnchLog`   | 254   | the id of the other CF |
+| `hmAnchLog`   | 253   | the id of the other CF |
 
 Settings for Crazyflie 2
 
 | Parameter     | Value | Comment                |
 |---------------|--------------------------------|
-| `hmId`        | 254   | the anchor id to use   |
+| `hmId`        | 253   | the anchor id to use   |
 | `hmTwr`       | 1     | to start transmission  |
-| `hmAnchLog`   | 255   | the id of the other CF |
+| `hmAnchLog`   | 254   | the id of the other CF |
 
 The distance to the other Crazyflie is available in the `tdoa3.hmDist` log variable.
 
diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 972aa8e0ac..2a254d7cd5 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -116,7 +116,7 @@ static logVarId_t estimatedPosLogZ;
 
 static const locoAddress_t base_address = 0xcfbc;
 
-static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T, const uint8_t anchorId);
+static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T);
 
 typedef enum {
   txOwnPosNot = 0,
@@ -293,18 +293,21 @@ static void rxcallback(dwDevice_t *dev) {
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
     const bool doExcludeId = ctx.isTwrActive;
     const uint8_t excludedId = ctx.anchorId;
+    const bool timeIsGood = tdoaEngineProcessPacketFiltered(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, doExcludeId, excludedId);
 #else
     const bool doExcludeId = false;
     const uint8_t excludedId = 0;
-#endif
     tdoaEngineProcessPacketFiltered(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, doExcludeId, excludedId);
+#endif
 
     tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
     handleLppPacket(dataLength, rangeDataLength, &rxPacket, &anchorCtx);
 
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
     if (ctx.isTwrActive) {
-      processTwoWayRanging(&anchorCtx, now_ms, txAn_in_cl_An, rxAn_by_T_in_cl_T, anchorId);
+      if (timeIsGood) {
+        processTwoWayRanging(&anchorCtx, now_ms, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+      }
     }
 #endif
 
@@ -320,26 +323,7 @@ static void setRadioInReceiveMode(dwDevice_t *dev) {
 
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
 
-static bool twrOutlierFilter(const tdoaAnchorContext_t* anchorCtx, const float distance) {
-  // Simplistic outlier filter for TWR samples in hybrid mode
-
-  tdoaAnchorInfo_t* info = anchorCtx->anchorInfo;
-  info->twrHistoryIndex = (info->twrHistoryIndex + 1) % TWR_HISTORY_LENGTH;
-
-  float32_t stddev;
-  arm_std_f32(info->twrHistory, TWR_HISTORY_LENGTH, &stddev);
-
-  float32_t mean;
-  arm_mean_f32(info->twrHistory, TWR_HISTORY_LENGTH, &mean);
-
-  float32_t diff = fabsf(mean - distance);
-  info->twrHistory[info->twrHistoryIndex] = distance;
-
-  bool sampleAccepted = diff < (TWR_OUTLIER_TH * stddev);
-  return sampleAccepted;
-}
-
-static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T, const uint8_t anchorId) {
+static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T) {
   // We assume updateRemoteData() has been called before this function
   // and that the remote data from the current packet is in the storage, as we read data from the storage.
 
@@ -358,27 +342,23 @@ static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uin
         int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
 
         float distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ;
+        point_t position;
+        if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
+          distanceMeasurement_t measurement = {
+            .distance = distance,
+            .stdDev = ctx.twrStdDev,
+            .x = position.x,
+            .y = position.y,
+            .z = position.z,
+          };
+
+          if (tdoaStorageGetId(anchorCtx) == ctx.logDistAnchorId) {
+            ctx.logDistance = measurement.distance;
+          }
 
-        bool sampleAccepted = twrOutlierFilter(anchorCtx, distance);
-        if (sampleAccepted) {
-          point_t position;
-          if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
-            distanceMeasurement_t measurement = {
-              .distance = distance,
-              .stdDev = ctx.twrStdDev,
-              .x = position.x,
-              .y = position.y,
-              .z = position.z,
-            };
-
-            if (anchorId == ctx.logDistAnchorId) {
-              ctx.logDistance = measurement.distance;
-            }
-
-            if (ctx.useTwrForPositionEstimation) {
-              estimatorEnqueueDistance(&measurement);
-              STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
-            }
+          if (ctx.useTwrForPositionEstimation) {
+            estimatorEnqueueDistance(&measurement);
+            STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);
           }
         }
       }
@@ -512,6 +492,8 @@ static void setTxData(dwDevice_t *dev)
     *pos = ctx.twrTxPos;
 
     lppLength = 2 + sizeof(struct lppShortAnchorPos_s);
+  } else {
+    lppLength = 0;
   }
 
 
@@ -678,13 +660,14 @@ static void Initialize(dwDevice_t *dev) {
   ctx.isReceivingPackets = false;
 
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
-  ctx.anchorId = 255;
+  ctx.anchorId = 254;
   ctx.txSeqNr = 0;
   ctx.latestTransmissionTime_ms = 0;
   ctx.nextTxTick = 0;
   ctx.isTwrActive = false;
   ctx.twrSendPosition = txOwnPosNot;
   ctx.twrStdDev = 0.25;
+  ctx.useTwrForPositionEstimation = false;
 
   ctx.averageTxDelay = 1000.0f / ANCHOR_MAX_TX_FREQ;
   ctx.nextTxDelayEvaluationTime_ms = 0;
@@ -745,7 +728,8 @@ PARAM_ADD(PARAM_FLOAT, stddev, &ctx.tdoaStdDev)
 
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
   /**
-   * @brief Anchor id used when transmitting packets in hybrid mode [m]
+   * @brief Anchor id used when transmitting packets in hybrid mode. Don't use 255 as this means broadcast and is
+   * received by all nodes.
    */
   PARAM_ADD(PARAM_UINT8, hmId, &ctx.anchorId)
 
diff --git a/src/utils/interface/tdoa/tdoaEngine.h b/src/utils/interface/tdoa/tdoaEngine.h
index ec8b06295f..022a03e3eb 100644
--- a/src/utils/interface/tdoa/tdoaEngine.h
+++ b/src/utils/interface/tdoa/tdoaEngine.h
@@ -41,7 +41,7 @@ void tdoaEngineInit(tdoaEngineState_t* state, const uint32_t now_ms, tdoaEngineS
 
 void tdoaEngineGetAnchorCtxForPacketProcessing(tdoaEngineState_t* engineState, const uint8_t anchorId, const uint32_t currentTime_ms, tdoaAnchorContext_t* anchorCtx);
 void tdoaEngineProcessPacket(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T);
-void tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const bool doExcludeId, const uint8_t excludedId);
+bool tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const bool doExcludeId, const uint8_t excludedId);
 
 #define TDOA_ENGINE_TRUNCATE_TO_ANCHOR_TS_BITMAP 0x00FFFFFFFF
 static inline uint64_t tdoaEngineTruncateToAnchorTimeStamp(uint64_t fullTimeStamp) {
diff --git a/src/utils/src/tdoa/tdoaEngine.c b/src/utils/src/tdoa/tdoaEngine.c
index 4c7140d415..6175a2cb5a 100644
--- a/src/utils/src/tdoa/tdoaEngine.c
+++ b/src/utils/src/tdoa/tdoaEngine.c
@@ -169,7 +169,7 @@ static bool matchYoungestAnchor(tdoaEngineState_t* engineState, tdoaAnchorContex
       if (!doExcludeId || (excludedId != candidateAnchorId)) {
         if (tdoaStorageGetTimeOfFlight(anchorCtx, candidateAnchorId)) {
           if (tdoaStorageGetCreateAnchorCtx(engineState->anchorInfoArray, candidateAnchorId, now_ms, otherAnchorCtx)) {
-            uint32_t updateTime = otherAnchorCtx->anchorInfo->lastUpdateTime;
+            uint32_t updateTime = tdoaStorageGetLastUpdateTime(otherAnchorCtx);
             if (updateTime > youmgestUpdateTime) {
               if (engineState->matching.seqNr[index] == tdoaStorageGetSeqNr(otherAnchorCtx)) {
                 youmgestUpdateTime = updateTime;
@@ -224,7 +224,7 @@ void tdoaEngineProcessPacket(tdoaEngineState_t* engineState, tdoaAnchorContext_t
   tdoaEngineProcessPacketFiltered(engineState, anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, false, 0);
 }
 
-void tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const bool doExcludeId, const uint8_t excludedId) {
+bool tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T, const bool doExcludeId, const uint8_t excludedId) {
   bool timeIsGood = updateClockCorrection(anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T, &engineState->stats);
   if (timeIsGood) {
     STATS_CNT_RATE_EVENT(&engineState->stats.timeIsGood);
@@ -236,4 +236,5 @@ void tdoaEngineProcessPacketFiltered(tdoaEngineState_t* engineState, tdoaAnchorC
       enqueueTDOA(&otherAnchorCtx, anchorCtx, tdoaDistDiff, engineState);
     }
   }
+  return timeIsGood;
 }

From c365fc9df4faed7526d03875e33511230ba2fcdd Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 9 Nov 2023 15:46:30 +0100
Subject: [PATCH 08/13] Empty anchor list when anchors are not seen

---
 src/deck/drivers/src/lpsTdoa3Tag.c | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 2a254d7cd5..e65ab36f2f 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -422,7 +422,7 @@ static dwTime_t findTransmitTimeAsSoonAsPossible(dwDevice_t *dev) {
   return transmitTime;
 }
 
-static int populateTxData(rangePacket3_t *rangePacket) {
+static int populateTxData(rangePacket3_t *rangePacket, const uint32_t now_ms) {
   // rangePacket->header.type already populated
   rangePacket->header.seq = ctx.txSeqNr;
   rangePacket->header.txTimeStamp = ctx.txT_in_cl_T;
@@ -433,7 +433,7 @@ static int populateTxData(rangePacket3_t *rangePacket) {
   // Consider a more clever selection of which anchors to include as remote data.
   // This implementation will give a somewhat randomized set but can probably be improved
   uint8_t ids[MAX_NR_OF_ANCHORS_IN_TX];
-  uint8_t anchorCount = tdoaStorageGetListOfAnchorIds(tdoaEngineState.anchorInfoArray, ids, MAX_NR_OF_ANCHORS_IN_TX);
+  uint8_t anchorCount = tdoaStorageGetListOfActiveAnchorIds(tdoaEngineState.anchorInfoArray, ids, MAX_NR_OF_ANCHORS_IN_TX, now_ms);
 
   for (uint8_t i = 0; i < anchorCount; i++) {
     remoteAnchorDataFull_t* anchorData = (remoteAnchorDataFull_t*) anchorDataPtr;
@@ -459,7 +459,7 @@ static int populateTxData(rangePacket3_t *rangePacket) {
 }
 
 // Set TX data in the radio TX buffer
-static void setTxData(dwDevice_t *dev)
+static void setTxData(dwDevice_t *dev, const uint32_t now_ms)
 {
   static packet_t txPacket;
   static bool firstEntry = true;
@@ -475,7 +475,7 @@ static void setTxData(dwDevice_t *dev)
 
   txPacket.sourceAddress = (base_address & 0xffffffffffffff00) | ctx.anchorId;
 
-  int rangePacketSize = populateTxData((rangePacket3_t *)txPacket.payload);
+  int rangePacketSize = populateTxData((rangePacket3_t *)txPacket.payload, now_ms);
 
   if (ctx.twrSendPosition != txOwnPosLocked) {
       // Store current estimated position
@@ -496,20 +496,19 @@ static void setTxData(dwDevice_t *dev)
     lppLength = 0;
   }
 
-
   dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH + rangePacketSize + lppLength);
 }
 
 
 // Setup the radio to send a ranging packet
-static void setupTx(dwDevice_t *dev) {
+static void setupTx(dwDevice_t *dev, const uint32_t now_ms) {
   dwTime_t txTime = findTransmitTimeAsSoonAsPossible(dev);
   ctx.txT_in_cl_T = txTime.low32;
   ctx.txSeqNr = (ctx.txSeqNr + 1) & 0x7f;
 
   dwIdle(dev);
 
-  setTxData(dev);
+  setTxData(dev, now_ms);
 
   dwNewTransmit(dev);
   dwSetDefaults(dev);
@@ -559,7 +558,7 @@ static uint32_t startNextEvent(dwDevice_t *dev, const uint32_t now) {
     if (!isTxPending) {
       if (ctx.nextTxTick < now) {
         const uint32_t now_ms = T2M(now);
-        setupTx(dev);
+        setupTx(dev, now_ms);
         isTxPending = true;
         ctx.latestTransmissionTime_ms = now_ms;
         STATS_CNT_RATE_EVENT(&ctx.cntPacketsTransmitted);

From d3735f7c3caba0576b58fca3111293a20e5132a5 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 9 Nov 2023 15:56:14 +0100
Subject: [PATCH 09/13] Log distance also when not using estimator

---
 src/deck/drivers/src/lpsTdoa3Tag.c | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index e65ab36f2f..648a60dcd4 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -342,6 +342,11 @@ static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uin
         int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
 
         float distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ;
+
+        if (tdoaStorageGetId(anchorCtx) == ctx.logDistAnchorId) {
+          ctx.logDistance = distance;
+        }
+
         point_t position;
         if (tdoaStorageGetAnchorPosition(anchorCtx, &position)) {
           distanceMeasurement_t measurement = {
@@ -352,10 +357,6 @@ static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uin
             .z = position.z,
           };
 
-          if (tdoaStorageGetId(anchorCtx) == ctx.logDistAnchorId) {
-            ctx.logDistance = measurement.distance;
-          }
-
           if (ctx.useTwrForPositionEstimation) {
             estimatorEnqueueDistance(&measurement);
             STATS_CNT_RATE_EVENT(&ctx.cntTwrToEstimator);

From 2d911975224420df438c0bbf733d6f7585bd88fd Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 9 Nov 2023 17:51:40 +0100
Subject: [PATCH 10/13] Add tof to anchor list

---
 src/deck/drivers/src/lpsTdoa2Tag.c      |  2 +-
 src/deck/drivers/src/lpsTdoa3Tag.c      | 25 +++++++++++-----
 src/utils/interface/tdoa/tdoaStorage.h  | 16 +++++++----
 src/utils/src/tdoa/tdoaEngine.c         |  6 ++--
 src/utils/src/tdoa/tdoaStorage.c        | 38 ++++++++++++++++++-------
 test/utils/src/tdoa/test_tdoa_storage.c | 32 +++++++++++++++++----
 6 files changed, 85 insertions(+), 34 deletions(-)

diff --git a/src/deck/drivers/src/lpsTdoa2Tag.c b/src/deck/drivers/src/lpsTdoa2Tag.c
index 4f914b61c5..8f739413c3 100644
--- a/src/deck/drivers/src/lpsTdoa2Tag.c
+++ b/src/deck/drivers/src/lpsTdoa2Tag.c
@@ -116,7 +116,7 @@ static void updateRemoteData(tdoaAnchorContext_t* anchorCtx, const rangePacket2_
       if (hasDistance) {
         int64_t tof = packet->distances[i];
         if (isValidTimeStamp(tof)) {
-          tdoaStorageSetTimeOfFlight(anchorCtx, remoteId, tof);
+          tdoaStorageSetRemoteTimeOfFlight(anchorCtx, remoteId, tof);
 
           if (isConsecutiveIds(previousAnchor, anchorId)) {
             logAnchorDistance[anchorId] = packet->distances[previousAnchor];
diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 648a60dcd4..835e6cb35a 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -109,6 +109,10 @@ for improved position estimation.
 #define SHORT_LPP 0xF0
 #define LPP_SHORT_ANCHOR_POSITION 0x01
 
+// Maximum acceptable age of time of flight information to be used in transmissions. The ToF information is used by
+// other CFs for TDoA positioning. A longer time is acceptable if the transmitting CF is not moving.
+#define MAX_AGE_OF_TOF_MS 200
+
 // Log ids for estimated position
 static logVarId_t estimatedPosLogX;
 static logVarId_t estimatedPosLogY;
@@ -116,7 +120,7 @@ static logVarId_t estimatedPosLogZ;
 
 static const locoAddress_t base_address = 0xcfbc;
 
-static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T);
+static void processTwoWayRanging(tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T);
 
 typedef enum {
   txOwnPosNot = 0,
@@ -220,7 +224,7 @@ static int updateRemoteData(tdoaAnchorContext_t* anchorCtx, const void* payload)
     if (hasDistance) {
       int64_t tof = anchorData->distance;
       if (isValidTimeStamp(tof)) {
-        tdoaStorageSetTimeOfFlight(anchorCtx, remoteId, tof);
+        tdoaStorageSetRemoteTimeOfFlight(anchorCtx, remoteId, tof);
 
         uint8_t anchorId = tdoaStorageGetId(anchorCtx);
         tdoaStats_t* stats = &tdoaEngineState.stats;
@@ -323,7 +327,7 @@ static void setRadioInReceiveMode(dwDevice_t *dev) {
 
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
 
-static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T) {
+static void processTwoWayRanging(tdoaAnchorContext_t* anchorCtx, const uint32_t now_ms, const uint64_t txAn_in_cl_An, const uint64_t rxAn_by_T_in_cl_T) {
   // We assume updateRemoteData() has been called before this function
   // and that the remote data from the current packet is in the storage, as we read data from the storage.
 
@@ -342,6 +346,7 @@ static void processTwoWayRanging(const tdoaAnchorContext_t* anchorCtx, const uin
         int64_t tof_T = (t_since_tx_T - t_in_anchor_T) / 2;
 
         float distance = SPEED_OF_LIGHT * (tof_T - LOCODECK_ANTENNA_DELAY) / LOCODECK_TS_FREQ;
+        tdoaStorageSetTimeOfFlight(anchorCtx, tof_T, now_ms);
 
         if (tdoaStorageGetId(anchorCtx) == ctx.logDistAnchorId) {
           ctx.logDistance = distance;
@@ -446,11 +451,17 @@ static int populateTxData(rangePacket3_t *rangePacket, const uint32_t now_ms) {
     tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, id, now_ms, &anchorCtx);
 
     anchorData->id = id;
-    anchorData->seq = anchorCtx.anchorInfo->seqNr;
-    anchorData->rxTimeStamp = anchorCtx.anchorInfo->rxTime;
+    anchorData->seq = tdoaStorageGetSeqNr(&anchorCtx);
+    anchorData->rxTimeStamp = tdoaStorageGetRxTime(&anchorCtx);
 
-    // Not adding distance to other anchors, only support simple ranging in hybrid mode (TWR) to anchors for now.
-    anchorDataPtr += sizeof(remoteAnchorDataShort_t);
+    uint64_t tof = tdoaStorageGetTimeOfFlight(&anchorCtx, now_ms - MAX_AGE_OF_TOF_MS);
+    if (tof > 0 && tof <= 0xfffful) {
+      anchorData->distance = tof;
+      anchorDataPtr += sizeof(remoteAnchorDataFull_t);
+      anchorData->seq |= 0x80;  // Set bit to indicate tof is included
+    } else {
+      anchorDataPtr += sizeof(remoteAnchorDataShort_t);
+    }
 
     remoteAnchorCount++;
   }
diff --git a/src/utils/interface/tdoa/tdoaStorage.h b/src/utils/interface/tdoa/tdoaStorage.h
index ea2a0c4529..85aadd4a09 100644
--- a/src/utils/interface/tdoa/tdoaStorage.h
+++ b/src/utils/interface/tdoa/tdoaStorage.h
@@ -40,13 +40,12 @@ typedef struct {
 
   point_t position; // The coordinates of the anchor
 
-  tdoaTimeOfFlight_t tof[TOF_PER_ANCHOR_COUNT];
+  tdoaTimeOfFlight_t remoteTof[TOF_PER_ANCHOR_COUNT];
   tdoaRemoteAnchorData_t remoteAnchorData[REMOTE_ANCHOR_DATA_COUNT];
 
   #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
-  // Outlier filter for TWR in hybrid mode
-  float twrHistory[TWR_HISTORY_LENGTH];
-  uint8_t twrHistoryIndex;
+  uint64_t tof;
+  uint32_t tofTime_ms;
   #endif
 } tdoaAnchorInfo_t;
 
@@ -83,8 +82,13 @@ int64_t tdoaStorageGetRemoteRxTime(const tdoaAnchorContext_t* anchorCtx, const u
 bool tdoaStorageGetRemoteRxTimeSeqNr(const tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, int64_t* rxTime, uint8_t* seqNr);
 void tdoaStorageSetRemoteRxTime(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t remoteRxTime, const uint8_t remoteSeqNr);
 void tdoaStorageGetRemoteSeqNrList(const tdoaAnchorContext_t* anchorCtx, int* remoteCount, uint8_t seqNr[], uint8_t id[]);
-int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint8_t otherAnchor);
-void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof);
+int64_t tdoaStorageGetRemoteTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint8_t otherAnchor);
+void tdoaStorageSetRemoteTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof);
+
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint32_t oldestAcceptableTime_ms);
+void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const int64_t tof, const uint32_t currentTime_ms);
+#endif
 
 // Mainly for test
 bool tdoaStorageIsAnchorInStorage(tdoaAnchorInfo_t anchorStorage[], const uint8_t anchor);
diff --git a/src/utils/src/tdoa/tdoaEngine.c b/src/utils/src/tdoa/tdoaEngine.c
index 6175a2cb5a..46fd2fc350 100644
--- a/src/utils/src/tdoa/tdoaEngine.c
+++ b/src/utils/src/tdoa/tdoaEngine.c
@@ -113,7 +113,7 @@ static bool updateClockCorrection(tdoaAnchorContext_t* anchorCtx, const int64_t
 static int64_t calcTDoA(const tdoaAnchorContext_t* otherAnchorCtx, const tdoaAnchorContext_t* anchorCtx, const int64_t txAn_in_cl_An, const int64_t rxAn_by_T_in_cl_T) {
   const uint8_t otherAnchorId = tdoaStorageGetId(otherAnchorCtx);
 
-  const int64_t tof_Ar_to_An_in_cl_An = tdoaStorageGetTimeOfFlight(anchorCtx, otherAnchorId);
+  const int64_t tof_Ar_to_An_in_cl_An = tdoaStorageGetRemoteTimeOfFlight(anchorCtx, otherAnchorId);
   const int64_t rxAr_by_An_in_cl_An = tdoaStorageGetRemoteRxTime(anchorCtx, otherAnchorId);
   const double clockCorrection = tdoaStorageGetClockCorrection(anchorCtx);
 
@@ -145,7 +145,7 @@ static bool matchRandomAnchor(tdoaEngineState_t* engineState, tdoaAnchorContext_
     const uint8_t candidateAnchorId = engineState->matching.id[index];
     if (!doExcludeId || (excludedId != candidateAnchorId)) {
       if (tdoaStorageGetCreateAnchorCtx(engineState->anchorInfoArray, candidateAnchorId, now_ms, otherAnchorCtx)) {
-        if (engineState->matching.seqNr[index] == tdoaStorageGetSeqNr(otherAnchorCtx) && tdoaStorageGetTimeOfFlight(anchorCtx, candidateAnchorId)) {
+        if (engineState->matching.seqNr[index] == tdoaStorageGetSeqNr(otherAnchorCtx) && tdoaStorageGetRemoteTimeOfFlight(anchorCtx, candidateAnchorId)) {
           return true;
         }
       }
@@ -167,7 +167,7 @@ static bool matchYoungestAnchor(tdoaEngineState_t* engineState, tdoaAnchorContex
     for (int index = 0; index < remoteCount; index++) {
       const uint8_t candidateAnchorId = engineState->matching.id[index];
       if (!doExcludeId || (excludedId != candidateAnchorId)) {
-        if (tdoaStorageGetTimeOfFlight(anchorCtx, candidateAnchorId)) {
+        if (tdoaStorageGetRemoteTimeOfFlight(anchorCtx, candidateAnchorId)) {
           if (tdoaStorageGetCreateAnchorCtx(engineState->anchorInfoArray, candidateAnchorId, now_ms, otherAnchorCtx)) {
             uint32_t updateTime = tdoaStorageGetLastUpdateTime(otherAnchorCtx);
             if (updateTime > youmgestUpdateTime) {
diff --git a/src/utils/src/tdoa/tdoaStorage.c b/src/utils/src/tdoa/tdoaStorage.c
index 9fa7d785ec..cb6e5a4cbf 100644
--- a/src/utils/src/tdoa/tdoaStorage.c
+++ b/src/utils/src/tdoa/tdoaStorage.c
@@ -187,6 +187,22 @@ void tdoaStorageSetRxTxData(tdoaAnchorContext_t* anchorCtx, int64_t rxTime, int6
   anchorInfo->lastUpdateTime = now;
 }
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint32_t oldestAcceptableTime_ms) {
+  if (anchorCtx->anchorInfo->tofTime_ms < oldestAcceptableTime_ms) {
+    return 0;
+  }
+
+  return anchorCtx->anchorInfo->tof;
+}
+
+void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const int64_t tof, const uint32_t currentTime_ms) {
+  anchorCtx->anchorInfo->tof = tof;
+  anchorCtx->anchorInfo->tofTime_ms = currentTime_ms;
+}
+
+#endif
+
 double tdoaStorageGetClockCorrection(const tdoaAnchorContext_t* anchorCtx) {
   return clockCorrectionEngineGet(&anchorCtx->anchorInfo->clockCorrectionStorage);
 }
@@ -264,14 +280,14 @@ void tdoaStorageGetRemoteSeqNrList(const tdoaAnchorContext_t* anchorCtx, int* re
   *remoteCount = count;
 }
 
-int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint8_t otherAnchor) {
+int64_t tdoaStorageGetRemoteTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const uint8_t otherAnchor) {
   const tdoaAnchorInfo_t* anchorInfo = anchorCtx->anchorInfo;
 
   for (int i = 0; i < TOF_PER_ANCHOR_COUNT; i++) {
-    if (otherAnchor == anchorInfo->tof[i].id) {
+    if (otherAnchor == anchorInfo->remoteTof[i].id) {
       uint32_t now = anchorCtx->currentTime_ms;
-      if (anchorInfo->tof[i].endOfLife > now) {
-        return anchorInfo->tof[i].tof;
+      if (anchorInfo->remoteTof[i].endOfLife > now) {
+        return anchorInfo->remoteTof[i].tof;
       }
       break;
     }
@@ -280,7 +296,7 @@ int64_t tdoaStorageGetTimeOfFlight(const tdoaAnchorContext_t* anchorCtx, const u
   return 0;
 }
 
-void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof) {
+void tdoaStorageSetRemoteTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t remoteAnchor, const int64_t tof) {
   tdoaAnchorInfo_t* anchorInfo = anchorCtx->anchorInfo;
 
   int indexToUpdate = 0;
@@ -288,20 +304,20 @@ void tdoaStorageSetTimeOfFlight(tdoaAnchorContext_t* anchorCtx, const uint8_t re
   uint32_t oldestTime = 0xFFFFFFFF;
 
   for (int i = 0; i < TOF_PER_ANCHOR_COUNT; i++) {
-    if (remoteAnchor == anchorInfo->tof[i].id) {
+    if (remoteAnchor == anchorInfo->remoteTof[i].id) {
       indexToUpdate = i;
       break;
     }
 
-    if (anchorInfo->tof[i].endOfLife < oldestTime) {
-      oldestTime = anchorInfo->tof[i].endOfLife;
+    if (anchorInfo->remoteTof[i].endOfLife < oldestTime) {
+      oldestTime = anchorInfo->remoteTof[i].endOfLife;
       indexToUpdate = i;
     }
   }
 
-  anchorInfo->tof[indexToUpdate].id = remoteAnchor;
-  anchorInfo->tof[indexToUpdate].tof = tof;
-  anchorInfo->tof[indexToUpdate].endOfLife = now + TOF_VALIDITY_PERIOD;
+  anchorInfo->remoteTof[indexToUpdate].id = remoteAnchor;
+  anchorInfo->remoteTof[indexToUpdate].tof = tof;
+  anchorInfo->remoteTof[indexToUpdate].endOfLife = now + TOF_VALIDITY_PERIOD;
 }
 
 bool tdoaStorageIsAnchorInStorage(tdoaAnchorInfo_t anchorStorage[], const uint8_t anchor) {
diff --git a/test/utils/src/tdoa/test_tdoa_storage.c b/test/utils/src/tdoa/test_tdoa_storage.c
index bcfd605801..31b5c3fb7a 100644
--- a/test/utils/src/tdoa/test_tdoa_storage.c
+++ b/test/utils/src/tdoa/test_tdoa_storage.c
@@ -599,7 +599,7 @@ void testThatNoTimeOfFlightIsReturnedWhenRemoteAnchorIsNotInStorage() {
   tdoaStorageGetCreateAnchorCtx(storage, anchor, storageTime, &context);
 
   // Test
-  int64_t actual = tdoaStorageGetTimeOfFlight(&context, remoteAnchor);
+  int64_t actual = tdoaStorageGetRemoteTimeOfFlight(&context, remoteAnchor);
 
   // Assert
   TEST_ASSERT_EQUAL_INT64(expected, actual);
@@ -617,7 +617,7 @@ void testThatTimeOfFlightIsReturnedWhenSet() {
   fixtureSetTof(&context, anchor, storageTime, remoteAnchor, expected);
 
   // Test
-  int64_t actual = tdoaStorageGetTimeOfFlight(&context, remoteAnchor);
+  int64_t actual = tdoaStorageGetRemoteTimeOfFlight(&context, remoteAnchor);
 
   // Assert
   TEST_ASSERT_EQUAL_INT64(expected, actual);
@@ -638,7 +638,7 @@ void testThatTimeOfFlightIsReturnedWhenSetASecondTime() {
   fixtureSetTof(&context, anchor, storageTime, remoteAnchor, expected);
 
   // Test
-  int64_t actual = tdoaStorageGetTimeOfFlight(&context, remoteAnchor);
+  int64_t actual = tdoaStorageGetRemoteTimeOfFlight(&context, remoteAnchor);
 
   // Assert
   TEST_ASSERT_EQUAL_INT64(expected, actual);
@@ -675,13 +675,33 @@ void testThatTofReplacesTheOldestEntryWhenStorageIsFull() {
   fixtureSetTof(&context, anchor, verificationStorageTime, verificationRemoteAnchor, verificationTof);
 
   // Assert
-  const int64_t actualVerification = tdoaStorageGetTimeOfFlight(&context, verificationRemoteAnchor);
+  const int64_t actualVerification = tdoaStorageGetRemoteTimeOfFlight(&context, verificationRemoteAnchor);
   TEST_ASSERT_EQUAL_INT64(verificationTof, actualVerification);
 
-  const int64_t actualReplaced = tdoaStorageGetTimeOfFlight(&context, oldestRemoteAnchor);
+  const int64_t actualReplaced = tdoaStorageGetRemoteTimeOfFlight(&context, oldestRemoteAnchor);
   TEST_ASSERT_EQUAL_INT64(0, actualReplaced);
 }
 
+#ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
+// Disabled for now as the unit test system does not support kconfig
+void _testThatTimeOfFlightIsSet() {
+  // Fixture
+  uint32_t storageTime_ms = 1234;
+  int64_t expectedToF = 4747474747;
+
+  tdoaAnchorContext_t context;
+  tdoaStorageGetCreateAnchorCtx(storage, 0, 0, &context);
+
+  // Test
+  tdoaStorageSetTimeOfFlight(&context, expectedToF, storageTime_ms);
+
+  // Assert
+  TEST_ASSERT_EQUAL_UINT64(expectedToF, tdoaStorageGetTimeOfFlight(&context, storageTime_ms - 1));
+  TEST_ASSERT_EQUAL_UINT64(expectedToF, tdoaStorageGetTimeOfFlight(&context, storageTime_ms));
+  TEST_ASSERT_EQUAL_UINT64(0, tdoaStorageGetTimeOfFlight(&context, storageTime_ms + 1));
+}
+#endif
+
 
 // Helpers ///////////////
 
@@ -692,5 +712,5 @@ static void fixtureSetRemoteRxTime(tdoaAnchorContext_t* context, const uint8_t a
 
 static void fixtureSetTof(tdoaAnchorContext_t* context, const uint8_t anchor, const uint32_t storageTime, const uint8_t remoteAnchor, const uint64_t tof) {
   tdoaStorageGetCreateAnchorCtx(storage, anchor, storageTime, context);
-  tdoaStorageSetTimeOfFlight(context, remoteAnchor, tof);
+  tdoaStorageSetRemoteTimeOfFlight(context, remoteAnchor, tof);
 }

From 90fb1b8acafd0f596d411689f981cce691d6a77a Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 10 Nov 2023 10:39:11 +0100
Subject: [PATCH 11/13] Add parameter for tof age

---
 src/deck/drivers/src/lpsTdoa3Tag.c | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/deck/drivers/src/lpsTdoa3Tag.c b/src/deck/drivers/src/lpsTdoa3Tag.c
index 835e6cb35a..64e865069e 100644
--- a/src/deck/drivers/src/lpsTdoa3Tag.c
+++ b/src/deck/drivers/src/lpsTdoa3Tag.c
@@ -109,10 +109,6 @@ for improved position estimation.
 #define SHORT_LPP 0xF0
 #define LPP_SHORT_ANCHOR_POSITION 0x01
 
-// Maximum acceptable age of time of flight information to be used in transmissions. The ToF information is used by
-// other CFs for TDoA positioning. A longer time is acceptable if the transmitting CF is not moving.
-#define MAX_AGE_OF_TOF_MS 200
-
 // Log ids for estimated position
 static logVarId_t estimatedPosLogX;
 static logVarId_t estimatedPosLogY;
@@ -192,6 +188,10 @@ static struct {
   // If TWR data should be used for position estimation
   bool useTwrForPositionEstimation;
 
+  // Maximum acceptable age of time of flight information to be used in transmissions. The ToF information is used by
+  // other CFs for TDoA positioning. A longer time is acceptable if the transmitting CF is not moving.
+  uint32_t maxAgeOfTof_ms;
+
   statsCntRateLogger_t cntPacketsTransmitted;
   statsCntRateLogger_t cntTwrSeqNrOk;
   statsCntRateLogger_t cntTwrToEstimator;
@@ -454,7 +454,7 @@ static int populateTxData(rangePacket3_t *rangePacket, const uint32_t now_ms) {
     anchorData->seq = tdoaStorageGetSeqNr(&anchorCtx);
     anchorData->rxTimeStamp = tdoaStorageGetRxTime(&anchorCtx);
 
-    uint64_t tof = tdoaStorageGetTimeOfFlight(&anchorCtx, now_ms - MAX_AGE_OF_TOF_MS);
+    uint64_t tof = tdoaStorageGetTimeOfFlight(&anchorCtx, now_ms - ctx.maxAgeOfTof_ms);
     if (tof > 0 && tof <= 0xfffful) {
       anchorData->distance = tof;
       anchorDataPtr += sizeof(remoteAnchorDataFull_t);
@@ -679,6 +679,7 @@ static void Initialize(dwDevice_t *dev) {
   ctx.twrSendPosition = txOwnPosNot;
   ctx.twrStdDev = 0.25;
   ctx.useTwrForPositionEstimation = false;
+  ctx.maxAgeOfTof_ms = 200;
 
   ctx.averageTxDelay = 1000.0f / ANCHOR_MAX_TX_FREQ;
   ctx.nextTxDelayEvaluationTime_ms = 0;
@@ -767,6 +768,11 @@ PARAM_ADD(PARAM_FLOAT, stddev, &ctx.tdoaStdDev)
    */
   PARAM_ADD(PARAM_UINT8, hmTwrEstPos, &ctx.useTwrForPositionEstimation)
 
+  /**
+   * @brief Max age of tof to include in transmitted packets.
+   */
+  PARAM_ADD(PARAM_UINT32, hmTofAge, &ctx.maxAgeOfTof_ms)
+
   /**
    * @brief Select an anchor id to use for logging distance to a specific anchor. The distance is available in the
    * tdoa3.hmDist log variable. Used in hybrid mode.

From ea279f62f6f54206fdd94ff2d86a90380475248b Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 10 Nov 2023 10:47:46 +0100
Subject: [PATCH 12/13] Updated test

---
 test/utils/src/tdoa/test_tdoa_storage.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/test/utils/src/tdoa/test_tdoa_storage.c b/test/utils/src/tdoa/test_tdoa_storage.c
index 31b5c3fb7a..9f3e9ea41c 100644
--- a/test/utils/src/tdoa/test_tdoa_storage.c
+++ b/test/utils/src/tdoa/test_tdoa_storage.c
@@ -682,9 +682,9 @@ void testThatTofReplacesTheOldestEntryWhenStorageIsFull() {
   TEST_ASSERT_EQUAL_INT64(0, actualReplaced);
 }
 
+// Not possible to put the ifdef outside the test function due to the way the test framework is implemented
+void testThatTimeOfFlightIsSet() {
 #ifdef CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE
-// Disabled for now as the unit test system does not support kconfig
-void _testThatTimeOfFlightIsSet() {
   // Fixture
   uint32_t storageTime_ms = 1234;
   int64_t expectedToF = 4747474747;
@@ -699,8 +699,8 @@ void _testThatTimeOfFlightIsSet() {
   TEST_ASSERT_EQUAL_UINT64(expectedToF, tdoaStorageGetTimeOfFlight(&context, storageTime_ms - 1));
   TEST_ASSERT_EQUAL_UINT64(expectedToF, tdoaStorageGetTimeOfFlight(&context, storageTime_ms));
   TEST_ASSERT_EQUAL_UINT64(0, tdoaStorageGetTimeOfFlight(&context, storageTime_ms + 1));
-}
 #endif
+}
 
 
 // Helpers ///////////////

From a2d2056cf227b03dbb58704c63eecdb8e97daeb3 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 10 Nov 2023 10:57:53 +0100
Subject: [PATCH 13/13] Disable hybrid mode in random build

---
 configs/all.config | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/configs/all.config b/configs/all.config
index 63bccc37d3..abfe7fcf74 100644
--- a/configs/all.config
+++ b/configs/all.config
@@ -15,3 +15,6 @@
 # Some decks use UART1 and can not be enabled when debug printing is directed to UART1
 # Turn it off to verify the deck drivers.
 # CONFIG_DEBUG_PRINT_ON_UART1 is not set
+#
+# Do not build with hybrid mode as we run out of CCM memory in combination with some other configurations
+# CONFIG_DECK_LOCO_TDOA3_HYBRID_MODE is not set