I’m working on a project to calculate the distances between anchors and a tag using triangulation and Kalman filtering, among other methods. I’ve shared the main part of my code below. I’m using UWB DW1000 modules and coding in the Keil 5.36 development environment.
This is my first time tackling something like this, and I’m feeling a bit lost. I’m trying to automate the distance calculations to figure out the tag’s coordinates and also automate the angle calculations. In the end, I want to display the distances from Anchor 1, Anchor 2, and Anchor 3 to the tag.
Can anyone point out what might be wrong with the code? Do I need to clean up any memory, or should I rethink how the functions are set up? I would really appreciate any help; I’m at a bit of a dead end here!
Sorry for comments on another language, I don’t have the energy to translate them. 🙁
#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;
}
You need to sign in to view this answers
