DWM1000DISCOVERY TWR方式定位程序分析

DWM1000DISCOVERY默认程序使用的是STM32Cube_FW_F0_V1.5.0_DWM1000DISCOVERY_FACTORY_20160412.zip这个版本，这个程序可以在分享的资料的Code目录下找到，改程序是基于STM32官方STM32Cube_FW_F0_V1.5.0程序修改过来的，只保留了MDK工程目录。

对应的DWM1000DISCOVERY开发板请到如下地址查看：

https://item.taobao.com/item.htm?spm=a230r.1.14.15.4auZz7&id=528003782720&ns=1&abbucket=12#detail

Keil编译：

因此首先需要安装MDK5.14版本的Keil，在Tool目录下也共享了安装程序，请自行安装。Keil搞定之后，找到如下目录\STM32Cube_FW_F0_V1.5.0_justin\Projects\STM32F072RB-Nucleo\DWM1000DISCOVERY\MDK-ARM，就可以开启Keil，编译；有的朋友可能有如下问题：

因为代码是从linux下移植过来的，编译器不同，所以结构体和联合体这里定义会有所不同解决的方法是在联合体前面加入如下程序：

#pragma anon_unions

加入之后，就可以顺利编译通过。

也有的朋友出现如下错误：

STM32F072RB-Nucleo\STM32F072RB-Nucleo.axf: Error: L6411E: No compatible library exists with a definition of startup symbol __main.

问题原因是MDK与ADS冲突了，解决方法如下

方法一：卸载，同时把ADS1.2的环境变量删除：

我的电脑->属性->高级-环境变量-path里面把C:\Program Files\ARM\ADSv1_2等相似的5个变量给删除了就可以了
法二：MDK与ADS共存
在我的电脑->属性->高级-环境变量-path里面增加一个变量：
增加环境变量： ARMCC5LIB
变量值：C:\Keil\ARM\ARMCC\lib

代码分析：

代码只有一个，包括标签Tag和基站Anchor的程序，通过串口USART1配置不同的角色，配置角色和地址的过程可以参考：

<a target=_blank href="http://blog.csdn.net/xingqingly/article/details/51121481" target="_blank">http://blog.csdn.net/xingqingly/article/details/51121481</a>

注意角色不同，但是地址不能配置为相同的，也就是说Tag地址默认是0，Anchor地址一定不要再出现0，地址在整个系统中是唯一的，这点要注意，否则Tag和Anchor的地址相同，无法通信。</span>

main函数大致流程如下：

(1) 首先初始化外设接口，这里我们主要用到GPIO控制LED，I2C1控制eeprom和lps25h，串口USART1用于配置地址和角色，SPI1控制DWM1000，USB目前没用到。

<pre name="code" class="cpp">/* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_USART1_UART_Init();
  MX_SPI1_Init();
  MX_USB_DEVICE_Init();

初始化完成后，所有的LED亮：

<pre name="code" class="cpp"> // Light up all LEDs to test
  ledOn(ledRanging);
  ledOn(ledSync);
  ledOn(ledMode);

(2) 初始化外设，自测，首先自测的是lps25h，目前板子上已经不焊接改芯片，所以 这部分代码必须注释掉。

<span style="font-size:14px;"></span><pre name="code" class="cpp">  // Initializing pressure sensor (if present ...)
  lps25hInit(&hi2c1);
  printf("TEST\t: Initializing pressure sensor ... ");
  if (lps25hTestConnection()) {
    printf("[OK]\r\n");
    lps25hSetEnabled(true);
  } else {
    printf("[FAIL] (%u)\r\n", (unsigned int)hi2c1.ErrorCode);
    selftestPasses = false;
  }

  printf("TEST\t: Pressure sensor self-test ... ");
  if (lps25hSelfTest()) {
    printf("[OK]\r\n");
  } else {
    printf("[FAIL]\r\n");
    selftestPasses = false;
  }

(3) 初始化eeprom，进行简单的读操作，验证是否正常

<span style="font-size:14px;"></span><pre name="code" class="cpp">// Initializing i2c eeprom
  eepromInit(&hi2c1);
  printf("TEST\t: EEPROM self-test ... ");
  if (eepromTest()) {
    printf("[OK]\r\n");
  } else {
    printf("[FAIL]\r\n");
    selftestPasses = false;
  }

(4) 最后自检dwm1000

<span style="font-size:14px;"></span><pre name="code" class="cpp"> printf("TEST\t: Initialize DWM1000 ... ");
  dwInit(dwm, &dwOps);       // Init libdw
  dwOpsInit(dwm);
  result = dwConfigure(dwm); // Configure the dw1000 chip
  if (result == 0) {
    printf("[OK]\r\n");
    dwEnableAllLeds(dwm);
  } else {
    printf("[ERROR]: %s\r\n", dwStrError(result));
    selftestPasses = false;
  }

(5) 以上外设，如果任何一个自检失败，都会将selftestPasses参数设置为false，也就是自检失败，这时code将会停住在while(1)中。所以如果哪个外设没有，一定要将其注释掉。

<span style="font-size:14px;"></span><pre name="code" class="cpp"> if (!selftestPasses) {
    printf("TEST\t: One or more self-tests failed, blocking startup!\r\n");
    while(1);
  }

(6) 自检成功后，接下来对eeprom进行配置，eeprom内部存储格式，参考如下文档：
lps-node-firmware EEPROM数据格式

  cfgInit();

  if (cfgReadU8(cfgAddress, &address[0])) {
    printf("CONFIG\t: Address is 0x%X\r\n", address[0]);
  } else {
    printf("CONFIG\t: Address not found!\r\n");
  }

  if (cfgReadU8(cfgMode, &mode)) {
    printf("CONFIG\t: Mode is ");
    switch (mode) {
      case modeAnchor: printf("Anchor\r\n"); break;
      case modeTag: printf("Tag\r\n"); break;
      case modeSniffer: printf("Sniffer\r\n"); break;
      default: printf("UNKNOWN\r\n"); break;
    }
  } else {
    printf("Device mode: Not found!\r\n");
  }

  uint8_t anchorListSize = 0;
  if (cfgFieldSize(cfgAnchorlist, &anchorListSize)) {
    if (cfgReadU8list(cfgAnchorlist, (uint8_t*)&anchors, anchorListSize)) {
      printf("CONFIG\t: Tag mode anchor list (%i): ", anchorListSize);
      for (i = 0; i < anchorListSize; i++) {
        printf("0x%02X ", anchors[i]);
      }
      printf("\r\n");
    } else {
      printf("CONFIG\t: Tag mode anchor list: Not found!\r\n");
    }
  }

总的来讲就是存储当前节点的角色，基站anchor有多少个，等下次重新上电的时候直接读取eeprom记录的角色和地址信息，来执行相应的程序。

<span style="font-size:14px;">之后，所有的LED灯灭，然后进入配置dwm参数的程序：</span>

<span style="font-size:14px;"></span><pre name="code" class="cpp">  ledOff(ledRanging);
  ledOff(ledSync);
  ledOff(ledMode);

(7) dwm1000无线参数配置

<span style="font-size:14px;"></span><pre name="code" class="cpp">dwTime_t delay = {.full = ANTENNA_DELAY/2};
  dwSetAntenaDelay(dwm, delay);

  dwAttachSentHandler(dwm, txcallback);
  dwAttachReceivedHandler(dwm, rxcallback);

  dwNewConfiguration(dwm);
  dwSetDefaults(dwm);
  dwEnableMode(dwm, MODE_SHORTDATA_FAST_ACCURACY);
  dwSetChannel(dwm, CHANNEL_2);
  dwSetPreambleCode(dwm, PREAMBLE_CODE_64MHZ_9);

  dwCommitConfiguration(dwm);

需要注意的是几点：

<span style="font-size:14px;">注册回调函数txcallback和rxcallback，这里的程序会在dwm1000中断引脚IRQ触发的时候调用的函数，可以追下中断处理函数：</span>

<span style="font-size:14px;"></span><pre name="code" class="cpp">void EXTI0_1_IRQHandler(void)
{
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0);
}

会发现调用了dwHandleInterrupt函数

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  switch (GPIO_Pin) {
    case DWM_IRQ_PIN:
      do{
          dwHandleInterrupt(dev);
      } while(checkIrq() != 0); //while IRS line active (ARM can only do edge sensitive interrupts)
      HAL_NVIC_ClearPendingIRQ(DWM_IRQn);
      break;
    case GPIO_PIN_12:
      //instance_notify_DW1000_inIDLE(1);
      break;
    default:
      break;
  }
}

展开 dwHandleInterrupt函数会发现调用了

<span style="font-size:14px;"></span><pre name="code" class="cpp">void dwHandleInterrupt(dwDevice_t *dev) {
	// read current status and handle via callbacks
	dwReadSystemEventStatusRegister(dev);
	if(dwIsClockProblem(dev) /* TODO and others */ && _handleError != 0) {
		(*_handleError)();
	}
	if(dwIsTransmitDone(dev) && dev->handleSent != 0) {
		(*dev->handleSent)(dev);
		dwClearTransmitStatus(dev);
	}
	if(dwIsReceiveTimestampAvailable(dev) && _handleReceiveTimestampAvailable != 0) {
		(*_handleReceiveTimestampAvailable)();
		dwClearReceiveTimestampAvailableStatus(dev);
	}
	if(dwIsReceiveFailed(dev) && _handleReceiveFailed != 0) {
		(*_handleReceiveFailed)();
		dwClearReceiveStatus(dev);
		if(dev->permanentReceive) {
			dwNewReceive(dev);
			dwStartReceive(dev);
		}
	} else if(dwIsReceiveTimeout(dev) && dev->handleReceiveTimeout != 0) {
		(*dev->handleReceiveTimeout)(dev);
		dwClearReceiveStatus(dev);
		if(dev->permanentReceive) {
			dwNewReceive(dev);
			dwStartReceive(dev);
		}
	} else if(dwIsReceiveDone(dev) && dev->handleReceived != 0) {
		(*dev->handleReceived)(dev);
		dwClearReceiveStatus(dev);
		if(dev->permanentReceive) {
			dwNewReceive(dev);
			dwStartReceive(dev);
		}
	}
	// clear all status that is left unhandled
	dwClearAllStatus(dev);
}

其中：

<span style="font-size:14px;">(*dev->handleSent)(dev)是处理发送，那因为之前注册回调函数</span><span style="font-size:14px;">txcallback，所以这里其实调用的就是main中的</span><span style="font-size:14px;">txcallback；</span>

<span style="font-size:14px;">dev->permanentReceive是处理接收，之前注册的回调函数是r</span><span style="font-size:14px;">xcallback</span><span style="font-size:14px;">，所以这里其实调用的就是main中的</span><span style="font-size:14px;">r</span><span style="font-size:14px;">xcallback。</span><span style="font-size:14px;">
</span><span style="font-size:14px;">
</span><span style="font-size:14px;">(8) 初始化dwm1000无线发送包头和局域网ID </span>

<span style="font-size:14px;"></span><pre name="code" class="cpp">// Initialize the packet in the TX buffer
  MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
  txPacket.pan = 0xbccf;

(9) 如果角色为基站anchor或者低功耗sniffer（目前sniffer没有使用），dwm1000直接进入接收状态，之后的sniffer先不做分析。

<span style="font-size:14px;"></span><pre name="code" class="cpp">  if (mode == modeAnchor || mode == modeSniffer) {
    dwNewReceive(dwm);
    dwSetDefaults(dwm);
    dwStartReceive(dwm);
  }

(10) 进入while(1)大循环，其中配置地址和角色的地方可以看到：

<span style="font-size:14px;"></span><pre name="code" class="cpp"> // Accepts serial commands
    if (HAL_UART_Receive(&huart1, (uint8_t*)&ch, 1, 0) == HAL_OK) {
      bool configChanged = false;
      if (ch >= '0' && ch <= '9') {
        printf("Updating address from 0x%02X to 0x%02X\r\n", address[0], ch - '0');
        cfgWriteU8(cfgAddress, ch - '0');
        if (cfgReadU8(cfgAddress, &address[0])) {
          printf("Device address: 0x%X\r\n", address[0]);
        } else {
          printf("Device address: Not found!\r\n");
        }
        configChanged = true;
      }
      if (ch == 'A' || ch == 'T' || ch == 'S' || ch == 'a' || ch == 't' || ch == 's') {
        // Print current mode
        if (cfgReadU8(cfgMode, &mode)) {
          printf("Previous device mode: ");
          switch (mode) {
            case modeAnchor: printf("Anchor\r\n"); break;
            case modeTag: printf("Tag\r\n"); break;
            case modeSniffer: printf("Sniffer\r\n"); break;
            default: printf("UNKNOWN\r\n"); break;
          }
        } else {
          printf("Previous device mode: Not found!\r\n");
        }

        // Write new changes
        switch (ch) {
          case 'A': case 'a': cfgWriteU8(cfgMode, modeAnchor); break;
          case 'T': case 't': cfgWriteU8(cfgMode, modeTag); break;
          case 'S': case 's': cfgWriteU8(cfgMode, modeSniffer); break;
        }

        // Print out to verify
        if (cfgReadU8(cfgMode, &mode)) {
          printf("New device mode: ");
          switch (mode) {
            case modeAnchor: printf("Anchor\r\n"); break;
            case modeTag: printf("Tag\r\n"); break;
            case modeSniffer: printf("Sniffer\r\n"); break;
            default: printf("UNKNOWN\r\n"); break;
          }
        } else {
          printf("New device mode: Not found!\r\n");
        }
        configChanged = true;
      }

      if (ch == 'D' || ch == 'd') {
        configChanged = true;
        printf("Resetting EEPROM configuration...");
        if (cfgReset())
          printf("OK\r\n");
        else
          printf("ERROR\r\n");
      }
      if (configChanged) {
        printf("EEPROM configuration changed, restart for it to take effect!\r\n");
      }

    }

(11) 如果角色是标签Tag，需要主动发送数据给基站进行测距

<span style="font-size:14px;"></span><pre name="code" class="cpp">if (mode == modeTag) {
       for (i=0; i<anchorListSize; i++) {
         printf ("Interrogating anchor %d\r\n", anchors[i]);
         base_address[0] = anchors[i];
         dwIdle(dwm);

         txPacket.payload[TYPE] = POLL;
         txPacket.payload[SEQ] = ++curr_seq;

         memcpy(txPacket.sourceAddress, address, 8);
         memcpy(txPacket.destAddress, base_address, 8);

         dwNewTransmit(dwm);
         dwSetDefaults(dwm);
         dwSetData(dwm, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);

         dwWaitForResponse(dwm, true);
         dwStartTransmit(dwm);

         HAL_Delay(30);
       }
     }

<span style="font-size:14px;">测距的方法是根据TWR方式做的，需要标签Tag和基站之间交互数据计算时间差来最后得出距离，具体方法可以参考：</span>

<span style="font-size:14px;">aps013_dw1000_and_two_way_ranging.pdf这篇文档，另外</span>注意到这里发送的第一个数据类型是：poll

<span style="font-size:14px;">txPacket.payload[TYPE] = POLL;</span>

<span style="font-size:14px;">单纯看程序有很多时间节点，会有点晕，所以这里画出时间轴，分别标出标签tag和基站anchor获取的时间戳：</span>

<span style="font-size:14px;"><img src="http://img.blog.csdn.net/20160415123728779" alt="" />

最后基站anchor会发送report信息给标签tag，那report中包含的信息有基站这边前面所记录的poll_rx， answer_tx和final_rx，这样再结合标签tag之前获取的</span><span style="font-size:14px;">poll_tx， answer_rx和final_tx，然后参考：</span><span style="font-size:14px;">dw1000_user_manual_2.06.pdf 附录3</span><span style="font-size:14px;">: Two-Way Ranging的介绍12.3.2  Using three messages这种方法，得到飞行时间信息：</span>

<span style="font-size:14px;"><img src="http://img.blog.csdn.net/20160415124305255" alt="" />
</span>

<span style="font-size:14px;">也就是程序中：</span>

<span style="font-size:14px;"></span><pre name="code" class="cpp">tprop_ctn = ((tround1*tround2) - (treply1*treply2)) / (tround1 + tround2 + treply1 + treply2);

之后再乘以光速C，得到距离信息distance：

<pre name="code" class="cpp" style="font-size:14px;">tprop = tprop_ctn/tsfreq;
      distance = C * tprop;

      printf("distance %d: %5dmm\r\n", rxPacket.sourceAddress[0], (unsigned int)(distance*1000));

注意printf会打印到串口1距离信息，所以标签Tag这边需要接串口到PC获取距离。

<span style="font-size:14px;">以上是程序的大致流程，感谢大家的大力支持。</span>

<span style="font-size:14px;">






</span>

<span style="font-size:14px;">

</span>

<span style="font-size:14px;">



</span>

<span style="font-size:14px;">
</span>

<span style="font-size:14px;">
</span>