Мобильная версияЯ впервые сталкиваюсь с чем-то подобным и чувствую себя немного растерянным. Я пытаюсь автоматизировать расчеты расстояний, чтобы определить координаты метки, а также автоматизировать расчеты углов. В конце концов, я хочу отобразить расстояния от привязки 1, привязки 2 и привязки 3 до тега.
Может ли кто-нибудь указать, что может быть не так с кодом? Нужно ли мне очищать память или стоит переосмыслить настройку функций? Я был бы очень признателен за любую помощь; Я здесь немного в тупике!
Извините за комментарии на другом языке, у меня нет сил их переводить.
#include "stm32f10x.h" // Стандартная библиотека для работы с STM32
#include
#include
#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
#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 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 = calculate_distance(anchor_x, anchor_y, 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 * 1000) & 0xFF); // Младший байт
angle_msg[LOCATION_INFO_START_IDX + (len++)] = (uint8)(((distances * 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 = (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;
}
Подробнее здесь: https://stackoverflow.com/questions/791 ... 00-modules
