Codes

ODS – généralités

1. Le problème : Le Temps relatif

Dans ce réseau UWB (Ultra-Wideband), chaque ancre (Ai) possède une horloge interne qui dérive par rapport à celle de l’ancre de référence (AR).

  • Elles n’ont pas le même « zéro » (Offset).
  • Elles ne battent pas la mesure à la même vitesse (Skew ou dérive).

L’objectif de ce protocole est de calculer ce Skew (kRi ) pour pouvoir convertir n’importe quelle durée mesurée par l’ancre i en une durée « standard » (celle de la référence R).

2. Le diagramme de séquence

Figure 1 : ODS-UWB synchronization protocol / source : Mme Dalce

Le diagramme montre une séquence d’événements physiques qui délimitent un intervalle de temps précis :

  • L’événement START  : Le mobile lance un signal. Tout le monde le reçoit. C’est le « CLAP » de départ.
    • L’ancre N le voit à tN1.
    • L’ancre de référence le voit a tR1
  • L’événement REQUEST : L’ancre de référence attend un peu, puis envoie un message direct aux autres ancres.
    • L’ancre N le voit a tN2.
    • Les Ancre répondent chacun leur tour (RESPONSE) avec leurs mesures (tN1, tN2) et leur temps d’envoie du RESPONSE (tN3).
    • L’ancre de référence reçoit ce message a tN4
  • Synchronisation (TWR)
    • L’ancre référence obtient les 4 temps clés (tN1, tN2, tN3 et tN4) .
    • Elle peut donc calculer le  Temps de Vol réel et effectuer la correction de la dérive d’horloge (Skew) pour le TDOA.

Code client 

//CLIENT

#include <SPI.h>

// HARDWARE COMPATIBILITY
// Forces the DecaDuino library to use the specific pinout and SPI configuration
// for the Decawave DWM1001-DEV module (nRF52832 MCU).
#define ARDUINO_DWM1001_DEV 
#include <DecaDuino.h>

// ADDRESSING 
#define ADDR_CLIENT 0x0010 // Unique ID of this mobile tag
#define ADDR_S1     0x0001 // Not used, but kept for protocol consistency
#define PAN_ID      0xDECA // Network ID
#define BROADCAST   0xFFFF // Broadcast address

// PROTOCOL DEFINITION 
// We only need the clap signal.
#define ODS_MSG_CLAP        0x01

// PACKET STRUCTURE
// __attribute__((packed)) ensures no memory padding is added by the compiler.
struct MacHeader { 
    uint16_t fc;   // Frame Control
    uint8_t seq;   // Sequence Number
    uint16_t pan;  // PAN ID
    uint16_t dest; // Destination Addr
    uint16_t src;  // Source Addr
} __attribute__((packed));

struct ODS_Packet {
    MacHeader header;
    uint8_t   messageType; 
    // Payload is empty for the clap
} __attribute__((packed));

//  DRIVER
#ifdef ARDUINO_DWM1001_DEV
  DecaDuino decaduino(SS1, DW_IRQ); // Use DWM1001 specific pins
#else
  DecaDuino decaduino; // Use default Arduino/Teensy pins
#endif

uint8_t rxBuffer[128]; // Not used for RX, but required for init
uint16_t rxLen;
int seqID = 0; // Packet counter

// SETUP ROUTINE
void setup() {
  Serial.begin(115200);
  // Blocking wait for Serial connection (max 3 seconds) to allow debug monitoring
  while (!Serial && millis() < 3000);
  
  

  // Initialize DW1000 radio chip
  if (!decaduino.init()) { Serial.println("Init failed"); while(1); }
  

  
  Serial.println("--- CLIENT ODS PRET ---");
  delay(1000);
}

// --- MAIN LOOP ---
void loop() {
  // Process driver engine (check interruptions)
  decaduino.engine();
  seqID++; // Increment sequence number

  Serial.print("Blink #"); Serial.println(seqID);
  
  // 1. PREPARE PACKET
  ODS_Packet packet;
  packet.header.fc = 0x8841;      // Standard Data Frame
  packet.header.pan = PAN_ID;
  packet.header.dest = BROADCAST; // Target: All Anchors
  packet.header.src = ADDR_CLIENT;
  packet.header.seq = seqID;
  packet.messageType = ODS_MSG_CLAP;
  
  // 2. TRANSMISSION (TX)
  // pdDataRequest sends the bytes to the radio buffer and triggers TX immediately.
  // Size is strictly 10 bytes (Header + Type).
  decaduino.pdDataRequest((uint8_t*)&packet, 10); 
  

  // 3. WAIT FOR PHYSICAL TRANSMISSION
  // We block here until the radio chip confirms the last bit has left the antenna.
  while (!decaduino.hasTxSucceeded()) decaduino.engine();

  // 4. WAIT BEFORE NEXT CLAP
  // The loop finishes here. The radio automatically returns to IDLE state (Rx/Tx OFF).
  delay(3000); // Wait 3 seconds before next clap

Explication code client

Le  nœud Client est basé sur une architecture unidirectionnelle asynchrone.

Principe de fonctionnement : Le « Blink »

Le nœud mobile fonctionne selon un modèle appelé « Blink » (ou Fire and Forget). Son rôle est exclusivement d’émettre un signal de présence périodique, sans se soucier de l’infrastructure environnante.

Le cycle de vie du mobile est le suivant :

  • Réveil : Le processeur sort de veille.
  • Émission : Il envoie un paquet court (le CLAP) en mode Broadcast.
  • Sommeil : Dès que l’envoi est confirmé physiquement, il coupe sa radio et retourne en veille.

Automate client

Figure 2 : Automate client

Code Ancre de référence

#include <SPI.h>

// --- HARDWARE COMPATIBILITY ---
// Define if using the DWM1001-DEV board
#define ARDUINO_DWM1001_DEV 
#include <DecaDuino.h>

// --- CONFIGURATION ---
#define ADDR_REF     0x0001   // Address of THIS Reference Anchor (The Time Base)
#define ADDR_N2      0x0002   // Address of Secondary Anchor 2 (Neighbor)
#define ADDR_N3      0x0003   // Address of Secondary Anchor 3 (Neighbor)
#define PAN_ID       0xDECA   // Personal Area Network ID
#define BROADCAST    0xFFFF   // Broadcast address
#define MAX_ANCHORS  5        // Max capacity of the system (Secondary anchors)

// DW1000 Time Unit: 1 ms approx 63897600 ticks
#define TICKS_PER_MS 63897600ULL 

// --- PROTOCOL DEFINITIONS ---
#define ODS_MSG_CLAP        0x01 // Step 1: Mobile tag emits signal (Event 0)
#define ODS_MSG_REQ_TS      0x02 // Step 2: Reference Anchor requests timestamps
#define ODS_MSG_RESP_TS     0x03 // Step 3: Secondary Anchors respond with data
// REMOVED: ODS_MSG_FINAL_SYNC (Protocol ends after data collection)

// --- PACKET STRUCTURES ---
struct MacHeader { 
    uint16_t fc;    // Frame Control
    uint8_t seq;    // Sequence Number
    uint16_t pan;   // PAN ID
    uint16_t dest;  // Destination Address
    uint16_t src;   // Source Address
} __attribute__((packed));

struct PayloadReqTS { 
    uint8_t count;                   // Number of anchors targeted
    uint16_t targetList[MAX_ANCHORS];// List of IDs (Neighbors)
} __attribute__((packed));

struct PayloadRespTS { 
    uint64_t t1_RX_CLAP; // Time Neighbor received CLAP
    uint64_t t2_RX_REQ;  // Time Neighbor received REQ
    uint64_t t3_TX_RESP; // Time Neighbor sent RESP
} __attribute__((packed));

struct ODS_Packet {
    MacHeader header;
    uint8_t   messageType;
    union {
        PayloadReqTS  reqInfo;
        PayloadRespTS respInfo;
    } payload;
} __attribute__((packed));

// --- GLOBAL VARIABLES ---
// List of Secondary Anchors (Neighbors) to synchronize
uint16_t neighborsList[MAX_ANCHORS] = {ADDR_N2, ADDR_N3}; 
int neighborsCount = 2; 

// Storage for timestamps (Index 0 corresponds to neighborsList[0], etc.)
// N stands for "Neighbor" (Secondary Anchor)
uint64_t ti1_neighbors[MAX_ANCHORS]; // t1: CLAP arrival at Neighbor (N)
uint64_t ti2_neighbors[MAX_ANCHORS]; // t2: REQ arrival at Neighbor (N)
uint64_t ti3_neighbors[MAX_ANCHORS]; // t3: RESP departure from Neighbor (N)
uint64_t ti4_neighbors[MAX_ANCHORS]; // t4: RESP arrival at Reference (Ref)

// R stands for "Reference" (This Anchor)
uint64_t tR1_RxClap = 0; // Time Reference received CLAP
uint64_t tR2_TxReq  = 0; // Time Reference sent REQ

bool responseReceived[MAX_ANCHORS]; // Track who replied

// State Machine
enum State { STATE_IDLE, STATE_WAIT_RESPONSES };
State currentState = STATE_IDLE;
unsigned long timeoutStart = 0;

// Hardware Driver
#ifdef ARDUINO_DWM1001_DEV
  DecaDuino decaduino(SS1, DW_IRQ); 
#else
  DecaDuino decaduino; 
#endif
uint8_t rxBuffer[128];
uint16_t rxLen;

void setup() {
  Serial.begin(115200);
  while (!Serial && millis() < 3000);

  #ifndef ARDUINO_DWM1001_DEV
    SPI.setSCK(14); 
  #endif

  if (!decaduino.init()) { Serial.println("Init failed"); while(1); }
  
  decaduino.setRxBuffer(rxBuffer, &rxLen);
  decaduino.plmeRxEnableRequest();
  Serial.println("--- REFERENCE ANCHOR (S1) READY ---");
}

void loop() {
  decaduino.engine();

  switch (currentState) {
    
    // --- STATE 1: IDLE (Waiting for Mobile CLAP) ---
    case STATE_IDLE:
      if (decaduino.rxFrameAvailable()) {
        ODS_Packet* pack = (ODS_Packet*)rxBuffer;
        
        // If we receive the CLAP (Reference Event from Mobile)
        if (pack->messageType == ODS_MSG_CLAP) {
            // 1. Capture exact arrival time at Reference (tR1)
            tR1_RxClap = decaduino.getLastRxTimestamp();
            
            // 2. Reset data structures for this new cycle
            for(int i=0; i<MAX_ANCHORS; i++) { 
                ti1_neighbors[i]=0; ti2_neighbors[i]=0; ti3_neighbors[i]=0; ti4_neighbors[i]=0;
                responseReceived[i] = false; 
            }

            // 3. Send the Group Request (Delayed) to capture precise tR2
            sendGroupRequestDelayed();
            
            // 4. Transition to waiting state
            currentState = STATE_WAIT_RESPONSES;
            timeoutStart = millis();
            decaduino.plmeRxEnableRequest();
        } else {
             // Not a CLAP, just listen again
             decaduino.plmeRxEnableRequest();
        }
      }
      break;

    // --- STATE 2: COLLECTING RESPONSES FROM SECONDARY ANCHORS ---
    case STATE_WAIT_RESPONSES:
      // Timeout safety (500ms max to wait for responses)
      if (millis() - timeoutStart > 500) { 
        Serial.println("Timeout ! Resetting to IDLE.");
        currentState = STATE_IDLE;
        decaduino.plmeRxEnableRequest();
        break;
      }

      if (decaduino.rxFrameAvailable()) {
         ODS_Packet* pack = (ODS_Packet*)rxBuffer;

         if (pack->messageType == ODS_MSG_RESP_TS) {
             uint16_t sender = pack->header.src;
             
             // Identify which neighbor sent this message
             for (int i=0; i<neighborsCount; i++) {
                 if (neighborsList[i] == sender) {
                     // Save Timestamps from Payload (Neighbor's local clock)
                     ti1_neighbors[i] = pack->payload.respInfo.t1_RX_CLAP;
                     ti2_neighbors[i] = pack->payload.respInfo.t2_RX_REQ;
                     ti3_neighbors[i] = pack->payload.respInfo.t3_TX_RESP;
                     
                     // Save arrival time at Reference (Reference's local clock)
                     ti4_neighbors[i] = decaduino.getLastRxTimestamp();
                     
                     responseReceived[i] = true;
                 }
             }
             
             // Check if ALL neighbors have responded
             bool allGood = true;
             for (int i=0; i<neighborsCount; i++) { if(!responseReceived[i]) allGood = false; }
             
             if (allGood) {
                 // All data collected. Process outputs.
                 printJSON(); 
                 
                 // Calculate Skew for each neighbor using the primitive formula
                 for(int i=0; i<neighborsCount; i++) {
                    CalculSkew_Primitive(i);
                 }
                 
                 // Cycle finished. No ACK sent. Return to IDLE.
                 currentState = STATE_IDLE;
             }
         }
         decaduino.plmeRxEnableRequest();
      }
      break;
  }
}

// Function to send the Request broadcast with a precise delay
void sendGroupRequestDelayed() {
  ODS_Packet packet;
  packet.header.fc = 0x8841; packet.header.pan = PAN_ID;
  packet.header.src = ADDR_REF; packet.header.dest = BROADCAST; 
  packet.messageType = ODS_MSG_REQ_TS;
  packet.payload.reqInfo.count = neighborsCount;
  
  // Fill target list
  for(int i=0; i<neighborsCount; i++) packet.payload.reqInfo.targetList[i] = neighborsList[i];

  // Calculate future time: Now + 20ms
  // 20ms * Ticks_per_ms (approx 63897 ticks for 1ms)
  uint64_t delayTicks = 20000 * 63897; 
  uint64_t now = decaduino.getSystemTimeCounter();
  
  // Align for hardware (mask lower 9 bits)
  tR2_TxReq = decaduino.alignDelayedTransmissionTS(now + delayTicks);
  
  // Send with Delayed Transmission enabled
  // Packet contains the count of targets + list of addresses
  decaduino.pdDataRequest((uint8_t*)&packet, 11+(2*neighborsCount), true, tR2_TxReq);
  
  // Wait for TX to complete
  while (!decaduino.hasTxSucceeded()) decaduino.engine();
}

// Output data for Python processing
void printJSON() {
    Serial.println("{");
    Serial.println("  \"anchor_Ref\": {");
    Serial.print("    \"tR1\": "); decaduino.printUint64(tR1_RxClap); Serial.println(",");
    Serial.print("    \"tR2\": "); decaduino.printUint64(tR2_TxReq); Serial.println("");
    Serial.println("  },");
    Serial.println("  \"neighbors\": [");
    
    for(int i=0; i<neighborsCount; i++) {
        Serial.println("    {");
        Serial.print("      \"id\": \"0x"); Serial.print(neighborsList[i], HEX); Serial.println("\",");
        Serial.print("      \"ti1\": "); decaduino.printUint64(ti1_neighbors[i]); Serial.println(",");
        Serial.print("      \"ti2\": "); decaduino.printUint64(ti2_neighbors[i]); Serial.println(",");
        Serial.print("      \"ti3\": "); decaduino.printUint64(ti3_neighbors[i]); Serial.println(",");
        Serial.print("      \"ti4\": "); decaduino.printUint64(ti4_neighbors[i]); Serial.println("");
        Serial.print("    }");
        if(i < neighborsCount-1) Serial.println(","); else Serial.println("");
    }
    Serial.println("  ]");
    Serial.println("}");
    Serial.println("___END_JSON___");
}

// "Primitive" Skew Calculation (Demonstration logic)
// See "Problems and Solutions" page for the Iterative Algorithm
void CalculSkew_Primitive(int i) {
    uint64_t tR1 = tR1_RxClap;
    uint64_t tR2 = tR2_TxReq;
    uint64_t tN1 = ti1_neighbors[i]; // Neighbor t1
    uint64_t tN2 = ti2_neighbors[i]; // Neighbor t2
    uint64_t tN3 = ti3_neighbors[i]; // Neighbor t3
    uint64_t tN4 = ti4_neighbors[i]; // Neighbor t4 (Received at Ref)

    // Calculate Round Trip Time (Ref -> Neighbor -> Ref)
    int64_t T_round = (int64_t)tN4 - (int64_t)tR2;
    
    // Calculate Reply Time at Neighbor (Processing time)
    // PROBLEM: This duration is measured in Neighbor's time units (skewed)
    int64_t T_reply = (int64_t)tN3 - (int64_t)tN2;
    
    // Estimate Time of Flight (Primitive ToF)
    int64_t tof_us = (T_round - T_reply) / 2; 

    // Intervals comparison
    int64_t ref_interval = (int64_t)tR2 - (int64_t)tR1; // Interval between events at Reference
    int64_t neighbor_interval = (int64_t)tN2 - (int64_t)tN1; // Interval between events at Neighbor
    
    // Primitive correction of reference interval using ToF
    int64_t denominator = ref_interval - (2 * tof_us); 
    
    double skew_ratio = 0.0;
    if (denominator != 0) {
        // Ratio = Neighbor_Ticks / Reference_Ticks
        skew_ratio = (double)neighbor_interval / (double)denominator;
    }

    Serial.print("Neighbor 0x"); Serial.print(neighborsList[i], HEX); Serial.println(":");
    Serial.print("  ToF (ticks): "); Serial.println((long)tof_us);
    Serial.print("  Skew (Ratio N/R) : ");
    Serial.println(skew_ratio, 6);
}

Explication

1. Initialisation et Écoute (État IDLE)

Le module est en écoute permanente. Le cycle démarre dès la réception du signal CLAP émis par le mobile (Tag).

  • Action : Capture immédiate du timestamp matériel de réception (tR1).
  • Réaction : L’ancre prépare l’envoi d’une REQUÊTE à toutes les ancres voisines.
  • Précision : Elle utilise une transmission différée (sendGroupRequestDelayed). Cela lui permet de définir l’heure exacte d’émission future (tR2) et de l’enregistrer avant même que l’onde radio ne parte.

2. Collecte des Données (État WAIT_RESPONSES)

L’ancre passe en mode attente pour récolter les mesures des voisins (N).

  • Réception : Pour chaque réponse reçue, elle extrait les temps mesurés par le voisin tN1, tN2, tN3 et note l’heure d’arrivée chez elle (tN4).
  • Stockage : Les 4 temps clés de chaque voisin sont stockés dans des tableaux indexés.
  • Sécurité : Un Timeout de 500ms force le retour à l’état initial si un voisin ne répond pas, évitant le blocage du système.

3. Le Calcul du Skew « Primitif » (CalculSkew_Primitive)

Une fois toutes les réponses reçues, le code tente d’estimer la dérive d’horloge (Skew) de chaque voisin via une approche géométrique directe.

A. Calcul du Temps de Vol (ToF)

Il estime la distance entre les ancres par une méthode d’aller-retour classique :

ToF = (Temps Aller-retour – Temps de traitement ) / 2

  • Temps Aller-Retour = tN4 – tR2 (Vu par la Référence)
  • Temps Traitement = tN3 – tN2 (Vu par le Voisin)

B. Calcul du Ratio de Skew

Il compare ensuite les intervalles de temps pour obtenir le ratio de vitesse des horloges :

Skew = Intervalle Voisin / Intervalle Référence corrigé

  • L’intervalle Voisin : tN2 – tN1 (Durée brute mesurée par le voisin).
  • L’intervalle Référence : tR2 – tR1 auquel on retire le ToF calculé juste avant (pour compenser le retard dû à la distance).

4. Limitation de cette Approche (Pourquoi ce n’est pas optimal)

Cette méthode contient un défaut logique appelé « L’œuf et la Poule » :

  • Pour calculer le ToF, on soustrait le temps de traitement du Voisin (tN3 – tN2)
  • Or, ce temps est mesuré avec l’horloge du Voisin qui est fausse puisqu’elle dérive.
  • L’erreur de l’horloge contamine le calcul de la distance, qui à son tour contamine le calcul final du Skew.

Cette méthode « primitive » offre une certaine précision , mais elle est insuffisante pour le TDoA centimétrique, ce qui justifie l’utilisation d’un algorithme itératif (détaillé dans la section problème rencontrés et solutions apportées.

Automate

Code serveur secondaire

#include <SPI.h>

// --- HARDWARE COMPATIBILITY ---
// Define for DWM1001-DEV board specific pinout
#define ARDUINO_DWM1001_DEV 
#include <DecaDuino.h>

// --- CONFIGURATION ---
#define MY_ADDR      0x0002   // Address of this specific Slave (Change for other boards)
#define ADDR_S1      0x0001   // Master Address
#define PAN_ID       0xDECA   // Personal Area Network ID
#define BROADCAST    0xFFFF   // Broadcast address

// DW1000 Time Unit conversion: approx 63,897,600 ticks = 1 millisecond
#define TICKS_PER_MS 63897600ULL 

// --- PROTOCOL DEFINITIONS ---
// Protocol steps
#define ODS_MSG_CLAP        0x01 // Step 1: Reference signal (Reference event)
#define ODS_MSG_REQ_TS      0x02 // Step 2: Master requests timestamps
#define ODS_MSG_RESP_TS     0x03 // Step 3: Slave responds with data
// REMOVED: ODS_MSG_FINAL_SYNC (No longer used)

#define MAX_SLAVES 5 // Maximum number of slaves addressed in one request

// --- DATA STRUCTURES (Packed to avoid padding issues) ---

// Standard 802.15.4 MAC Header
struct MacHeader { 
    uint16_t fc;    // Frame Control
    uint8_t seq;    // Sequence Number
    uint16_t pan;   // PAN ID
    uint16_t dest;  // Destination Address
    uint16_t src;   // Source Address
} __attribute__((packed));

// Payload for Step 2: Request Timestamps (Sent by Master)
struct PayloadReqTS { 
    uint8_t count;                  // How many slaves are targeted?
    uint16_t targetList[MAX_SLAVES];// List of slave addresses to respond
} __attribute__((packed));

// Payload for Step 3: Response Timestamps (Sent by Slave)
struct PayloadRespTS { 
    uint64_t t1_RX_CLAP; // Timestamp: When the CLAP was received
    uint64_t t2_RX_REQ;  // Timestamp: When the REQUEST was received
    uint64_t t3_TX_RESP; // Timestamp: When this RESPONSE is transmitted
} __attribute__((packed));

// Main Packet Union: Combines Header and Payloads
struct ODS_Packet {
    MacHeader header;
    uint8_t   messageType;
    union {
        PayloadReqTS  reqInfo;
        PayloadRespTS respInfo;
        // Removed finalInfo
    } payload;
} __attribute__((packed));

// --- GLOBAL VARIABLES ---
#ifdef ARDUINO_DWM1001_DEV
  // Initialize DecaDuino with specific SS and IRQ pins for DWM1001
  DecaDuino decaduino(SS1, DW_IRQ); 
#else
  // Default initialization
  DecaDuino decaduino; 
#endif

uint8_t rxBuffer[128]; // Buffer for incoming data
uint16_t rxLen;        // Length of received data

// Variables to store critical timestamps
uint64_t t1_Clap = 0;
uint64_t t2_Req  = 0;

void setup() {
  Serial.begin(115200);
  // Wait for Serial to be ready (useful for debugging boot issues)
  while (!Serial && millis() < 3000);

  #ifndef ARDUINO_DWM1001_DEV
    SPI.setSCK(14); // Specific clock pin for generic ESP32 setups
  #endif

  // Initialize the UWB radio
  if (!decaduino.init()) { 
    Serial.println("Init failed"); 
    while(1); // Halt if hardware fails
  }
  
  // Set up RX buffer
  decaduino.setRxBuffer(rxBuffer, &rxLen);
  
  // Enable Receiver immediately
  decaduino.plmeRxEnableRequest();
  
  Serial.print("--- SLAVE 0x"); 
  Serial.print(MY_ADDR, HEX); 
  Serial.println(" READY ---");
}

void loop() {
  // Main driver engine (handles IRQ and state transitions)
  decaduino.engine();

  // Check if a packet has been successfully received
  if (decaduino.rxFrameAvailable()) {
    ODS_Packet* pack = (ODS_Packet*)rxBuffer;

    // --- 1. HANDLE CLAP MSG (Reference Signal) ---
    if (pack->messageType == ODS_MSG_CLAP) {
      // Hardware automatically captures the exact arrival time
      t1_Clap = decaduino.getLastRxTimestamp();
      Serial.println("[Slave] t1 (CLAP) saved.");
    }

    // --- 2. HANDLE REQUEST MSG (Master asks for data) ---
    else if (pack->messageType == ODS_MSG_REQ_TS) {
      // Capture the arrival time of this request immediately
      t2_Req = decaduino.getLastRxTimestamp(); 
      
      // Parse the payload to see if I am in the target list
      int count = pack->payload.reqInfo.count;
      int myIndex = -1;
      
      for (int i=0; i<count; i++) {
          if (pack->payload.reqInfo.targetList[i] == MY_ADDR) { 
            myIndex = i; // Found myself! Save the index.
            break; 
          }
      }

      // If I am targeted, calculate response time
      if (myIndex != -1) {
          // --- CALCULATE DELAYED TX TIME (t3) ---
          // Logic: Processing Time + (Slot Time * Index)
          // 5ms basic delay + 10ms for each position in the list
          uint64_t delayTicks = (5 + (myIndex * 10)) * TICKS_PER_MS;
          
          // t3 = t2 (Request Arrival) + Delay
          uint64_t future_t3 = t2_Req + delayTicks;
          
          // IMPORTANT: Hardware requires masking lower 9 bits for delayed TX
          // This ensures the timestamp is valid for the DW1000 chip
          uint64_t aligned_t3 = decaduino.alignDelayedTransmissionTS(future_t3);
          
          // Schedule the response
          sendResponseDelayed(aligned_t3);
      }
    }
    
    // Re-enable Receiver to listen for next messages
    decaduino.plmeRxEnableRequest();
  }
}

// Function to construct and schedule the packet transmission
void sendResponseDelayed(uint64_t t3) {
  ODS_Packet packet;
  
  // 1. Fill Header
  packet.header.fc = 0x8841; // Standard Data Frame
  packet.header.pan = PAN_ID;
  packet.header.dest = ADDR_S1; 
  packet.header.src = MY_ADDR;
  
  // 2. Set Message Type
  packet.messageType = ODS_MSG_RESP_TS;
  
  // 3. Fill Payload with captured timestamps
  packet.payload.respInfo.t1_RX_CLAP = t1_Clap; // When CLAP arrived
  packet.payload.respInfo.t2_RX_REQ  = t2_Req;  // When REQ arrived
  packet.payload.respInfo.t3_TX_RESP = t3;      // When this packet LEAVES

  // 4. Send Request to Radio
  // Size = Header(9) + Type(1) + 3*8 bytes = 34 bytes
  // true = Enable Delayed Transmission
  // t3 = The exact time the radio must transmit
  decaduino.pdDataRequest((uint8_t*)&packet, 34, true, t3);
  
  // 5. Wait for Transmission to complete (Blocking for simplicity)
  while (!decaduino.hasTxSucceeded()) {
    decaduino.engine();
  }
  Serial.println("[Slave] Response scheduled and sent.");
}

Explication

Ce code implémente la machine d’état d’une ancre esclave. Son rôle est purement réactif : capturer des temps précis et les transmettre selon un ordonnancement strict.

Voici les 3 étapes fonctionnelles du code :

1. Acquisition Temporelle (Timestamping Hardware)

Le module est en réception continue (RX Enable).

  • À la réception du message de référence (START), il capture le Timestamp t1 (temps d’arrivée physique du signal).
  • À la réception de la requête (REQ), il capture le Timestamp t2.
  • Note : Ces valeurs proviennent directement des registres du chip UWB pour une précision nanoseconde.

2. Filtrage et Ordonnancement (TDMA)

Le code analyse le payload de la requête :

  • Filtrage d’adresse : Il vérifie si son MY_ADDR est présent dans la liste cible.
  • Calcul du Slot : S’il est ciblé, il déduit son index i. Il calcule alors l’instant futur d’émission t3 selon une logique TDMA (Time Division Multiple Access) pour éviter toute collision RF avec les autres ancres.

3. Transmission Différée (Delayed TX)

C’est l’étape critique pour le TDoA.

  • Le code construit un paquet contenant les données brutes : {t1, t2, t3}.
  • Il ne demande pas une émission immédiate, mais une Transmission Différée (« Send at t3 »).
  • Le contrôleur radio attendra matériellement que son horloge interne atteigne exactement t3 pour envoyer le signal.

En résumé : Ce code transforme le microcontrôleur en un capteur de temps passif, qui ne s’active que pour renvoyer ses mesures (t1, t2) à un instant (t3) déterministe.

Automate

Figure 4 : Automate ancre (serveur secondaire)

Généralités : TDoA (Time Difference of Arrival)

Le TDoA (Time Difference of Arrival) est une technique de localisation par multilatération hyperbolique. Contrairement aux méthodes basées sur le temps de vol absolu (ToA) qui nécessitent une synchronisation entre le mobile et les ancres, le TDoA exploite la différence de temps d’arrivée d’un signal émis par le mobile et reçu par plusieurs ancres synchronisées entre elles.

1. Principe Géométrique : L’Intersection d’Hyperboles

Le principe fondamental repose sur la propriété géométrique suivante : l’ensemble des points pour lesquels la différence de distance par rapport à deux points fixes (les foyers) est constante constitue une hyperbole.

Dans notre configuration :

  • Les Foyers (S) : Ce sont les ancres fixes dont les positions sont connues. (Ancre R = S1, Ancre N = S2, S3)
  • Le Mobile (M) : C’est sa position que l’on cherche à déterminer
  • La Constante : La différence de distance mesurée est proportionnelle à la différence de temps d’arrivée des signaux multiplied par la vitesse de la lumière c.

Distance(M, S1) – Distance(M, S2) = Constante

Une seule mesure entre une paire d’ancres (ex: S1-S2) place le mobile sur une première courbe hyperbolique. Pour déterminer la position unique (x, y) du mobile M, il est nécessaire de générer une seconde courbe via une autre paire (ex: S1-S3). L’intersection de ces deux courbes donne la position du mobile.

2. Formulation mathématique du problème

La résolution de la position repose sur l’égalité entre la différence de temps mesurée et la différence de distance géométrique. L’équation fondamentale pour une paire d’ancres constituée d’une ancre de référence R ( S1) et d’une ancre secondaire j (ex: S2 ou S3) est la suivante :

Où :

  • eR,j : La différence de temps de vol observée.
  • tR : Le temps de réception du signal « CLAP » par l’ancre de référence S1.
  • tj : Le temps de réception du signal « CLAP » par l’ancre secondaire Sj.
  • (x, y) : La position inconnue du mobile M.
  • (XR, YR ) : La position connue de l’ancre de référence S1.
  • (xj, yj) : La position connue de l’ancre secondaire Sj.
  • c : La vitesse de la lumière ( 3 * 108 m/s).

3. Le rôle du protocole ODS : conversion de Fuseau Horaire

L’équation ci-dessus n’est valide que si tR et tj sont exprimés dans le même référentiel temporel. Or, chaque module DWM1001 possède sa propre horloge locale qui dérive indépendamment.

C’est ici que le protocole ODS intervient

Le timestamp brut tj mesuré par l’ancre secondaire est inutilisable tel quel. Grâce aux échanges de paquets de synchronisation, l’algorithme ODS calcule les paramètres de correction linéaire (offset et dérive) pour convertir tj dans le « fuseau horaire » de l’ancre de référence S1.

Code python

import json
import re
import numpy as np
from scipy.optimize import least_squares

# =============================================================================
# 1. CONFIGURATION (Mapping logical IDs to physical hardware)
# =============================================================================

# Physical labels on the DWM1001 boxes
ID_ANCRE_S1_REF = "14"   # Master (Reference) anchor
ID_ANCRE_S2_0x2 = "45"   # Slave 1
ID_ANCRE_S3_0x3 = "43"   # Slave 2
ID_CLIENT_MOB = "9"      # Mobile node to locate

# Antenna delay calibration (in meters)
ANTENNA_BIAS_M = -185.0 

# RAW UWB DATA (contains timestamps in hexadecimal and synchronization info)
RAW_DATA_INPUT = """
Slave 0x3:
  ToF (ticks): -960
  T_Master_real (ticks): 1295611255
  T_Slave (ticks): 1295615541
Skew (Ratio S/M) : 1.000003
{
  "anchor_R": {
    "tR1": 000000615244238b,
    "tR2": 000000619f81128e
  },
  "slaves": [
    {
      "id": "0x2",
      "ti1": 000000ca6e718cd9,
      "ti2": 000000cabbae6f87,
      "ti3": 000000caceb9a68e,
      "ti4": 00000061b28c54ac
    },
    {
      "id": "0x3",
      "ti1": 00000053cc6e92a4,
      "ti2": 0000005419ab90ce,
      "ti3": 0000005452ccc88e,
      "ti4": 00000061d8a2481c
    }
  ]
}
"""

# =============================================================================
# 2. COORDINATES DATABASE (X, Y, Z positions of all nodes in the room)
# =============================================================================

NODES_DB = {
    "dwm1001-14" : np.array([-1.19, 4.578, 2.658]),
    "dwm1001-43" : np.array([3.339, 7.565, 2.65]),
    "dwm1001-45" : np.array([1.311, 8.989, 2.65]),
    "dwm1001-9" : np.array([-4.005, -1.674, 2.65]),
    # ... (other nodes omitted for brevity)
}

# Physical Constants: Speed of light and Decawave time unit (ticks)
C = 299702547.235
TIMEBASE = 15.65e-12

# =============================================================================
# 3. HELPER FUNCTIONS
# =============================================================================

def clean_and_parse_json(raw_text):
    """ Fixes malformed JSON by adding quotes to hex values and parsing it """
    start_index = raw_text.find('{')
    if start_index == -1: return None
    json_part = raw_text[start_index:]
    pattern = r':\s*(0x[0-9a-fA-F]+|[0-9a-fA-F]{5,})'
    fixed_json = re.sub(pattern, r': "\1"', json_part)
    try:
        return json.loads(fixed_json)
    except json.JSONDecodeError as e:
        print(f"JSON Error: {e}")
        return None

def get_val(val):
    """ Converts a string (hex or dec) into an integer """
    if isinstance(val, str): return int(val, 16)
    return int(val)

def tdoa_residuals(vars, anchors_positions, measured_diffs):
    """ Cost function for the solver: minimizes the difference between 
        theoretical and measured distance differences (Hyperbolic positioning) """
    x, y = vars
    est_pos = np.array([x, y, anchors_positions[0][2]])
    ref_pos = anchors_positions[0]
    
    residuals = []
    for i in range(1, len(anchors_positions)):
        slave_pos = anchors_positions[i]
        measure = measured_diffs[i-1] 
        dist_ref = np.linalg.norm(est_pos - ref_pos)
        dist_slave = np.linalg.norm(est_pos - slave_pos)
        # Residual = (Estimated distance difference) - (Measured distance difference)
        residuals.append((dist_slave - dist_ref) - measure)
        
    return residuals

# =============================================================================
# 4. MAIN EXECUTION (Synchronization logic and Solver)
# =============================================================================

if __name__ == "__main__":
    print(f"=== TDOA CALCULATION WITH ODS CORRECTION ===")
    
    try:
        # Mapping JSON roles to their physical locations in the database
        ROLE_TO_PHYSICAL = {
            "anchor_R": ID_ANCRE_S1_REF,
            "0x2": ID_ANCRE_S2_0x2,
            "0x3": ID_ANCRE_S3_0x3
        }

        pos_S1 = NODES_DB[f"dwm1001-{ID_ANCRE_S1_REF}"]
        anchors_solver = [pos_S1]
        dist_diffs_solver = []

    except KeyError as e:
        print(f"ERROR: Physical ID {e} not found in database.")
        exit()

    data = clean_and_parse_json(RAW_DATA_INPUT)
    
    if data:
        # Reference timestamps from the Master Anchor
        tR1 = get_val(data["anchor_R"]["tR1"]) # Start message arrival
        tR2 = get_val(data["anchor_R"]["tR2"]) # Request message departure
        
        for slave in data["slaves"]:
            role_json = slave["id"]
            
            if role_json in ROLE_TO_PHYSICAL:
                id_physique = ROLE_TO_PHYSICAL[role_json]
                db_key = f"dwm1001-{id_physique}"
                
                if db_key in NODES_DB:
                    # Slave timestamps
                    ti1 = get_val(slave["ti1"]) # Start message arrival (Slave clock)
                    ti2 = get_val(slave["ti2"]) # Request message arrival (Slave clock)
                    ti3 = get_val(slave["ti3"]) # Response message departure (Slave clock)
                    ti4 = get_val(slave["ti4"]) # Response message arrival (Master clock)
                    
                    pos_Si = NODES_DB[db_key]
                    
                    # 1. SYNCHRONIZATION: Calculate Time of Flight (ToF) between Master and Slave
                    round_trip = ti4 - tR2
                    processing = ti3 - ti2
                    tof_ticks = (round_trip - processing) / 2.0
                    tof_raw_m = (tof_ticks * TIMEBASE) * C
                    # Apply antenna delay correction
                    tof_corrected_m = tof_raw_m - ANTENNA_BIAS_M
                    tof_corr_ticks = tof_corrected_m / C / TIMEBASE
                    
                    # 2. ODS LOGIC: Normalize Slave timestamp to the Master's time reference
                    arrival_req = tR2 + tof_corr_ticks
                    ti1_norm = arrival_req - (ti2 - ti1)
                    
                    # 3. TDOA: Calculate distance difference (Mobile to Slave vs Mobile to Master)
                    tdoa_dist = (ti1_norm - tR1) * TIMEBASE * C
                    
                    anchors_solver.append(pos_Si)
                    dist_diffs_solver.append(tdoa_dist)

        # 4. SOLVER: find the best (X,Y) coordinates
        if len(dist_diffs_solver) >= 2:
            # Initial guess: mean position of anchors
            guess = np.mean(anchors_solver, axis=0)[:2]
            res = least_squares(tdoa_residuals, guess, args=(anchors_solver, dist_diffs_solver))
            
            print(f"\nRESULT: X={res.x[0]:.3f}, Y={res.x[1]:.3f}")
            
            # Error calculation compared to real coordinates
            client_key = f"dwm1001-{ID_CLIENT_MOB}"
            if client_key in NODES_DB:
                real = NODES_DB[client_key]
                err = np.linalg.norm(res.x - real[:2])
                print(f"REAL POSITION ERROR: {err:.3f} m")

1. Préparation et nettoyage des données

Le code commence par « nettoyer » les données brutes qui sortent de la plateforme FitIotLab. Comme les cartes envoient des messages un peu bruts avec des grands nombres en hexadécimal, on utilise des expressions régulières (Regex) dans la fonction clean_and_parse_json. Le but est de transformer ce texte en un format JSON propre que Python peut manipuler facilement comme un dictionnaire.

2. Le cœur du protocole : La synchronisation ODS

C’est la partie la plus critique. Comme les horloges de nos ancres (S1, S2, S3) ne sont pas synchronisées, on ne peut pas comparer leurs timestamps directement.

  • Calcul du ToF (Time of Flight) : Le code calcule le temps que met un message pour faire l’aller-retour entre l’ancre Maître (S1) et une ancre Esclave (Si).
  • Normalisation : En connaissant ce temps de vol, on peut recalculer à quel moment précis l’esclave a reçu le signal du mobile (ti1), mais en l’exprimant dans le référentiel de temps du Maître. C’est cette étape qui permet de « gommer » le décalage entre les horloges.

3. Calcul du TDoA (Time Difference of Arrival)

Une fois que tout le monde parle la « même langue temporelle », on calcule la différence de temps d’arrivée du signal du mobile entre le Maître et chaque Esclave. On multiplie ce temps par la vitesse de la lumière c pour obtenir une différence de distance. C’est ce qu’on appelle la mesure TDoA

4. Résolution

Pour trouver la position (x, y) finale, on utilise la fonction least_squares.

  • Pourquoi ? Parce qu’avec les obstacles et les imprécisions radio, nos hyperboles de localisation ne se croisent jamais parfaitement en un seul point.
  • Comment ? L’algorithme part d’une estimation (le « guess ») et ajuste les coordonnées (x, y) petit à petit. Il cherche le point qui minimise la somme des carrés des erreurs de distance. C’est ce qui nous donne la position la plus probable statistiquement.

Tests réalisés

Pour évaluer la fiabilité de notre système, on a réalisé une série de tests en faisant varier la topologie des nœuds, c’est-à-dire leur disposition et leur distance par rapport au mobile.

Voici ce qu’on a observé sur le terrain :

L’influence de la distance (Test 0)

Au début, on a testé une configuration où le mobile était assez éloigné des ancres (nœud 9 versus 14, 45, 43). Le résultat a été assez brutal avec une erreur de plus de 10 mètres, ce qui nous a montré que la géométrie du réseau est super importante pour la précision du TDoA.

Optimisation par axes (Tests 1 et 2)

On a ensuite essayé de regrouper les ancres plus près du mobile (nœud 43) en les alignant sur des axes spécifiques. En se focalisant sur l’axe X, on est descendu à 1,315 m d’erreur , et sur l’axe Y, on a obtenu notre meilleur score avec seulement 1 mètre d’écart.

1

2

L’impact des obstacles (Test 3)

On a voulu voir comment le système réagissait face à une cloison. En plaçant les ancres derrière des murs, l’erreur est remontée à 5,809 m. Ça confirme que les obstacles physiques perturbent pas mal le signal UWB et les calculs de temps de vol.

Les limites du système (Test 4)

Enfin, on a tenté une topologie « extrême » avec des ancres très éloignées les unes des autres à travers tout le bâtiment. Là, on a atteint les limites de notre protocole : le script a fini en « Timeout », car les messages RESPONSE n’arrivaient plus à temps ou étaient perdus à cause de la distance trop grande.

Synthèse

Pour résumer, ces tests nous ont permis de comprendre que pour avoir une localisation précise, il ne suffit pas que le code soit bon, il faut aussi que le placement des nœuds soit cohérent par rapport à la zone qu’on veut couvrir.

Retour en haut