Implement native ESP-IDF radar firmware
This commit is contained in:
15
main/CMakeLists.txt
Normal file
15
main/CMakeLists.txt
Normal file
@@ -0,0 +1,15 @@
|
||||
idf_component_register(
|
||||
SRCS
|
||||
"app_main.c"
|
||||
"app_settings.c"
|
||||
"audio_service.c"
|
||||
"board.c"
|
||||
"radar_protocol.c"
|
||||
"radar_service.c"
|
||||
"target_tracker.c"
|
||||
"ui.c"
|
||||
INCLUDE_DIRS "."
|
||||
REQUIRES driver esp_lcd esp_timer nvs_flash lvgl
|
||||
)
|
||||
|
||||
target_compile_options(${COMPONENT_LIB} PRIVATE -Wall -Wextra -Werror)
|
||||
172
main/app_main.c
Normal file
172
main/app_main.c
Normal file
@@ -0,0 +1,172 @@
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "esp_err.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "lvgl.h"
|
||||
|
||||
#include "app_settings.h"
|
||||
#include "audio_service.h"
|
||||
#include "board.h"
|
||||
#include "radar_service.h"
|
||||
#include "radick_config.h"
|
||||
#include "ui.h"
|
||||
|
||||
#define AUTO_DIM_BRIGHTNESS_PERCENT 12U
|
||||
#define UI_LOOP_DELAY_MS 5U
|
||||
|
||||
static const char *TAG = "radick";
|
||||
static uint8_t s_user_brightness = 78U;
|
||||
static bool s_audio_ready;
|
||||
static bool s_auto_dimmed;
|
||||
static uint64_t s_last_activity_ms;
|
||||
|
||||
static uint64_t app_now_ms(void)
|
||||
{
|
||||
return (uint64_t)esp_timer_get_time() / 1000U;
|
||||
}
|
||||
|
||||
static void on_sound_changed(bool enabled)
|
||||
{
|
||||
if (s_audio_ready) {
|
||||
audio_service_set_enabled(enabled);
|
||||
}
|
||||
const esp_err_t err = app_settings_set_sound(enabled);
|
||||
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
|
||||
ESP_LOGW(TAG, "could not persist sound setting: %s", esp_err_to_name(err));
|
||||
}
|
||||
}
|
||||
|
||||
static void on_brightness_changed(uint8_t percent, bool persist)
|
||||
{
|
||||
s_user_brightness = percent;
|
||||
s_auto_dimmed = false;
|
||||
s_last_activity_ms = app_now_ms();
|
||||
board_set_brightness(percent);
|
||||
|
||||
if (persist) {
|
||||
const esp_err_t err = app_settings_set_brightness(percent);
|
||||
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
|
||||
ESP_LOGW(TAG, "could not persist brightness: %s", esp_err_to_name(err));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void fade_in_backlight(uint8_t target_percent)
|
||||
{
|
||||
for (uint8_t brightness = 0U; brightness < target_percent;) {
|
||||
const uint8_t remaining = target_percent - brightness;
|
||||
brightness += remaining > 4U ? 4U : remaining;
|
||||
board_set_brightness(brightness);
|
||||
vTaskDelay(pdMS_TO_TICKS(6));
|
||||
}
|
||||
}
|
||||
|
||||
static float nearest_distance_m(const radar_snapshot_t *snapshot)
|
||||
{
|
||||
float nearest_mm = INFINITY;
|
||||
|
||||
for (size_t i = 0; i < RADAR_SERVICE_MAX_TARGETS; ++i) {
|
||||
if (!snapshot->targets[i].valid) {
|
||||
continue;
|
||||
}
|
||||
const float distance_mm = hypotf(snapshot->targets[i].x_mm,
|
||||
snapshot->targets[i].y_mm);
|
||||
if (distance_mm < nearest_mm) {
|
||||
nearest_mm = distance_mm;
|
||||
}
|
||||
}
|
||||
return isfinite(nearest_mm) ? nearest_mm / 1000.0F : NAN;
|
||||
}
|
||||
|
||||
static void update_auto_dim(bool target_present)
|
||||
{
|
||||
static uint32_t previous_touch_ms;
|
||||
const uint64_t current_ms = app_now_ms();
|
||||
const uint32_t touch_ms = board_last_touch_ms();
|
||||
|
||||
if (target_present || touch_ms != previous_touch_ms) {
|
||||
s_last_activity_ms = current_ms;
|
||||
previous_touch_ms = touch_ms;
|
||||
if (s_auto_dimmed) {
|
||||
board_set_brightness(s_user_brightness);
|
||||
s_auto_dimmed = false;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (!s_auto_dimmed &&
|
||||
current_ms - s_last_activity_ms >= RADICK_AUTO_DIM_MS) {
|
||||
const uint8_t dimmed = s_user_brightness < AUTO_DIM_BRIGHTNESS_PERCENT
|
||||
? s_user_brightness
|
||||
: AUTO_DIM_BRIGHTNESS_PERCENT;
|
||||
board_set_brightness(dimmed);
|
||||
s_auto_dimmed = true;
|
||||
}
|
||||
}
|
||||
|
||||
void app_main(void)
|
||||
{
|
||||
app_settings_t settings;
|
||||
const esp_err_t settings_err = app_settings_init(&settings);
|
||||
if (settings_err != ESP_OK) {
|
||||
ESP_LOGW(TAG, "using default settings: %s", esp_err_to_name(settings_err));
|
||||
}
|
||||
s_user_brightness = settings.brightness_percent;
|
||||
s_last_activity_ms = app_now_ms();
|
||||
|
||||
ESP_ERROR_CHECK(board_init());
|
||||
|
||||
const esp_err_t audio_err = audio_service_init();
|
||||
s_audio_ready = audio_err == ESP_OK;
|
||||
if (s_audio_ready) {
|
||||
audio_service_set_enabled(settings.sound_enabled);
|
||||
} else {
|
||||
ESP_LOGW(TAG, "audio unavailable: %s", esp_err_to_name(audio_err));
|
||||
}
|
||||
|
||||
ESP_ERROR_CHECK(radar_service_init());
|
||||
|
||||
const ui_config_t ui_config = {
|
||||
.sound_enabled = settings.sound_enabled,
|
||||
.touch_available = board_touch_available(),
|
||||
.brightness_percent = settings.brightness_percent,
|
||||
.sound_changed = on_sound_changed,
|
||||
.brightness_changed = on_brightness_changed,
|
||||
};
|
||||
ui_init(&ui_config);
|
||||
|
||||
radar_snapshot_t snapshot;
|
||||
radar_service_get_snapshot(&snapshot);
|
||||
ui_update(&snapshot);
|
||||
(void)lv_timer_handler();
|
||||
fade_in_backlight(s_user_brightness);
|
||||
|
||||
uint64_t next_ui_update_ms = 0U;
|
||||
for (;;) {
|
||||
const uint64_t current_ms = app_now_ms();
|
||||
if (current_ms >= next_ui_update_ms) {
|
||||
radar_service_get_snapshot(&snapshot);
|
||||
ui_update(&snapshot);
|
||||
|
||||
const float nearest_m = nearest_distance_m(&snapshot);
|
||||
if (s_audio_ready) {
|
||||
if (isfinite(nearest_m)) {
|
||||
audio_service_set_nearest_distance(nearest_m);
|
||||
} else {
|
||||
audio_service_clear_target();
|
||||
}
|
||||
}
|
||||
update_auto_dim(snapshot.target_count > 0U);
|
||||
next_ui_update_ms = current_ms + RADICK_UI_REFRESH_MS;
|
||||
}
|
||||
|
||||
(void)lv_timer_handler();
|
||||
vTaskDelay(pdMS_TO_TICKS(UI_LOOP_DELAY_MS));
|
||||
}
|
||||
}
|
||||
81
main/app_settings.c
Normal file
81
main/app_settings.c
Normal file
@@ -0,0 +1,81 @@
|
||||
#include "app_settings.h"
|
||||
|
||||
#include "esp_log.h"
|
||||
#include "nvs.h"
|
||||
#include "nvs_flash.h"
|
||||
|
||||
#define SETTINGS_NAMESPACE "radick"
|
||||
#define SETTINGS_KEY_BRIGHTNESS "brightness"
|
||||
#define SETTINGS_KEY_SOUND "sound"
|
||||
#define DEFAULT_BRIGHTNESS_PERCENT 78U
|
||||
#define DEFAULT_SOUND_ENABLED true
|
||||
|
||||
static const char *TAG = "settings";
|
||||
static nvs_handle_t s_nvs;
|
||||
static bool s_ready;
|
||||
|
||||
static esp_err_t init_nvs_partition(void)
|
||||
{
|
||||
esp_err_t err = nvs_flash_init();
|
||||
if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
|
||||
err = nvs_flash_erase();
|
||||
if (err != ESP_OK) {
|
||||
return err;
|
||||
}
|
||||
err = nvs_flash_init();
|
||||
}
|
||||
return err;
|
||||
}
|
||||
|
||||
esp_err_t app_settings_init(app_settings_t *settings)
|
||||
{
|
||||
if (settings == NULL) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
settings->brightness_percent = DEFAULT_BRIGHTNESS_PERCENT;
|
||||
settings->sound_enabled = DEFAULT_SOUND_ENABLED;
|
||||
|
||||
esp_err_t err = init_nvs_partition();
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGW(TAG, "NVS unavailable: %s", esp_err_to_name(err));
|
||||
return err;
|
||||
}
|
||||
|
||||
err = nvs_open(SETTINGS_NAMESPACE, NVS_READWRITE, &s_nvs);
|
||||
if (err != ESP_OK) {
|
||||
return err;
|
||||
}
|
||||
s_ready = true;
|
||||
|
||||
uint8_t value = 0;
|
||||
if (nvs_get_u8(s_nvs, SETTINGS_KEY_BRIGHTNESS, &value) == ESP_OK &&
|
||||
value >= 10U && value <= 100U) {
|
||||
settings->brightness_percent = value;
|
||||
}
|
||||
if (nvs_get_u8(s_nvs, SETTINGS_KEY_SOUND, &value) == ESP_OK) {
|
||||
settings->sound_enabled = value != 0U;
|
||||
}
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t app_settings_set_brightness(uint8_t percent)
|
||||
{
|
||||
if (percent < 10U || percent > 100U) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
if (!s_ready) {
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
esp_err_t err = nvs_set_u8(s_nvs, SETTINGS_KEY_BRIGHTNESS, percent);
|
||||
return err == ESP_OK ? nvs_commit(s_nvs) : err;
|
||||
}
|
||||
|
||||
esp_err_t app_settings_set_sound(bool enabled)
|
||||
{
|
||||
if (!s_ready) {
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
esp_err_t err = nvs_set_u8(s_nvs, SETTINGS_KEY_SOUND, enabled ? 1U : 0U);
|
||||
return err == ESP_OK ? nvs_commit(s_nvs) : err;
|
||||
}
|
||||
15
main/app_settings.h
Normal file
15
main/app_settings.h
Normal file
@@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "esp_err.h"
|
||||
|
||||
typedef struct {
|
||||
uint8_t brightness_percent;
|
||||
bool sound_enabled;
|
||||
} app_settings_t;
|
||||
|
||||
esp_err_t app_settings_init(app_settings_t *settings);
|
||||
esp_err_t app_settings_set_brightness(uint8_t percent);
|
||||
esp_err_t app_settings_set_sound(bool enabled);
|
||||
436
main/audio_service.c
Normal file
436
main/audio_service.c
Normal file
@@ -0,0 +1,436 @@
|
||||
#include "audio_service.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "driver/i2s_std.h"
|
||||
#include "esp_log.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
|
||||
/* CrowPanel 7.0-inch V3 onboard NS4168 connections. */
|
||||
#define AUDIO_I2S_PORT I2S_NUM_0
|
||||
#define AUDIO_PIN_BCLK 42
|
||||
#define AUDIO_PIN_LRCLK 18
|
||||
#define AUDIO_PIN_DOUT 17
|
||||
|
||||
#define AUDIO_SAMPLE_RATE_HZ 16000U
|
||||
#define AUDIO_DMA_BUFFER_COUNT 4
|
||||
#define AUDIO_DMA_FRAMES 128U
|
||||
#define AUDIO_CHANNEL_COUNT 2U
|
||||
|
||||
#define PING_DURATION_MS 70U
|
||||
#define PING_TOTAL_FRAMES ((AUDIO_SAMPLE_RATE_HZ * PING_DURATION_MS) / 1000U)
|
||||
#define PING_ATTACK_FRAMES ((AUDIO_SAMPLE_RATE_HZ * 4U) / 1000U)
|
||||
#define PING_RELEASE_FRAMES ((AUDIO_SAMPLE_RATE_HZ * 12U) / 1000U)
|
||||
#define PING_PEAK_AMPLITUDE 7000
|
||||
|
||||
#define DISTANCE_MIN_M 0.3f
|
||||
#define DISTANCE_MAX_M 8.0f
|
||||
#define PING_FREQUENCY_NEAR_HZ 1800U
|
||||
#define PING_FREQUENCY_FAR_HZ 700U
|
||||
#define PING_INTERVAL_NEAR_MS 120U
|
||||
#define PING_INTERVAL_FAR_MS 900U
|
||||
|
||||
#define SINE_TABLE_BITS 8U
|
||||
#define SINE_TABLE_SIZE (1U << SINE_TABLE_BITS)
|
||||
#define AUDIO_TASK_STACK_BYTES 3072U
|
||||
#define AUDIO_WRITE_TIMEOUT_MS 25U
|
||||
|
||||
typedef struct {
|
||||
bool enabled;
|
||||
bool has_target;
|
||||
float nearest_distance_m;
|
||||
} audio_state_snapshot_t;
|
||||
|
||||
static const char *TAG = "audio_service";
|
||||
static portMUX_TYPE s_state_lock = portMUX_INITIALIZER_UNLOCKED;
|
||||
static bool s_initialized;
|
||||
static bool s_initializing;
|
||||
static bool s_enabled = true;
|
||||
static bool s_has_target;
|
||||
static float s_nearest_distance_m;
|
||||
static TaskHandle_t s_audio_task;
|
||||
static i2s_chan_handle_t s_tx_channel;
|
||||
|
||||
/* Kept out of the worker stack: 512 B each for the LUT and stereo DMA chunk. */
|
||||
static int16_t s_sine_table[SINE_TABLE_SIZE];
|
||||
static int16_t s_pcm_chunk[AUDIO_DMA_FRAMES * AUDIO_CHANNEL_COUNT];
|
||||
|
||||
static void audio_task(void *context);
|
||||
|
||||
static void init_sine_table(void)
|
||||
{
|
||||
const float phase_scale = (2.0f * 3.14159265358979323846f) / (float)SINE_TABLE_SIZE;
|
||||
|
||||
for (uint32_t i = 0; i < SINE_TABLE_SIZE; ++i) {
|
||||
s_sine_table[i] = (int16_t)(sinf((float)i * phase_scale) * 32767.0f);
|
||||
}
|
||||
}
|
||||
|
||||
static audio_state_snapshot_t state_snapshot(void)
|
||||
{
|
||||
audio_state_snapshot_t snapshot;
|
||||
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
snapshot.enabled = s_enabled;
|
||||
snapshot.has_target = s_has_target;
|
||||
snapshot.nearest_distance_m = s_nearest_distance_m;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
return snapshot;
|
||||
}
|
||||
|
||||
static void notify_audio_task(TaskHandle_t task)
|
||||
{
|
||||
if (task != NULL) {
|
||||
xTaskNotifyGive(task);
|
||||
}
|
||||
}
|
||||
|
||||
static uint32_t map_frequency_hz(float distance_m)
|
||||
{
|
||||
const float position = (distance_m - DISTANCE_MIN_M) /
|
||||
(DISTANCE_MAX_M - DISTANCE_MIN_M);
|
||||
const float span = (float)(PING_FREQUENCY_NEAR_HZ - PING_FREQUENCY_FAR_HZ);
|
||||
|
||||
return (uint32_t)((float)PING_FREQUENCY_NEAR_HZ - (position * span) + 0.5f);
|
||||
}
|
||||
|
||||
static uint32_t map_interval_ms(float distance_m)
|
||||
{
|
||||
const float position = (distance_m - DISTANCE_MIN_M) /
|
||||
(DISTANCE_MAX_M - DISTANCE_MIN_M);
|
||||
const float span = (float)(PING_INTERVAL_FAR_MS - PING_INTERVAL_NEAR_MS);
|
||||
|
||||
return (uint32_t)((float)PING_INTERVAL_NEAR_MS + (position * span) + 0.5f);
|
||||
}
|
||||
|
||||
static uint32_t envelope_q15(uint32_t frame)
|
||||
{
|
||||
if (frame < PING_ATTACK_FRAMES) {
|
||||
return (frame * 32767U) / PING_ATTACK_FRAMES;
|
||||
}
|
||||
|
||||
const uint32_t frames_after = PING_TOTAL_FRAMES - frame - 1U;
|
||||
if (frames_after < PING_RELEASE_FRAMES) {
|
||||
return (frames_after * 32767U) / PING_RELEASE_FRAMES;
|
||||
}
|
||||
|
||||
return 32767U;
|
||||
}
|
||||
|
||||
static bool write_pcm(const int16_t *samples, size_t frame_count)
|
||||
{
|
||||
const uint8_t *source = (const uint8_t *)samples;
|
||||
const size_t total_bytes = frame_count * AUDIO_CHANNEL_COUNT * sizeof(int16_t);
|
||||
size_t offset = 0;
|
||||
|
||||
while (offset < total_bytes) {
|
||||
size_t bytes_written = 0;
|
||||
const esp_err_t err = i2s_channel_write(s_tx_channel,
|
||||
source + offset,
|
||||
total_bytes - offset,
|
||||
&bytes_written,
|
||||
AUDIO_WRITE_TIMEOUT_MS);
|
||||
if (err != ESP_OK || bytes_written == 0U) {
|
||||
ESP_LOGW(TAG, "I2S write failed: %s (%u/%u bytes)",
|
||||
esp_err_to_name(err),
|
||||
(unsigned)offset,
|
||||
(unsigned)total_bytes);
|
||||
return false;
|
||||
}
|
||||
offset += bytes_written;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool target_still_audible(void)
|
||||
{
|
||||
const audio_state_snapshot_t snapshot = state_snapshot();
|
||||
return snapshot.enabled && snapshot.has_target;
|
||||
}
|
||||
|
||||
static bool play_ping(uint32_t frequency_hz)
|
||||
{
|
||||
uint32_t phase = 0;
|
||||
const uint32_t phase_step = (uint32_t)(((uint64_t)frequency_hz << 32) /
|
||||
AUDIO_SAMPLE_RATE_HZ);
|
||||
|
||||
for (uint32_t first_frame = 0; first_frame < PING_TOTAL_FRAMES;
|
||||
first_frame += AUDIO_DMA_FRAMES) {
|
||||
if (!target_still_audible()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t frame_count = PING_TOTAL_FRAMES - first_frame;
|
||||
if (frame_count > AUDIO_DMA_FRAMES) {
|
||||
frame_count = AUDIO_DMA_FRAMES;
|
||||
}
|
||||
|
||||
for (uint32_t i = 0; i < frame_count; ++i) {
|
||||
const uint32_t frame = first_frame + i;
|
||||
const uint32_t envelope = envelope_q15(frame);
|
||||
const int32_t wave = s_sine_table[phase >> (32U - SINE_TABLE_BITS)];
|
||||
int32_t sample = (wave * PING_PEAK_AMPLITUDE) >> 15;
|
||||
sample = (sample * (int32_t)envelope) >> 15;
|
||||
phase += phase_step;
|
||||
|
||||
s_pcm_chunk[i * AUDIO_CHANNEL_COUNT] = (int16_t)sample;
|
||||
s_pcm_chunk[(i * AUDIO_CHANNEL_COUNT) + 1U] = (int16_t)sample;
|
||||
}
|
||||
|
||||
if (!write_pcm(s_pcm_chunk, frame_count)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* Queue one DMA ring of zero samples. Once these writes complete, every tone
|
||||
* buffer has reached the peripheral, so it is safe to clear and stop the DMA
|
||||
* without clipping the 70 ms envelope tail.
|
||||
*/
|
||||
static void drain_with_silence(void)
|
||||
{
|
||||
memset(s_pcm_chunk, 0, sizeof(s_pcm_chunk));
|
||||
for (uint32_t i = 0; i < AUDIO_DMA_BUFFER_COUNT; ++i) {
|
||||
if (!write_pcm(s_pcm_chunk, AUDIO_DMA_FRAMES)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void stop_output(bool *i2s_running, bool drain)
|
||||
{
|
||||
if (!*i2s_running) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (drain) {
|
||||
drain_with_silence();
|
||||
}
|
||||
(void)i2s_channel_disable(s_tx_channel);
|
||||
*i2s_running = false;
|
||||
}
|
||||
|
||||
static void audio_task(void *context)
|
||||
{
|
||||
(void)context;
|
||||
bool i2s_running = false;
|
||||
bool active_session = false;
|
||||
TickType_t last_ping_tick = 0;
|
||||
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
s_audio_task = xTaskGetCurrentTaskHandle();
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
for (;;) {
|
||||
const audio_state_snapshot_t snapshot = state_snapshot();
|
||||
|
||||
if (!snapshot.enabled || !snapshot.has_target) {
|
||||
stop_output(&i2s_running, false);
|
||||
active_session = false;
|
||||
(void)ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
|
||||
continue;
|
||||
}
|
||||
|
||||
const TickType_t now = xTaskGetTickCount();
|
||||
if (active_session) {
|
||||
const TickType_t interval_ticks =
|
||||
pdMS_TO_TICKS(map_interval_ms(snapshot.nearest_distance_m));
|
||||
const TickType_t elapsed_ticks = now - last_ping_tick;
|
||||
if (elapsed_ticks < interval_ticks) {
|
||||
(void)ulTaskNotifyTake(pdTRUE, interval_ticks - elapsed_ticks);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (!i2s_running) {
|
||||
if (i2s_channel_enable(s_tx_channel) != ESP_OK) {
|
||||
ESP_LOGE(TAG, "Could not start I2S output");
|
||||
(void)ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(250));
|
||||
continue;
|
||||
}
|
||||
i2s_running = true;
|
||||
}
|
||||
|
||||
last_ping_tick = xTaskGetTickCount();
|
||||
active_session = true;
|
||||
const bool completed = play_ping(map_frequency_hz(snapshot.nearest_distance_m));
|
||||
stop_output(&i2s_running, completed);
|
||||
}
|
||||
}
|
||||
|
||||
esp_err_t audio_service_init(void)
|
||||
{
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
if (s_initialized) {
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
return ESP_OK;
|
||||
}
|
||||
if (s_initializing) {
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
s_initializing = true;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
init_sine_table();
|
||||
|
||||
i2s_chan_config_t channel_config =
|
||||
I2S_CHANNEL_DEFAULT_CONFIG(AUDIO_I2S_PORT, I2S_ROLE_MASTER);
|
||||
channel_config.dma_desc_num = AUDIO_DMA_BUFFER_COUNT;
|
||||
channel_config.dma_frame_num = AUDIO_DMA_FRAMES;
|
||||
channel_config.auto_clear = true;
|
||||
|
||||
const i2s_std_config_t standard_config = {
|
||||
.clk_cfg = I2S_STD_CLK_DEFAULT_CONFIG(AUDIO_SAMPLE_RATE_HZ),
|
||||
.slot_cfg = I2S_STD_PHILIPS_SLOT_DEFAULT_CONFIG(
|
||||
I2S_DATA_BIT_WIDTH_16BIT, I2S_SLOT_MODE_STEREO),
|
||||
.gpio_cfg = {
|
||||
.mclk = I2S_GPIO_UNUSED,
|
||||
.bclk = AUDIO_PIN_BCLK,
|
||||
.ws = AUDIO_PIN_LRCLK,
|
||||
.dout = AUDIO_PIN_DOUT,
|
||||
.din = I2S_GPIO_UNUSED,
|
||||
.invert_flags = {
|
||||
.mclk_inv = false,
|
||||
.bclk_inv = false,
|
||||
.ws_inv = false,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
esp_err_t err = i2s_new_channel(&channel_config, &s_tx_channel, NULL);
|
||||
if (err == ESP_OK) {
|
||||
err = i2s_channel_init_std_mode(s_tx_channel, &standard_config);
|
||||
}
|
||||
|
||||
if (err != ESP_OK) {
|
||||
if (s_tx_channel != NULL) {
|
||||
(void)i2s_del_channel(s_tx_channel);
|
||||
s_tx_channel = NULL;
|
||||
}
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
s_initializing = false;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
ESP_LOGE(TAG, "NS4168 I2S initialisation failed: %s", esp_err_to_name(err));
|
||||
return err;
|
||||
}
|
||||
|
||||
const BaseType_t task_created = xTaskCreate(audio_task,
|
||||
"proximity_audio",
|
||||
AUDIO_TASK_STACK_BYTES,
|
||||
NULL,
|
||||
tskIDLE_PRIORITY + 4,
|
||||
NULL);
|
||||
if (task_created != pdPASS) {
|
||||
(void)i2s_del_channel(s_tx_channel);
|
||||
s_tx_channel = NULL;
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
s_initializing = false;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
return ESP_ERR_NO_MEM;
|
||||
}
|
||||
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
s_initialized = true;
|
||||
s_initializing = false;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
ESP_LOGI(TAG, "NS4168 ready: 16 kHz, stereo, BCLK=%d LRCLK=%d DOUT=%d",
|
||||
AUDIO_PIN_BCLK, AUDIO_PIN_LRCLK, AUDIO_PIN_DOUT);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
void audio_service_set_enabled(bool enabled)
|
||||
{
|
||||
TaskHandle_t task;
|
||||
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
s_enabled = enabled;
|
||||
task = s_audio_task;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
notify_audio_task(task);
|
||||
}
|
||||
|
||||
bool audio_service_is_enabled(void)
|
||||
{
|
||||
bool enabled;
|
||||
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
enabled = s_enabled;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
return enabled;
|
||||
}
|
||||
|
||||
void audio_service_set_nearest_distance(float distance_m)
|
||||
{
|
||||
if (!isfinite(distance_m) || distance_m <= 0.0f) {
|
||||
audio_service_clear_target();
|
||||
return;
|
||||
}
|
||||
|
||||
if (distance_m < DISTANCE_MIN_M) {
|
||||
distance_m = DISTANCE_MIN_M;
|
||||
} else if (distance_m > DISTANCE_MAX_M) {
|
||||
distance_m = DISTANCE_MAX_M;
|
||||
}
|
||||
|
||||
TaskHandle_t task;
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
s_nearest_distance_m = distance_m;
|
||||
s_has_target = true;
|
||||
task = s_audio_task;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
notify_audio_task(task);
|
||||
}
|
||||
|
||||
void audio_service_clear_target(void)
|
||||
{
|
||||
TaskHandle_t task;
|
||||
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
s_has_target = false;
|
||||
s_nearest_distance_m = 0.0f;
|
||||
task = s_audio_task;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
notify_audio_task(task);
|
||||
}
|
||||
|
||||
bool audio_service_has_target(void)
|
||||
{
|
||||
bool has_target;
|
||||
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
has_target = s_has_target;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
return has_target;
|
||||
}
|
||||
|
||||
bool audio_service_get_nearest_distance(float *distance_m)
|
||||
{
|
||||
bool has_target;
|
||||
float distance;
|
||||
|
||||
taskENTER_CRITICAL(&s_state_lock);
|
||||
has_target = s_has_target;
|
||||
distance = s_nearest_distance_m;
|
||||
taskEXIT_CRITICAL(&s_state_lock);
|
||||
|
||||
if (has_target && distance_m != NULL) {
|
||||
*distance_m = distance;
|
||||
}
|
||||
return has_target;
|
||||
}
|
||||
61
main/audio_service.h
Normal file
61
main/audio_service.h
Normal file
@@ -0,0 +1,61 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "esp_err.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Initialise the CrowPanel V3 NS4168 proximity-ping output.
|
||||
*
|
||||
* The service owns I2S0 and GPIOs 42 (BCLK), 18 (LRCLK), and 17 (DOUT).
|
||||
* It starts a small worker task; no audio is emitted until a target distance is
|
||||
* supplied. Calling this function more than once after a successful start is
|
||||
* harmless.
|
||||
*/
|
||||
esp_err_t audio_service_init(void);
|
||||
|
||||
/**
|
||||
* @brief Enable or mute proximity pings.
|
||||
*
|
||||
* This call only updates service state and wakes the audio worker. It never
|
||||
* waits for I2S/DMA and is safe to call from normal application tasks.
|
||||
*/
|
||||
void audio_service_set_enabled(bool enabled);
|
||||
|
||||
/** @return The current user-visible enabled/muted setting. */
|
||||
bool audio_service_is_enabled(void);
|
||||
|
||||
/**
|
||||
* @brief Publish the current nearest target distance in metres.
|
||||
*
|
||||
* Finite positive values are clamped to the supported 0.3--8.0 m audio range.
|
||||
* Passing a non-finite or non-positive value has the same effect as
|
||||
* audio_service_clear_target(). This call never waits for I2S/DMA.
|
||||
*/
|
||||
void audio_service_set_nearest_distance(float distance_m);
|
||||
|
||||
/**
|
||||
* @brief Indicate that the radar currently has no target.
|
||||
*
|
||||
* Any active ping is silenced by the audio worker at its next small DMA chunk.
|
||||
*/
|
||||
void audio_service_clear_target(void);
|
||||
|
||||
/** @return true if a valid nearest-target distance is currently published. */
|
||||
bool audio_service_has_target(void);
|
||||
|
||||
/**
|
||||
* @brief Read the published nearest-target distance.
|
||||
*
|
||||
* @param[out] distance_m Receives the clamped distance when non-NULL.
|
||||
* @return true when a target is present, false otherwise.
|
||||
*/
|
||||
bool audio_service_get_nearest_distance(float *distance_m);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
360
main/board.c
Normal file
360
main/board.c
Normal file
@@ -0,0 +1,360 @@
|
||||
#include "board.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "driver/i2c.h"
|
||||
#include "driver/ledc.h"
|
||||
#include "esp_check.h"
|
||||
#include "esp_heap_caps.h"
|
||||
#include "esp_lcd_panel_ops.h"
|
||||
#include "esp_lcd_panel_rgb.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "lvgl.h"
|
||||
|
||||
#include "radick_config.h"
|
||||
|
||||
#define BOARD_I2C_PORT I2C_NUM_0
|
||||
#define BOARD_I2C_FREQ_HZ 400000
|
||||
#define BOARD_I2C_TIMEOUT_MS 30
|
||||
#define GT911_READ_TIMEOUT_MS 5
|
||||
#define GT911_REG_PRODUCT_ID 0x8140
|
||||
#define GT911_REG_POINT_STATUS 0x814E
|
||||
#define GT911_REG_FIRST_POINT 0x814F
|
||||
#define LVGL_DRAW_LINES 48
|
||||
#define LVGL_TICK_MS 2
|
||||
|
||||
static const char *TAG = "board";
|
||||
|
||||
static esp_lcd_panel_handle_t s_panel;
|
||||
static lv_disp_draw_buf_t s_draw_buf;
|
||||
static lv_disp_drv_t s_display_driver;
|
||||
static lv_indev_drv_t s_input_driver;
|
||||
static lv_color_t *s_buffer_a;
|
||||
static lv_color_t *s_buffer_b;
|
||||
static esp_timer_handle_t s_lvgl_tick_timer;
|
||||
|
||||
static bool s_touch_available;
|
||||
static uint8_t s_touch_address;
|
||||
static bool s_touch_pressed;
|
||||
static uint16_t s_touch_x;
|
||||
static uint16_t s_touch_y;
|
||||
static int64_t s_touch_sample_us;
|
||||
static volatile uint32_t s_last_touch_ms;
|
||||
static uint8_t s_brightness;
|
||||
|
||||
static esp_err_t i2c_write(uint8_t address, const uint8_t *data, size_t length)
|
||||
{
|
||||
return i2c_master_write_to_device(BOARD_I2C_PORT, address, data, length,
|
||||
pdMS_TO_TICKS(BOARD_I2C_TIMEOUT_MS));
|
||||
}
|
||||
|
||||
static esp_err_t pca9557_write(uint8_t reg, uint8_t value)
|
||||
{
|
||||
const uint8_t payload[2] = {reg, value};
|
||||
return i2c_write(RADICK_PCA9557_ADDRESS, payload, sizeof(payload));
|
||||
}
|
||||
|
||||
static esp_err_t gt911_read(uint8_t address, uint16_t reg, uint8_t *data, size_t length)
|
||||
{
|
||||
const uint8_t command[2] = {(uint8_t)(reg >> 8), (uint8_t)reg};
|
||||
return i2c_master_write_read_device(BOARD_I2C_PORT, address,
|
||||
command, sizeof(command), data, length,
|
||||
pdMS_TO_TICKS(GT911_READ_TIMEOUT_MS));
|
||||
}
|
||||
|
||||
static esp_err_t gt911_write_byte(uint8_t address, uint16_t reg, uint8_t value)
|
||||
{
|
||||
const uint8_t payload[3] = {(uint8_t)(reg >> 8), (uint8_t)reg, value};
|
||||
return i2c_write(address, payload, sizeof(payload));
|
||||
}
|
||||
|
||||
static esp_err_t init_i2c(void)
|
||||
{
|
||||
const i2c_config_t config = {
|
||||
.mode = I2C_MODE_MASTER,
|
||||
.sda_io_num = RADICK_TOUCH_SDA_GPIO,
|
||||
.scl_io_num = RADICK_TOUCH_SCL_GPIO,
|
||||
.sda_pullup_en = GPIO_PULLUP_DISABLE,
|
||||
.scl_pullup_en = GPIO_PULLUP_DISABLE,
|
||||
.master.clk_speed = BOARD_I2C_FREQ_HZ,
|
||||
.clk_flags = 0,
|
||||
};
|
||||
|
||||
ESP_RETURN_ON_ERROR(i2c_param_config(BOARD_I2C_PORT, &config), TAG,
|
||||
"I2C configuration failed");
|
||||
return i2c_driver_install(BOARD_I2C_PORT, config.mode, 0, 0, 0);
|
||||
}
|
||||
|
||||
static esp_err_t reset_v3_touch(void)
|
||||
{
|
||||
// Vendor-required V3 sequence. IO0 is GT911 RESET and IO1 is INT.
|
||||
ESP_RETURN_ON_ERROR(pca9557_write(0x02, 0x00), TAG,
|
||||
"PCA9557 polarity reset failed");
|
||||
ESP_RETURN_ON_ERROR(pca9557_write(0x01, 0xFC), TAG,
|
||||
"PCA9557 output setup failed");
|
||||
ESP_RETURN_ON_ERROR(pca9557_write(0x03, 0xFC), TAG,
|
||||
"PCA9557 pin setup failed");
|
||||
vTaskDelay(pdMS_TO_TICKS(20));
|
||||
|
||||
ESP_RETURN_ON_ERROR(pca9557_write(0x01, 0xFD), TAG,
|
||||
"GT911 reset release failed");
|
||||
vTaskDelay(pdMS_TO_TICKS(100));
|
||||
|
||||
// Leave RESET driven high and release INT to an input.
|
||||
return pca9557_write(0x03, 0xFE);
|
||||
}
|
||||
|
||||
static bool find_gt911(void)
|
||||
{
|
||||
const uint8_t addresses[] = {RADICK_GT911_ADDRESS, RADICK_GT911_ALT_ADDRESS};
|
||||
uint8_t product_id[4];
|
||||
|
||||
for (size_t i = 0; i < sizeof(addresses); ++i) {
|
||||
if (gt911_read(addresses[i], GT911_REG_PRODUCT_ID,
|
||||
product_id, sizeof(product_id)) == ESP_OK) {
|
||||
s_touch_address = addresses[i];
|
||||
ESP_LOGI(TAG, "GT911 at 0x%02x, product %.4s", s_touch_address,
|
||||
(const char *)product_id);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool poll_touch(uint16_t *x, uint16_t *y)
|
||||
{
|
||||
if (!s_touch_available) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t status = 0;
|
||||
const int64_t now_us = esp_timer_get_time();
|
||||
if (gt911_read(s_touch_address, GT911_REG_POINT_STATUS, &status, 1) != ESP_OK) {
|
||||
if ((now_us - s_touch_sample_us) > 100000) {
|
||||
s_touch_pressed = false;
|
||||
}
|
||||
return s_touch_pressed;
|
||||
}
|
||||
|
||||
if ((status & 0x80U) != 0U) {
|
||||
const uint8_t count = status & 0x0FU;
|
||||
if (count > 0U && count <= 5U) {
|
||||
uint8_t point[8];
|
||||
if (gt911_read(s_touch_address, GT911_REG_FIRST_POINT,
|
||||
point, sizeof(point)) == ESP_OK) {
|
||||
// The vendor's ROTATION_NORMAL + map() path ultimately yields
|
||||
// these native landscape coordinates unchanged.
|
||||
s_touch_x = (uint16_t)point[1] | ((uint16_t)point[2] << 8);
|
||||
s_touch_y = (uint16_t)point[3] | ((uint16_t)point[4] << 8);
|
||||
if (s_touch_x >= RADICK_LCD_H_RES) {
|
||||
s_touch_x = RADICK_LCD_H_RES - 1;
|
||||
}
|
||||
if (s_touch_y >= RADICK_LCD_V_RES) {
|
||||
s_touch_y = RADICK_LCD_V_RES - 1;
|
||||
}
|
||||
s_touch_pressed = true;
|
||||
s_touch_sample_us = now_us;
|
||||
s_last_touch_ms = (uint32_t)(now_us / 1000);
|
||||
}
|
||||
} else {
|
||||
s_touch_pressed = false;
|
||||
s_touch_sample_us = now_us;
|
||||
}
|
||||
(void)gt911_write_byte(s_touch_address, GT911_REG_POINT_STATUS, 0);
|
||||
} else if ((now_us - s_touch_sample_us) > 100000) {
|
||||
s_touch_pressed = false;
|
||||
}
|
||||
|
||||
*x = s_touch_x;
|
||||
*y = s_touch_y;
|
||||
return s_touch_pressed;
|
||||
}
|
||||
|
||||
static void lvgl_touch_read(lv_indev_drv_t *driver, lv_indev_data_t *data)
|
||||
{
|
||||
(void)driver;
|
||||
uint16_t x = 0;
|
||||
uint16_t y = 0;
|
||||
const bool pressed = poll_touch(&x, &y);
|
||||
data->state = pressed ? LV_INDEV_STATE_PR : LV_INDEV_STATE_REL;
|
||||
if (pressed) {
|
||||
data->point.x = x;
|
||||
data->point.y = y;
|
||||
}
|
||||
}
|
||||
|
||||
static void lvgl_flush(lv_disp_drv_t *driver, const lv_area_t *area,
|
||||
lv_color_t *pixels)
|
||||
{
|
||||
(void)driver;
|
||||
const esp_err_t err = esp_lcd_panel_draw_bitmap(s_panel,
|
||||
area->x1, area->y1,
|
||||
area->x2 + 1, area->y2 + 1,
|
||||
pixels);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG, "LCD flush failed: %s", esp_err_to_name(err));
|
||||
}
|
||||
lv_disp_flush_ready(driver);
|
||||
}
|
||||
|
||||
static void lvgl_tick(void *context)
|
||||
{
|
||||
(void)context;
|
||||
lv_tick_inc(LVGL_TICK_MS);
|
||||
}
|
||||
|
||||
static esp_err_t init_backlight(void)
|
||||
{
|
||||
const ledc_timer_config_t timer = {
|
||||
.speed_mode = LEDC_LOW_SPEED_MODE,
|
||||
.duty_resolution = LEDC_TIMER_10_BIT,
|
||||
.timer_num = LEDC_TIMER_0,
|
||||
.freq_hz = 5000,
|
||||
.clk_cfg = LEDC_AUTO_CLK,
|
||||
};
|
||||
const ledc_channel_config_t channel = {
|
||||
.gpio_num = RADICK_LCD_BACKLIGHT_GPIO,
|
||||
.speed_mode = LEDC_LOW_SPEED_MODE,
|
||||
.channel = LEDC_CHANNEL_0,
|
||||
.intr_type = LEDC_INTR_DISABLE,
|
||||
.timer_sel = LEDC_TIMER_0,
|
||||
.duty = 0,
|
||||
.hpoint = 0,
|
||||
};
|
||||
|
||||
ESP_RETURN_ON_ERROR(ledc_timer_config(&timer), TAG,
|
||||
"backlight timer init failed");
|
||||
return ledc_channel_config(&channel);
|
||||
}
|
||||
|
||||
static esp_err_t init_rgb_panel(void)
|
||||
{
|
||||
const esp_lcd_rgb_panel_config_t panel_config = {
|
||||
.clk_src = LCD_CLK_SRC_DEFAULT,
|
||||
.timings = {
|
||||
.pclk_hz = 15000000,
|
||||
.h_res = RADICK_LCD_H_RES,
|
||||
.v_res = RADICK_LCD_V_RES,
|
||||
.hsync_pulse_width = 48,
|
||||
.hsync_back_porch = 40,
|
||||
.hsync_front_porch = 40,
|
||||
.vsync_pulse_width = 31,
|
||||
.vsync_back_porch = 13,
|
||||
.vsync_front_porch = 1,
|
||||
.flags = {
|
||||
.pclk_active_neg = true,
|
||||
},
|
||||
},
|
||||
.data_width = 16,
|
||||
.bits_per_pixel = 16,
|
||||
.num_fbs = 1,
|
||||
.bounce_buffer_size_px = RADICK_LCD_H_RES * 10,
|
||||
.dma_burst_size = 64,
|
||||
.hsync_gpio_num = RADICK_LCD_HSYNC_GPIO,
|
||||
.vsync_gpio_num = RADICK_LCD_VSYNC_GPIO,
|
||||
.de_gpio_num = RADICK_LCD_DE_GPIO,
|
||||
.pclk_gpio_num = RADICK_LCD_PCLK_GPIO,
|
||||
.disp_gpio_num = GPIO_NUM_NC,
|
||||
.data_gpio_nums = {
|
||||
GPIO_NUM_15, GPIO_NUM_7, GPIO_NUM_6, GPIO_NUM_5, GPIO_NUM_4,
|
||||
GPIO_NUM_9, GPIO_NUM_46, GPIO_NUM_3, GPIO_NUM_8, GPIO_NUM_16,
|
||||
GPIO_NUM_1, GPIO_NUM_14, GPIO_NUM_21, GPIO_NUM_47, GPIO_NUM_48,
|
||||
GPIO_NUM_45,
|
||||
},
|
||||
.flags = {
|
||||
.fb_in_psram = true,
|
||||
},
|
||||
};
|
||||
|
||||
ESP_RETURN_ON_ERROR(esp_lcd_new_rgb_panel(&panel_config, &s_panel), TAG,
|
||||
"RGB panel allocation failed");
|
||||
ESP_RETURN_ON_ERROR(esp_lcd_panel_reset(s_panel), TAG,
|
||||
"RGB panel reset failed");
|
||||
return esp_lcd_panel_init(s_panel);
|
||||
}
|
||||
|
||||
static esp_err_t init_lvgl(void)
|
||||
{
|
||||
lv_init();
|
||||
|
||||
const size_t pixels = RADICK_LCD_H_RES * LVGL_DRAW_LINES;
|
||||
const size_t bytes = pixels * sizeof(lv_color_t);
|
||||
s_buffer_a = heap_caps_malloc(bytes, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
|
||||
s_buffer_b = heap_caps_malloc(bytes, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
|
||||
if (s_buffer_a == NULL || s_buffer_b == NULL) {
|
||||
return ESP_ERR_NO_MEM;
|
||||
}
|
||||
|
||||
lv_disp_draw_buf_init(&s_draw_buf, s_buffer_a, s_buffer_b, pixels);
|
||||
lv_disp_drv_init(&s_display_driver);
|
||||
s_display_driver.hor_res = RADICK_LCD_H_RES;
|
||||
s_display_driver.ver_res = RADICK_LCD_V_RES;
|
||||
s_display_driver.flush_cb = lvgl_flush;
|
||||
s_display_driver.draw_buf = &s_draw_buf;
|
||||
lv_disp_drv_register(&s_display_driver);
|
||||
|
||||
lv_indev_drv_init(&s_input_driver);
|
||||
s_input_driver.type = LV_INDEV_TYPE_POINTER;
|
||||
s_input_driver.read_cb = lvgl_touch_read;
|
||||
lv_indev_drv_register(&s_input_driver);
|
||||
|
||||
const esp_timer_create_args_t timer_args = {
|
||||
.callback = lvgl_tick,
|
||||
.name = "lvgl_tick",
|
||||
};
|
||||
ESP_RETURN_ON_ERROR(esp_timer_create(&timer_args, &s_lvgl_tick_timer), TAG,
|
||||
"LVGL tick timer create failed");
|
||||
return esp_timer_start_periodic(s_lvgl_tick_timer, LVGL_TICK_MS * 1000);
|
||||
}
|
||||
|
||||
esp_err_t board_init(void)
|
||||
{
|
||||
ESP_RETURN_ON_ERROR(init_backlight(), TAG, "backlight init failed");
|
||||
ESP_RETURN_ON_ERROR(init_i2c(), TAG, "touch I2C init failed");
|
||||
|
||||
const esp_err_t reset_result = reset_v3_touch();
|
||||
if (reset_result == ESP_OK) {
|
||||
s_touch_available = find_gt911();
|
||||
} else {
|
||||
ESP_LOGW(TAG, "V3 touch reset failed: %s", esp_err_to_name(reset_result));
|
||||
}
|
||||
if (!s_touch_available) {
|
||||
ESP_LOGW(TAG, "GT911 not found; display will remain usable without touch");
|
||||
}
|
||||
|
||||
ESP_RETURN_ON_ERROR(init_rgb_panel(), TAG, "LCD init failed");
|
||||
ESP_RETURN_ON_ERROR(init_lvgl(), TAG, "LVGL init failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
void board_set_brightness(uint8_t percent)
|
||||
{
|
||||
if (percent > 100U) {
|
||||
percent = 100U;
|
||||
}
|
||||
const uint32_t max_duty = (1U << LEDC_TIMER_10_BIT) - 1U;
|
||||
const uint32_t duty = (max_duty * percent) / 100U;
|
||||
if (ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty) == ESP_OK) {
|
||||
(void)ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
|
||||
s_brightness = percent;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t board_get_brightness(void)
|
||||
{
|
||||
return s_brightness;
|
||||
}
|
||||
|
||||
bool board_touch_available(void)
|
||||
{
|
||||
return s_touch_available;
|
||||
}
|
||||
|
||||
uint32_t board_last_touch_ms(void)
|
||||
{
|
||||
return s_last_touch_ms;
|
||||
}
|
||||
16
main/board.h
Normal file
16
main/board.h
Normal file
@@ -0,0 +1,16 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "esp_err.h"
|
||||
|
||||
// Initializes the CrowPanel V3 RGB panel, backlight, I2C touch reset sequence,
|
||||
// GT911 polling, and LVGL display/input drivers.
|
||||
esp_err_t board_init(void);
|
||||
|
||||
void board_set_brightness(uint8_t percent);
|
||||
uint8_t board_get_brightness(void);
|
||||
|
||||
bool board_touch_available(void);
|
||||
uint32_t board_last_touch_ms(void);
|
||||
5
main/idf_component.yml
Normal file
5
main/idf_component.yml
Normal file
@@ -0,0 +1,5 @@
|
||||
dependencies:
|
||||
idf: ">=5.3,<5.4"
|
||||
lvgl/lvgl:
|
||||
version: "8.3.11"
|
||||
public: true
|
||||
198
main/radar_protocol.c
Normal file
198
main/radar_protocol.c
Normal file
@@ -0,0 +1,198 @@
|
||||
#include "radar_protocol.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
const uint8_t rd03d_frame_header[RD03D_FRAME_HEADER_SIZE] = {
|
||||
0xAAU, 0xFFU, 0x03U, 0x00U,
|
||||
};
|
||||
|
||||
const uint8_t rd03d_frame_tail[RD03D_FRAME_TAIL_SIZE] = {
|
||||
0x55U, 0xCCU,
|
||||
};
|
||||
|
||||
const uint8_t rd03d_cmd_enable[RD03D_CMD_ENABLE_SIZE] = {
|
||||
0xFDU, 0xFCU, 0xFBU, 0xFAU, 0x04U, 0x00U, 0xFFU,
|
||||
0x00U, 0x01U, 0x00U, 0x04U, 0x03U, 0x02U, 0x01U,
|
||||
};
|
||||
|
||||
const uint8_t rd03d_cmd_multi[RD03D_CMD_MULTI_SIZE] = {
|
||||
0xFDU, 0xFCU, 0xFBU, 0xFAU, 0x02U, 0x00U,
|
||||
0x90U, 0x00U, 0x04U, 0x03U, 0x02U, 0x01U,
|
||||
};
|
||||
|
||||
const uint8_t rd03d_cmd_end[RD03D_CMD_END_SIZE] = {
|
||||
0xFDU, 0xFCU, 0xFBU, 0xFAU, 0x02U, 0x00U,
|
||||
0xFEU, 0x00U, 0x04U, 0x03U, 0x02U, 0x01U,
|
||||
};
|
||||
|
||||
static uint16_t decode_u16_le(const uint8_t *bytes)
|
||||
{
|
||||
return (uint16_t)((uint16_t)bytes[0] |
|
||||
(uint16_t)((uint16_t)bytes[1] << 8U));
|
||||
}
|
||||
|
||||
int16_t rd03d_decode_sign_magnitude(uint8_t low, uint8_t high)
|
||||
{
|
||||
const uint16_t raw = (uint16_t)((uint16_t)low |
|
||||
(uint16_t)((uint16_t)high << 8U));
|
||||
const int16_t magnitude = (int16_t)(raw & UINT16_C(0x7FFF));
|
||||
|
||||
return (raw & UINT16_C(0x8000)) != 0U ? magnitude : (int16_t)-magnitude;
|
||||
}
|
||||
|
||||
bool rd03d_decode_frame(const uint8_t frame[RD03D_FRAME_SIZE], rd03d_frame_t *out)
|
||||
{
|
||||
size_t target_index;
|
||||
|
||||
if (frame == NULL || out == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (memcmp(frame, rd03d_frame_header, RD03D_FRAME_HEADER_SIZE) != 0 ||
|
||||
memcmp(frame + RD03D_FRAME_SIZE - RD03D_FRAME_TAIL_SIZE,
|
||||
rd03d_frame_tail,
|
||||
RD03D_FRAME_TAIL_SIZE) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
memset(out, 0, sizeof(*out));
|
||||
for (target_index = 0U; target_index < RD03D_MAX_TARGETS; ++target_index) {
|
||||
const uint8_t *payload = frame + RD03D_FRAME_HEADER_SIZE +
|
||||
target_index * RD03D_TARGET_DATA_SIZE;
|
||||
rd03d_target_t *target = &out->targets[target_index];
|
||||
uint8_t combined = 0U;
|
||||
size_t byte_index;
|
||||
|
||||
for (byte_index = 0U; byte_index < RD03D_TARGET_DATA_SIZE; ++byte_index) {
|
||||
combined |= payload[byte_index];
|
||||
}
|
||||
if (combined == 0U) {
|
||||
continue;
|
||||
}
|
||||
|
||||
target->x_mm = rd03d_decode_sign_magnitude(payload[0], payload[1]);
|
||||
target->y_mm = rd03d_decode_sign_magnitude(payload[2], payload[3]);
|
||||
target->speed_cm_s = rd03d_decode_sign_magnitude(payload[4], payload[5]);
|
||||
target->resolution_mm = decode_u16_le(payload + 6U);
|
||||
target->valid = true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void rd03d_parser_init(rd03d_parser_t *parser)
|
||||
{
|
||||
if (parser != NULL) {
|
||||
memset(parser, 0, sizeof(*parser));
|
||||
}
|
||||
}
|
||||
|
||||
static void parser_seek_header(rd03d_parser_t *parser, uint8_t byte)
|
||||
{
|
||||
if (byte == rd03d_frame_header[parser->header_matched]) {
|
||||
++parser->header_matched;
|
||||
if (parser->header_matched == RD03D_FRAME_HEADER_SIZE) {
|
||||
memcpy(parser->buffer, rd03d_frame_header, RD03D_FRAME_HEADER_SIZE);
|
||||
parser->buffered = RD03D_FRAME_HEADER_SIZE;
|
||||
parser->header_matched = 0U;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/* The header has only one non-empty proper prefix/suffix: a single AA. */
|
||||
parser->header_matched = byte == rd03d_frame_header[0] ? 1U : 0U;
|
||||
}
|
||||
|
||||
static void parser_resynchronise(rd03d_parser_t *parser)
|
||||
{
|
||||
size_t offset;
|
||||
size_t suffix_length;
|
||||
|
||||
/* Prefer a complete header already present inside the rejected window. */
|
||||
for (offset = 1U;
|
||||
offset + RD03D_FRAME_HEADER_SIZE <= RD03D_FRAME_SIZE;
|
||||
++offset) {
|
||||
if (memcmp(parser->buffer + offset,
|
||||
rd03d_frame_header,
|
||||
RD03D_FRAME_HEADER_SIZE) == 0) {
|
||||
parser->buffered = RD03D_FRAME_SIZE - offset;
|
||||
memmove(parser->buffer, parser->buffer + offset, parser->buffered);
|
||||
parser->header_matched = 0U;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* Otherwise preserve a partial header at the end for a split next frame. */
|
||||
parser->buffered = 0U;
|
||||
parser->header_matched = 0U;
|
||||
for (suffix_length = RD03D_FRAME_HEADER_SIZE - 1U;
|
||||
suffix_length > 0U;
|
||||
--suffix_length) {
|
||||
if (memcmp(parser->buffer + RD03D_FRAME_SIZE - suffix_length,
|
||||
rd03d_frame_header,
|
||||
suffix_length) == 0) {
|
||||
parser->header_matched = suffix_length;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool rd03d_parser_push(rd03d_parser_t *parser, uint8_t byte, rd03d_frame_t *out)
|
||||
{
|
||||
rd03d_frame_t decoded;
|
||||
|
||||
if (parser == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
++parser->bytes_received;
|
||||
if (parser->buffered == 0U) {
|
||||
parser_seek_header(parser, byte);
|
||||
return false;
|
||||
}
|
||||
|
||||
parser->buffer[parser->buffered++] = byte;
|
||||
if (parser->buffered < RD03D_FRAME_SIZE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (rd03d_decode_frame(parser->buffer, &decoded)) {
|
||||
parser->buffered = 0U;
|
||||
parser->header_matched = 0U;
|
||||
++parser->frames_decoded;
|
||||
if (out != NULL) {
|
||||
*out = decoded;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
++parser->malformed_frames;
|
||||
parser_resynchronise(parser);
|
||||
return false;
|
||||
}
|
||||
|
||||
size_t rd03d_parser_feed(rd03d_parser_t *parser,
|
||||
const uint8_t *data,
|
||||
size_t length,
|
||||
rd03d_frame_callback_t callback,
|
||||
void *context)
|
||||
{
|
||||
size_t index;
|
||||
size_t decoded_count = 0U;
|
||||
|
||||
if (parser == NULL || (data == NULL && length != 0U)) {
|
||||
return 0U;
|
||||
}
|
||||
|
||||
for (index = 0U; index < length; ++index) {
|
||||
rd03d_frame_t frame;
|
||||
|
||||
if (rd03d_parser_push(parser, data[index], &frame)) {
|
||||
++decoded_count;
|
||||
if (callback != NULL) {
|
||||
callback(&frame, context);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return decoded_count;
|
||||
}
|
||||
86
main/radar_protocol.h
Normal file
86
main/radar_protocol.h
Normal file
@@ -0,0 +1,86 @@
|
||||
#ifndef RADICK_RADAR_PROTOCOL_H
|
||||
#define RADICK_RADAR_PROTOCOL_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* RD-03D V1 multi-target UART protocol constants. */
|
||||
#define RD03D_UART_BAUD_RATE 256000U
|
||||
#define RD03D_MAX_TARGETS 3U
|
||||
#define RD03D_TARGET_DATA_SIZE 8U
|
||||
#define RD03D_FRAME_HEADER_SIZE 4U
|
||||
#define RD03D_FRAME_TAIL_SIZE 2U
|
||||
#define RD03D_FRAME_SIZE 30U
|
||||
|
||||
#define RD03D_CMD_ENABLE_SIZE 14U
|
||||
#define RD03D_CMD_MULTI_SIZE 12U
|
||||
#define RD03D_CMD_END_SIZE 12U
|
||||
|
||||
extern const uint8_t rd03d_frame_header[RD03D_FRAME_HEADER_SIZE];
|
||||
extern const uint8_t rd03d_frame_tail[RD03D_FRAME_TAIL_SIZE];
|
||||
extern const uint8_t rd03d_cmd_enable[RD03D_CMD_ENABLE_SIZE];
|
||||
extern const uint8_t rd03d_cmd_multi[RD03D_CMD_MULTI_SIZE];
|
||||
extern const uint8_t rd03d_cmd_end[RD03D_CMD_END_SIZE];
|
||||
|
||||
typedef struct {
|
||||
int16_t x_mm;
|
||||
int16_t y_mm;
|
||||
int16_t speed_cm_s;
|
||||
uint16_t resolution_mm;
|
||||
bool valid;
|
||||
} rd03d_target_t;
|
||||
|
||||
typedef struct {
|
||||
rd03d_target_t targets[RD03D_MAX_TARGETS];
|
||||
} rd03d_frame_t;
|
||||
|
||||
/*
|
||||
* The parser owns no dynamic memory and is safe to place in static storage or
|
||||
* on a task stack. Its fields are public so the same header works in ESP-IDF
|
||||
* and small host-side tests without an allocator or opaque platform handle.
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t buffer[RD03D_FRAME_SIZE];
|
||||
size_t buffered;
|
||||
size_t header_matched;
|
||||
uint64_t bytes_received;
|
||||
uint32_t frames_decoded;
|
||||
uint32_t malformed_frames;
|
||||
} rd03d_parser_t;
|
||||
|
||||
typedef void (*rd03d_frame_callback_t)(const rd03d_frame_t *frame, void *context);
|
||||
|
||||
/* Decode the RD-03D's unusual sign/magnitude value (bit 15 means positive). */
|
||||
int16_t rd03d_decode_sign_magnitude(uint8_t low, uint8_t high);
|
||||
|
||||
/* Decode one complete frame. Returns false unless both header and tail match. */
|
||||
bool rd03d_decode_frame(const uint8_t frame[RD03D_FRAME_SIZE], rd03d_frame_t *out);
|
||||
|
||||
void rd03d_parser_init(rd03d_parser_t *parser);
|
||||
|
||||
/*
|
||||
* Feed one byte. A complete decoded frame is copied to out (when non-NULL),
|
||||
* and true is returned. Noise and malformed frames are consumed internally.
|
||||
*/
|
||||
bool rd03d_parser_push(rd03d_parser_t *parser, uint8_t byte, rd03d_frame_t *out);
|
||||
|
||||
/*
|
||||
* Feed an arbitrary UART chunk. The callback is invoked once per frame when
|
||||
* non-NULL; the return value always reports the number of decoded frames.
|
||||
*/
|
||||
size_t rd03d_parser_feed(rd03d_parser_t *parser,
|
||||
const uint8_t *data,
|
||||
size_t length,
|
||||
rd03d_frame_callback_t callback,
|
||||
void *context);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* RADICK_RADAR_PROTOCOL_H */
|
||||
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;
|
||||
}
|
||||
30
main/radar_service.h
Normal file
30
main/radar_service.h
Normal file
@@ -0,0 +1,30 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "esp_err.h"
|
||||
|
||||
#define RADAR_SERVICE_MAX_TARGETS 3
|
||||
|
||||
typedef struct {
|
||||
bool valid;
|
||||
uint32_t id;
|
||||
float x_mm;
|
||||
float y_mm;
|
||||
float speed_cm_s;
|
||||
uint16_t resolution_mm;
|
||||
uint32_t age_ms;
|
||||
} radar_target_view_t;
|
||||
|
||||
typedef struct {
|
||||
bool sensor_online;
|
||||
uint8_t target_count;
|
||||
uint32_t frame_count;
|
||||
uint32_t invalid_frame_count;
|
||||
uint32_t last_frame_age_ms;
|
||||
radar_target_view_t targets[RADAR_SERVICE_MAX_TARGETS];
|
||||
} radar_snapshot_t;
|
||||
|
||||
esp_err_t radar_service_init(void);
|
||||
void radar_service_get_snapshot(radar_snapshot_t *snapshot);
|
||||
39
main/radick_config.h
Normal file
39
main/radick_config.h
Normal file
@@ -0,0 +1,39 @@
|
||||
#pragma once
|
||||
|
||||
#include "driver/gpio.h"
|
||||
|
||||
// This firmware intentionally targets the Basic CrowPanel V3.0. V1/V2 route
|
||||
// the touch interrupt through GPIO38 and are not pin-compatible with Radick.
|
||||
#define RADICK_LCD_H_RES 800
|
||||
#define RADICK_LCD_V_RES 480
|
||||
|
||||
#define RADICK_LCD_DE_GPIO GPIO_NUM_41
|
||||
#define RADICK_LCD_VSYNC_GPIO GPIO_NUM_40
|
||||
#define RADICK_LCD_HSYNC_GPIO GPIO_NUM_39
|
||||
#define RADICK_LCD_PCLK_GPIO GPIO_NUM_0
|
||||
#define RADICK_LCD_BACKLIGHT_GPIO GPIO_NUM_2
|
||||
|
||||
#define RADICK_TOUCH_SDA_GPIO GPIO_NUM_19
|
||||
#define RADICK_TOUCH_SCL_GPIO GPIO_NUM_20
|
||||
#define RADICK_PCA9557_ADDRESS 0x18
|
||||
#define RADICK_GT911_ADDRESS 0x5D
|
||||
#define RADICK_GT911_ALT_ADDRESS 0x14
|
||||
|
||||
// Safer split-connector wiring for V3:
|
||||
// RD-03D TX -> GPIO_D pin 1 (GPIO38)
|
||||
// RD-03D RX <- UART pin 2 (GPIO43)
|
||||
// This avoids connecting the radar TX output to GPIO44, where the onboard
|
||||
// CH340C TX output is already electrically connected.
|
||||
#define RADICK_RADAR_RX_GPIO GPIO_NUM_38
|
||||
#define RADICK_RADAR_TX_GPIO GPIO_NUM_43
|
||||
#define RADICK_RADAR_UART_NUM UART_NUM_1
|
||||
#define RADICK_RADAR_BAUD_RATE 256000
|
||||
|
||||
#define RADICK_AUDIO_DOUT_GPIO GPIO_NUM_17
|
||||
#define RADICK_AUDIO_LRCLK_GPIO GPIO_NUM_18
|
||||
#define RADICK_AUDIO_BCLK_GPIO GPIO_NUM_42
|
||||
|
||||
#define RADICK_UI_REFRESH_MS 50U
|
||||
#define RADICK_SENSOR_OFFLINE_MS 1500U
|
||||
#define RADICK_TARGET_EXPIRE_MS 650U
|
||||
#define RADICK_AUTO_DIM_MS 60000U
|
||||
387
main/target_tracker.c
Normal file
387
main/target_tracker.c
Normal file
@@ -0,0 +1,387 @@
|
||||
#include "target_tracker.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <string.h>
|
||||
|
||||
#define TRACKER_DEFAULT_POSITION_ALPHA 0.35F
|
||||
#define TRACKER_DEFAULT_SPEED_ALPHA 0.45F
|
||||
#define TRACKER_DEFAULT_MOTION_ALPHA 0.60F
|
||||
#define TRACKER_DEFAULT_ASSOCIATION_MM 1800.0F
|
||||
#define TRACKER_DEFAULT_MAX_MISSED_FRAMES 6U
|
||||
|
||||
typedef struct {
|
||||
const target_tracker_t *tracker;
|
||||
const rd03d_target_t *detections;
|
||||
size_t active_indices[TARGET_TRACKER_CAPACITY];
|
||||
size_t active_count;
|
||||
size_t detection_count;
|
||||
float gate_squared;
|
||||
int current_assignment[TARGET_TRACKER_CAPACITY];
|
||||
int best_assignment[TARGET_TRACKER_CAPACITY];
|
||||
int best_match_count;
|
||||
float best_cost;
|
||||
} association_search_t;
|
||||
|
||||
static bool alpha_is_valid(float alpha)
|
||||
{
|
||||
return alpha >= 0.0F && alpha <= 1.0F;
|
||||
}
|
||||
|
||||
void target_tracker_default_config(target_tracker_config_t *config)
|
||||
{
|
||||
if (config == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
config->position_ema_alpha = TRACKER_DEFAULT_POSITION_ALPHA;
|
||||
config->speed_ema_alpha = TRACKER_DEFAULT_SPEED_ALPHA;
|
||||
config->motion_ema_alpha = TRACKER_DEFAULT_MOTION_ALPHA;
|
||||
config->max_association_distance_mm = TRACKER_DEFAULT_ASSOCIATION_MM;
|
||||
config->max_missed_frames = TRACKER_DEFAULT_MAX_MISSED_FRAMES;
|
||||
}
|
||||
|
||||
static target_tracker_config_t sanitise_config(const target_tracker_config_t *input)
|
||||
{
|
||||
target_tracker_config_t result;
|
||||
|
||||
target_tracker_default_config(&result);
|
||||
if (input == NULL) {
|
||||
return result;
|
||||
}
|
||||
|
||||
if (alpha_is_valid(input->position_ema_alpha)) {
|
||||
result.position_ema_alpha = input->position_ema_alpha;
|
||||
}
|
||||
if (alpha_is_valid(input->speed_ema_alpha)) {
|
||||
result.speed_ema_alpha = input->speed_ema_alpha;
|
||||
}
|
||||
if (alpha_is_valid(input->motion_ema_alpha)) {
|
||||
result.motion_ema_alpha = input->motion_ema_alpha;
|
||||
}
|
||||
if (input->max_association_distance_mm > 0.0F) {
|
||||
result.max_association_distance_mm = input->max_association_distance_mm;
|
||||
}
|
||||
result.max_missed_frames = input->max_missed_frames;
|
||||
return result;
|
||||
}
|
||||
|
||||
void target_tracker_init(target_tracker_t *tracker,
|
||||
const target_tracker_config_t *config)
|
||||
{
|
||||
if (tracker == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
memset(tracker, 0, sizeof(*tracker));
|
||||
tracker->config = sanitise_config(config);
|
||||
tracker->next_id = 1U;
|
||||
}
|
||||
|
||||
void target_tracker_reset(target_tracker_t *tracker)
|
||||
{
|
||||
target_tracker_config_t config;
|
||||
|
||||
if (tracker == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
config = tracker->config;
|
||||
memset(tracker, 0, sizeof(*tracker));
|
||||
tracker->config = config;
|
||||
tracker->next_id = 1U;
|
||||
}
|
||||
|
||||
static float association_cost(const association_search_t *search,
|
||||
size_t active_offset,
|
||||
size_t detection_index)
|
||||
{
|
||||
const target_track_state_t *track =
|
||||
&search->tracker->tracks[search->active_indices[active_offset]];
|
||||
const rd03d_target_t *detection = &search->detections[detection_index];
|
||||
const float frames_ahead =
|
||||
(float)track->snapshot.last_seen_age_frames + 1.0F;
|
||||
const float predicted_x = track->last_observed_x_mm +
|
||||
track->velocity_x_mm_per_frame * frames_ahead;
|
||||
const float predicted_y = track->last_observed_y_mm +
|
||||
track->velocity_y_mm_per_frame * frames_ahead;
|
||||
const float dx = predicted_x - (float)detection->x_mm;
|
||||
const float dy = predicted_y - (float)detection->y_mm;
|
||||
|
||||
return dx * dx + dy * dy;
|
||||
}
|
||||
|
||||
static void search_associations(association_search_t *search,
|
||||
size_t active_offset,
|
||||
unsigned int used_detection_mask,
|
||||
int match_count,
|
||||
float total_cost)
|
||||
{
|
||||
size_t detection_index;
|
||||
|
||||
if (active_offset == search->active_count) {
|
||||
if (match_count > search->best_match_count ||
|
||||
(match_count == search->best_match_count &&
|
||||
total_cost < search->best_cost)) {
|
||||
search->best_match_count = match_count;
|
||||
search->best_cost = total_cost;
|
||||
memcpy(search->best_assignment,
|
||||
search->current_assignment,
|
||||
sizeof(search->best_assignment));
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
for (detection_index = 0U;
|
||||
detection_index < search->detection_count;
|
||||
++detection_index) {
|
||||
const unsigned int detection_bit = 1U << detection_index;
|
||||
float cost;
|
||||
|
||||
if ((used_detection_mask & detection_bit) != 0U) {
|
||||
continue;
|
||||
}
|
||||
cost = association_cost(search, active_offset, detection_index);
|
||||
if (cost > search->gate_squared) {
|
||||
continue;
|
||||
}
|
||||
|
||||
search->current_assignment[active_offset] = (int)detection_index;
|
||||
search_associations(search,
|
||||
active_offset + 1U,
|
||||
used_detection_mask | detection_bit,
|
||||
match_count + 1,
|
||||
total_cost + cost);
|
||||
}
|
||||
|
||||
search->current_assignment[active_offset] = -1;
|
||||
search_associations(search,
|
||||
active_offset + 1U,
|
||||
used_detection_mask,
|
||||
match_count,
|
||||
total_cost);
|
||||
}
|
||||
|
||||
static uint32_t allocate_id(target_tracker_t *tracker)
|
||||
{
|
||||
uint32_t id = tracker->next_id++;
|
||||
|
||||
if (id == 0U) {
|
||||
id = tracker->next_id++;
|
||||
}
|
||||
if (tracker->next_id == 0U) {
|
||||
tracker->next_id = 1U;
|
||||
}
|
||||
return id;
|
||||
}
|
||||
|
||||
static void initialise_track(target_tracker_t *tracker,
|
||||
target_track_state_t *track,
|
||||
const rd03d_target_t *detection)
|
||||
{
|
||||
memset(track, 0, sizeof(*track));
|
||||
track->active = true;
|
||||
track->last_observed_x_mm = (float)detection->x_mm;
|
||||
track->last_observed_y_mm = (float)detection->y_mm;
|
||||
track->snapshot.id = allocate_id(tracker);
|
||||
track->snapshot.x_mm = (float)detection->x_mm;
|
||||
track->snapshot.y_mm = (float)detection->y_mm;
|
||||
track->snapshot.speed_cm_s = (float)detection->speed_cm_s;
|
||||
track->snapshot.resolution_mm = detection->resolution_mm;
|
||||
track->snapshot.age_frames = 1U;
|
||||
track->snapshot.last_seen_age_frames = 0U;
|
||||
track->snapshot.visible = true;
|
||||
track->snapshot.observed_this_frame = true;
|
||||
}
|
||||
|
||||
static void update_matched_track(target_track_state_t *track,
|
||||
const rd03d_target_t *detection,
|
||||
const target_tracker_config_t *config)
|
||||
{
|
||||
const float gap = (float)track->snapshot.last_seen_age_frames + 1.0F;
|
||||
const float measured_velocity_x =
|
||||
((float)detection->x_mm - track->last_observed_x_mm) / gap;
|
||||
const float measured_velocity_y =
|
||||
((float)detection->y_mm - track->last_observed_y_mm) / gap;
|
||||
const float position_keep = 1.0F - config->position_ema_alpha;
|
||||
const float speed_keep = 1.0F - config->speed_ema_alpha;
|
||||
const float motion_keep = 1.0F - config->motion_ema_alpha;
|
||||
|
||||
track->velocity_x_mm_per_frame =
|
||||
config->motion_ema_alpha * measured_velocity_x +
|
||||
motion_keep * track->velocity_x_mm_per_frame;
|
||||
track->velocity_y_mm_per_frame =
|
||||
config->motion_ema_alpha * measured_velocity_y +
|
||||
motion_keep * track->velocity_y_mm_per_frame;
|
||||
track->last_observed_x_mm = (float)detection->x_mm;
|
||||
track->last_observed_y_mm = (float)detection->y_mm;
|
||||
|
||||
track->snapshot.x_mm = config->position_ema_alpha * (float)detection->x_mm +
|
||||
position_keep * track->snapshot.x_mm;
|
||||
track->snapshot.y_mm = config->position_ema_alpha * (float)detection->y_mm +
|
||||
position_keep * track->snapshot.y_mm;
|
||||
track->snapshot.speed_cm_s =
|
||||
config->speed_ema_alpha * (float)detection->speed_cm_s +
|
||||
speed_keep * track->snapshot.speed_cm_s;
|
||||
track->snapshot.resolution_mm = detection->resolution_mm;
|
||||
track->snapshot.last_seen_age_frames = 0U;
|
||||
track->snapshot.observed_this_frame = true;
|
||||
}
|
||||
|
||||
size_t target_tracker_update(target_tracker_t *tracker,
|
||||
const rd03d_target_t *detections,
|
||||
size_t detection_count)
|
||||
{
|
||||
rd03d_target_t valid_detections[TARGET_TRACKER_CAPACITY];
|
||||
bool detection_used[TARGET_TRACKER_CAPACITY] = {false, false, false};
|
||||
association_search_t search;
|
||||
size_t valid_count = 0U;
|
||||
size_t track_index;
|
||||
size_t active_offset;
|
||||
size_t detection_index;
|
||||
|
||||
if (tracker == NULL || (detections == NULL && detection_count != 0U)) {
|
||||
return 0U;
|
||||
}
|
||||
|
||||
for (detection_index = 0U;
|
||||
detection_index < detection_count && valid_count < TARGET_TRACKER_CAPACITY;
|
||||
++detection_index) {
|
||||
if (detections[detection_index].valid) {
|
||||
valid_detections[valid_count++] = detections[detection_index];
|
||||
}
|
||||
}
|
||||
|
||||
memset(&search, 0, sizeof(search));
|
||||
search.tracker = tracker;
|
||||
search.detections = valid_detections;
|
||||
search.detection_count = valid_count;
|
||||
search.gate_squared = tracker->config.max_association_distance_mm *
|
||||
tracker->config.max_association_distance_mm;
|
||||
search.best_match_count = -1;
|
||||
search.best_cost = FLT_MAX;
|
||||
|
||||
for (track_index = 0U; track_index < TARGET_TRACKER_CAPACITY; ++track_index) {
|
||||
target_track_state_t *track = &tracker->tracks[track_index];
|
||||
|
||||
if (!track->active) {
|
||||
continue;
|
||||
}
|
||||
track->snapshot.observed_this_frame = false;
|
||||
if (track->snapshot.age_frames != UINT32_MAX) {
|
||||
++track->snapshot.age_frames;
|
||||
}
|
||||
search.active_indices[search.active_count++] = track_index;
|
||||
}
|
||||
|
||||
search_associations(&search, 0U, 0U, 0, 0.0F);
|
||||
|
||||
for (active_offset = 0U;
|
||||
active_offset < search.active_count;
|
||||
++active_offset) {
|
||||
target_track_state_t *track =
|
||||
&tracker->tracks[search.active_indices[active_offset]];
|
||||
const int assigned_detection = search.best_assignment[active_offset];
|
||||
|
||||
if (assigned_detection >= 0) {
|
||||
detection_used[(size_t)assigned_detection] = true;
|
||||
update_matched_track(track,
|
||||
&valid_detections[(size_t)assigned_detection],
|
||||
&tracker->config);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (track->snapshot.last_seen_age_frames != UINT16_MAX) {
|
||||
++track->snapshot.last_seen_age_frames;
|
||||
}
|
||||
if (track->snapshot.last_seen_age_frames >
|
||||
tracker->config.max_missed_frames) {
|
||||
memset(track, 0, sizeof(*track));
|
||||
}
|
||||
}
|
||||
|
||||
for (detection_index = 0U; detection_index < valid_count; ++detection_index) {
|
||||
if (detection_used[detection_index]) {
|
||||
continue;
|
||||
}
|
||||
target_track_state_t *destination = NULL;
|
||||
for (track_index = 0U;
|
||||
track_index < TARGET_TRACKER_CAPACITY;
|
||||
++track_index) {
|
||||
if (!tracker->tracks[track_index].active) {
|
||||
destination = &tracker->tracks[track_index];
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (destination == NULL) {
|
||||
/* Current detections take precedence over retained missing tracks. */
|
||||
for (track_index = 0U;
|
||||
track_index < TARGET_TRACKER_CAPACITY;
|
||||
++track_index) {
|
||||
target_track_state_t *candidate = &tracker->tracks[track_index];
|
||||
if (candidate->snapshot.observed_this_frame) {
|
||||
continue;
|
||||
}
|
||||
if (destination == NULL ||
|
||||
candidate->snapshot.last_seen_age_frames >
|
||||
destination->snapshot.last_seen_age_frames) {
|
||||
destination = candidate;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (destination != NULL) {
|
||||
initialise_track(tracker, destination,
|
||||
&valid_detections[detection_index]);
|
||||
}
|
||||
}
|
||||
|
||||
return target_tracker_snapshot(tracker, NULL, 0U);
|
||||
}
|
||||
|
||||
size_t target_tracker_update_frame(target_tracker_t *tracker,
|
||||
const rd03d_frame_t *frame)
|
||||
{
|
||||
if (frame == NULL) {
|
||||
return target_tracker_update(tracker, NULL, 0U);
|
||||
}
|
||||
return target_tracker_update(tracker, frame->targets, RD03D_MAX_TARGETS);
|
||||
}
|
||||
|
||||
size_t target_tracker_snapshot(const target_tracker_t *tracker,
|
||||
target_snapshot_t *out,
|
||||
size_t capacity)
|
||||
{
|
||||
const target_snapshot_t *ordered[TARGET_TRACKER_CAPACITY];
|
||||
size_t count = 0U;
|
||||
size_t track_index;
|
||||
size_t index;
|
||||
|
||||
if (tracker == NULL) {
|
||||
return 0U;
|
||||
}
|
||||
|
||||
for (track_index = 0U; track_index < TARGET_TRACKER_CAPACITY; ++track_index) {
|
||||
if (tracker->tracks[track_index].active) {
|
||||
size_t insertion = count;
|
||||
|
||||
while (insertion > 0U &&
|
||||
ordered[insertion - 1U]->id >
|
||||
tracker->tracks[track_index].snapshot.id) {
|
||||
ordered[insertion] = ordered[insertion - 1U];
|
||||
--insertion;
|
||||
}
|
||||
ordered[insertion] = &tracker->tracks[track_index].snapshot;
|
||||
++count;
|
||||
}
|
||||
}
|
||||
|
||||
if (out == NULL) {
|
||||
return count;
|
||||
}
|
||||
if (capacity > count) {
|
||||
capacity = count;
|
||||
}
|
||||
for (index = 0U; index < capacity; ++index) {
|
||||
out[index] = *ordered[index];
|
||||
}
|
||||
return count;
|
||||
}
|
||||
85
main/target_tracker.h
Normal file
85
main/target_tracker.h
Normal file
@@ -0,0 +1,85 @@
|
||||
#ifndef RADICK_TARGET_TRACKER_H
|
||||
#define RADICK_TARGET_TRACKER_H
|
||||
|
||||
#include "radar_protocol.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define TARGET_TRACKER_CAPACITY RD03D_MAX_TARGETS
|
||||
|
||||
typedef struct {
|
||||
float position_ema_alpha;
|
||||
float speed_ema_alpha;
|
||||
float motion_ema_alpha;
|
||||
float max_association_distance_mm;
|
||||
uint16_t max_missed_frames;
|
||||
} target_tracker_config_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t id;
|
||||
float x_mm;
|
||||
float y_mm;
|
||||
float speed_cm_s;
|
||||
uint16_t resolution_mm;
|
||||
uint32_t age_frames;
|
||||
uint16_t last_seen_age_frames;
|
||||
bool visible;
|
||||
bool observed_this_frame;
|
||||
} target_snapshot_t;
|
||||
|
||||
/* Public state keeps the module allocator-free; consumers should use snapshot. */
|
||||
typedef struct {
|
||||
target_snapshot_t snapshot;
|
||||
float last_observed_x_mm;
|
||||
float last_observed_y_mm;
|
||||
float velocity_x_mm_per_frame;
|
||||
float velocity_y_mm_per_frame;
|
||||
bool active;
|
||||
} target_track_state_t;
|
||||
|
||||
typedef struct {
|
||||
target_tracker_config_t config;
|
||||
target_track_state_t tracks[TARGET_TRACKER_CAPACITY];
|
||||
uint32_t next_id;
|
||||
} target_tracker_t;
|
||||
|
||||
void target_tracker_default_config(target_tracker_config_t *config);
|
||||
|
||||
/* A NULL config selects conservative defaults suitable for the 10 Hz stream. */
|
||||
void target_tracker_init(target_tracker_t *tracker,
|
||||
const target_tracker_config_t *config);
|
||||
|
||||
/* Clear tracks and restart IDs at one while preserving the current config. */
|
||||
void target_tracker_reset(target_tracker_t *tracker);
|
||||
|
||||
/*
|
||||
* Advance the tracker by exactly one radar frame. Only detections with valid
|
||||
* set participate, and at most the first three valid detections are used.
|
||||
* Returns the number of tracks retained after the update.
|
||||
*/
|
||||
size_t target_tracker_update(target_tracker_t *tracker,
|
||||
const rd03d_target_t *detections,
|
||||
size_t detection_count);
|
||||
|
||||
size_t target_tracker_update_frame(target_tracker_t *tracker,
|
||||
const rd03d_frame_t *frame);
|
||||
|
||||
/*
|
||||
* Copy visible tracks ordered by stable ID. Passing NULL/zero capacity is a
|
||||
* valid way to query the number of visible tracks.
|
||||
*/
|
||||
size_t target_tracker_snapshot(const target_tracker_t *tracker,
|
||||
target_snapshot_t *out,
|
||||
size_t capacity);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* RADICK_TARGET_TRACKER_H */
|
||||
504
main/ui.c
Normal file
504
main/ui.c
Normal file
@@ -0,0 +1,504 @@
|
||||
#include "ui.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "lvgl.h"
|
||||
|
||||
#define MAIN_WIDTH 610
|
||||
#define SIDEBAR_WIDTH 190
|
||||
#define RADAR_TOP 52
|
||||
#define RADAR_HEIGHT 428
|
||||
#define RADAR_CENTER_X 300
|
||||
#define RADAR_CENTER_Y 420
|
||||
#define RADAR_RADIUS 340
|
||||
#define RADAR_MAX_MM 8000.0f
|
||||
#define DEGREES_TO_RADIANS 0.01745329251994329577f
|
||||
|
||||
static const uint32_t COLOR_BACKGROUND = 0x071015;
|
||||
static const uint32_t COLOR_PANEL = 0x0D191F;
|
||||
static const uint32_t COLOR_CARD = 0x13232A;
|
||||
static const uint32_t COLOR_GRID = 0x1F4145;
|
||||
static const uint32_t COLOR_GRID_MAJOR = 0x2C6665;
|
||||
static const uint32_t COLOR_ACCENT = 0x4BE1C1;
|
||||
static const uint32_t COLOR_TEXT = 0xEAF3F2;
|
||||
static const uint32_t COLOR_MUTED = 0x70858A;
|
||||
static const uint32_t COLOR_DANGER = 0xFF6B6B;
|
||||
static const uint32_t TARGET_COLORS[3] = {0xFF6B6B, 0xFFB454, 0x52C9F3};
|
||||
|
||||
static ui_config_t s_config;
|
||||
static radar_snapshot_t s_snapshot;
|
||||
static lv_obj_t *s_radar;
|
||||
static lv_obj_t *s_status_dot;
|
||||
static lv_obj_t *s_status_label;
|
||||
static lv_obj_t *s_frame_age_label;
|
||||
static lv_obj_t *s_nearest_value;
|
||||
static lv_obj_t *s_empty_label;
|
||||
static lv_obj_t *s_sound_button;
|
||||
static lv_obj_t *s_sound_label;
|
||||
static lv_obj_t *s_brightness_slider;
|
||||
static lv_obj_t *s_target_rows[3];
|
||||
static lv_obj_t *s_target_row_dots[3];
|
||||
static lv_obj_t *s_target_row_labels[3];
|
||||
static lv_obj_t *s_target_tags[3];
|
||||
static bool s_sound_enabled;
|
||||
|
||||
static lv_color_t color(uint32_t hex)
|
||||
{
|
||||
return lv_color_hex(hex);
|
||||
}
|
||||
|
||||
static void remove_default_style(lv_obj_t *object)
|
||||
{
|
||||
lv_obj_remove_style_all(object);
|
||||
lv_obj_clear_flag(object, LV_OBJ_FLAG_SCROLLABLE);
|
||||
}
|
||||
|
||||
static lv_obj_t *make_label(lv_obj_t *parent, const char *text,
|
||||
const lv_font_t *font, uint32_t text_color)
|
||||
{
|
||||
lv_obj_t *label = lv_label_create(parent);
|
||||
lv_label_set_text(label, text);
|
||||
lv_obj_set_style_text_font(label, font, 0);
|
||||
lv_obj_set_style_text_color(label, color(text_color), 0);
|
||||
return label;
|
||||
}
|
||||
|
||||
static float distance_to_radius(float distance_mm)
|
||||
{
|
||||
if (distance_mm <= 0.0f) {
|
||||
return 0.0f;
|
||||
}
|
||||
if (distance_mm <= 2000.0f) {
|
||||
return (distance_mm / 2000.0f) * 0.50f * RADAR_RADIUS;
|
||||
}
|
||||
if (distance_mm <= 4000.0f) {
|
||||
return (0.50f + ((distance_mm - 2000.0f) / 2000.0f) * 0.25f) * RADAR_RADIUS;
|
||||
}
|
||||
if (distance_mm <= 6000.0f) {
|
||||
return (0.75f + ((distance_mm - 4000.0f) / 2000.0f) * 0.15f) * RADAR_RADIUS;
|
||||
}
|
||||
if (distance_mm >= RADAR_MAX_MM) {
|
||||
return RADAR_RADIUS;
|
||||
}
|
||||
return (0.90f + ((distance_mm - 6000.0f) / 2000.0f) * 0.10f) * RADAR_RADIUS;
|
||||
}
|
||||
|
||||
static uint8_t target_color_index(uint32_t id)
|
||||
{
|
||||
return id == 0U ? 0U : (uint8_t)((id - 1U) % 3U);
|
||||
}
|
||||
|
||||
static bool target_to_point(const radar_target_view_t *target,
|
||||
lv_coord_t origin_x, lv_coord_t origin_y,
|
||||
lv_point_t *point)
|
||||
{
|
||||
if (!target->valid || target->y_mm <= 0.0f) {
|
||||
return false;
|
||||
}
|
||||
const float distance = sqrtf(target->x_mm * target->x_mm +
|
||||
target->y_mm * target->y_mm);
|
||||
if (distance < 100.0f || distance > RADAR_MAX_MM) {
|
||||
return false;
|
||||
}
|
||||
const float angle = atan2f(target->x_mm, target->y_mm);
|
||||
const float radius = distance_to_radius(distance);
|
||||
point->x = origin_x + RADAR_CENTER_X + (lv_coord_t)lroundf(sinf(angle) * radius);
|
||||
point->y = origin_y + RADAR_CENTER_Y - (lv_coord_t)lroundf(cosf(angle) * radius);
|
||||
return true;
|
||||
}
|
||||
|
||||
static void draw_line(lv_draw_ctx_t *ctx, lv_color_t line_color,
|
||||
lv_coord_t width, lv_point_t start, lv_point_t end)
|
||||
{
|
||||
lv_draw_line_dsc_t descriptor;
|
||||
lv_draw_line_dsc_init(&descriptor);
|
||||
descriptor.color = line_color;
|
||||
descriptor.width = width;
|
||||
descriptor.opa = LV_OPA_COVER;
|
||||
lv_draw_line(ctx, &descriptor, &start, &end);
|
||||
}
|
||||
|
||||
static void draw_radar_event(lv_event_t *event)
|
||||
{
|
||||
if (lv_event_get_code(event) != LV_EVENT_DRAW_MAIN) {
|
||||
return;
|
||||
}
|
||||
|
||||
lv_obj_t *object = lv_event_get_target(event);
|
||||
lv_draw_ctx_t *ctx = lv_event_get_draw_ctx(event);
|
||||
lv_area_t coordinates;
|
||||
lv_obj_get_coords(object, &coordinates);
|
||||
const lv_point_t center = {
|
||||
.x = coordinates.x1 + RADAR_CENTER_X,
|
||||
.y = coordinates.y1 + RADAR_CENTER_Y,
|
||||
};
|
||||
|
||||
const uint16_t distances[] = {2000, 4000, 6000, 8000};
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
lv_draw_arc_dsc_t arc;
|
||||
lv_draw_arc_dsc_init(&arc);
|
||||
arc.color = color(i == 3 ? COLOR_GRID_MAJOR : COLOR_GRID);
|
||||
arc.width = i == 3 ? 2 : 1;
|
||||
arc.opa = LV_OPA_COVER;
|
||||
lv_draw_arc(ctx, &arc, ¢er,
|
||||
(uint16_t)lroundf(distance_to_radius(distances[i])), 210, 330);
|
||||
}
|
||||
|
||||
const int16_t angles[] = {-60, -30, 0, 30, 60};
|
||||
for (size_t i = 0; i < 5; ++i) {
|
||||
const float radians = (float)angles[i] * DEGREES_TO_RADIANS;
|
||||
const lv_point_t edge = {
|
||||
.x = center.x + (lv_coord_t)lroundf(sinf(radians) * RADAR_RADIUS),
|
||||
.y = center.y - (lv_coord_t)lroundf(cosf(radians) * RADAR_RADIUS),
|
||||
};
|
||||
draw_line(ctx, color(angles[i] == 0 ? COLOR_GRID_MAJOR : COLOR_GRID),
|
||||
angles[i] == 0 ? 2 : 1, center, edge);
|
||||
}
|
||||
|
||||
lv_draw_rect_dsc_t sensor;
|
||||
lv_draw_rect_dsc_init(&sensor);
|
||||
sensor.radius = LV_RADIUS_CIRCLE;
|
||||
sensor.bg_color = color(s_snapshot.sensor_online ? COLOR_ACCENT : COLOR_MUTED);
|
||||
sensor.bg_opa = LV_OPA_COVER;
|
||||
const lv_area_t sensor_area = {
|
||||
.x1 = center.x - 6, .y1 = center.y - 6,
|
||||
.x2 = center.x + 6, .y2 = center.y + 6,
|
||||
};
|
||||
lv_draw_rect(ctx, &sensor, &sensor_area);
|
||||
|
||||
for (size_t i = 0; i < RADAR_SERVICE_MAX_TARGETS; ++i) {
|
||||
const radar_target_view_t *target = &s_snapshot.targets[i];
|
||||
lv_point_t point;
|
||||
if (!target_to_point(target, coordinates.x1, coordinates.y1, &point)) {
|
||||
continue;
|
||||
}
|
||||
const lv_color_t target_color = color(TARGET_COLORS[target_color_index(target->id)]);
|
||||
|
||||
lv_draw_rect_dsc_t halo;
|
||||
lv_draw_rect_dsc_init(&halo);
|
||||
halo.radius = LV_RADIUS_CIRCLE;
|
||||
halo.bg_opa = LV_OPA_TRANSP;
|
||||
halo.border_color = target_color;
|
||||
halo.border_opa = LV_OPA_COVER;
|
||||
halo.border_width = 2;
|
||||
const lv_area_t halo_area = {
|
||||
.x1 = point.x - 13, .y1 = point.y - 13,
|
||||
.x2 = point.x + 13, .y2 = point.y + 13,
|
||||
};
|
||||
lv_draw_rect(ctx, &halo, &halo_area);
|
||||
|
||||
lv_draw_rect_dsc_t dot;
|
||||
lv_draw_rect_dsc_init(&dot);
|
||||
dot.radius = LV_RADIUS_CIRCLE;
|
||||
dot.bg_color = target_color;
|
||||
dot.bg_opa = LV_OPA_COVER;
|
||||
const lv_area_t dot_area = {
|
||||
.x1 = point.x - 5, .y1 = point.y - 5,
|
||||
.x2 = point.x + 5, .y2 = point.y + 5,
|
||||
};
|
||||
lv_draw_rect(ctx, &dot, &dot_area);
|
||||
}
|
||||
}
|
||||
|
||||
static void update_sound_button(void)
|
||||
{
|
||||
lv_label_set_text(s_sound_label, s_sound_enabled ? "SOUND ON" : "SOUND OFF");
|
||||
lv_obj_set_style_bg_color(s_sound_button,
|
||||
color(s_sound_enabled ? COLOR_ACCENT : COLOR_CARD), 0);
|
||||
lv_obj_set_style_text_color(s_sound_label,
|
||||
color(s_sound_enabled ? COLOR_BACKGROUND : COLOR_MUTED), 0);
|
||||
}
|
||||
|
||||
static void sound_button_event(lv_event_t *event)
|
||||
{
|
||||
if (lv_event_get_code(event) != LV_EVENT_CLICKED) {
|
||||
return;
|
||||
}
|
||||
s_sound_enabled = !s_sound_enabled;
|
||||
update_sound_button();
|
||||
if (s_config.sound_changed != NULL) {
|
||||
s_config.sound_changed(s_sound_enabled);
|
||||
}
|
||||
}
|
||||
|
||||
static void brightness_event(lv_event_t *event)
|
||||
{
|
||||
const lv_event_code_t code = lv_event_get_code(event);
|
||||
if (code != LV_EVENT_VALUE_CHANGED && code != LV_EVENT_RELEASED) {
|
||||
return;
|
||||
}
|
||||
if (s_config.brightness_changed != NULL) {
|
||||
const uint8_t value = (uint8_t)lv_slider_get_value(s_brightness_slider);
|
||||
s_config.brightness_changed(value, code == LV_EVENT_RELEASED);
|
||||
}
|
||||
}
|
||||
|
||||
static void create_header(lv_obj_t *root)
|
||||
{
|
||||
lv_obj_t *title = make_label(root, "RADICK", &lv_font_montserrat_24, COLOR_TEXT);
|
||||
lv_obj_set_pos(title, 22, 13);
|
||||
|
||||
lv_obj_t *subtitle = make_label(root, "RD-03D / 24 GHz / 3 TARGET",
|
||||
&lv_font_montserrat_12, COLOR_MUTED);
|
||||
lv_obj_set_pos(subtitle, 140, 22);
|
||||
|
||||
lv_obj_t *line = lv_obj_create(root);
|
||||
remove_default_style(line);
|
||||
lv_obj_set_size(line, MAIN_WIDTH - 40, 1);
|
||||
lv_obj_set_pos(line, 20, 50);
|
||||
lv_obj_set_style_bg_color(line, color(COLOR_GRID), 0);
|
||||
lv_obj_set_style_bg_opa(line, LV_OPA_COVER, 0);
|
||||
}
|
||||
|
||||
static void create_radar(lv_obj_t *root)
|
||||
{
|
||||
s_radar = lv_obj_create(root);
|
||||
remove_default_style(s_radar);
|
||||
lv_obj_set_pos(s_radar, 0, RADAR_TOP);
|
||||
lv_obj_set_size(s_radar, MAIN_WIDTH, RADAR_HEIGHT);
|
||||
lv_obj_add_event_cb(s_radar, draw_radar_event, LV_EVENT_DRAW_MAIN, NULL);
|
||||
|
||||
const uint16_t distances[] = {2000, 4000, 6000, 8000};
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
char text[8];
|
||||
snprintf(text, sizeof(text), "%um", (unsigned)(distances[i] / 1000U));
|
||||
lv_obj_t *label = make_label(s_radar, text, &lv_font_montserrat_12,
|
||||
i == 3 ? COLOR_GRID_MAJOR : COLOR_MUTED);
|
||||
lv_obj_set_pos(label, RADAR_CENTER_X + 7,
|
||||
RADAR_CENTER_Y - (lv_coord_t)distance_to_radius(distances[i]) - 8);
|
||||
}
|
||||
|
||||
s_empty_label = make_label(s_radar, "WAITING FOR RADAR",
|
||||
&lv_font_montserrat_16, COLOR_MUTED);
|
||||
lv_obj_align(s_empty_label, LV_ALIGN_TOP_MID, 0, 32);
|
||||
|
||||
for (size_t i = 0; i < RADAR_SERVICE_MAX_TARGETS; ++i) {
|
||||
s_target_tags[i] = make_label(s_radar, "", &lv_font_montserrat_12, COLOR_TEXT);
|
||||
lv_obj_set_style_bg_color(s_target_tags[i], color(COLOR_BACKGROUND), 0);
|
||||
lv_obj_set_style_bg_opa(s_target_tags[i], LV_OPA_70, 0);
|
||||
lv_obj_set_style_pad_hor(s_target_tags[i], 5, 0);
|
||||
lv_obj_set_style_pad_ver(s_target_tags[i], 3, 0);
|
||||
lv_obj_set_style_radius(s_target_tags[i], 4, 0);
|
||||
lv_obj_add_flag(s_target_tags[i], LV_OBJ_FLAG_HIDDEN);
|
||||
}
|
||||
}
|
||||
|
||||
static lv_obj_t *create_card(lv_obj_t *parent, lv_coord_t y, lv_coord_t height)
|
||||
{
|
||||
lv_obj_t *card = lv_obj_create(parent);
|
||||
lv_obj_clear_flag(card, LV_OBJ_FLAG_SCROLLABLE);
|
||||
lv_obj_set_pos(card, 12, y);
|
||||
lv_obj_set_size(card, SIDEBAR_WIDTH - 24, height);
|
||||
lv_obj_set_style_bg_color(card, color(COLOR_CARD), 0);
|
||||
lv_obj_set_style_bg_opa(card, LV_OPA_COVER, 0);
|
||||
lv_obj_set_style_border_width(card, 0, 0);
|
||||
lv_obj_set_style_radius(card, 10, 0);
|
||||
lv_obj_set_style_pad_all(card, 0, 0);
|
||||
return card;
|
||||
}
|
||||
|
||||
static void create_sidebar(lv_obj_t *root)
|
||||
{
|
||||
lv_obj_t *sidebar = lv_obj_create(root);
|
||||
remove_default_style(sidebar);
|
||||
lv_obj_set_pos(sidebar, MAIN_WIDTH, 0);
|
||||
lv_obj_set_size(sidebar, SIDEBAR_WIDTH, 480);
|
||||
lv_obj_set_style_bg_color(sidebar, color(COLOR_PANEL), 0);
|
||||
lv_obj_set_style_bg_opa(sidebar, LV_OPA_COVER, 0);
|
||||
|
||||
s_status_dot = lv_obj_create(sidebar);
|
||||
remove_default_style(s_status_dot);
|
||||
lv_obj_set_size(s_status_dot, 9, 9);
|
||||
lv_obj_set_pos(s_status_dot, 14, 19);
|
||||
lv_obj_set_style_radius(s_status_dot, LV_RADIUS_CIRCLE, 0);
|
||||
lv_obj_set_style_bg_opa(s_status_dot, LV_OPA_COVER, 0);
|
||||
|
||||
s_status_label = make_label(sidebar, "RADAR OFFLINE",
|
||||
&lv_font_montserrat_14, COLOR_MUTED);
|
||||
lv_obj_set_pos(s_status_label, 31, 14);
|
||||
s_frame_age_label = make_label(sidebar, "NO DATA",
|
||||
&lv_font_montserrat_12, COLOR_MUTED);
|
||||
lv_obj_set_pos(s_frame_age_label, 31, 31);
|
||||
|
||||
lv_obj_t *nearest_card = create_card(sidebar, 58, 94);
|
||||
lv_obj_t *nearest_title = make_label(nearest_card, "NEAREST",
|
||||
&lv_font_montserrat_12, COLOR_MUTED);
|
||||
lv_obj_set_pos(nearest_title, 12, 10);
|
||||
s_nearest_value = make_label(nearest_card, "--.- m",
|
||||
&lv_font_montserrat_40, COLOR_TEXT);
|
||||
lv_obj_set_pos(s_nearest_value, 10, 33);
|
||||
|
||||
lv_obj_t *targets_title = make_label(sidebar, "TARGETS",
|
||||
&lv_font_montserrat_12, COLOR_MUTED);
|
||||
lv_obj_set_pos(targets_title, 14, 167);
|
||||
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
s_target_rows[i] = create_card(sidebar, 186 + (lv_coord_t)i * 53, 45);
|
||||
s_target_row_dots[i] = lv_obj_create(s_target_rows[i]);
|
||||
remove_default_style(s_target_row_dots[i]);
|
||||
lv_obj_set_size(s_target_row_dots[i], 8, 8);
|
||||
lv_obj_set_pos(s_target_row_dots[i], 11, 18);
|
||||
lv_obj_set_style_radius(s_target_row_dots[i], LV_RADIUS_CIRCLE, 0);
|
||||
lv_obj_set_style_bg_color(s_target_row_dots[i], color(TARGET_COLORS[i]), 0);
|
||||
lv_obj_set_style_bg_opa(s_target_row_dots[i], LV_OPA_COVER, 0);
|
||||
s_target_row_labels[i] = make_label(s_target_rows[i], "--",
|
||||
&lv_font_montserrat_12, COLOR_MUTED);
|
||||
lv_obj_set_pos(s_target_row_labels[i], 29, 8);
|
||||
}
|
||||
|
||||
s_sound_button = lv_btn_create(sidebar);
|
||||
lv_obj_set_pos(s_sound_button, 12, 351);
|
||||
lv_obj_set_size(s_sound_button, SIDEBAR_WIDTH - 24, 42);
|
||||
lv_obj_set_style_shadow_width(s_sound_button, 0, 0);
|
||||
lv_obj_set_style_border_width(s_sound_button, 0, 0);
|
||||
lv_obj_set_style_radius(s_sound_button, 9, 0);
|
||||
lv_obj_add_event_cb(s_sound_button, sound_button_event, LV_EVENT_CLICKED, NULL);
|
||||
s_sound_label = make_label(s_sound_button, "", &lv_font_montserrat_14, COLOR_TEXT);
|
||||
lv_obj_center(s_sound_label);
|
||||
update_sound_button();
|
||||
|
||||
lv_obj_t *brightness_title = make_label(sidebar, "BRIGHTNESS",
|
||||
&lv_font_montserrat_12, COLOR_MUTED);
|
||||
lv_obj_set_pos(brightness_title, 14, 411);
|
||||
s_brightness_slider = lv_slider_create(sidebar);
|
||||
lv_obj_set_pos(s_brightness_slider, 14, 442);
|
||||
lv_obj_set_size(s_brightness_slider, SIDEBAR_WIDTH - 28, 8);
|
||||
lv_slider_set_range(s_brightness_slider, 10, 100);
|
||||
lv_slider_set_value(s_brightness_slider, s_config.brightness_percent, LV_ANIM_OFF);
|
||||
lv_obj_set_style_bg_color(s_brightness_slider, color(COLOR_CARD), LV_PART_MAIN);
|
||||
lv_obj_set_style_bg_color(s_brightness_slider, color(COLOR_ACCENT), LV_PART_INDICATOR);
|
||||
lv_obj_set_style_bg_color(s_brightness_slider, color(COLOR_TEXT), LV_PART_KNOB);
|
||||
lv_obj_set_style_pad_all(s_brightness_slider, 6, LV_PART_KNOB);
|
||||
lv_obj_set_ext_click_area(s_brightness_slider, 12);
|
||||
lv_obj_add_event_cb(s_brightness_slider, brightness_event, LV_EVENT_VALUE_CHANGED, NULL);
|
||||
lv_obj_add_event_cb(s_brightness_slider, brightness_event, LV_EVENT_RELEASED, NULL);
|
||||
|
||||
if (!s_config.touch_available) {
|
||||
lv_obj_t *touch_warning = make_label(sidebar, "TOUCH OFFLINE",
|
||||
&lv_font_montserrat_12, COLOR_DANGER);
|
||||
lv_obj_align(touch_warning, LV_ALIGN_BOTTOM_MID, 0, -5);
|
||||
}
|
||||
}
|
||||
|
||||
void ui_init(const ui_config_t *config)
|
||||
{
|
||||
memset(&s_snapshot, 0, sizeof(s_snapshot));
|
||||
if (config != NULL) {
|
||||
s_config = *config;
|
||||
} else {
|
||||
memset(&s_config, 0, sizeof(s_config));
|
||||
s_config.brightness_percent = 75;
|
||||
}
|
||||
s_sound_enabled = s_config.sound_enabled;
|
||||
|
||||
lv_obj_t *root = lv_scr_act();
|
||||
lv_obj_clear_flag(root, LV_OBJ_FLAG_SCROLLABLE);
|
||||
lv_obj_set_style_bg_color(root, color(COLOR_BACKGROUND), 0);
|
||||
lv_obj_set_style_bg_opa(root, LV_OPA_COVER, 0);
|
||||
lv_obj_set_style_pad_all(root, 0, 0);
|
||||
|
||||
create_header(root);
|
||||
create_radar(root);
|
||||
create_sidebar(root);
|
||||
}
|
||||
|
||||
void ui_update(const radar_snapshot_t *snapshot)
|
||||
{
|
||||
if (snapshot == NULL || s_radar == NULL) {
|
||||
return;
|
||||
}
|
||||
const bool online = snapshot->sensor_online;
|
||||
const bool radar_changed =
|
||||
snapshot->frame_count != s_snapshot.frame_count ||
|
||||
snapshot->target_count != s_snapshot.target_count ||
|
||||
online != s_snapshot.sensor_online;
|
||||
s_snapshot = *snapshot;
|
||||
lv_obj_set_style_bg_color(s_status_dot,
|
||||
color(online ? COLOR_ACCENT : COLOR_DANGER), 0);
|
||||
lv_label_set_text(s_status_label, online ? "RADAR ONLINE" : "RADAR OFFLINE");
|
||||
lv_obj_set_style_text_color(s_status_label,
|
||||
color(online ? COLOR_TEXT : COLOR_DANGER), 0);
|
||||
|
||||
if (online) {
|
||||
char age_text[24];
|
||||
snprintf(age_text, sizeof(age_text), "%lu FR / %lums",
|
||||
(unsigned long)snapshot->frame_count,
|
||||
(unsigned long)snapshot->last_frame_age_ms);
|
||||
lv_label_set_text(s_frame_age_label, age_text);
|
||||
} else {
|
||||
lv_label_set_text(s_frame_age_label, "NO SENSOR DATA");
|
||||
}
|
||||
|
||||
float nearest = INFINITY;
|
||||
for (size_t i = 0; i < RADAR_SERVICE_MAX_TARGETS; ++i) {
|
||||
const radar_target_view_t *target = &snapshot->targets[i];
|
||||
if (target->valid) {
|
||||
const float distance = sqrtf(target->x_mm * target->x_mm +
|
||||
target->y_mm * target->y_mm);
|
||||
if (distance < nearest) {
|
||||
nearest = distance;
|
||||
}
|
||||
|
||||
char row_text[44];
|
||||
snprintf(row_text, sizeof(row_text), "T%lu %.2fm\n%+.0f cm/s",
|
||||
(unsigned long)target->id, distance / 1000.0f,
|
||||
target->speed_cm_s);
|
||||
lv_label_set_text(s_target_row_labels[i], row_text);
|
||||
const uint8_t color_index = target_color_index(target->id);
|
||||
lv_obj_set_style_text_color(s_target_row_labels[i], color(COLOR_TEXT), 0);
|
||||
lv_obj_set_style_bg_color(s_target_row_dots[i],
|
||||
color(TARGET_COLORS[color_index]), 0);
|
||||
lv_obj_clear_flag(s_target_row_dots[i], LV_OBJ_FLAG_HIDDEN);
|
||||
|
||||
lv_point_t point;
|
||||
if (target_to_point(target, 0, 0, &point)) {
|
||||
char tag[24];
|
||||
snprintf(tag, sizeof(tag), "T%lu %.1fm",
|
||||
(unsigned long)target->id, distance / 1000.0f);
|
||||
lv_label_set_text(s_target_tags[i], tag);
|
||||
lv_obj_set_style_text_color(s_target_tags[i],
|
||||
color(TARGET_COLORS[color_index]), 0);
|
||||
lv_coord_t tag_x = point.x + 15;
|
||||
if (tag_x > MAIN_WIDTH - 82) {
|
||||
tag_x = point.x - 78;
|
||||
}
|
||||
lv_coord_t tag_y = point.y - 12;
|
||||
if (tag_y < 0) {
|
||||
tag_y = 0;
|
||||
}
|
||||
lv_obj_set_pos(s_target_tags[i], tag_x, tag_y);
|
||||
lv_obj_clear_flag(s_target_tags[i], LV_OBJ_FLAG_HIDDEN);
|
||||
} else {
|
||||
lv_obj_add_flag(s_target_tags[i], LV_OBJ_FLAG_HIDDEN);
|
||||
}
|
||||
} else {
|
||||
lv_label_set_text(s_target_row_labels[i], "--");
|
||||
lv_obj_set_style_text_color(s_target_row_labels[i], color(COLOR_MUTED), 0);
|
||||
lv_obj_add_flag(s_target_row_dots[i], LV_OBJ_FLAG_HIDDEN);
|
||||
lv_obj_add_flag(s_target_tags[i], LV_OBJ_FLAG_HIDDEN);
|
||||
}
|
||||
}
|
||||
|
||||
if (isfinite(nearest)) {
|
||||
char nearest_text[16];
|
||||
snprintf(nearest_text, sizeof(nearest_text), "%.1f m", nearest / 1000.0f);
|
||||
lv_label_set_text(s_nearest_value, nearest_text);
|
||||
} else {
|
||||
lv_label_set_text(s_nearest_value, "--.- m");
|
||||
}
|
||||
|
||||
if (!online) {
|
||||
lv_label_set_text(s_empty_label, "WAITING FOR RADAR");
|
||||
lv_obj_clear_flag(s_empty_label, LV_OBJ_FLAG_HIDDEN);
|
||||
} else if (snapshot->target_count == 0U) {
|
||||
lv_label_set_text(s_empty_label, "AREA CLEAR");
|
||||
lv_obj_clear_flag(s_empty_label, LV_OBJ_FLAG_HIDDEN);
|
||||
} else {
|
||||
lv_obj_add_flag(s_empty_label, LV_OBJ_FLAG_HIDDEN);
|
||||
}
|
||||
if (radar_changed) {
|
||||
lv_obj_invalidate(s_radar);
|
||||
}
|
||||
}
|
||||
17
main/ui.h
Normal file
17
main/ui.h
Normal file
@@ -0,0 +1,17 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "radar_service.h"
|
||||
|
||||
typedef struct {
|
||||
bool sound_enabled;
|
||||
bool touch_available;
|
||||
uint8_t brightness_percent;
|
||||
void (*sound_changed)(bool enabled);
|
||||
void (*brightness_changed)(uint8_t percent, bool persist);
|
||||
} ui_config_t;
|
||||
|
||||
void ui_init(const ui_config_t *config);
|
||||
void ui_update(const radar_snapshot_t *snapshot);
|
||||
Reference in New Issue
Block a user