[UWB之TDOA]lps-node-firmware代码中uwb_tdoa_anchor2.c代码解析

https://github.com/bitcraze/lps-node-firmware代码中uwb_tdoa_anchor2.c代码解析：


根据该文件的注释，TDOA测距的实现使用的是TDMA的原理，所谓的TDMA,就是根据时隙决定谁来发送什么类型的数据，或者决定由谁来使用该时隙，这里uwb_tdoa_anchor2的实现就是根据slot号决定由谁来进行tdoa测距的业务实现； 

/*
 * This anchor algorithm is using TDMA to divide frames in 8 timeslots. Each
 * anchor is sending a packet in one timeslot, anchor n sends its packet in
 * timeslot n. The slot time is of 2ms.
 *
 * Each packet contains (assuming the packet is sent by anchor n):
 *   - A list of 8 IDs that contains the sequence number of the packets
 *     - At index n: The sequence number of this packet
 *     - At index != n: The sequence number of the last packet received by
 *       anchor 'index'
 *   - A list of 8 timestamps that contains
 *     - At index n: The TX timestamp of the current packet in anchor n time
 *     - At index != n: The RX timestamp of all other packets from previous
 *                      frame in anchor n clock. If the previous packet was
 *                      invalid the timestamp is 0
 *   - A list of 7 distances, the distance from this anchor to the other
 *     anchors in the system expressed in this anchor clock. The distance to
 *     the current anchor is reserved.
 *
 * This is enough info for an observer to calculate the time of departure
 * of any packets in this anchor clock, and so to calculate the difference time
 * of arrivale of the packets at the tag.
 */


这里uwb模块就没有了anchor和tag的概念，统一都是anchor， 但有主anchor和从anchor两种角色，测距主要在主站完成，从站在固定的slot位发送测距请求，主站完成测距过程，如果在非指定的slot发送测距请求，则主站认为是无效的测距请求，不予处理；



主站和从站上电后，从站都处于RX状态，等待主站发送第一个同步时间戳的包，主站则发送第一个时间戳同步包：

static uint32_t tdoa2UwbEvent(dwDevice_t *dev, uwbEvent_t event)
{
  if (ctx.state == synchronizedState) {
    return slotStep(dev, event);
  } else {
    if (ctx.anchorId == 0) {
      //标识主站，发送第一个同步时间戳的包
      dwGetSystemTimestamp(dev, &ctx.tdmaFrameStart);
      ctx.tdmaFrameStart.full = TDMA_LAST_FRAME(ctx.tdmaFrameStart.full) + 2*TDMA_FRAME_LEN;
      ctx.state = synchronizedState;
      setupTx(dev);

      ctx.slotState = slotTxDone;
      updateSlot();
    } 
    //。。。。。
}


测距包的数据结构：

#define NSLOTS 8

#define TDMA_SLOT_BITS 26 // 26: 2ms timeslot

#define TDMA_NSLOT_BITS 3


// Timeout for receiving a packet in a timeslot

#define RECEIVE_TIMEOUT 300


// Timeout for receiving a service packet after we TX ours

#define RECEIVE_SERVICE_TIMEOUT 800


#define TS_TX_SIZE 4


// Packet formats

#define PACKET_TYPE_TDOA2 0x22


typedef struct rangePacket_s {

  uint8_t type;

  uint8_t pid[NSLOTS];  // Packet id of the timestamps

  uint8_t timestamps[NSLOTS][TS_TX_SIZE];  // Relevant time for anchors

  uint16_t distances[NSLOTS];

} __attribute__((packed)) rangePacket_t;


模块初始化代码：

static void setTxData(dwDevice_t *dev){
  //….
  if (firstEntry) {
    MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);

    memcpy(txPacket.sourceAddress, base_address, 8);
    txPacket.sourceAddress[0] = ctx.anchorId;
    memcpy(txPacket.destAddress, base_address, 8);
    txPacket.destAddress[0] = 0xff;

    txPacket.payload[0] = PACKET_TYPE_TDOA2;

    firstEntry = false;
  }
  //….
}

// Initialize/reset the agorithm
static void tdoa2Init(uwbConfig_t * config, dwDevice_t *dev)
{
  ctx.anchorId = config->address[0];
  ctx.state = syncTdmaState;
  ctx.slot = NSLOTS-1;
  ctx.nextSlot = 0;
  memset(ctx.txTimestamps, 0, sizeof(ctx.txTimestamps));
  memset(ctx.rxTimestamps, 0, sizeof(ctx.rxTimestamps));
}



其中主站和从站均使用如下的状态机：

// FSM states

enum state_e {

  syncTdmaState = 0, // Anchors 1 to 5 starts here and rise up to synchronizedState

  syncTimeState,

  synchronizedState, // Anchor 0 is always here!

};


enum slotState_e {

  slotRxDone,

  slotTxDone,

};


主站的状态机：

state

syncTdmaState ->  synchronizedState -> syncTdmaState

slotState

slotTxDone -> slotRxDone->slotTxDone




从站的状态机：

state

syncTdmaState ->  synchronizedState

slotState

slotRxDone -> slotTxDone->slotRxDone




ctx.anchorId = 0 的为主站

具体逻辑在 tdoa2UwbEvent方法中，

    if (ctx.anchorId == 0) {
      //标识主站
      dwGetSystemTimestamp(dev, &ctx.tdmaFrameStart);
      ctx.tdmaFrameStart.full = TDMA_LAST_FRAME(ctx.tdmaFrameStart.full) + 2*TDMA_FRAME_LEN;
      ctx.state = synchronizedState;
      setupTx(dev);

      ctx.slotState = slotTxDone;
      updateSlot();
    } else {
      //标识从站：也可以理解为被测距站
      switch (event) {
        case eventPacketReceived: {
            if (rxPacket.sourceAddress[0] == 0 && rxPacket.payload[0] == PACKET_TYPE_TDOA2) {
              //收到主站的同步请求
	}
        default:
          // Start the receiver waiting for a packet from anchor 0
          dwIdle(dev);
          dwSetReceiveWaitTimeout(dev, RECEIVE_TIMEOUT);
          dwWriteSystemConfigurationRegister(dev);

          dwNewReceive(dev);
          dwSetDefaults(dev);
          dwStartReceive(dev);
   }


从站在收到主站的同步时间戳请求后，修改开始发送时间，配置延时发送数据

              //收到主站的同步请求
              rangePacket_t * rangePacket = (rangePacket_t *)rxPacket.payload;

              // Resync local frame start to packet from anchor 0
              dwTime_t pkTxTime = { .full = 0 };
              memcpy(&pkTxTime, rangePacket->timestamps[0], TS_TX_SIZE);
              ctx.tdmaFrameStart.full = rxTime.full - (pkTxTime.full - TDMA_LAST_FRAME(pkTxTime.full));

              ctx.tdmaFrameStart.full += TDMA_FRAME_LEN;

              setupTx(dev);
              ctx.slotState = slotRxDone;
              ctx.state = synchronizedState;





每个槽位发送包的逻辑在这里：

// Setup the radio to send a packet in the next timeslot
static void setupTx(dwDevice_t *dev)
{
  ctx.packetIds[ctx.anchorId] = ctx.pid++;
  dwTime_t txTime = transmitTimeForSlot(ctx.nextSlot);
  ctx.txTimestamps[ctx.anchorId] = txTime.low32;

  dwSetReceiveWaitTimeout(dev, RECEIVE_SERVICE_TIMEOUT);
  dwWriteSystemConfigurationRegister(dev);

  dwNewTransmit(dev);
  dwSetDefaults(dev);
  setTxData(dev);
  dwSetTxRxTime(dev, txTime);

  dwWaitForResponse(dev, true);
  dwStartTransmit(dev);
}


关键是这行代码：

 dwTime_t txTime = transmitTimeForSlot(ctx.nextSlot);


/* Calculate the transmit time for a given timeslot in the current frame */
static dwTime_t transmitTimeForSlot(int slot)
{
  dwTime_t transmitTime = { .full = 0 };

  // Calculate start of the slot
  transmitTime.full = ctx.tdmaFrameStart.full + slot*TDMA_SLOT_LEN;
  // Add guard and preamble time
  transmitTime.full += TDMA_GUARD_LENGTH;
  transmitTime.full += PREAMBLE_LENGTH;

  // DW1000 can only schedule time with 9 LSB at 0, adjust for it
  adjustTxRxTime(&transmitTime);

  return transmitTime;
}



几点待优化考虑的点：

1、如果主站附件多于8个从站如何处理，从站标识如何确定，依靠取模解决，排队如何进行，或者说调整为最大的slot？

2、TDOA测距的精度如何确定？


TDMA,也就是时分多址,非常好理解,同样的一段频谱在同时同地给不同的人使用,那就会产生强干扰，那就不同时给不同的用户使用，就是同样的一段频谱在时间上进行划分（时隙），然后分给不同的用户使用，每个用户只在属于自己的时隙里通信，这样就可以避免掉同频干扰了，但如果时隙不够给不同的用户分配时，则需要排队，也就是排队进房间进行通信。



TDMA技术说明

多址技术分为频分多址FDMA、时分多址TDMA、码分多址CDMA、空分多址SDMA。


1．频分多址（FDMA）技术


是让不同的地球通信站占用不同频率的信道进行通信。因为各个用户使用着不同频率的信道，所以相互没有干扰。早期的移动通信就是采用这个技术。


2．时分多址（TDMA）技术


这种多址技术是让若干个地球站共同使用一个信道，但是我们把一个载波在不同的时间上进行切片，分为8个时隙给8个用户用，由于占用的时间不同，所以相互之间不会干扰。显然，在相同信道数的情况下，采用时分多址要比频分多址能容纳更多的用户。


3．码分多址（CDMA）技术


这种多址技术也是多个地球站共同使用一个信道。但是每个地球站都被分配有一个独特的“码序列”，与所有别的“码序列”都不相同且正交，所以各个用户相互之间也没有干扰。因为是靠不同的“码序列”来区分不同的地球站，所以叫做“码分多址”。采用CDMA技术可以比时分多址方式容纳更多的用户。 



https://blog.csdn.net/whushenlei/article/details/41745993

MAC TDMA系统的设计围绕着时钟同步和时隙调度两个方面。


时钟同步：在通信系统中时钟的同步是一个很重要的问题。


Beacon帧是WLAN网络中一种很重要的管理帧，将本地时钟的替换成接收到的时间戳从而完成了时钟的同步。在本系统中只保留一个路由节点的Beacon帧功能，从而使系统中所有的节点都与此时钟时间同步。


时隙调度：时隙调度是指节点只是特定的时间发送数据帧或管理帧，而在其他时刻处于等待状态。在传统的802.11协议中Beacon帧是通过这六个相应的定时器完成定时发送Beacon帧的，本方案正是利用了这六个定时器的来完成时隙调度。


呱牛笔记



-------------------广告线---------------
项目、合作，欢迎勾搭，邮箱：promall@qq.com


本文为呱牛笔记原创文章,转载无需和我联系,但请注明来自呱牛笔记 ,it3q.com

请先登录后发表评论
  • 最新评论
  • 总共0条评论