我正在开发一个项目，使用三角测量和卡尔曼滤波等方法来计算锚点和标签之间的距离。我在下面分享了代码的主要部分。我正在使用 UWB DW1000 模块并在 Keil 5.36 开发环境中进行编码。
这是我第一次处理这样的事情，我感到有点失落。我正在尝试自动进行距离计算，以找出标签的坐标，并自动进行角度计算。最后，我想显示从 Anchor 1、Anchor 2、Anchor 3 到标签的距离。
任何人都可以指出代码可能有什么问题吗？我是否需要清理任何内存，或者我应该重新考虑如何设置函数？我真的很感激任何帮助；我在这里有点走进了死胡同！
抱歉对另一种语言的评论，我没有精力翻译它们。 :(
#include "stm32f10x.h" // Стандартная библиотека для работы с STM32
#include <stdio.h>
#include <string.h>
#include "deca_device_api.h" // API для взаимодействия с устройством DW1000
#include "deca_regs.h" // Регистры DW1000
#include "deca_sleep.h" // Функции для организации задержек
#include "lcd.h" // Работа с LCD дисплеем
#include "port.h" // Порты и интерфейсы
#include "lcd_oled.h" // Драйвер OLED дисплея
#include "trilateration.h" // Модуль для работы с триангуляцией
#include <math.h>
#include "kalman.h"
#include "AT24C02.h" // Работа с EEPROM
#include "stm32_eval.h" // Библиотека для отладки STM32
#include "lib.h"
extern char dist_str[16]; // Строка для отображения дистанции
extern uint8_t TAG_ID; // Идентификатор метки
extern uint8_t MASTER_TAG; // Идентификатор главной метки
extern uint8_t SLAVE_TAG_START_INDEX; // Начальный индекс меток-слейвов
extern uint8_t ANCHOR_IND;
extern uint8 Semaphore[MAX_SLAVE_TAG];
/* Массив с координатами якорей */
vec3d AnchorList[ANCHOR_MAX_NUM];
/* Лучшее решение для позиции метки */
vec3d tag_best_solution;
/* Массив расстояний до якорей */
int Anthordistance[ANCHOR_MAX_NUM];
/* Счётчик количества измерений для каждого якоря */
int Anthordistance_count[ANCHOR_MAX_NUM];
/* Константа для обновления расстояний до якорей */
int ANCHOR_REFRESH_COUNT_set = 5;
#define ANCHOR_REFRESH_COUNT ANCHOR_REFRESH_COUNT_set
#ifdef __GNUC__
/* Если используется компилятор GCC/RAISONANCE, маленькая версия printf вызывает __io_putchar() */
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE * f)
#endif /* __GNUC__ */
/* Функция для вывода содержимого регистров */
void dwt_dumpregisters(char * str, size_t strSize) {
uint32 reg = 0; // Переменная для хранения значения регистра
uint8 buff[5]; // Буфер для чтения данных из устройства
int i; // Индекс для циклов
int cnt;
#if(0)
// Чтение и вывод регистров от 0x00 до 0x3F
for (i = 0; i < 0x3F; i++) { // Цикл для чтения всех регистров от 0 до 0x3F
dwt_readfromdevice(i, 0, 5, buff);
// Форматируем строку с результатом и добавляем в строку str
str += cnt = sprintf(str, "reg[%02X]=%02X%02X%02X%02X%02X", i, buff[4], buff[3], buff[2], buff[1], buff[0]); // Форматирование строки с данными
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x20
for (i = 0; i <= 32; i += 4) {
reg = dwt_read32bitoffsetreg(0x20, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x20, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x21
for (i = 0; i <= 44; i += 4) {
reg = dwt_read32bitoffsetreg(0x21, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x21, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x23
for (i = 0; i <= 0x20; i += 4) {
reg = dwt_read32bitoffsetreg(0x23, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x23, i, reg);
str += cnt = sprintf(str, "\n");
}
#else
// Чтение регистров для регистров 0x24
for (i = 0; i <= 12; i += 4) {
reg = dwt_read32bitoffsetreg(0x24, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x24, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x27
for (i = 0; i <= 44; i += 4) {
reg = dwt_read32bitoffsetreg(0x27, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x27, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x28
for (i = 0; i <= 64; i += 4) {
reg = dwt_read32bitoffsetreg(0x28, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x28, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x2A
for (i = 0; i < 20; i += 4) {
reg = dwt_read32bitoffsetreg(0x2A, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x2A, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x2B
for (i = 0; i < 24; i += 4) {
reg = dwt_read32bitoffsetreg(0x2B, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x2B, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x2f
for (i = 0; i < 40; i += 4) {
reg = dwt_read32bitoffsetreg(0x2f, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x2f, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x31
for (i = 0; i < 84; i += 4) {
reg = dwt_read32bitoffsetreg(0x31, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x31, i, reg);
str += cnt = sprintf(str, "\n");
}
// Чтение регистров для регистров 0x36 = PMSC_ID
for (i = 0; i <= 48; i += 4) {
reg = dwt_read32bitoffsetreg(0x36, i);
str += cnt = sprintf(str, "reg[%02X:%02X]=%08X", 0x36, i, reg);
str += cnt = sprintf(str, "\n");
}
#endif
}
void Anchor_Array_Init(void) {
int anchor_index = 0;
for (anchor_index = 0; anchor_index < ANCHOR_MAX_NUM; anchor_index++) {
Anthordistance[anchor_index] = 0; // Обнуляем расстояние
Anthordistance_count[anchor_index] = 0; // Обнуляем счётчик измерений
}
}
void Semaphore_Init(void) {
int tag_index = 0;
// Инициализируем семафоры для всех меток
for (tag_index = 0; tag_index < MAX_SLAVE_TAG; tag_index++) {
Semaphore[tag_index] = 0; // Все семафоры устанавливаем в 0 (неактивные)
}
}
int Sum_Tag_Semaphore_request(void) {
int tag_index = 0;
int sum_request = 0;
for (tag_index = SLAVE_TAG_START_INDEX; tag_index < MAX_SLAVE_TAG; tag_index++) {
sum_request += Semaphore[tag_index]; // Суммируем значение семафора
}
return sum_request; // Возвращаем общее количество запросов
}
void Tag_Measure_Dis(void) {
uint8 dest_anthor = 0, frame_len = 0;
float final_distance = 0;
frame_seq_nb = 0;
for (dest_anthor = 0; dest_anthor < ANCHOR_MAX_NUM; dest_anthor++) {
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb; // Индекс кадра
tx_poll_msg[ALL_MSG_TAG_IDX] = TAG_ID; // Добавление ID метки
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0); // Записываем данные в DW1000
dwt_writetxfctrl(sizeof(tx_poll_msg), 0); // Контроль передачи
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
dwt_rxenable(0);
uint32 tick1 = portGetTickCount(); // Получаем текущее время
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) {
if ((portGetTickCount() - tick1) > 350) {
break;
}
};
GPIO_SetBits(GPIOA, GPIO_Pin_1);
if (status_reg & SYS_STATUS_RXFCG) {
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
// Проверяем, совпадает ли идентификатор метки с текущим TAG_ID
if (rx_buffer[ALL_MSG_TAG_IDX] != TAG_ID)
continue;
rx_buffer[ALL_MSG_TAG_IDX] = 0;
rx_buffer[ALL_MSG_SN_IDX] = 0;
// Сравниваем полученный кадр с ожидаемым ответом
if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0) {
uint32 final_tx_time;
/* Получаем временные метки отправки запроса и приема ответа */
poll_tx_ts = get_tx_timestamp_u64(); // Временная метка отправки запроса
resp_rx_ts = get_rx_timestamp_u64(); // Временная метка получения ответа
/* Рассчитываем время для отправки финального сообщения. См. примечание 9. */
final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(final_tx_time); // Устанавливаем отложенную передачу с рассчитанным временем
/* Временная метка финальной передачи — это программируемое время плюс задержка антенны */
final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;
/* Записываем все временные метки в финальное сообщение. См. примечание 10. */
final_msg_set_ts( & tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts); // Временная метка отправки запроса
final_msg_set_ts( & tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts); // Временная метка получения ответа
final_msg_set_ts( & tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts); // Временная метка финальной передачи
/* Записываем и отправляем финальное сообщение. См. примечание 7. */
tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb; // Устанавливаем номер кадра
tx_final_msg[ALL_MSG_TAG_IDX] = TAG_ID; // Устанавливаем идентификатор метки
dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0); // Записываем данные для передачи
dwt_writetxfctrl(sizeof(tx_final_msg), 0); // Устанавливаем параметры контроля передачи
/* Запускаем передачу с отложенной отправкой */
dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
// Ожидаем получения кадра или ошибки
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) {
if ((portGetTickCount() - tick1) > 500) { // Если прошло более 500 мс
break; // Прерываем ожидание
}
};
/* Увеличиваем номер последовательности кадра после передачи (по модулю 256) */
if (status_reg & SYS_STATUS_RXFCG) { // Если кадр был успешно принят
/* Очищаем флаг успешного приема/передачи в регистре статуса DW1000 */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* Читаем принятый кадр в локальный буфер */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; // Получаем длину кадра
if (frame_len <= RX_BUF_LEN) { // Если длина кадра допустима
dwt_readrxdata(rx_buffer, frame_len, 0); // Читаем данные из буфера приема
}
// Проверяем идентификатор метки
if (rx_buffer[ALL_MSG_TAG_IDX] != TAG_ID)
continue; // Если не совпадает, продолжаем выполнение
rx_buffer[ALL_MSG_TAG_IDX] = 0; // Очищаем идентификатор для упрощения проверки
/* Очищаем поле номера последовательности кадра */
rx_buffer[ALL_MSG_SN_IDX] = 0;
// Проверяем, совпадают ли принятые данные с ожидаемым сообщением расстояния
if (memcmp(rx_buffer, distance_msg, ALL_MSG_COMMON_LEN) == 0) {
// Обрабатываем финальное расстояние
// final_distance = rx_buffer[10] + (float)rx_buffer[11]/100;
Anthordistance[rx_buffer[12]] += (rx_buffer[10] * 1000 + rx_buffer[11] * 10);
Anthordistance_count[rx_buffer[12]]++; // Увеличиваем счетчик измерений
{
int Anchor_Index = 0; // Индекс якоря
// Проверяем, не достигло ли количество измерений порогового значения
while (Anchor_Index < ANCHOR_MAX_NUM) {
if (Anthordistance_count[Anchor_Index] >= ANCHOR_REFRESH_COUNT) {
distance_mange(); // Вызываем функцию управления расстояниями
// Очищаем все данные после обновления
Anchor_Index = 0;
while (Anchor_Index < ANCHOR_MAX_NUM) {
Anthordistance_count[Anchor_Index] = 0; // Обнуляем счетчики измерений
Anthordistance[Anchor_Index] = 0; // Обнуляем расстояния
Anchor_Index++;
}
break; // Прерываем цикл
}
Anchor_Index++; // Переходим к следующему якорю
}
}
}
} else {
/* Очищаем все ошибки приема в регистре статуса DW1000 */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
} else {
/* Очищаем ошибки приема */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
/* Выполняем задержку между обменами дальностью (опционально). */
// deca_sleep(RNG_DELAY_MS); // Комментарий: задержка может быть использована для регулировки времени между запросами дальности
/* Увеличиваем номер последовательности кадра для следующего сообщения (по модулю 256). */
frame_seq_nb++; // Это необходимо для уникальной идентификации каждого отправляемого кадра
}
}
// Переменная для хранения финальной дистанции.
double final_distance = 0;
int main(void) {
// Переменные для индексов якорей и тегов (устройств).
uint8 anthor_index = 0;
uint8 tag_index = 0;
// Флаги для управления семафорами.
uint8 Semaphore_Enable = 0;
uint8 Waiting_TAG_Release_Semaphore = 0;
// Переменная для хранения длины кадров.
int8 frame_len = 0;
// Инициализация аппаратных модулей платы.
peripherals_init();
// Вывод приветственного сообщения.
printf("hello dwm1000!\r\n");
/* Сброс и инициализация чипа DW1000.
* На время инициализации тактовые сигналы DW1000 должны быть настроены на частоту кристалла.
* После инициализации скорость SPI можно увеличить для оптимальной производительности.
*/
reset_DW1000(); // Сброс линии RSTn на чипе DW1000 на низкий уровень на заданный период.
spi_set_rate_low(); // Установка низкой скорости SPI.
// Проверка инициализации DW1000.
if (dwt_initialise(DWT_LOADUCODE) == -1) {
printf("dwm1000 init fail!\r\n");
OLED_ShowString(0, 0, "INIT FAIL"); // Отображение ошибки на OLED.
// Постоянная индикация ошибки и мигание светодиода.
while (1) {
//STM_EVAL_LEDOn(LED1);
GPIO_SetBits(GPIOC, GPIO_Pin_13); // Включение светодиода.
deca_sleep(1000); // Задержка в 1 секунду.
//STM_EVAL_LEDOff(LED1);
GPIO_ResetBits(GPIOC, GPIO_Pin_13); // Выключение светодиода.
deca_sleep(1000); // Задержка в 1 секунду.
}
}
spi_set_rate_high(); // Установка высокой скорости SPI.
/* Конфигурация DW1000 с параметрами, установленными в переменной config. */
dwt_configure( & config);
dwt_setleds(1); // Включение светодиодов DW1000 для индикации работы.
/* Применение значений задержки антенны по умолчанию для передачи и приёма. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
OLED_ShowString(0, 0, "INIT PASS"); // Отображение успешной инициализации на OLED.
printf("init pass!\r\n");
printf("AIT-BU01-DB V100 T2020-5-17\r\n"); // Вывод версии прошивки устройства.
// Координаты для якорей.
AnchorList[0].x = 0.12;
AnchorList[0].y = 0.34;
AnchorList[0].z = 0;
AnchorList[1].x = 0.25;
AnchorList[1].y = 0;
AnchorList[1].z = 0;
AnchorList[2].x = 0;
AnchorList[2].y = 0;
AnchorList[2].z = 0;
int rx_ant_delay = 32880; // Задержка антенны при приёме.
int index = 0;
extern UserSet UserSetNow; // Экземпляр структуры пользовательских настроек.
// Буфер для хранения данных, считанных из памяти.
uint16_t buff[3] = {
1,
0,
0xff
};
FLASH_ReadMoreData(USER_FLASH_BASE, buff, 3); // Чтение данных из флеш-памяти.
if (buff[0] == 1) {
UserSetNow.ANCHOR_TAG = 1; // Якорь.
} else if (buff[0] == 0) {
UserSetNow.ANCHOR_TAG = 0; // Тег.
} else {
UserSetNow.ANCHOR_TAG = 1; // По умолчанию якорь.
}
//#ifdef ANTHOR
// Если устройство является якорем.
if (UserSetNow.ANCHOR_TAG == 1) {
// Установка идентификатора якоря.
if (buff[1] >= 0 && buff[1] <= 255) {
UserSetNow.ID = buff[1];
ANCHOR_IND = UserSetNow.ID;
}
printf("device:anchor ID:%d\r\n", ANCHOR_IND);
// Инициализация массива якорей.
Anchor_Array_Init();
// Бесконечный цикл для инициации обмена данными.
OLED_ShowString(0, 0, "DS TWR ANTHOR"); // Отображение режима работы.
//OLED_ShowString(0,2,"Distance:");
//KalMan_PramInit();
ANTHOR_MEASURE(); // Вызов функции измерений для якоря.
}
//#endif
//#ifdef TAG
// Если устройство является тегом.
if (UserSetNow.ANCHOR_TAG == 0) {
// Установка идентификатора тега.
if (buff[1] >= 0 && buff[1] <= 255) {
UserSetNow.ID = buff[1];
TAG_ID = UserSetNow.ID;
MASTER_TAG = TAG_ID;
}
printf("device:TAG ID:%d\r\n", UserSetNow.ID);
if (TAG_ID == MASTER_TAG) {
OLED_ShowString(0, 0, "DS MASTER TAG:"); // Тег является главным.
} else {
OLED_ShowString(0, 0, "DS SLAVE TAG:"); // Тег является подчинённым.
}
// Установка задержки для ответа и таймаута приёма.
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
//OLED_ShowString(0,2,"Distance:"); chnage by johhn
if (TAG_ID == MASTER_TAG) {
Semaphore_Enable = 1; // Разрешение работы семафора для главного тега.
Semaphore_Init(); // Инициализация семафора.
Waiting_TAG_Release_Semaphore = 0;
} else {
Semaphore_Enable = 0;
}
//Master TAG0
// Вызов функции измерений для тега.
TAG_MEASURE();
}
//#endif
}
#define Filter_N 5 // Максимальное количество фильтров, используемых в системе
const int Filter_D_set = 5; // Установка количества данных для каждого фильтра
#define Filter_D Filter_D_set // Каждому фильтру соответствует "Filter_D" данных
int Value_Buf[Filter_N][Filter_D] = { // Массив для хранения значений фильтров
0 // Инициализация всех значений в ноль
};
// Фильтрация данных с помощью усреднения значений.
int filter_index[Filter_N] = { // Индексы для отслеживания текущей позиции записи для каждого фильтра
0 // Инициализация индексов для всех фильтров
};
// Функция фильтрации данных
int filter(int input, int fliter_idx) {
char count = 0; // Счетчик для цикла
int sum = 0; // Переменная для хранения суммы значений фильтра
if (input > 0) { // Если входное значение положительное
// Запись входного значения в буфер фильтра
Value_Buf[fliter_idx][filter_index[fliter_idx]++] = input;
// Если индекс превышает размер фильтра, сбросить его в 0
if (filter_index[fliter_idx] == Filter_D) filter_index[fliter_idx] = 0;
// Подсчет суммы значений в буфере
for (count = 0; count < Filter_D; count++) {
sum += Value_Buf[fliter_idx][count]; // Суммируем значения
}
return (int)(sum / Filter_D); // Возвращаем среднее значение
} else { // Если входное значение не положительное
for (count = 0; count < Filter_D; count++) {
sum += Value_Buf[fliter_idx][count]; // Суммируем значения
}
return (int)(sum / Filter_D); // Возвращаем среднее значение
}
}
// Управление процессом измерения расстояний.
static void distance_mange(void) {
{
int Anchor_Index = 0; // Индекс для обхода якорей
while (Anchor_Index < ANCHOR_MAX_NUM) { // Цикл по всем якорям
if (Anthordistance_count[Anchor_Index] > 0) { // Проверка, есть ли измеренные расстояния
// Применение фильтрации для каждого якоря.
Anthordistance[Anchor_Index] = filter((int)(Anthordistance[Anchor_Index] / Anthordistance_count[Anchor_Index]), Anchor_Index);
}
Anchor_Index++; // Переход к следующему якорю
}
}
// Вычисление углов и отправка данных на первый якорь.
compute_angle_send_to_anthor0(Anthordistance[0], Anthordistance[1], Anthordistance[2]);
// Отображение расстояний для каждого якоря.
for (int j = 0; j < ANCHOR_MAX_NUM; j++) {
if (Anthordistance_count[j] > 0) { // Проверка, есть ли данные для якоря
sprintf(dist_str, "an%d:%3.2fm", j, (float) Anthordistance[j] / 1000); // Форматирование строки с расстоянием
printf("%s\r\n", dist_str); // Вывод расстояния в консоль
}
}
// Отображение расстояния для первого якоря на OLED
if (Anthordistance_count[0] > 0) {
sprintf(dist_str, "an0:%3.2fm", (float) Anthordistance[0] / 1000);
//printf("%s\r\n",dist_str);
OLED_ShowString(0, 2 + j * 2, dist_str); // Отображение на OLED
}
// Отображение расстояния для второго якоря на OLED
if (Anthordistance_count[1] > 0) {
sprintf(dist_str, "an1:%3.2fm", (float) Anthordistance[1] / 1000);
//printf("%s\r\n",dist_str);
OLED_ShowString(0, 4, " "); // Очистка предыдущего вывода
OLED_ShowString(0, 4, dist_str); // Отображение нового значения
}
// Отображение расстояния для третьего якоря на OLED
if (Anthordistance_count[2] > 0) {
sprintf(dist_str, "%3.2fm", (float) Anthordistance[2] / 1000);
OLED_ShowString(0, 6, " "); // Очистка предыдущего вывода
OLED_ShowString(0, 6, dist_str); // Отображение нового значения
}
}
// Координаты якорей (вышек) в системе координат (X, Y)
#define ANCHOR0_X 0.0
#define ANCHOR0_Y 0.0
#define ANCHOR1_X 5.0
#define ANCHOR1_Y 0.0
#define ANCHOR2_X 2.5
#define ANCHOR2_Y 4.33
// Макросы для количества якорей
#define ANCHOR_MAX_NUM 3
#define TAG_ID 0x01
// Прототипы функций
static void compute_angle_send_to_anthor0(float tag_x, float tag_y);
// Управление процессом измерения расстояний.
static void distance_mange(void) {
// Объявление переменных для хранения координат тега
float tag_x = 0.0; // X координата тега
float tag_y = 0.0; // Y координата тега
// Проверка, что количество измеренных расстояний до якорей больше нуля
if (Anthordistance_count[0] > 0 && Anthordistance_count[1] > 0 && Anthordistance_count[2] > 0) {
// Координаты якорей
float x1 = anchor[0].x;
float y1 = anchor[0].y;
float d1 = Anthordistance[0] / 1000.0; // Расстояние до якоря 1 в метрах
float x2 = anchor[1].x;
float y2 = anchor[1].y;
float d2 = Anthordistance[1] / 1000.0; // Расстояние до якоря 2 в метрах
float x3 = anchor[2].x;
float y3 = anchor[2].y;
float d3 = Anthordistance[2] / 1000.0; // Расстояние до якоря 3 в метрах
// Решение системы уравнений для нахождения координат тега (x, y)
float A = 2 * (x2 - x1);
float B = 2 * (y2 - y1);
float C = d1 * d1 - d2 * d2 - x1 * x1 + x2 * x2 - y1 * y1 + y2 * y2;
float D = 2 * (x3 - x2);
float E = 2 * (y3 - y2);
float F = d2 * d2 - d3 * d3 - x2 * x2 + x3 * x3 - y2 * y2 + y3 * y3;
// Вычисление координат тега
tag_x = (C - F * (A / D)) / (B - E * (A / D));
tag_y = (C - A * tag_x) / B;
// Отправка вычисленных координат тега на сервер или другим компонентам
compute_angle_send_to_anthor0(tag_x, tag_y);
} else {
// Обработка случая, когда расстояния не измерены
// Например, можно установить координаты в (0, 0) или вывести сообщение об ошибке
tag_x = 0.0;
tag_y = 0.0;
compute_angle_send_to_anthor0(tag_x, tag_y); // Отправка (0, 0)
}
// Обновление значения текущего времени (если необходимо)
// current_time = get_current_time();
}
// Отображение расстояний для якорей на OLED
for (int j = 0; j < ANCHOR_MAX_NUM; j++) {
if (Anthordistance_count[j] > 0) { // Проверка, есть ли данные для якоря
sprintf(dist_str, "an%d:%3.2fm", j, (float) Anthordistance[j] / 1000); // Форматирование строки с расстоянием
OLED_ShowString(0, 2 + j * 2, dist_str); // Отображение на OLED
} else {
OLED_ShowString(0, 2 + j * 2, "an%d: --- "); // Отображение пустого значения, если данных нет
}
}
// Функция для расчёта расстояний и углов в реальном времени
static void compute_angle_send_to_anthor0(float tag_x, float tag_y) {
float distances[ANCHOR_MAX_NUM];
for (int i = 0; i < ANCHOR_MAX_NUM; i++) {
distances[i] = calculate_distance(anchor_x[i], anchor_y[i], tag_x, tag_y); // Расстояние до всех якорей
}
// Получение углов с использованием вычисленных расстояний
float angle = compute_angle(distances[0], distances[1], distances[2]);
// Вывод угла на OLED
sprintf(dist_str, "angle: %3.2f degrees", angle);
OLED_ShowString(0, 6, " "); // Очистка предыдущего вывода
OLED_ShowString(0, 6, dist_str); // Отображение угла на OLED
// Условия для управления автомобилем в зависимости от угла
if (distances[0] > 1) { // Если расстояние больше 1 метра
if (angle > 110) {
printf("turn right\r\n"); // Поворот вправо
} else if (angle < 75) {
printf("turn left\r\n"); // Поворот влево
} else {
printf("forward\r\n"); // Движение вперед
}
} else {
printf("stay here\r\n"); // Остановка на месте
}
// Блок для отправки информации о местоположении
uint8 len = 0; // Переменная для отслеживания длины сообщения
angle_msg[LOCATION_FLAG_IDX] = 1; // Установка флага местоположения
angle_msg[LOCATION_INFO_START_IDX + (len++)] = 'm';
angle_msg[LOCATION_INFO_START_IDX + (len++)] = 'r';
angle_msg[LOCATION_INFO_START_IDX + (len++)] = 0x02; // Начало сообщения
angle_msg[LOCATION_INFO_START_IDX + (len++)] = TAG_ID; // ID тега
// Добавление расстояний в сообщение
for (int i = 0; i < ANCHOR_MAX_NUM; i++) {
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)((distances[i] * 1000) & 0xFF); // Младший байт
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)(((distances[i] * 1000) >> 8) & 0xFF); // Старший байт
}
// Добавление символов конца строки
angle_msg[LOCATION_INFO_START_IDX + (len++)] = '\n'; // Символ новой строки
angle_msg[LOCATION_INFO_START_IDX + (len++)] = '\r'; // Возврат каретки
angle_msg[LOCATION_INFO_LEN_IDX] = len; // Записываем фактическую длину сообщения
// Отправка данных с помощью библиотеки dwt
angle_msg[ALL_MSG_SN_IDX] = framenum; // Номер кадра
angle_msg[ALL_MSG_TAG_IDX] = TAG_ID; // Идентификатор тега
dwt_writetxdata(sizeof(angle_msg), angle_msg, 0);
dwt_writetxfctrl(sizeof(angle_msg), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE);
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)) {};
framenum++; // Увеличение номера кадра
}
static void final_msg_set_ts(uint8 * ts_field, uint64 ts) {
if (ts_field == NULL) return; // Проверка указателя
for (int i = 0; i < FINAL_MSG_TS_LEN; i++) {
// Запись младшего байта временной метки (ts) в текущий элемент массива ts_field
ts_field[i] = (uint8) ts;
// Сдвиг временной метки вправо на 8 бит, чтобы получить следующий байт
ts >>= 8;
}
float distance1 = distances[0];
float distance2 = distances[1];
float distance3 = distances[2];
// Проверка, чтобы убедиться, что расстояния больше нуля
if (distance1 > 0 && distance2 > 0 && distance3 > 0) {
float angle = compute_angle(distance1, distance2, distance3); // Вычисление угла
printf("Computed angle: %f\n", angle);
}
}
#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t * file, uint32_t line) {
while (1) {}
}
#endif
// Функция, используемая для передачи символов через USART (например, при использовании printf)
PUTCHAR_PROTOTYPE {
/* Здесь можно реализовать свою версию функции fputc, которая отправляет символ через USART */
USART_ClearFlag(EVAL_COM1, USART_FLAG_TC);
USART_SendData(EVAL_COM1, (uint8_t) ch);
while (USART_GetFlagStatus(EVAL_COM1, USART_FLAG_TC) == RESET) {}
return ch;
}
for (i = 0; i <= 32; i += 4) {for (i = 0; i < 32; i += 4) {同样适用于
for (i = 0; i <= 44; i += 4) {