Radick/main/radar_service.c

#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;
}