Радар на ESP32 с двухъядерной оптимизацией: плавное сканирование и фильтрация ложных отражений
При создании простого радиолокатора на базе ESP32 и ультразвукового датчика HC‑SR04 многие сталкиваются с проблемами: дрожание сервопривода, зависания дисплея, ложные срабатывания. В этой статье я покажу, как решить эти проблемы, используя оба ядра ESP32, атомарный доступ к данным и интеллектуальный фильтр объектов.
Почему обычный подход не работает?
В типичном Arduino‑скетче весь код выполняется последовательно: измеряем расстояние, поворачиваем серву, обновляем дисплей. Но у HC‑SR04 есть задержка измерения (до 25 мс), сервопривод требует плавных перемещений, а дисплей TFT — постоянной отрисовки. В результате угол сканирования начинает «дёргаться», изображение обновляется рывками, а ложные отражения засоряют экран.
Решение — использовать FreeRTOS, встроенную в ESP32, и распределить нагрузку на два ядра. Одно ядро отвечает исключительно за управление сервоприводом, второе — за измерения, логику и вывод на дисплей. Такая архитектура даёт плавное движение сервы и стабильную картинку.
Общая структура кода
Весь код разделён на несколько логических блоков:
- Статический слой — сетка, шкалы, панель, которые рисуются один раз.
- Динамический слой — текущий луч, обнаруженные объекты, показания.
- Фильтр объектов — структура, хранящая каждый объект с количеством попаданий и временем последнего обнаружения.
- Две задачи FreeRTOS —
servoTask(ядро 0) иmainTask(ядро 1).
Статический слой
Статический слой рисуется один раз в setup(). Он включает фон, сетку радиусов (40, 80, 120, 160, 200 пикселей), радиальные линии через каждые 30 градусов, верхнюю и нижнюю панели. Это позволяет избежать перерисовки одних и тех же элементов в каждом цикле, экономя ресурсы.
void drawStaticLayer() {
tft.fillScreen(COLOR_BG);
tft.fillRect(0, 0, DISPLAY_WIDTH, 30, COLOR_PANEL);
tft.setTextColor(COLOR_TEXT);
tft.setCursor(50, 5);
tft.print("ESP32 RADAR");
for (int r = 40; r <= RADAR_RADIUS; r += 40) {
tft.drawCircle(RADAR_CENTER_X, RADAR_CENTER_Y, r, COLOR_GRID);
}
for (int a = -60; a <= 60; a += 30) {
float rad = radians(a);
int x = RADAR_CENTER_X + RADAR_RADIUS * sin(rad);
int y = RADAR_CENTER_Y - RADAR_RADIUS * cos(rad);
tft.drawLine(RADAR_CENTER_X, RADAR_CENTER_Y, x, y, COLOR_GRID);
}
}Задача управления сервоприводом (ядро 0)
Это задача с высоким приоритетом. Она работает независимо, плавно изменяя угол с шагом SCAN_STEP (2 градуса) и задержкой SERVO_UPDATE_MS (15 мс). Направление меняется при достижении границ сектора (от 0 до SCAN_ANGLE). Текущий угол сохраняется в атомарную переменную currentAngle, доступную второй задаче без блокировок.
void servoTask(void *pvParameters) {
int localAngle = 0;
int localDir = 1;
while (1) {
localAngle += localDir * SCAN_STEP;
if (localAngle >= SCAN_ANGLE) {
localAngle = SCAN_ANGLE;
localDir = -1;
}
if (localAngle <= 0) {
localAngle = 0;
localDir = 1;
}
currentAngle = localAngle; // атомарная запись
int servoAngle = map(localAngle, 0, SCAN_ANGLE, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
radarServo.write(servoAngle);
vTaskDelay(pdMS_TO_TICKS(SERVO_UPDATE_MS));
}
}Почему переменная
currentAngleобъявлена какvolatile? Это указание компилятору, что значение может измениться в любой момент из другой задачи. В ESP32 чтение/запись целого числа являются атомарными, поэтому мьютексы не нужны. Это повышает стабильность и избегает блокировок.
Основная задача (ядро 1)
На втором ядре выполняется всё остальное: измерение расстояния, обновление списка объектов, отрисовка динамического слоя. Задержка vTaskDelay(1) даёт процессору время на обслуживание других системных задач и сбрасывает сторожевой таймер (watchdog).
void mainTask(void *pvParameters) {
float lastDistance = 0;
TickType_t lastDisplay = xTaskGetTickCount();
while (1) {
int angle = currentAngle; // атомарное чтение
float d = measureDistance();
if (d > 0) {
lastDistance = d;
updateObjects(d, angle);
}
if ((xTaskGetTickCount() - lastDisplay) >= pdMS_TO_TICKS(DISPLAY_UPDATE_MS)) {
drawDynamicLayer(angle, lastDistance);
lastDisplay = xTaskGetTickCount();
}
vTaskDelay(pdMS_TO_TICKS(1));
}
}Фильтр ложных отражений
Ультразвуковой датчик часто даёт случайные выбросы. Чтобы отсечь их, мы используем структуру RadarObject, в которой хранятся координаты, количество попаданий (hits), время последнего обнаружения и флаг активности.
struct RadarObject {
float distance;
int angle;
uint8_t hits;
unsigned long lastSeen;
bool active;
};В функции updateObjects() происходит следующее:
- Если обнаруженное расстояние соответствует уже существующему объекту (угол ±6°, дистанция ±10 см), увеличиваем счётчик
hits. - Если объект не найден, но расстояние в допустимых пределах, создаём новый объект.
- Удаляем объекты, которые не обновлялись более 1.2 секунды.
Объекты отображаются на экране только если hits >= 2. Это надёжно отсекает одиночные ложные срабатывания.
Динамический слой
Динамический слой перерисовывается с частотой DISPLAY_UPDATE_MS (20 мс). Сначала очищается область радара (ниже заголовка и выше нижней панели). Затем рисуются все активные объекты с цветом, зависящим от расстояния: красный (<40 см), жёлтый (40–80 см), зелёный (>80 см). После этого рисуется луч от центра до края радара под текущим углом.
void drawDynamicLayer(int angle, float lastDist) {
tft.fillRect(0, 30, DISPLAY_WIDTH, DISPLAY_HEIGHT - 70, COLOR_BG);
for (int i = 0; i < 30; i++) {
if (objects[i].active && objects[i].hits >= 2) {
float d = objects[i].distance * 2; // масштабирование: 1 см = 2 пикселя
float r = radians(objects[i].angle - 60);
int x = RADAR_CENTER_X + d * sin(r);
int y = RADAR_CENTER_Y - d * cos(r);
uint16_t c = (objects[i].distance < 40) ? COLOR_NEAR :
(objects[i].distance < 80) ? COLOR_MID : COLOR_FAR;
tft.fillCircle(x, y, 4, c);
}
}
// луч
float rad = radians(angle - 60);
int lx = RADAR_CENTER_X + RADAR_RADIUS * sin(rad);
int ly = RADAR_CENTER_Y - RADAR_RADIUS * cos(rad);
tft.drawLine(RADAR_CENTER_X, RADAR_CENTER_Y, lx, ly, COLOR_SWEEP);
// текстовая информация
tft.setTextColor(COLOR_SWEEP, COLOR_PANEL);
tft.setCursor(20, DISPLAY_HEIGHT - 30);
tft.print("A:");
tft.print(angle);
tft.print(" D:");
tft.print(lastDist, 1);
tft.print(" O:");
tft.print(objectCount);
}Обратите внимание на преобразование координат: угол отсчитывается от вертикали вниз (радар «смотрит» вниз), поэтому используется angle - 60, чтобы 0° соответствовал левому краю, а 120° — правому. Расстояние умножается на 2 для соответствия масштабу экрана.
Измерение расстояния HC‑SR04
Функция measureDistance() стандартная, но с увеличенным таймаутом (25000 мкс), чтобы измерять до 100 см. Если расстояние превышает MAX_DISTANCE или нет эха, возвращается 0.
Управление сервоприводом
Сервопривод подключается к выводу 14. Диапазон углов сканирования (30–150°) отображается на сектор 120° (от 0 до SCAN_ANGLE). Это позволяет использовать стандартные сервы с углом поворота 180°.
Запуск задач в setup()
В setup() инициализируются пины, сервопривод, дисплей, рисуется статический слой. Затем создаются две задачи с привязкой к конкретным ядрам с помощью xTaskCreatePinnedToCore().
xTaskCreatePinnedToCore(
servoTask,
"ServoTask",
4096,
NULL,
2,
NULL,
0 // ядро 0
);
xTaskCreatePinnedToCore(
mainTask,
"MainTask",
8192,
NULL,
1,
NULL,
1 // ядро 1
);Размер стека для mainTask увеличен до 8 КБ, так как в ней используются функции библиотеки TFT, требующие больше памяти.
Заключение
Представленный код демонстрирует эффективное использование двухъядерной архитектуры ESP32 для создания плавного и надёжного радиолокатора. Основные преимущества:
- Плавное сканирование — сервопривод управляется в отдельном ядре с фиксированной задержкой, без влияния измерений и отрисовки.
- Стабильность — атомарный доступ к разделяемой переменной исключает взаимоблокировки.
- Фильтрация ложных отражений — каждый объект требует минимум двух подтверждений перед отображением.
- Экономия ресурсов — статический слой рисуется один раз, динамический обновляется только там, где необходимо.
Этот проект легко дорабатывать: добавить звуковую индикацию, запись данных на SD‑карту или передачу по Wi‑Fi. Надеюсь, код послужит хорошей основой для ваших экспериментов с ESP32 и робототехникой.
// ОПТИМИЗИРОВАННЫЙ РАДАР ESP32
// Разделение на два ядра: управление сервоприводом (ядро 0) и логика + дисплей (ядро 1)
// Фильтр ложных отражений, статический слой, динамический слой
// Исправлена стабильность: убраны мьютексы (атомарный доступ), добавлены задержки
#include <TFT_eSPI.h>
#include <SPI.h>
#include <ESP32Servo.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
// ==================== НАСТРОЙКИ ====================
#define TRIG_PIN 12
#define ECHO_PIN 13
#define SERVO_PIN 14
#define MIN_DISTANCE 2
#define MAX_DISTANCE 100
#define SCAN_ANGLE 120
#define SERVO_MIN_ANGLE 30
#define SERVO_MAX_ANGLE 150
#define DISPLAY_WIDTH 240
#define DISPLAY_HEIGHT 280
#define RADAR_CENTER_X 120
#define RADAR_CENTER_Y 250
#define RADAR_RADIUS 200
#define SCAN_STEP 2
#define SERVO_UPDATE_MS 15
#define DISPLAY_UPDATE_MS 20
#define COLOR_BG 0x0000
#define COLOR_GRID 0x03E0
#define COLOR_SWEEP 0x07E0
#define COLOR_TEXT 0xFFFF
#define COLOR_NEAR 0xF800
#define COLOR_MID 0xFFE0
#define COLOR_FAR 0x07E0
#define COLOR_PANEL 0x3186
TFT_eSPI tft;
Servo radarServo;
// Разделяемая переменная – текущий угол (атомарна на ESP32)
volatile int currentAngle = 0;
int scanDir = 1; // используется только в серво‑задаче
// ==================== СТРУКТУРА ОБЪЕКТА ====================
struct RadarObject {
float distance;
int angle;
uint8_t hits; // фильтр ложных отражений
unsigned long lastSeen;
bool active;
};
RadarObject objects[30];
int objectCount = 0;
// ==================== СТАТИЧЕСКИЙ СЛОЙ ====================
void drawStaticLayer() {
tft.fillScreen(COLOR_BG);
tft.fillRect(0, 0, DISPLAY_WIDTH, 30, COLOR_PANEL);
tft.setTextColor(COLOR_TEXT);
tft.setTextSize(2);
tft.setCursor(50, 5);
tft.print("ESP32 RADAR");
tft.drawFastHLine(0, 30, DISPLAY_WIDTH, COLOR_SWEEP);
for (int r = 40; r <= RADAR_RADIUS; r += 40) {
tft.drawCircle(RADAR_CENTER_X, RADAR_CENTER_Y, r, COLOR_GRID);
}
for (int a = -60; a <= 60; a += 30) {
float rad = radians(a);
int x = RADAR_CENTER_X + RADAR_RADIUS * sin(rad);
int y = RADAR_CENTER_Y - RADAR_RADIUS * cos(rad);
tft.drawLine(RADAR_CENTER_X, RADAR_CENTER_Y, x, y, COLOR_GRID);
}
tft.fillRect(0, DISPLAY_HEIGHT - 40, DISPLAY_WIDTH, 40, COLOR_PANEL);
tft.drawFastHLine(0, DISPLAY_HEIGHT - 40, DISPLAY_WIDTH, COLOR_SWEEP);
}
// ==================== HC-SR04 ====================
float measureDistance() {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
long duration = pulseIn(ECHO_PIN, HIGH, 25000);
if (duration <= 0) return 0;
float d = duration * 0.034 / 2;
if (d > MAX_DISTANCE) return 0;
return d;
}
// ==================== ФИЛЬТР ОБЪЕКТОВ ====================
void updateObjects(float dist, int angle) {
unsigned long now = millis();
bool found = false;
for (int i = 0; i < 30; i++) {
if (objects[i].active && abs(objects[i].angle - angle) < 6) {
if (abs(objects[i].distance - dist) < 10) {
objects[i].distance = dist;
objects[i].hits++;
objects[i].lastSeen = now;
found = true;
break;
}
}
}
if (!found && dist >= MIN_DISTANCE) {
for (int i = 0; i < 30; i++) {
if (!objects[i].active) {
objects[i] = {dist, angle, 1, now, true};
objectCount++;
break;
}
}
}
for (int i = 0; i < 30; i++) {
if (objects[i].active && (now - objects[i].lastSeen > 1200)) {
objects[i].active = false;
if (objectCount > 0) objectCount--;
}
}
}
// ==================== ДИНАМИЧЕСКИЙ СЛОЙ ====================
void drawDynamicLayer(int angle, float lastDist) {
// Очистка области радара (ниже заголовка и выше панели)
tft.fillRect(0, 30, DISPLAY_WIDTH, DISPLAY_HEIGHT - 70, COLOR_BG);
// Объекты с достаточным количеством попаданий
for (int i = 0; i < 30; i++) {
if (objects[i].active && objects[i].hits >= 2) {
float d = objects[i].distance * 2;
float r = radians(objects[i].angle - 60);
int x = RADAR_CENTER_X + d * sin(r);
int y = RADAR_CENTER_Y - d * cos(r);
uint16_t c = (objects[i].distance < 40) ? COLOR_NEAR :
(objects[i].distance < 80) ? COLOR_MID : COLOR_FAR;
tft.fillCircle(x, y, 4, c);
}
}
// Луч
float rad = radians(angle - 60);
int lx = RADAR_CENTER_X + RADAR_RADIUS * sin(rad);
int ly = RADAR_CENTER_Y - RADAR_RADIUS * cos(rad);
tft.drawLine(RADAR_CENTER_X, RADAR_CENTER_Y, lx, ly, COLOR_SWEEP);
// Информация
tft.setTextColor(COLOR_SWEEP, COLOR_PANEL);
tft.setTextSize(2);
tft.setCursor(20, DISPLAY_HEIGHT - 30);
tft.print("A:");
tft.print(angle);
tft.print(" D:");
tft.print(lastDist, 1);
tft.print(" O:");
tft.print(objectCount);
}
// ==================== ЗАДАЧА УПРАВЛЕНИЯ СЕРВОПРИВОДОМ (ядро 0) ====================
void servoTask(void *pvParameters) {
int localAngle = 0;
int localDir = 1;
while (1) {
localAngle += localDir * SCAN_STEP;
if (localAngle >= SCAN_ANGLE) {
localAngle = SCAN_ANGLE;
localDir = -1;
}
if (localAngle <= 0) {
localAngle = 0;
localDir = 1;
}
// Атомарная запись угла (доступна другой задаче)
currentAngle = localAngle;
int servoAngle = map(localAngle, 0, SCAN_ANGLE, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
radarServo.write(servoAngle);
vTaskDelay(pdMS_TO_TICKS(SERVO_UPDATE_MS));
}
}
// ==================== ОСНОВНАЯ ЗАДАЧА (ядро 1) ====================
void mainTask(void *pvParameters) {
float lastDistance = 0;
TickType_t lastDisplay = xTaskGetTickCount();
while (1) {
// Читаем текущий угол (атомарно)
int angle = currentAngle;
float d = measureDistance();
if (d > 0) {
lastDistance = d;
updateObjects(d, angle);
}
if ((xTaskGetTickCount() - lastDisplay) >= pdMS_TO_TICKS(DISPLAY_UPDATE_MS)) {
drawDynamicLayer(angle, lastDistance);
lastDisplay = xTaskGetTickCount();
}
// Короткая задержка, чтобы отдать процессор и сбросить watchdog
vTaskDelay(pdMS_TO_TICKS(1));
}
}
// ==================== SETUP ====================
void setup() {
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
radarServo.attach(SERVO_PIN);
radarServo.write(map(0, 0, SCAN_ANGLE, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE));
tft.init();
tft.setRotation(0);
drawStaticLayer();
// Создаём задачи на разных ядрах
// Ядро 0 – сервопривод
xTaskCreatePinnedToCore(
servoTask,
"ServoTask",
4096, // стек (байт)
NULL,
2, // приоритет выше для плавности
NULL,
0
);
// Ядро 1 – логика, измерения, отображение
xTaskCreatePinnedToCore(
mainTask,
"MainTask",
8192, // стек (байт) – больше из‑за работы с дисплеем
NULL,
1,
NULL,
1
);
}
// ==================== LOOP ====================
// В Arduino loop ничего не делаем, всё в задачах
void loop() {
vTaskDelay(pdMS_TO_TICKS(1000));
}Об авторе
