1. Alcance
La distancia (longitud) es una de las cantidades físicas más utilizadas. La medida de la distancia es la más común en nuestra vida diaria, como la medida de la longitud y el tamaño de los objetos en la vida diaria y el trabajo, y la medida de la altura humana. .
La longitud es una medida del espacio unidimensional y es la distancia de un punto a otro. La unidad internacional de longitud es "metro" ("m"), las unidades comúnmente utilizadas son kilómetros (km), metros (m), decímetros (dm), centímetros (cm), milímetros (mm), micrómetros (μm), nanómetros (nm) y así sucesivamente. El estándar nacional para la medición de longitud es GBT1219-2000.
2. Método de rango
2.1 Medición de la distancia de la regla de medición
Usando una regla con una escala para medir directamente la distancia, este método es fácil de operar y tiene una alta precisión.
2.2 Telemetría con línea de visión
Algunos instrumentos de medición usan estadios para medir la distancia, y los instrumentos usan la óptica y la trigonometría para determinar la distancia entre dos puntos. Los instrumentos de medición de línea de visión comunes incluyen teodolito, placa plana, nivel, etc. Estos instrumentos pasan a través del cable de línea de visión dentro del telescopio. La distancia entre el cable de línea de visión en la escala se denomina lectura de línea de visión. La distancia entre el instrumento y la escala es una función de la distancia entre las escalas La precisión de medición de la mayoría de los instrumentos de medición de línea de visión puede alcanzar 1/400. El principio de medición de distancia del instrumento de nivel es el siguiente:
2.3 Alcance ultrasónico
Algunos dispositivos usan ondas ultrasónicas para medir distancias. El principio de la medición ultrasónica de distancias es usar transmisores ultrasónicos para emitir ondas ultrasónicas y comenzar a cronometrar mientras emiten ondas ultrasónicas. Las ondas ultrasónicas se propagan en el aire y regresan inmediatamente cuando encuentran obstáculos. El receptor ultrasónico recibe ondas reflejadas Detenga inmediatamente el temporizador. La velocidad de propagación de las ondas ultrasónicas en el aire es de 340m/s, según el tiempo t (segundos) registrado por el temporizador, se puede calcular la distancia s entre el punto de emisión y el obstáculo: s=340*t/2
2.4 Alcance láser
El telémetro láser es un instrumento que utiliza láser para medir la distancia del objetivo. Existen aproximadamente tres métodos de medición de distancia: método de pulso, método de fase y método de reflexión triangular . El principio de funcionamiento del telémetro de láser pulsado es relativamente simple. Aquí hay una breve descripción del telémetro de láser pulsado. Su principio de funcionamiento es disparar un rayo láser de pulso corto al objetivo. El componente recibe el rayo láser reflejado y el temporizador mide el tiempo desde la emisión hasta la recepción del rayo láser, y calcula la distancia desde el observador hasta el objetivo.
El láser se propaga en el aire a la velocidad c, y el tiempo que requiere el láser para ir y venir entre dos puntos A y B es t, entonces la distancia D entre los dos puntos A y B se puede expresar de la siguiente manera.
En la fórmula: D es la distancia entre la estación A y B; c es la velocidad de propagación de la luz en la atmósfera; t es el tiempo que tarda la luz en ir y venir entre A y B una vez.
Además de los métodos de rango mencionados anteriormente, también hay un método de rango más popular en los últimos años (que es el protagonista del que vamos a hablar hoy): el rango UWB .
3. Tecnología y aplicación UWB
La tecnología UWB es una tecnología de comunicación inalámbrica que utiliza frecuencias superiores a 1 GHz . UWB no utiliza una portadora sinusoidal, sino pulsos estrechos de nanosegundos para transmitir datos . UWB ocupa un amplio rango de espectro y su velocidad de transmisión de datos puede alcanzar cientos de megabits por segundo. La tecnología UWB utiliza pulsos de banda base ultraancha para la comunicación y se utiliza principalmente en sistemas de comunicación para radares militares, posicionamiento y bajas tasas de intercepción. En febrero de 2002, la Comisión Federal de Comunicaciones de los Estados Unidos emitió normas preliminares sobre el espectro y la potencia utilizados por los equipos UWB civiles. En este reglamento, el sistema de comunicación que transmite un ancho de banda de datos superior a 500 MHz en la banda de frecuencia de 3,1~10,6 GHz se denomina sistema UWB.
La tecnología UWB utiliza un pulso con un ciclo de trabajo muy bajo como tecnología de espectro ensanchado sin portadora de la portadora de información y modula directamente el pulso de choque con un tiempo de subida y bajada muy pronunciado. El pulso de choque generalmente adopta un pulso gaussiano de un solo ciclo, y un bit de información puede asignarse a cientos de tales pulsos. El ancho del pulso de ciclo único es del orden de nanosegundos y tiene un espectro de frecuencia muy amplio.
La tecnología UWB resuelve los principales problemas de comunicación que han afectado a la tecnología de comunicación inalámbrica tradicional durante muchos años.La tecnología UWB tiene las siguientes características:
1. Realización simple de la estructura del sistema
2. Transmisión de datos de alta velocidad
3. Bajo consumo de energía
4. Alta seguridad
6. Posicionamiento preciso
7. Bajo costo del sistema
Realización de UWB La realización de
la estructura del sistema UWB es relativamente simple, y el transmisor UWB excita directamente al transmisor con pulsos de bajo costo. Durante la transmisión de datos de alta velocidad, generalmente se requiere que el rango de transmisión de las señales UWB esté dentro de los 10 m, y su velocidad de transmisión puede alcanzar más de 500 Mbit/s. El sistema UWB utiliza pulsos de intervalo para enviar datos, tiene un ciclo de trabajo muy bajo y el consumo de energía del sistema puede ser muy bajo. Durante la comunicación de alta velocidad, el consumo de energía del sistema es solo de unos pocos cientos de μW a decenas de mW. La imagen muestra la estructura básica del circuito del sistema de comunicación inalámbrica de banda ultraancha:
Aplicación UWB
1. Aplicación de comunicación , la velocidad de comunicación basada en la tecnología UWB puede alcanzar 1 Gb/s. Cuando la distancia de transmisión es muy corta, la ventaja de velocidad de la comunicación UWB es más obvia. Es muy adecuado para VR, transferencia de archivos de teléfonos móviles, redes inalámbricas y otros campos.
2. Las aplicaciones de alcance , basadas en la tecnología UWB, pueden lograr una medición de distancia de alta precisión y resolver problemas como el alcance y la puntería. Adecuado para seguimiento de distancia social, prevención de pérdida de artículos, etc.
3. Aplicaciones de posicionamiento . Basado en la tecnología UWB, se puede realizar un posicionamiento de alta precisión en interiores y exteriores. Esta es la aplicación más ventajosa de la tecnología UWB. El posicionamiento UWB se puede aplicar al posicionamiento de alta precisión en varios entornos complejos. Por ejemplo: gestión de la ubicación del personal penitenciario, seguimiento de la trayectoria de la actividad, gestión de la ubicación del personal en lugares de trabajo de alto riesgo, etc.
4. Aplicaciones de encriptación y autenticación , debido a la alta seguridad de la comunicación UWB, es adecuado para pago móvil, autenticación, tarjeta de crédito, tarjeta de identificación, tarjeta de deslizamiento, etc.
4. Medio de medición y algoritmo
Antes de explicar el principio del rango UWB, primero comprendamos las dos probabilidades en el proceso de rango:
1. El medio de rango , el medio o la herramienta utilizada para la medición de distancia.
2. Algoritmo de medición de distancia , el método de cálculo de distancia utilizado en la medición de distancia.
Con base en estos dos conceptos, utilizaremos el algoritmo de rango medio y rango para los varios métodos de medición de distancia mencionados en el segundo capítulo.
Regla de medición de distancia
Medio de medición: regla de medición con escala.
Algoritmo de medición de distancia: el método de diferencia mínima, encuentre la escala con la diferencia más pequeña entre el objeto medido y la escala de escala, y use este valor de escala como la distancia de medición.
Telemetría con línea de visión
Medio de telemetría: imágenes ópticas.
Algoritmo de alcance: Método de aproximación, ajustando la distancia focal óptica o la posición, de modo que la línea de visión en la lente óptica esté cerca del objetivo. Cuando la línea de visión en la lente óptica coincida con el objetivo, la distancia se determina de acuerdo con la parámetros ópticos.
Medición de distancia ultrasónica
Medio de medición de distancia: ultrasónico.
Algoritmo de distancia: tiempo de vuelo (TOF), al registrar la diferencia de tiempo entre la onda ultrasónica que se transmite y la recepción del eco ultrasónico, el valor de la distancia se obtiene multiplicando la velocidad de propagación del sonido por el tiempo.
Medición de distancia por láser
Medio de medición de distancia: láser.
Algoritmo de distancia: tiempo de vuelo (TOF), al registrar la diferencia de tiempo entre la emisión del láser y la recepción del eco del láser, el valor de la distancia se obtiene multiplicando la velocidad de la luz por el tiempo.
Medio de medición de distancia de medición de distancia UWB
: onda electromagnética UWB.
Algoritmo de rango: tiempo de vuelo (TOF), la información de error de tiempo en la señal de radio recibida, la diferencia de tiempo se calcula de acuerdo con la marca de tiempo y el valor de distancia se obtiene multiplicando la velocidad de propagación de radio por el tiempo.
Método de diferencia de sello de tiempo
Podemos usar el método de sello de tiempo para calcular la diferencia de tiempo. Usamos un ejemplo para describir el algoritmo de sello de tiempo :
Por ejemplo, un amigo nos escribió una carta en papel, y al final de la carta, el amigo escribió down La fecha en que se envió la carta fue el 1 de octubre de 2022. Después de un largo viaje, el cartero entregó la carta en nuestra casa. Abrimos la carta y leímos la fecha (1 de octubre de 2022) y luego según la fecha de recepción del sobre, octubre de 2022 El 8 de agosto, en base a estas dos fechas, se calculó que la carta tardó un total de 8 días desde el envío hasta la recepción .
Este es el algoritmo de marca de tiempo. Agregamos tiempo durante el proceso de comunicación, y el receptor puede calcular la diferencia de tiempo entre el envío y la recepción de la información en función de la hora en que se recibe la información (la hora del receptor y del remitente están sincronizadas).
Calculamos que se tarda un total de 8 días desde el envío hasta la recepción de la carta, suponiendo que la velocidad media del cartero enviando cartas es de 200km por día, por lo que podemos concluir que la distancia entre la casa de nuestro amigo y nuestra casa es de unos 1600km, que se mide por la distancia del método de diferencia de marca de tiempo .
5. Principio y algoritmo de rango UWB
Principio de rango UWB
El principio del rango UWB es utilizar el método de diferencia de marca de tiempo para calcular el tiempo de vuelo de las ondas electromagnéticas , calculando así la distancia. Hay dos tipos de entidades en la aplicación de rango UWB:
1. La estación base, que está instalada de forma fija en algún lugar para calcular la distancia y generar datos.
2. La etiqueta se instala en el objeto de destino que se va a medir.
La estación base y la etiqueta usan comunicación UWB, y hay una marca de tiempo en el proceso de comunicación, por lo que la estación base puede calcular la distancia entre la etiqueta y la estación base de acuerdo con la marca de tiempo . El diagrama de bloques de la implementación de la marca de tiempo UWB es el siguiente:
La etiqueta inicia la comunicación y los datos de comunicación contienen la información de tiempo t1 de envío de datos. Cuando la estación base recibe los datos, primero registra el tiempo de recepción t2 y luego extrae el tiempo de envío de la información recibida (suponiendo que el tiempo de la estación base y la etiqueta están sincronizadas), según El tiempo de vuelo t=t2-t1 se calcula a partir de la marca de tiempo de envío y la marca de tiempo de recepción , y la distancia desde la etiqueta hasta la estación base se calcula d= (c * t) /2 , donde c es la velocidad de la luz.
La descripción anterior es solo una breve introducción al principio básico de la distancia de medición UWB mediante el método de marca de tiempo, pero la aplicación real no es tan simple como se muestra en la figura anterior. En la aplicación práctica, el algoritmo de medición de distancia UWB se divide en: rango bidireccional (TWR) y rango bidireccional bilateral (ADSTWR).
Algoritmo de rango UWB
Rango bidireccional (TWR) El
rango bidireccional también se denomina TWR (Two-Way-Ranging). La estación base inicia una solicitud de rango y registra el tiempo t1. Después de que la etiqueta recibe la solicitud, registra el tiempo de recepción t2, y luego la etiqueta responde con una respuesta, respondiendo La información de respuesta incluye dos tiempos t2 (tiempo de solicitud de recepción) y t3 (tiempo de solicitud de respuesta). Cuando la estación base recibe la respuesta, registra el tiempo de recepción t4. En este momento, el la estación base tiene 4 tiempos: t1, t2, t3, t4, entonces El tiempo de vuelo T=( (t4 - t1) - (t3 - t2) )/ 2 , calcule la distancia D= T*c entre la estación base y el etiqueta. El diagrama de bloques del algoritmo es el siguiente:
Pregunta 1: ¿Por qué no usar t2 - t1 o t4 - t3 directamente para el tiempo de vuelo?
Debido a que el tiempo de los dos dispositivos, la estación base y la etiqueta, no está sincronizado, ¡Es muy difícil que los dos dispositivos sincronicen el tiempo!
Pregunta 2: ¿Cómo resolver el problema de sincronización de tiempo?
Observamos cuidadosamente los cuatro tiempos t1, t2, t3 y t4. No es difícil encontrar que t1 y t4 son los tiempos registrados por la estación base, y estos dos tiempos están sincronizados, t2 y t3 son los tiempos registrados por la estación base. etiqueta Estos dos tiempos son sincrónicos. Entonces usamos (t4 - t1) - (t3 - t2) para obtener el tiempo de vuelo, de modo que no se requiera la sincronización de tiempo de los dos dispositivos, la estación base y la etiqueta .
¿El algoritmo TWR resuelve perfectamente el problema? De hecho, este no es el caso. Tomemos como ejemplo la diferencia horaria (t4 - t1) registrada por la estación base. ¿Hay algún error en esta diferencia horaria? Suponemos que la frecuencia principal de la MCU es 10Mhz, y el contador en el temporizador aumentará en uno cada ciclo de reloj. Sabemos el tiempo de acuerdo con el contador . Por ejemplo, el valor del contador es 800. En este momento, pensamos que el tiempo es 80us.
Pero, ¿la frecuencia principal de la MCU realmente funciona a 10Mhz? ¡Es imposible que cada MCU funcione a 10Mhz absolutos! Hay un error en la frecuencia principal de trabajo de cada MCU. Suponiendo que el error de frecuencia principal de la MCU es del 1%, el tiempo real correspondiente cuando el valor del contador es 800 es 79.2us (80 * 0.99), el error de tiempo es 0.8 nosotros, y el error de distancia es de 120 m!
De acuerdo con el cálculo, se puede ver que cuando la diferencia de tiempo registrada por la estación base es mayor, el error de tiempo será mayor. t = T * P, donde t es el tiempo de error , T es la diferencia horaria registrada y P es el error de reloj.
Rango bidireccional bilateral (ADSTWR)
ADSTWR (Asymetic double side two way range) es una versión actualizada y mejorada de TWR (para mejorar el problema de grandes errores en el algoritmo TWR), ADS-TWR agrega una transmisión basada en TWR y mejora el método de cálculo de tiempo. El diagrama de bloques del algoritmo es el siguiente:
De acuerdo con estas tres comunicaciones, se pueden calcular dos tiempos de vuelo:
T1 = ( (t4 - t1) - (t3 - t2) ) / 2
T2 = ( (t6 - t3) - (t5 - t4) ) / 2
Dado que el tiempo entre las dos mediciones es muy corto, se puede considerar que la distancia no ha cambiado y el tiempo de vuelo también es igual, por lo que:
T1 = T2
Podemos encontrar el tiempo promedio de vuelo por dos tiempos de vuelo:
T~ = ( T1 + T2 )/2 = ( ( (t4 - t1) - (t3 - t2) ) + ( (t6 - t3) - (t5 - t4) ) ) / 4
Convirtamos la fórmula:
Ta = t4 - t1
Tb = t3 - t2
Tc = t6 - t3
Td = t5 - t4
entonces:
T1 = ( Ta - Tb ) / 2
T2 = (Tc - Td ) / 2
T~ = ( ( Ta - Tb) + ( Tc - Td ) ) / 4
Este algoritmo es el mismo que el algoritmo TWR, ¡así que el error es el mismo! ¡Así que tenemos que cambiar el algoritmo!
Un conocido:
T1 = ( Ta - Tb ) / 2
T2 = (Tc - Td ) / 2
T1 = T2
Convertir:
Ta = 2*T1 + Tb
Tc = 2*T2 + Td
Ta*Tc = ( 2*T1 + Tb ) *(2*T2 + Td)
= ( 2*T1 + Tb ) *(2*T1 + Td)
= 4*T1*T1 + 2*T1*Td + 2*T1*Tb + Tb*Td
entonces:
Ta*Tc - Tb*Td = 4*T1*T1 + 2*T1*Td + 2*T1*Tb
Ta*Tc - Tb*Td = T1*(4*T1 + 2*Td + 2*Tb )
en:
4*T1 = (2*T1) + ( 2*T2) = (Ta - Tb) + (Tc - Td )
entonces:
Ta*Tc - Tb*Td = T1*( (Ta - Tb) + (Tc - Td ) + 2*Td + 2*Tb )
Ta*Tc - Tb*Td = T1*(Ta + Tc + Td + Tb)
y terminar con:
T1 = (Ta*Tc - Tb*Td ) /(Ta + Tc + Td + Tb)
El error generado por este algoritmo tiene poco que ver con el tiempo de procesamiento de la respuesta enviada después de recibir la trama. El mismo error de reloj ADSTWR tiene mayor precisión de rango que TWR (este algoritmo se usa en nuestro diseño de algoritmo de software) .
6. Diseño integrado de rango UWB
Esta sección presenta el diseño de hardware y software de la estación base UWB. La MCU del circuito de la estación base usa STM32F103, la estación base usa el chip DW1000 para la comunicación UWB y la estación base usa una batería de litio para la fuente de alimentación. El diagrama esquemático del circuito de la estación base es el siguiente:
El circuito de posicionamiento UWB se basa en el sistema de posicionamiento UWB de la serie DecaWave DW1000. DW1000 utiliza la interfaz SPI. Los circuitos relevantes son los siguientes:
arquitectura de software
La arquitectura general del software integrado se muestra en la figura: se divide principalmente en capa de controlador, capa DW_API y capa de aplicación.
La capa del controlador realiza principalmente la comunicación SPI entre STM32 y DWIC, utiliza la biblioteca HAL para el desarrollo, completa la configuración de SPI, la configuración de interrupción, la configuración de IIC, la configuración del puerto serie, la configuración de vigilancia, etc., se conecta a la capa DW_API y completa el controlador construcción de capas.
La capa DW_API utiliza la API oficial de DecaWave para el trasplante y la construcción. La API encapsula funciones comunes. Las funciones principales de DWIC se realizan básicamente leyendo y escribiendo los registros correspondientes. Puede obtener una comprensión general de las funciones API comunes a través del ejemplo simple en el API oficial combinada con DW1000_Software_API_Guide.pdf.
La capa de aplicación es el código de implementación para realizar las funciones principales del rango TWR, completar la lectura del estado del sistema, la configuración de parámetros, la transmisión y recepción de datos TWR, el cálculo TOF, el filtro Kalman, la transmisión de datos en serie y otras funciones.
Flujo de trabajo del programa principal
El programa principal se encuentra en Src/application/dw_main.c, que principalmente completa la inicialización de los parámetros del dispositivo y la operación de la estación base o la máquina de estado de etiquetas de acuerdo con el estado del interruptor DIP, y finalmente
empaqueta y envía los datos seriales. El proceso de trabajo de la estación base lo realiza la máquina de estado, que completa todo el proceso de ADSTWR de ciclo único. El proceso de trabajo principal se muestra en la figura: el código fuente del código es el
siguiente :
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;
}
}
Código relacionado con el algoritmo de rango :
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;
La fórmula correspondiente al código:
Gracias:
CSDN: https://blog.csdn.net/li_man_man_man
Titulares de hoy: https://www.toutiao.com/article/7149576260891443724
La creación no es fácil, espero que a los amigos les guste, reenvíen, comenten, ¡sigan
! ¡Los comentarios y la atención serán mi fuerza impulsora para actualizaciones continuas!