激光测距仪原理图解（手持式激光测距仪用法图解） - 原点资讯

驱动层主要实现 STM32 与 DWIC 的 SPI 通信，使用 HAL 库进行开发，完成SPI 配置、中断配置、IIC 配置、串口配置、看门狗配置等，对接到 DW_API 层，完成驱动层搭建。

DW_API 层使用 DecaWave 官方 API 进行移植搭建，API 将常用功能进行函数封装，DWIC 的主要功能实现基本通过读写相应的寄存器实现，通过官方 API 内的简单 example 结合
DW1000_Software_API_Guide.pdf》可大概了解常用 API 功能。

应用层是实现 TWR 测距主要功能的实现代码，完成系统的状态读取，参数配置，TWR 据收发，TOF 计算，卡尔曼滤波，串口数据发送等功能。

主程序工作流程
主程序位于 Src/application/dw_main.c，主要完成设备参数初始化和按拨码开
关状态进行基站或标签状态机运行，最后进行串口数据打包发送。基站工作过程由状态机实现，完成单周期ADSTWR 全过程，主要工作流程如图：

激光测距仪原理图解,手持式激光测距仪用法图解(21)

代码源码如下：

void anchor_app(void) { switch (state) { case STA_INIT_poll_BLINK: //初始化接收机，接收poll消息 dwt_setpreambledetecttimeout(0); //清除前导码超时，一直接收 dwt_setrxtimeout(0); //清除接收数据超时，一直接收 int ret = dwt_rxenable(DWT_START_RX_IMMEDIATE); //打开接收机，等待接收数据 rx_status = RX_WAIT; //清rx标志位，中断服务函数更改其状态 state = STA_WAIT_POLL_BLINK; /* 省略代码*/ break; case STA_WAIT_POLL_BLINK: //等待poll消息，中断回调函数状态变更 if(rx_status == RX_OK) //接收到数据 { rx_status = RX_WAIT; state = STA_recv_POLL_BLINK; } else if((rx_status == RX_TIMEOUT)||(rx_status == RX_ERROR)) //接收数据错误，重新开启接收 { state = STA_INIT_POLL_BLINK; } /* 省略代码*/ break; case STA_RECV_POLL_BLINK://接收处理poll消息 if(rx_buffer[FUNC_CODE_IDX] == FUNC_CODE_POLL) //判断收到的数据是POLL { range_nb = rx_buffer[RANGE_NB_IDX]; //取range_nb，resp发送时发送相同的range_nb recv_tag_id = rx_buffer[SENDER_SHORT_ADD_IDX]; //取发送标签的ID if(recv_tag_id >= inst_slot_number) //标签ID如果大于标签总容量则退出 { state = STA_INIT_POLL_BLINK; break; } range_time = portGetTickCnt(); //取得测距时间 poll_rx_ts = get_rx_timestamp_u64(); //获得poll_rx时间戳 /* 省略代码*/ } else //非POLL数据重新开启接收POLL { state = STA_INIT_POLL_BLINK; } /* 省略代码*/ break; case STA_SORR_RESP://根据基站ID按顺序进行发送或接收resp消息 if(sr > 0)// sr>0 处于resp阶段，按基站ID判断接收resp或发送resp { if(rr & 0x01)//当前该基站需发送resp { rr = 0; state = STA_SEND_RESP; } else//当前该基站需接收resp { dwt_setdelayedtrxtime(resp_rx_time); //设置接收机开启延时时间 dwt_setrxtimeout(inst_resp_rx_timeout); //设置接收数据超时时间 dwt_setpreambledetecttimeout(PRE_TIMEOUT); //设置接收前导码超时时间 int ret = dwt_rxenable(DWT_START_RX_DELAYED); //延时开启接收机 if(ret == DWT_ERROR) { state = STA_INIT_POLL_BLINK; break; } rr = rr >> 1; state = STA_WAIT_RESP; } sr = sr - 1; /* 省略代码*/ } else//准备接收final { /* 省略代码*/ dwt_setrxtimeout(inst_final_rx_timeout); //设置接收数据超时时间 dwt_setpreambledetecttimeout(PRE_TIMEOUT); //设置接收前导码超时时间 int ret = dwt_rxenable(DWT_START_RX_DELAYED); //延时开启接收机 if(ret == DWT_ERROR) { state = STA_INIT_POLL_BLINK; break; } state = STA_WAIT_FINAL; } break; case STA_SEND_RESP: //打包发送resp消息 { /* 省略代码*/ /* resp数据打包 */ tx_resp_msg[SEQ_NB_IDX] = frame_seq_nb ; tx_resp_msg[RANGE_NB_IDX] = range_nb; tx_resp_msg[SENDER_SHORT_ADD_IDX] = anc_id; tx_resp_msg[RECEIVER_SHORT_ADD_IDX] = recv_tag_id; tx_resp_msg[FUNC_CODE_IDX] = FUNC_CODE_RESP; /* 省略代码*/ state = STA_SORR_RESP; } break; case STA_WAIT_RESP: //等待接收resp消息，在中断回调函数内rx_status状态变更 if(rx_status == RX_OK) { rx_status = RX_WAIT; state = STA_RECV_RESP; } else if((rx_status == RX_TIMEOUT)||(rx_status == RX_ERROR)) { rx_status = RX_WAIT; state = STA_SORR_RESP; } break; case STA_RECV_RESP: //接收到其他基站的resp消息 { if(rx_buffer[FUNC_CODE_IDX] == FUNC_CODE_RESP)//正确接收到resp消息 { if(rx_buffer[RANGE_NB_IDX] == range_nb)//和当前测距具有相同的range_nb { uint8_t recv_anc_id = rx_buffer[SENDER_SHORT_ADD_IDX]; //取基站ID distance_report[recv_anc_id] = (int32_t)rx_buffer[RESP_MSG_PREV_DIS_IDX] << 24; distance_report[recv_anc_id] = (int32_t)rx_buffer[RESP_MSG_PREV_DIS_IDX 1] << 16; distance_report[recv_anc_id] = (int32_t)rx_buffer[RESP_MSG_PREV_DIS_IDX 2] << 8; distance_report[recv_anc_id] = (int32_t)rx_buffer[RESP_MSG_PREV_DIS_IDX 3]; } } state = STA_SORR_RESP; } break; case STA_WAIT_FINAL: //等待接收final消息，在中断回调函数内rx_status状态变更 if(rx_status == RX_OK) { { state = STA_RECV_FINAL; } } else if((rx_status == RX_TIMEOUT)||(rx_status == RX_ERROR)) { rx_status = RX_WAIT; state = STA_INIT_POLL_BLINK; range_status = RANGE_ERROR; } break; case STA_RECV_FINAL: //接收到final消息，数据处理 if ((rx_buffer[FUNC_CODE_IDX] == FUNC_CODE_FINAL) && (rx_buffer[RANGE_NB_IDX] == range_nb)) { resp_valid = rx_buffer[FINAL_MSG_FINAL_VALID_IDX]; if((resp_valid >> anc_id) & 0x01) //final消息中，本基站发送的resp消息是有效的,则进行距离计算 { uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts; uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32; double Ra, Rb, Da, Db; int64_t tof_dtu; double tof; resp_tx_ts = get_tx_timestamp_u64(); //取得resp_tx时间戳 final_rx_ts = get_rx_timestamp_u64(); //取得final_rx时间戳 /* 从final消息中，取得poll_tx时间戳，resp_rx时间戳，final_tx时间戳 */ final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts); final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP1_RX_TS_IDX anc_id * FINAL_MSG_TS_LEN], &resp_rx_ts); final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts); /* 计算飞行时间 */ poll_rx_ts_32 = (uint32_t)poll_rx_ts; resp_tx_ts_32 = (uint32_t)resp_tx_ts; final_rx_ts_32 = (uint32_t)final_rx_ts; Ra = (double)(resp_rx_ts - poll_tx_ts); Rb = (double)(final_rx_ts_32 - resp_tx_ts_32); Da = (double)(final_tx_ts - resp_rx_ts); Db = (double)(resp_tx_ts_32 - poll_rx_ts_32); tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra Rb Da Db)); tof = (int32)tof_dtu; if (tof > 0x7FFFFFFF) { tof -= 0x80000000; } tof = tof * DWT_TIME_UNITS; distance_now_m = tof * SPEED_OF_LIGHT; if(distance_now_m > 20000.000) { distance_now_m = -1; } distance_now_m = distance_now_m - (float)distance_offset_cm/100.0f; //将上次的测距值写入distance_report用于串口输出 distance_report[anc_id] = prev_range[recv_tag_id].distance; //更新prev_range为本次测距值 prev_range[recv_tag_id].distance = distance_now_m * 1000;//单位转换为mm prev_range[recv_tag_id].range_nb = range_nb; valid_report = resp_valid; /* 省略代码*/ } } dwt_forcetrxoff(); state = STA_INIT_POLL_BLINK; break; default: break; } }

测距算法相关代码：

poll_rx_ts_32 = (uint32_t)poll_rx_ts; resp_tx_ts_32 = (uint32_t)resp_tx_ts; final_rx_ts_32 = (uint32_t)final_rx_ts; Ra = (double)(resp_rx_ts - poll_tx_ts); Rb = (double)(final_rx_ts_32 - resp_tx_ts_32); Da = (double)(final_tx_ts - resp_rx_ts); Db = (double)(resp_tx_ts_32 - poll_rx_ts_32); tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra Rb Da Db)); tof = (int32)tof_dtu; tof = tof * DWT_TIME_UNITS; distance_now_m = tof * SPEED_OF_LIGHT;

代码对应的公式：

激光测距仪原理图解,手持式激光测距仪用法图解(22)