Implement native ESP-IDF radar firmware
This commit is contained in:
328
main/radar_service.c
Normal file
328
main/radar_service.c
Normal file
@@ -0,0 +1,328 @@
|
||||
#include "radar_service.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "driver/uart.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
|
||||
#include "radar_protocol.h"
|
||||
#include "radick_config.h"
|
||||
#include "target_tracker.h"
|
||||
|
||||
#define RADAR_RX_BUFFER_BYTES 4096
|
||||
#define RADAR_TX_BUFFER_BYTES 256
|
||||
#define RADAR_READ_CHUNK_BYTES 256
|
||||
#define RADAR_TASK_STACK_BYTES 4096
|
||||
#define RADAR_TASK_PRIORITY (tskIDLE_PRIORITY + 5)
|
||||
#define RADAR_BOOT_DELAY_MS 250U
|
||||
#define RADAR_COMMAND_RETRY_MS 5000U
|
||||
#define RADAR_COMMAND_MAX_ATTEMPTS 3U
|
||||
#define RADAR_MIN_DISTANCE_MM 100.0F
|
||||
#define RADAR_MAX_DISTANCE_MM 8000.0F
|
||||
|
||||
typedef struct {
|
||||
uint32_t id;
|
||||
uint64_t last_seen_ms;
|
||||
bool active;
|
||||
} track_seen_t;
|
||||
|
||||
static const char *TAG = "radar";
|
||||
static portMUX_TYPE s_snapshot_lock = portMUX_INITIALIZER_UNLOCKED;
|
||||
static rd03d_parser_t s_parser;
|
||||
static target_tracker_t s_tracker;
|
||||
static radar_snapshot_t s_published;
|
||||
static track_seen_t s_seen[RADAR_SERVICE_MAX_TARGETS];
|
||||
static uint64_t s_last_frame_ms;
|
||||
static bool s_has_frame;
|
||||
static bool s_started;
|
||||
|
||||
static uint64_t now_ms(void)
|
||||
{
|
||||
return (uint64_t)esp_timer_get_time() / 1000U;
|
||||
}
|
||||
|
||||
static uint32_t elapsed_ms_saturated(uint64_t now, uint64_t then)
|
||||
{
|
||||
if (now <= then) {
|
||||
return 0U;
|
||||
}
|
||||
const uint64_t elapsed = now - then;
|
||||
return elapsed > UINT32_MAX ? UINT32_MAX : (uint32_t)elapsed;
|
||||
}
|
||||
|
||||
static track_seen_t *find_seen(uint32_t id)
|
||||
{
|
||||
track_seen_t *empty = NULL;
|
||||
|
||||
for (size_t i = 0; i < RADAR_SERVICE_MAX_TARGETS; ++i) {
|
||||
if (s_seen[i].active && s_seen[i].id == id) {
|
||||
return &s_seen[i];
|
||||
}
|
||||
if (!s_seen[i].active && empty == NULL) {
|
||||
empty = &s_seen[i];
|
||||
}
|
||||
}
|
||||
return empty;
|
||||
}
|
||||
|
||||
static bool plausible_detection(const rd03d_target_t *target)
|
||||
{
|
||||
if (!target->valid || target->y_mm <= 0) {
|
||||
return false;
|
||||
}
|
||||
const float x = (float)target->x_mm;
|
||||
const float y = (float)target->y_mm;
|
||||
const float distance_squared = x * x + y * y;
|
||||
return distance_squared >=
|
||||
(RADAR_MIN_DISTANCE_MM * RADAR_MIN_DISTANCE_MM) &&
|
||||
distance_squared <=
|
||||
(RADAR_MAX_DISTANCE_MM * RADAR_MAX_DISTANCE_MM);
|
||||
}
|
||||
|
||||
static void publish_frame(const rd03d_frame_t *input, void *context)
|
||||
{
|
||||
(void)context;
|
||||
rd03d_frame_t frame = *input;
|
||||
target_snapshot_t tracked[RADAR_SERVICE_MAX_TARGETS];
|
||||
radar_snapshot_t next = {0};
|
||||
const uint64_t timestamp_ms = now_ms();
|
||||
|
||||
for (size_t i = 0; i < RD03D_MAX_TARGETS; ++i) {
|
||||
if (!plausible_detection(&frame.targets[i])) {
|
||||
memset(&frame.targets[i], 0, sizeof(frame.targets[i]));
|
||||
}
|
||||
}
|
||||
|
||||
target_tracker_update_frame(&s_tracker, &frame);
|
||||
const size_t tracked_count = target_tracker_snapshot(
|
||||
&s_tracker, tracked, RADAR_SERVICE_MAX_TARGETS);
|
||||
|
||||
/* Release disappeared IDs before allocating a slot to a new track. */
|
||||
for (size_t seen_index = 0;
|
||||
seen_index < RADAR_SERVICE_MAX_TARGETS;
|
||||
++seen_index) {
|
||||
if (!s_seen[seen_index].active) {
|
||||
continue;
|
||||
}
|
||||
bool retained = false;
|
||||
for (size_t track_index = 0; track_index < tracked_count; ++track_index) {
|
||||
if (tracked[track_index].id == s_seen[seen_index].id) {
|
||||
retained = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!retained) {
|
||||
memset(&s_seen[seen_index], 0, sizeof(s_seen[seen_index]));
|
||||
}
|
||||
}
|
||||
|
||||
next.sensor_online = true;
|
||||
next.frame_count = s_parser.frames_decoded;
|
||||
next.invalid_frame_count = s_parser.malformed_frames;
|
||||
|
||||
for (size_t i = 0; i < tracked_count && i < RADAR_SERVICE_MAX_TARGETS; ++i) {
|
||||
track_seen_t *seen = find_seen(tracked[i].id);
|
||||
if (seen == NULL) {
|
||||
continue;
|
||||
}
|
||||
if (!seen->active) {
|
||||
seen->active = true;
|
||||
seen->id = tracked[i].id;
|
||||
seen->last_seen_ms = timestamp_ms;
|
||||
}
|
||||
if (tracked[i].observed_this_frame) {
|
||||
seen->last_seen_ms = timestamp_ms;
|
||||
}
|
||||
|
||||
const uint32_t age_ms = elapsed_ms_saturated(timestamp_ms,
|
||||
seen->last_seen_ms);
|
||||
if (!tracked[i].visible || age_ms > RADICK_TARGET_EXPIRE_MS) {
|
||||
continue;
|
||||
}
|
||||
|
||||
radar_target_view_t *view = &next.targets[next.target_count++];
|
||||
view->valid = true;
|
||||
view->id = tracked[i].id;
|
||||
view->x_mm = tracked[i].x_mm;
|
||||
view->y_mm = tracked[i].y_mm;
|
||||
view->speed_cm_s = tracked[i].speed_cm_s;
|
||||
view->resolution_mm = tracked[i].resolution_mm;
|
||||
view->age_ms = age_ms;
|
||||
}
|
||||
|
||||
taskENTER_CRITICAL(&s_snapshot_lock);
|
||||
s_published = next;
|
||||
s_last_frame_ms = timestamp_ms;
|
||||
s_has_frame = true;
|
||||
taskEXIT_CRITICAL(&s_snapshot_lock);
|
||||
}
|
||||
|
||||
static void send_multi_target_command(void)
|
||||
{
|
||||
const int written = uart_write_bytes(RADICK_RADAR_UART_NUM,
|
||||
rd03d_cmd_multi,
|
||||
RD03D_CMD_MULTI_SIZE);
|
||||
if (written != (int)RD03D_CMD_MULTI_SIZE) {
|
||||
ESP_LOGW(TAG, "multi-target command short write: %d/%u",
|
||||
written, (unsigned)RD03D_CMD_MULTI_SIZE);
|
||||
}
|
||||
}
|
||||
|
||||
static void radar_task(void *context)
|
||||
{
|
||||
(void)context;
|
||||
uint8_t bytes[RADAR_READ_CHUNK_BYTES];
|
||||
unsigned int command_attempts = 0U;
|
||||
uint32_t observed_frame_count = 0U;
|
||||
uint64_t last_frame_activity_ms = 0U;
|
||||
bool ever_received_frame = false;
|
||||
bool outage_latched = false;
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(RADAR_BOOT_DELAY_MS));
|
||||
(void)uart_flush_input(RADICK_RADAR_UART_NUM);
|
||||
send_multi_target_command();
|
||||
++command_attempts;
|
||||
uint64_t next_retry_ms = now_ms() + RADAR_COMMAND_RETRY_MS;
|
||||
|
||||
for (;;) {
|
||||
const int count = uart_read_bytes(RADICK_RADAR_UART_NUM,
|
||||
bytes,
|
||||
sizeof(bytes),
|
||||
pdMS_TO_TICKS(20));
|
||||
if (count > 0) {
|
||||
(void)rd03d_parser_feed(&s_parser, bytes, (size_t)count,
|
||||
publish_frame, NULL);
|
||||
|
||||
taskENTER_CRITICAL(&s_snapshot_lock);
|
||||
s_published.invalid_frame_count = s_parser.malformed_frames;
|
||||
taskEXIT_CRITICAL(&s_snapshot_lock);
|
||||
}
|
||||
|
||||
const uint64_t timestamp_ms = now_ms();
|
||||
if (s_parser.frames_decoded != observed_frame_count) {
|
||||
observed_frame_count = s_parser.frames_decoded;
|
||||
last_frame_activity_ms = timestamp_ms;
|
||||
if (ever_received_frame && outage_latched) {
|
||||
/* A power-cycled module returns to its default target mode. */
|
||||
command_attempts = 0U;
|
||||
next_retry_ms = timestamp_ms;
|
||||
}
|
||||
ever_received_frame = true;
|
||||
outage_latched = false;
|
||||
} else if (ever_received_frame && !outage_latched &&
|
||||
timestamp_ms - last_frame_activity_ms >
|
||||
RADICK_SENSOR_OFFLINE_MS) {
|
||||
outage_latched = true;
|
||||
}
|
||||
|
||||
/* 0x90 is idempotent; bounded repeats also cover a default single-target stream. */
|
||||
if (command_attempts < RADAR_COMMAND_MAX_ATTEMPTS &&
|
||||
timestamp_ms >= next_retry_ms) {
|
||||
send_multi_target_command();
|
||||
++command_attempts;
|
||||
next_retry_ms = timestamp_ms + RADAR_COMMAND_RETRY_MS;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
esp_err_t radar_service_init(void)
|
||||
{
|
||||
if (s_started) {
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
const uart_config_t config = {
|
||||
.baud_rate = RADICK_RADAR_BAUD_RATE,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||
.rx_flow_ctrl_thresh = 0,
|
||||
.source_clk = UART_SCLK_DEFAULT,
|
||||
};
|
||||
|
||||
esp_err_t err = uart_driver_install(RADICK_RADAR_UART_NUM,
|
||||
RADAR_RX_BUFFER_BYTES,
|
||||
RADAR_TX_BUFFER_BYTES,
|
||||
0, NULL, 0);
|
||||
if (err != ESP_OK) {
|
||||
return err;
|
||||
}
|
||||
err = uart_param_config(RADICK_RADAR_UART_NUM, &config);
|
||||
if (err == ESP_OK) {
|
||||
err = uart_set_pin(RADICK_RADAR_UART_NUM,
|
||||
RADICK_RADAR_TX_GPIO,
|
||||
RADICK_RADAR_RX_GPIO,
|
||||
UART_PIN_NO_CHANGE,
|
||||
UART_PIN_NO_CHANGE);
|
||||
}
|
||||
if (err != ESP_OK) {
|
||||
(void)uart_driver_delete(RADICK_RADAR_UART_NUM);
|
||||
return err;
|
||||
}
|
||||
|
||||
rd03d_parser_init(&s_parser);
|
||||
target_tracker_init(&s_tracker, NULL);
|
||||
memset(&s_published, 0, sizeof(s_published));
|
||||
memset(s_seen, 0, sizeof(s_seen));
|
||||
|
||||
if (xTaskCreate(radar_task, "radar_uart", RADAR_TASK_STACK_BYTES,
|
||||
NULL, RADAR_TASK_PRIORITY, NULL) != pdPASS) {
|
||||
(void)uart_driver_delete(RADICK_RADAR_UART_NUM);
|
||||
return ESP_ERR_NO_MEM;
|
||||
}
|
||||
|
||||
s_started = true;
|
||||
ESP_LOGI(TAG, "RD-03D UART ready: RX GPIO%d, TX GPIO%d, %d baud",
|
||||
RADICK_RADAR_RX_GPIO, RADICK_RADAR_TX_GPIO,
|
||||
RADICK_RADAR_BAUD_RATE);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
void radar_service_get_snapshot(radar_snapshot_t *snapshot)
|
||||
{
|
||||
if (snapshot == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t last_frame_ms;
|
||||
bool has_frame;
|
||||
taskENTER_CRITICAL(&s_snapshot_lock);
|
||||
*snapshot = s_published;
|
||||
last_frame_ms = s_last_frame_ms;
|
||||
has_frame = s_has_frame;
|
||||
taskEXIT_CRITICAL(&s_snapshot_lock);
|
||||
|
||||
if (!has_frame) {
|
||||
snapshot->sensor_online = false;
|
||||
snapshot->last_frame_age_ms = UINT32_MAX;
|
||||
snapshot->target_count = 0U;
|
||||
memset(snapshot->targets, 0, sizeof(snapshot->targets));
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t since_frame_ms = elapsed_ms_saturated(now_ms(), last_frame_ms);
|
||||
snapshot->last_frame_age_ms = since_frame_ms;
|
||||
snapshot->sensor_online = since_frame_ms <= RADICK_SENSOR_OFFLINE_MS;
|
||||
|
||||
uint8_t retained = 0U;
|
||||
for (size_t i = 0; i < snapshot->target_count; ++i) {
|
||||
radar_target_view_t target = snapshot->targets[i];
|
||||
const uint64_t total_age = (uint64_t)target.age_ms + since_frame_ms;
|
||||
if (!snapshot->sensor_online || total_age > RADICK_TARGET_EXPIRE_MS) {
|
||||
continue;
|
||||
}
|
||||
target.age_ms = total_age > UINT32_MAX ? UINT32_MAX : (uint32_t)total_age;
|
||||
snapshot->targets[retained++] = target;
|
||||
}
|
||||
for (size_t i = retained; i < RADAR_SERVICE_MAX_TARGETS; ++i) {
|
||||
memset(&snapshot->targets[i], 0, sizeof(snapshot->targets[i]));
|
||||
}
|
||||
snapshot->target_count = retained;
|
||||
}
|
||||
Reference in New Issue
Block a user