diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..f52c8b1 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,28 @@ +name: build + +on: + push: + pull_request: + +permissions: + contents: read + +jobs: + host-tests: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Configure + run: cmake -S tests -B build-host -DCMAKE_BUILD_TYPE=Debug + - name: Build + run: cmake --build build-host --parallel + - name: Test + run: ctest --test-dir build-host --output-on-failure + + esp-idf: + runs-on: ubuntu-latest + container: espressif/idf:v5.3.4 + steps: + - uses: actions/checkout@v4 + - name: Build ESP32-S3 firmware + run: idf.py build diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e0be428 --- /dev/null +++ b/.gitignore @@ -0,0 +1,9 @@ +/build/ +/build-*/ +/.cache/ +/managed_components/ +/sdkconfig +/sdkconfig.old +*.bin +*.elf +*.map diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..fe72c3b --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.16) + +# Only discover the application and its declared transitive dependencies. +set(COMPONENTS main) +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +project(radick) diff --git a/README.md b/README.md index 3cfdcd4..9cc0fc4 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,119 @@ # Radick -Dick Finder \ No newline at end of file +Radick은 Ai-Thinker RD-03D V1 24 GHz 레이더와 Elecrow 7인치 CrowPanel 하나로 동작하는 독립형 다중 목표 레이더입니다. 원본 [Arduino-ESP32-Radarproject](https://github.com/Stevee87/Arduino-ESP32-Radarproject)의 레이더 표현과 거리 기반 경고음을 유지하면서, XIAO·Arduino GIGA·Wi-Fi·UDP를 모두 제거했습니다. 펌웨어는 Arduino 계층 없이 ESP-IDF 5.3과 LVGL 8로 작성되어 레이더 UART부터 화면까지 한 ESP32-S3 안에서 처리합니다. + +> [!IMPORTANT] +> 이 프로젝트는 **Basic CrowPanel 7.0 V3.0 (`DIS08070H`, ESP32-S3 N4R8)** 및 **RD-03D V1 (`S5KM312CL`, 15 × 44 mm)** 전용입니다. CrowPanel V1/V2와 RD-03D V2는 핀 및 프로토콜이 달라 지원하지 않습니다. + +## 필요한 하드웨어 + +- Elecrow Basic CrowPanel 7.0-inch HMI ESP32 Display V3.0 +- Ai-Thinker RD-03D V1 레이더 모듈 +- 보호회로가 있는 1셀 Li-ion/LiPo 배터리(공칭 3.7 V, 최대 4.2 V) +- 출력 리플 100 mV 이하의 5 V 승압 모듈(연속 200 mA 이상, 500 mA급 권장) +- 5 V 레이더 전원 스위치와 승압 모듈 제조사가 권장하는 입·출력 캐패시터 +- 경고음을 사용할 경우 J5 `SPK`에 맞는 스피커 + +배터리는 CrowPanel의 J1에 직접 연결할 수 있지만, RD-03D의 동작 전압은 4.5–5.5 V입니다. 따라서 레이더 VCC에는 배터리나 3V3 핀을 직접 연결하지 말고 반드시 별도 5 V 승압 출력을 사용해야 합니다. MCU/디스플레이는 CrowPanel 한 장뿐이지만, 이 승압 전원부는 생략할 수 없는 최소 부품입니다. + +## 배선 + +| RD-03D V1 | CrowPanel V3 / 전원 | 설명 | +|---|---|---| +| `VCC` | 5 V 승압 모듈 `OUT+` | **J10의 3V3 또는 배터리에 직접 연결 금지** | +| `GND` | 공통 GND(J7 pin 4 등) | CrowPanel·승압 모듈·레이더의 GND를 공통 연결 | +| `TX` | J7 `GPIO_D` pin 1, GPIO38 | ESP32-S3 UART1 RX | +| `RX` | J10 `UART` pin 2, GPIO43 | ESP32-S3 UART1 TX | + +배터리 전원은 아래처럼 분기합니다. + +```text +보호형 1S 배터리 ──> CrowPanel J1 BAT+/GND + └─────────> 5 V 저리플 승압 ──> RD-03D VCC/GND + +RD-03D TX ─────────> J7 pin 1 / GPIO38 +RD-03D RX <───────── J10 pin 2 / GPIO43 +``` + +J10 pin 1(GPIO44)은 온보드 CH340C의 TX 출력과 같은 네트이므로 레이더 TX를 연결하면 출력끼리 충돌할 수 있습니다. 이 때문에 레이더 수신과 송신을 서로 다른 커넥터로 나눈 것입니다. GPIO43은 CH340C RX와 레이더 RX라는 두 입력으로만 연결되어 안전하게 플래싱 경로를 유지합니다. 다만 부트 ROM의 송신도 레이더 RX에 들어가므로 **플래싱 중에는 레이더 5 V 전원을 끄는 것을 권장**합니다. + +RD-03D V1의 1.25 mm 4핀 커넥터는 데이터시트 기준 pin 1 `5V`, pin 2 `GND`, pin 3 `TX`, pin 4 `RX`입니다. 모듈 방향에 따라 좌우가 뒤집혀 보일 수 있으므로 실크와 pin 1 표시를 반드시 확인하십시오. + +J1 커넥터의 pin 1은 `BAT+`, pin 2는 `GND`입니다. 커넥터 규격만 믿지 말고 실제 배터리 극성을 멀티미터로 확인하십시오. CrowPanel에는 MCU가 읽을 수 있는 배터리 전압/잔량 회로가 없어 UI에 근거 없는 배터리 퍼센트를 표시하지 않습니다. 자세한 전체 핀맵과 전원 주의사항은 [하드웨어 문서](docs/HARDWARE.md)를 참고하십시오. + +## 동작 방식 + +- UART1, 256000 baud, 8N1로 RD-03D의 30바이트 프레임을 직접 수신 +- 스트림 노이즈·분할 프레임·손상 프레임 이후 자동 재동기화 +- 최대 세 목표를 전역 최근접 매칭과 이동 예측으로 추적해 슬롯 순서가 바뀌어도 ID 유지 +- 위치/속도 EMA로 화면 떨림 완화 +- 원본의 1 m 강제 병합과 2초 정지 고스트 로직을 제거해 가까운 두 사람을 별도 목표로 유지하고 잔상을 빠르게 제거 +- 0–8 m를 근거리 중심 비선형 스케일로 그려 작은 화면 이동도 알아보기 쉽게 표시 +- 가장 가까운 목표의 거리에 따라 온보드 NS4168 스피커의 핑 주기와 음높이를 변경 +- 터치로 음소거와 밝기를 조절하고, 60초 동안 터치와 목표가 없으면 자동 감광 +- 마지막 정상 프레임으로부터 1.5초가 지나면 명확한 `RADAR OFFLINE` 상태 표시 + +레이더 명령 전송은 앱 부팅 후 공식 멀티 타깃 명령 `0x90`만 사용합니다. 기본 단일 타깃 스트림이 이미 출력되는 경우도 놓치지 않도록 5초 간격으로 총 세 번만 멱등 전송하며 UART와 UI 태스크를 막지 않습니다. 센서 전원이 꺼졌다가 정상 프레임이 다시 들어오면 같은 제한된 설정 절차를 한 번 재수행합니다. + +## 빌드와 설치 + +권장 환경은 ESP-IDF **v5.3.4**입니다. 저장소 루트에서 다음을 실행합니다. + +```bash +. "$IDF_PATH/export.sh" +idf.py set-target esp32s3 +idf.py build +idf.py -p /dev/ttyUSB0 flash +``` + +첫 빌드에서 Component Manager가 LVGL 8.3.11을 받습니다. 애플리케이션 UART 콘솔은 레이더 핀과의 간섭을 피하려고 꺼 두었으므로 `monitor`에 런타임 로그가 표시되지 않는 것이 정상입니다. + +로컬 ESP-IDF 설치 없이 Docker로 빌드할 수도 있습니다. + +```bash +docker run --rm \ + -v "$PWD:/project" -w /project \ + espressif/idf:v5.3.4 idf.py build +``` + +## 호스트 테스트 + +레이더 프로토콜과 추적기는 ESP32 의존성 없이 테스트할 수 있습니다. + +```bash +cmake -S tests -B build-host +cmake --build build-host --parallel +ctest --test-dir build-host --output-on-failure +``` + +테스트는 sign-magnitude 디코딩, 모든 청크 분할 위치, 노이즈/손상/바이트 유실 후 복구, 연속 프레임, 슬롯 재정렬, 근접 목표 비병합, 누락/재획득, 서로 교차하는 두 목표의 ID 유지를 포함합니다. + +## 구조 + +```text +main/ +├── board.* RGB LCD, GT911/PCA9557, LVGL, 백라이트 +├── radar_protocol.* RD-03D 스트림 파서 +├── target_tracker.* 안정 ID, 예측, EMA, 만료 처리 +├── radar_service.* UART 태스크와 thread-safe 화면 스냅샷 +├── audio_service.* I2S 거리 경고음 +├── ui.* 800×480 단순 레이더 UI +├── app_settings.* NVS 밝기/음소거 저장 +└── app_main.c 서비스 연결, 자동 감광, LVGL 루프 +``` + +## 실기 확인 항목 + +전체 소프트웨어 빌드와 호스트 테스트 외에, 조립 후 다음 항목은 실제 하드웨어에서 확인해야 합니다. + +- 승압 출력이 레이더 연결 상태에서도 5 V 근처로 안정적인지 +- GT911 터치 방향과 RGB 패널 색/플리커 +- 안테나 방향 기준 좌우 좌표가 화면과 일치하는지 +- 스피커 임피던스에 맞춘 실제 음량과 전원 노이즈 +- 배터리 구동 시간과 충전 중 발열 + +레이더 사양은 Ai-Thinker의 [RD-03D V1 문서](https://docs.ai-thinker.com/en/Rd-03D_V1/), 디스플레이 핀은 Elecrow의 [CrowPanel 7-inch 저장소](https://github.com/Elecrow-RD/CrowPanel-7.0-HMI-ESP32-Display-800x480)와 V3 회로도를 기준으로 했습니다. + +## License + +[WTFPL v2](LICENSE) diff --git a/dependencies.lock b/dependencies.lock new file mode 100644 index 0000000..636a4f7 --- /dev/null +++ b/dependencies.lock @@ -0,0 +1,18 @@ +dependencies: + idf: + source: + type: idf + version: 5.3.4 + lvgl/lvgl: + component_hash: 948bff879a345149b83065535bbc4a026ce9f47498a22881e432a264b9098015 + dependencies: [] + source: + registry_url: https://components.espressif.com/ + type: service + version: 8.3.11 +direct_dependencies: +- idf +- lvgl/lvgl +manifest_hash: 22347424756d0bee5a8893b518dad72510d82d0ef82f426ce28777d8c1834201 +target: esp32s3 +version: 2.0.0 diff --git a/docs/HARDWARE.md b/docs/HARDWARE.md new file mode 100644 index 0000000..21b420c --- /dev/null +++ b/docs/HARDWARE.md @@ -0,0 +1,110 @@ +# CrowPanel V3 하드웨어와 핀맵 + +이 문서는 Radick이 대상으로 하는 **Basic CrowPanel 7.0 V3.0 / DIS08070H / ESP32-S3-WROOM-1-N4R8**의 핀 사용과 RD-03D V1 연결을 정리합니다. 같은 이름으로 판매된 V1/V2는 특히 터치 회로가 다르므로 이 펌웨어의 GPIO38 레이더 입력 배선을 그대로 사용하면 안 됩니다. + +## 외부 레이더 배선 + +| 기능 | ESP32-S3 | 물리 위치 | 방향 | +|---|---:|---|---| +| 레이더 데이터 RX | GPIO38 | J7 `GPIO_D` pin 1 | RD-03D TX → ESP32 | +| 레이더 명령 TX | GPIO43 | J10 `UART` pin 2 | ESP32 → RD-03D RX | +| 공통 접지 | GND | J7 pin 4 등 | 공통 | +| 레이더 전원 | 별도 5 V boost | 외부 전원부 | boost → RD-03D VCC | + +J7의 pin 2는 NC, pin 3은 3V3, pin 4는 GND입니다. J10은 pin 1 GPIO44/RX 표기, pin 2 GPIO43/TX 표기, pin 3 3V3, pin 4 GND입니다. 커넥터의 RX/TX 표기는 CrowPanel 관점입니다. + +J10의 GPIO43/44는 CH340C USB-UART와 전기적으로 공유됩니다. CH340C TX가 연결된 GPIO44를 레이더 TX 입력으로 쓰지 않고, V3에서 사용자 GPIO로 제공되는 GPIO38을 사용해 출력 충돌을 피합니다. GPIO43에는 ESP32 출력 하나와 레이더/CH340C 입력만 매달립니다. V3 회로도에는 GPIO38 관련 선택실장 저항 자리가 있으므로 보드 변형에서는 실장 상태를 확인하십시오. + +RD-03D V1의 1.25 mm 4핀 커넥터 순서는 pin 1 `5V`, pin 2 `GND`, pin 3 `TX`, pin 4 `RX`입니다. 커넥터를 바라보는 방향에 따라 순서가 뒤집혀 보일 수 있으므로 실크와 pin 1 표시를 기준으로 확인하십시오. + +## 배터리와 5 V 레일 + +- J1 PH2.0 pin 1: `BAT+` +- J1 PH2.0 pin 2: `GND` +- 배터리 범위: 보호회로가 있는 1S Li-ion/LiPo, 공칭 3.7 V, 최대 4.2 V +- 보드 충전 IC: 4054 계열 선형 충전기, 회로도 설정 약 500 mA +- RD-03D 요구 전원: 4.5–5.5 V, 5 V 권장, 전원 설계 여유 200 mA 이상 + +배터리 전압은 RD-03D 최저 동작 전압보다 낮습니다. 다음 연결이 필수입니다. + +```text +BAT+ ──┬── CrowPanel J1 pin 1 + └── switch ── 5 V low-ripple boost IN+ +GND ───┬── CrowPanel J1 pin 2 + └───────────── boost IN-/OUT- ── RD-03D GND +boost OUT+ ─────────────────────────────> RD-03D VCC +``` + +- 레이더 VCC를 J7/J10의 3V3에 연결하지 마십시오. +- 레이더 VCC를 배터리 BAT+에 직접 연결하지 마십시오. +- 승압 모듈은 연속 200 mA 이상이어야 하며 여유를 둔 500 mA급과 공식 사양의 출력 리플 100 mV 이하를 만족해야 합니다. +- 승압 모듈 데이터시트가 지정한 캐패시터와 짧은 전원/접지 배선을 사용하십시오. +- 보드 회로에는 MCU용 배터리 ADC나 fuel gauge가 없으므로 정확한 잔량 표시에는 외부 게이지가 필요합니다. +- 단순 선형 충전 회로에는 별도 power-path가 보이지 않습니다. 충전과 고부하 사용을 동시에 하면 충전 종료 판정과 발열에 영향을 줄 수 있으므로 실기에서 온도를 확인하고, 제품화할 때는 power-path가 있는 전원 설계를 사용하십시오. + +## 내부 사용 핀 + +### 800 × 480 RGB 패널 + +| 신호 | GPIO | 신호 | GPIO | +|---|---:|---|---:| +| D0 | 15 | D8 | 8 | +| D1 | 7 | D9 | 16 | +| D2 | 6 | D10 | 1 | +| D3 | 5 | D11 | 14 | +| D4 | 4 | D12 | 21 | +| D5 | 9 | D13 | 47 | +| D6 | 46 | D14 | 48 | +| D7 | 3 | D15 | 45 | +| PCLK | 0 | DE | 41 | +| HSYNC | 39 | VSYNC | 40 | +| Backlight PWM | 2 | | | + +Radick은 15 MHz pixel clock과 V3 예제/회로도의 porch 및 sync 값을 사용합니다. 프레임 버퍼는 8 MB octal PSRAM에 둡니다. + +### 터치와 I/O 확장기 + +| 기능 | 값 | +|---|---| +| I2C SDA | GPIO19 | +| I2C SCL | GPIO20 | +| PCA9557 | 0x18 | +| GT911 | V3 기본 0x5D, 0x14는 방어적으로 추가 탐색 | +| PCA IO0 | GT911 RESET | +| PCA IO1 | GT911 INT | + +V3 부팅 시 PCA9557 IO0/IO1을 출력 low로 두고 RESET을 high로 해제한 뒤 INT를 입력으로 전환합니다. 이 INT-low 리셋 순서는 정상적으로 GT911 주소 0x5D를 선택하며, 0x14 탐색은 이미 다른 상태로 기동한 변형에 대한 방어 수단입니다. 터치가 검출되지 않아도 화면과 레이더는 계속 동작하며 UI에 `TOUCH OFFLINE`을 표시합니다. GPIO19/20이 터치 I2C에 사용되므로 ESP32-S3 native USB D-/D+ 용도로 사용할 수 없습니다. + +### 오디오 + +| I2S 신호 | GPIO | +|---|---:| +| SDIN | 17 | +| LRCLK/WS | 18 | +| BCLK | 42 | + +온보드 NS4168 앰프 출력은 J5 `SPK`의 두 단자에 연결됩니다. BTL 출력이므로 어느 스피커 단자도 GND에 연결하면 안 됩니다. 펌웨어는 16 kHz, 16-bit stereo 신호로 같은 모노 핑을 양 채널에 보냅니다. + +### 사용하지 않는 SD 인터페이스 + +| 신호 | GPIO | +|---|---:| +| CS | 10 | +| MOSI | 11 | +| SCK | 12 | +| MISO | 13 | + +Radick은 SD 카드를 초기화하지 않아 메모리와 부팅 시간을 절약합니다. + +## UART 및 플래싱 문제 해결 + +1. 정상 프레임은 30바이트이며 `AA FF 03 00`으로 시작해 `55 CC`로 끝납니다. +2. UART 설정은 256000 baud, 8 data bits, no parity, 1 stop bit입니다. +3. 센서가 응답하지 않으면 먼저 RD-03D VCC에서 실제 5 V와 공통 GND를 확인하십시오. +4. TX/RX는 센서 기준입니다. RD TX는 GPIO38, RD RX는 GPIO43이어야 합니다. +5. ROM UART 트래픽이 레이더 RX에도 전달되므로 플래싱 중에는 레이더 5 V 전원을 끄는 것을 권장합니다. +6. 애플리케이션 콘솔은 의도적으로 비활성화되어 있습니다. 필요하면 개발 중에만 sdkconfig의 콘솔 설정을 바꾸되 GPIO43/44 공유를 고려하십시오. + +제품화하거나 UART 배선이 길다면 Ai-Thinker 권고에 따라 신호선에 10–100 Ω 직렬 저항과 적절한 ESD 보호를 추가하십시오. + +근거 자료: [Ai-Thinker RD-03D V1](https://docs.ai-thinker.com/en/Rd-03D_V1/), [Elecrow CrowPanel 7.0 repository](https://github.com/Elecrow-RD/CrowPanel-7.0-HMI-ESP32-Display-800x480), [ESP-IDF RGB LCD API](https://docs.espressif.com/projects/esp-idf/en/v5.3.3/esp32s3/api-reference/peripherals/lcd/rgb_lcd.html). diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt new file mode 100644 index 0000000..7ee7a57 --- /dev/null +++ b/main/CMakeLists.txt @@ -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) diff --git a/main/app_main.c b/main/app_main.c new file mode 100644 index 0000000..ae30af6 --- /dev/null +++ b/main/app_main.c @@ -0,0 +1,172 @@ +#include +#include +#include +#include + +#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)); + } +} diff --git a/main/app_settings.c b/main/app_settings.c new file mode 100644 index 0000000..7c08e1e --- /dev/null +++ b/main/app_settings.c @@ -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; +} diff --git a/main/app_settings.h b/main/app_settings.h new file mode 100644 index 0000000..d3db127 --- /dev/null +++ b/main/app_settings.h @@ -0,0 +1,15 @@ +#pragma once + +#include +#include + +#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); diff --git a/main/audio_service.c b/main/audio_service.c new file mode 100644 index 0000000..23b39ab --- /dev/null +++ b/main/audio_service.c @@ -0,0 +1,436 @@ +#include "audio_service.h" + +#include +#include +#include +#include + +#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; +} diff --git a/main/audio_service.h b/main/audio_service.h new file mode 100644 index 0000000..df6e032 --- /dev/null +++ b/main/audio_service.h @@ -0,0 +1,61 @@ +#pragma once + +#include + +#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 diff --git a/main/board.c b/main/board.c new file mode 100644 index 0000000..05f9c9b --- /dev/null +++ b/main/board.c @@ -0,0 +1,360 @@ +#include "board.h" + +#include +#include + +#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; +} diff --git a/main/board.h b/main/board.h new file mode 100644 index 0000000..8829107 --- /dev/null +++ b/main/board.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include + +#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); diff --git a/main/idf_component.yml b/main/idf_component.yml new file mode 100644 index 0000000..97cd545 --- /dev/null +++ b/main/idf_component.yml @@ -0,0 +1,5 @@ +dependencies: + idf: ">=5.3,<5.4" + lvgl/lvgl: + version: "8.3.11" + public: true diff --git a/main/radar_protocol.c b/main/radar_protocol.c new file mode 100644 index 0000000..8421eb6 --- /dev/null +++ b/main/radar_protocol.c @@ -0,0 +1,198 @@ +#include "radar_protocol.h" + +#include + +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; +} diff --git a/main/radar_protocol.h b/main/radar_protocol.h new file mode 100644 index 0000000..3713492 --- /dev/null +++ b/main/radar_protocol.h @@ -0,0 +1,86 @@ +#ifndef RADICK_RADAR_PROTOCOL_H +#define RADICK_RADAR_PROTOCOL_H + +#include +#include +#include + +#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 */ diff --git a/main/radar_service.c b/main/radar_service.c new file mode 100644 index 0000000..9f17fd5 --- /dev/null +++ b/main/radar_service.c @@ -0,0 +1,328 @@ +#include "radar_service.h" + +#include +#include +#include +#include + +#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; +} diff --git a/main/radar_service.h b/main/radar_service.h new file mode 100644 index 0000000..e0be3a7 --- /dev/null +++ b/main/radar_service.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +#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); diff --git a/main/radick_config.h b/main/radick_config.h new file mode 100644 index 0000000..78f095c --- /dev/null +++ b/main/radick_config.h @@ -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 diff --git a/main/target_tracker.c b/main/target_tracker.c new file mode 100644 index 0000000..3a811bb --- /dev/null +++ b/main/target_tracker.c @@ -0,0 +1,387 @@ +#include "target_tracker.h" + +#include +#include + +#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; +} diff --git a/main/target_tracker.h b/main/target_tracker.h new file mode 100644 index 0000000..cdb0b92 --- /dev/null +++ b/main/target_tracker.h @@ -0,0 +1,85 @@ +#ifndef RADICK_TARGET_TRACKER_H +#define RADICK_TARGET_TRACKER_H + +#include "radar_protocol.h" + +#include +#include +#include + +#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 */ diff --git a/main/ui.c b/main/ui.c new file mode 100644 index 0000000..bcf283f --- /dev/null +++ b/main/ui.c @@ -0,0 +1,504 @@ +#include "ui.h" + +#include +#include +#include + +#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); + } +} diff --git a/main/ui.h b/main/ui.h new file mode 100644 index 0000000..dcbed59 --- /dev/null +++ b/main/ui.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +#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); diff --git a/partitions.csv b/partitions.csv new file mode 100644 index 0000000..aadd2d6 --- /dev/null +++ b/partitions.csv @@ -0,0 +1,4 @@ +# Name, Type, SubType, Offset, Size, Flags +nvs, data, nvs, 0x9000, 24K, +phy_init, data, phy, 0xf000, 4K, +factory, app, factory, 0x10000, 3M, diff --git a/sdkconfig.defaults b/sdkconfig.defaults new file mode 100644 index 0000000..4319694 --- /dev/null +++ b/sdkconfig.defaults @@ -0,0 +1,47 @@ +# Target: Elecrow Basic CrowPanel 7.0-inch V3.0 (DIS08070H, ESP32-S3-N4R8) +CONFIG_IDF_TARGET="esp32s3" + +# Module memory configuration: 4 MB QIO flash + 8 MB octal PSRAM. +CONFIG_ESPTOOLPY_FLASHMODE_QIO=y +CONFIG_ESPTOOLPY_FLASH_MODE_AUTO_DETECT=n +CONFIG_ESPTOOLPY_FLASHFREQ_80M=y +CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y +CONFIG_PARTITION_TABLE_CUSTOM=y +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" +CONFIG_SPIRAM=y +CONFIG_SPIRAM_MODE_OCT=y +CONFIG_SPIRAM_SPEED_80M=y +CONFIG_SPIRAM_BOOT_INIT=y +CONFIG_SPIRAM_USE_CAPS_ALLOC=y +CONFIG_SPIRAM_MALLOC_ALWAYSINTERNAL=16384 +CONFIG_SPIRAM_MALLOC_RESERVE_INTERNAL=32768 +CONFIG_SPIRAM_FETCH_INSTRUCTIONS=y +CONFIG_SPIRAM_RODATA=y + +# The RGB panel is bandwidth-sensitive when its frame buffer lives in PSRAM. +CONFIG_ESP32S3_DATA_CACHE_LINE_64B=y +CONFIG_LCD_RGB_RESTART_IN_VSYNC=y +CONFIG_COMPILER_OPTIMIZATION_PERF=y +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y +CONFIG_FREERTOS_HZ=1000 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=6144 + +# GPIO43 is used by the radar TX path. Keep the application console off UART0. +# The ROM downloader still works through the onboard CH340C. +CONFIG_ESP_CONSOLE_NONE=y +CONFIG_ESP_CONSOLE_SECONDARY_NONE=y +CONFIG_BOOTLOADER_LOG_LEVEL_NONE=y +CONFIG_LOG_DEFAULT_LEVEL_WARN=y + +# LVGL 8 RGB565 configuration and only the fonts used by this UI. +CONFIG_LV_COLOR_DEPTH_16=y +CONFIG_LV_COLOR_16_SWAP=n +# CONFIG_LV_BUILD_EXAMPLES is not set +CONFIG_LV_FONT_MONTSERRAT_12=y +CONFIG_LV_FONT_MONTSERRAT_14=y +CONFIG_LV_FONT_MONTSERRAT_16=y +CONFIG_LV_FONT_MONTSERRAT_20=y +CONFIG_LV_FONT_MONTSERRAT_24=y +CONFIG_LV_FONT_MONTSERRAT_28=y +CONFIG_LV_FONT_MONTSERRAT_32=y +CONFIG_LV_FONT_MONTSERRAT_40=y diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 0000000..bc064cc --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.16) + +project(radick_host_tests LANGUAGES C) + +enable_testing() + +add_executable(radick_tests + test_main.c + ../main/radar_protocol.c + ../main/target_tracker.c +) + +target_include_directories(radick_tests PRIVATE ../main) +target_compile_features(radick_tests PRIVATE c_std_11) +target_compile_options(radick_tests PRIVATE + -Wall + -Wextra + -Wpedantic + -Wconversion + -Wshadow + -Werror +) + +add_test(NAME radick_core COMMAND radick_tests) diff --git a/tests/test_main.c b/tests/test_main.c new file mode 100644 index 0000000..5a7fe7c --- /dev/null +++ b/tests/test_main.c @@ -0,0 +1,572 @@ +#include "radar_protocol.h" +#include "target_tracker.h" + +#include +#include +#include +#include +#include +#include + +#define ARRAY_SIZE(array) (sizeof(array) / sizeof((array)[0])) + +static unsigned int tests_run; + +static void fail(const char *file, int line, const char *expression) +{ + (void)fprintf(stderr, "%s:%d: assertion failed: %s\n", file, line, expression); + exit(EXIT_FAILURE); +} + +#define ASSERT_TRUE(expression) \ + do { \ + if (!(expression)) { \ + fail(__FILE__, __LINE__, #expression); \ + } \ + } while (false) + +#define ASSERT_EQ_INT(expected, actual) \ + do { \ + const long long expected_value = (long long)(expected); \ + const long long actual_value = (long long)(actual); \ + if (expected_value != actual_value) { \ + fail(__FILE__, __LINE__, #expected " == " #actual); \ + } \ + } while (false) + +#define ASSERT_NEAR(expected, actual, tolerance) \ + do { \ + const double difference = fabs((double)(expected) - (double)(actual)); \ + if (difference > (double)(tolerance)) { \ + fail(__FILE__, __LINE__, #expected " ~= " #actual); \ + } \ + } while (false) + +#define RUN_TEST(function) \ + do { \ + function(); \ + ++tests_run; \ + (void)printf("ok %u - %s\n", tests_run, #function); \ + } while (false) + +static uint16_t encode_sign_magnitude(int16_t value) +{ + if (value >= 0) { + return (uint16_t)value | UINT16_C(0x8000); + } + return (uint16_t)(-(int32_t)value); +} + +static void put_u16_le(uint8_t *destination, uint16_t value) +{ + destination[0] = (uint8_t)(value & UINT16_C(0x00FF)); + destination[1] = (uint8_t)(value >> 8U); +} + +static void make_frame(uint8_t frame[RD03D_FRAME_SIZE], + const rd03d_target_t targets[RD03D_MAX_TARGETS]) +{ + size_t index; + + memset(frame, 0, RD03D_FRAME_SIZE); + memcpy(frame, rd03d_frame_header, RD03D_FRAME_HEADER_SIZE); + memcpy(frame + RD03D_FRAME_SIZE - RD03D_FRAME_TAIL_SIZE, + rd03d_frame_tail, + RD03D_FRAME_TAIL_SIZE); + + for (index = 0U; index < RD03D_MAX_TARGETS; ++index) { + uint8_t *payload; + + if (!targets[index].valid) { + continue; + } + payload = frame + RD03D_FRAME_HEADER_SIZE + + index * RD03D_TARGET_DATA_SIZE; + put_u16_le(payload, encode_sign_magnitude(targets[index].x_mm)); + put_u16_le(payload + 2U, encode_sign_magnitude(targets[index].y_mm)); + put_u16_le(payload + 4U, + encode_sign_magnitude(targets[index].speed_cm_s)); + put_u16_le(payload + 6U, targets[index].resolution_mm); + } +} + +static rd03d_target_t detection(int16_t x, + int16_t y, + int16_t speed, + uint16_t resolution) +{ + const rd03d_target_t result = { + .x_mm = x, + .y_mm = y, + .speed_cm_s = speed, + .resolution_mm = resolution, + .valid = true, + }; + return result; +} + +static void test_sign_magnitude_boundaries(void) +{ + ASSERT_EQ_INT(-782, rd03d_decode_sign_magnitude(0x0EU, 0x03U)); + ASSERT_EQ_INT(1713, rd03d_decode_sign_magnitude(0xB1U, 0x86U)); + ASSERT_EQ_INT(-16, rd03d_decode_sign_magnitude(0x10U, 0x00U)); + ASSERT_EQ_INT(0, rd03d_decode_sign_magnitude(0x00U, 0x00U)); + ASSERT_EQ_INT(0, rd03d_decode_sign_magnitude(0x00U, 0x80U)); + ASSERT_EQ_INT(-32767, rd03d_decode_sign_magnitude(0xFFU, 0x7FU)); + ASSERT_EQ_INT(32767, rd03d_decode_sign_magnitude(0xFFU, 0xFFU)); +} + +static void test_decode_complete_frame_and_resolution(void) +{ + const rd03d_target_t input[RD03D_MAX_TARGETS] = { + {.x_mm = -782, .y_mm = 1713, .speed_cm_s = -16, + .resolution_mm = 360U, .valid = true}, + {.x_mm = 32767, .y_mm = -32767, .speed_cm_s = 72, + .resolution_mm = 65535U, .valid = true}, + {0}, + }; + uint8_t bytes[RD03D_FRAME_SIZE]; + rd03d_frame_t frame; + + make_frame(bytes, input); + ASSERT_TRUE(rd03d_decode_frame(bytes, &frame)); + ASSERT_TRUE(frame.targets[0].valid); + ASSERT_EQ_INT(-782, frame.targets[0].x_mm); + ASSERT_EQ_INT(1713, frame.targets[0].y_mm); + ASSERT_EQ_INT(-16, frame.targets[0].speed_cm_s); + ASSERT_EQ_INT(360, frame.targets[0].resolution_mm); + ASSERT_TRUE(frame.targets[1].valid); + ASSERT_EQ_INT(32767, frame.targets[1].x_mm); + ASSERT_EQ_INT(-32767, frame.targets[1].y_mm); + ASSERT_EQ_INT(65535, frame.targets[1].resolution_mm); + ASSERT_TRUE(!frame.targets[2].valid); + + bytes[RD03D_FRAME_SIZE - 1U] ^= 0x01U; + ASSERT_TRUE(!rd03d_decode_frame(bytes, &frame)); +} + +typedef struct { + rd03d_frame_t frames[8]; + size_t count; +} frame_capture_t; + +static void capture_frame(const rd03d_frame_t *frame, void *context) +{ + frame_capture_t *capture = context; + + ASSERT_TRUE(capture->count < ARRAY_SIZE(capture->frames)); + capture->frames[capture->count++] = *frame; +} + +static void test_parser_all_split_points(void) +{ + const rd03d_target_t targets[RD03D_MAX_TARGETS] = { + {.x_mm = -1200, .y_mm = 3456, .speed_cm_s = 88, + .resolution_mm = 240U, .valid = true}, + {0}, + {0}, + }; + uint8_t bytes[RD03D_FRAME_SIZE]; + size_t split; + + make_frame(bytes, targets); + for (split = 0U; split <= RD03D_FRAME_SIZE; ++split) { + rd03d_parser_t parser; + frame_capture_t capture = {0}; + size_t decoded_count; + + rd03d_parser_init(&parser); + decoded_count = rd03d_parser_feed(&parser, + bytes, + split, + capture_frame, + &capture); + decoded_count += rd03d_parser_feed(&parser, + bytes + split, + RD03D_FRAME_SIZE - split, + capture_frame, + &capture); + ASSERT_EQ_INT(1, decoded_count); + ASSERT_EQ_INT(1, capture.count); + ASSERT_EQ_INT(-1200, capture.frames[0].targets[0].x_mm); + ASSERT_EQ_INT(1, parser.frames_decoded); + ASSERT_EQ_INT(0, parser.malformed_frames); + } +} + +static void test_parser_noise_corruption_and_resynchronisation(void) +{ + const rd03d_target_t first_targets[RD03D_MAX_TARGETS] = { + {.x_mm = -321, .y_mm = 2000, .speed_cm_s = 10, + .resolution_mm = 120U, .valid = true}, + {0}, + {0}, + }; + const rd03d_target_t second_targets[RD03D_MAX_TARGETS] = { + {.x_mm = 777, .y_mm = 2500, .speed_cm_s = -25, + .resolution_mm = 480U, .valid = true}, + {0}, + {0}, + }; + const uint8_t noise[] = {0x00U, 0xAAU, 0xAAU, 0xFFU, 0x02U, 0x99U}; + uint8_t bad[RD03D_FRAME_SIZE]; + uint8_t good[RD03D_FRAME_SIZE]; + uint8_t stream[sizeof(noise) + RD03D_FRAME_SIZE * 2U]; + rd03d_parser_t parser; + frame_capture_t capture = {0}; + size_t offset = 0U; + + make_frame(bad, first_targets); + make_frame(good, second_targets); + bad[RD03D_FRAME_SIZE - 2U] = 0x54U; + memcpy(stream + offset, noise, sizeof(noise)); + offset += sizeof(noise); + memcpy(stream + offset, bad, sizeof(bad)); + offset += sizeof(bad); + memcpy(stream + offset, good, sizeof(good)); + offset += sizeof(good); + + rd03d_parser_init(&parser); + ASSERT_EQ_INT(1, + rd03d_parser_feed(&parser, + stream, + offset, + capture_frame, + &capture)); + ASSERT_EQ_INT(1, capture.count); + ASSERT_EQ_INT(777, capture.frames[0].targets[0].x_mm); + ASSERT_EQ_INT(-25, capture.frames[0].targets[0].speed_cm_s); + ASSERT_EQ_INT(1, parser.malformed_frames); + ASSERT_EQ_INT(offset, parser.bytes_received); +} + +static void test_parser_recovers_from_dropped_byte_and_embedded_header(void) +{ + rd03d_target_t corrupt_targets[RD03D_MAX_TARGETS] = { + {.x_mm = -100, .y_mm = 1000, .speed_cm_s = 4, + .resolution_mm = 0U, .valid = true}, + {0}, + {0}, + }; + const rd03d_target_t good_targets[RD03D_MAX_TARGETS] = { + {.x_mm = 901, .y_mm = 3333, .speed_cm_s = 42, + .resolution_mm = 360U, .valid = true}, + {0}, + {0}, + }; + uint8_t corrupt[RD03D_FRAME_SIZE]; + uint8_t good[RD03D_FRAME_SIZE]; + uint8_t stream[RD03D_FRAME_SIZE * 2U - 1U]; + rd03d_parser_t parser; + frame_capture_t capture = {0}; + + make_frame(corrupt, corrupt_targets); + make_frame(good, good_targets); + /* A false header in the damaged payload must not prevent later recovery. */ + memcpy(corrupt + 12U, rd03d_frame_header, RD03D_FRAME_HEADER_SIZE); + memcpy(stream, corrupt, 20U); + memcpy(stream + 20U, corrupt + 21U, RD03D_FRAME_SIZE - 21U); + memcpy(stream + RD03D_FRAME_SIZE - 1U, good, RD03D_FRAME_SIZE); + + rd03d_parser_init(&parser); + ASSERT_EQ_INT(1, + rd03d_parser_feed(&parser, + stream, + sizeof(stream), + capture_frame, + &capture)); + ASSERT_EQ_INT(1, capture.count); + ASSERT_EQ_INT(901, capture.frames[0].targets[0].x_mm); + ASSERT_TRUE(parser.malformed_frames >= 1U); +} + +static void test_parser_multiple_frames_per_chunk(void) +{ + rd03d_target_t targets[RD03D_MAX_TARGETS] = { + {.x_mm = 1, .y_mm = 2, .speed_cm_s = 3, + .resolution_mm = 4U, .valid = true}, + {0}, + {0}, + }; + uint8_t stream[RD03D_FRAME_SIZE * 3U]; + rd03d_parser_t parser; + frame_capture_t capture = {0}; + size_t index; + + for (index = 0U; index < 3U; ++index) { + targets[0].x_mm = (int16_t)(100 + (int16_t)index); + make_frame(stream + index * RD03D_FRAME_SIZE, targets); + } + rd03d_parser_init(&parser); + ASSERT_EQ_INT(3, + rd03d_parser_feed(&parser, + stream, + sizeof(stream), + capture_frame, + &capture)); + ASSERT_EQ_INT(3, capture.count); + ASSERT_EQ_INT(100, capture.frames[0].targets[0].x_mm); + ASSERT_EQ_INT(102, capture.frames[2].targets[0].x_mm); +} + +static target_tracker_config_t deterministic_tracker_config(void) +{ + target_tracker_config_t config; + + target_tracker_default_config(&config); + config.position_ema_alpha = 0.5F; + config.speed_ema_alpha = 0.5F; + config.motion_ema_alpha = 1.0F; + config.max_association_distance_mm = 1500.0F; + config.max_missed_frames = 2U; + return config; +} + +static const target_snapshot_t *find_snapshot(const target_snapshot_t *snapshots, + size_t count, + uint32_t id) +{ + size_t index; + + for (index = 0U; index < count; ++index) { + if (snapshots[index].id == id) { + return &snapshots[index]; + } + } + return NULL; +} + +static void test_tracker_stable_ids_slot_reorder_and_ema(void) +{ + target_tracker_t tracker; + target_tracker_config_t config = deterministic_tracker_config(); + rd03d_target_t frame_one[] = { + {.x_mm = -1000, .y_mm = 2000, .speed_cm_s = 20, + .resolution_mm = 120U, .valid = true}, + {.x_mm = 1000, .y_mm = 2200, .speed_cm_s = -30, + .resolution_mm = 240U, .valid = true}, + }; + rd03d_target_t frame_two[] = { + {.x_mm = 1100, .y_mm = 2200, .speed_cm_s = -10, + .resolution_mm = 360U, .valid = true}, + {.x_mm = -800, .y_mm = 2000, .speed_cm_s = 40, + .resolution_mm = 480U, .valid = true}, + }; + target_snapshot_t snapshots[TARGET_TRACKER_CAPACITY]; + uint32_t left_id; + uint32_t right_id; + size_t count; + const target_snapshot_t *left; + const target_snapshot_t *right; + + target_tracker_init(&tracker, &config); + ASSERT_EQ_INT(2, + target_tracker_update(&tracker, + frame_one, + ARRAY_SIZE(frame_one))); + count = target_tracker_snapshot(&tracker, snapshots, ARRAY_SIZE(snapshots)); + ASSERT_EQ_INT(2, count); + left_id = snapshots[0].x_mm < 0.0F ? snapshots[0].id : snapshots[1].id; + right_id = snapshots[0].x_mm > 0.0F ? snapshots[0].id : snapshots[1].id; + + ASSERT_EQ_INT(2, + target_tracker_update(&tracker, + frame_two, + ARRAY_SIZE(frame_two))); + count = target_tracker_snapshot(&tracker, snapshots, ARRAY_SIZE(snapshots)); + left = find_snapshot(snapshots, count, left_id); + right = find_snapshot(snapshots, count, right_id); + ASSERT_TRUE(left != NULL); + ASSERT_TRUE(right != NULL); + ASSERT_NEAR(-900.0, left->x_mm, 0.001); + ASSERT_NEAR(30.0, left->speed_cm_s, 0.001); + ASSERT_EQ_INT(480, left->resolution_mm); + ASSERT_NEAR(1050.0, right->x_mm, 0.001); + ASSERT_NEAR(-20.0, right->speed_cm_s, 0.001); + ASSERT_EQ_INT(360, right->resolution_mm); + ASSERT_EQ_INT(2, left->age_frames); + ASSERT_EQ_INT(0, left->last_seen_age_frames); + ASSERT_TRUE(left->visible && left->observed_this_frame); +} + +static void test_tracker_does_not_merge_close_targets(void) +{ + target_tracker_t tracker; + rd03d_target_t detections[] = { + {.x_mm = 0, .y_mm = 2000, .speed_cm_s = 1, + .resolution_mm = 120U, .valid = true}, + {.x_mm = 500, .y_mm = 2000, .speed_cm_s = 2, + .resolution_mm = 120U, .valid = true}, + {.x_mm = 900, .y_mm = 2000, .speed_cm_s = 3, + .resolution_mm = 120U, .valid = true}, + }; + target_snapshot_t snapshots[TARGET_TRACKER_CAPACITY]; + + target_tracker_init(&tracker, NULL); + ASSERT_EQ_INT(3, + target_tracker_update(&tracker, + detections, + ARRAY_SIZE(detections))); + ASSERT_EQ_INT(3, + target_tracker_snapshot(&tracker, + snapshots, + ARRAY_SIZE(snapshots))); + ASSERT_TRUE(snapshots[0].id != snapshots[1].id); + ASSERT_TRUE(snapshots[1].id != snapshots[2].id); +} + +static void test_tracker_frame_age_removal_and_reacquisition(void) +{ + target_tracker_t tracker; + target_tracker_config_t config = deterministic_tracker_config(); + const rd03d_target_t first = detection(100, 2000, 10, 120U); + target_snapshot_t snapshot; + uint32_t original_id; + + target_tracker_init(&tracker, &config); + ASSERT_EQ_INT(1, target_tracker_update(&tracker, &first, 1U)); + ASSERT_EQ_INT(1, target_tracker_snapshot(&tracker, &snapshot, 1U)); + original_id = snapshot.id; + + ASSERT_EQ_INT(1, target_tracker_update(&tracker, NULL, 0U)); + ASSERT_EQ_INT(1, target_tracker_snapshot(&tracker, &snapshot, 1U)); + ASSERT_EQ_INT(1, snapshot.last_seen_age_frames); + ASSERT_TRUE(snapshot.visible); + ASSERT_TRUE(!snapshot.observed_this_frame); + + ASSERT_EQ_INT(1, target_tracker_update(&tracker, NULL, 0U)); + ASSERT_EQ_INT(1, target_tracker_snapshot(&tracker, &snapshot, 1U)); + ASSERT_EQ_INT(2, snapshot.last_seen_age_frames); + ASSERT_EQ_INT(0, target_tracker_update(&tracker, NULL, 0U)); + ASSERT_EQ_INT(0, target_tracker_snapshot(&tracker, &snapshot, 1U)); + + ASSERT_EQ_INT(1, target_tracker_update(&tracker, &first, 1U)); + ASSERT_EQ_INT(1, target_tracker_snapshot(&tracker, &snapshot, 1U)); + ASSERT_TRUE(snapshot.id != original_id); +} + +static void test_tracker_prediction_preserves_ids_through_crossing(void) +{ + target_tracker_t tracker; + target_tracker_config_t config = deterministic_tracker_config(); + rd03d_target_t detections[2]; + target_snapshot_t snapshots[2]; + uint32_t left_to_right_id; + uint32_t right_to_left_id; + size_t count; + const target_snapshot_t *left_to_right; + const target_snapshot_t *right_to_left; + + config.position_ema_alpha = 1.0F; + target_tracker_init(&tracker, &config); + detections[0] = detection(-600, 2000, 5, 100U); + detections[1] = detection(600, 2000, -5, 100U); + ASSERT_EQ_INT(2, target_tracker_update(&tracker, detections, 2U)); + count = target_tracker_snapshot(&tracker, snapshots, ARRAY_SIZE(snapshots)); + left_to_right_id = snapshots[0].x_mm < 0.0F ? snapshots[0].id : snapshots[1].id; + right_to_left_id = snapshots[0].x_mm > 0.0F ? snapshots[0].id : snapshots[1].id; + + detections[0] = detection(-200, 2000, 5, 100U); + detections[1] = detection(200, 2000, -5, 100U); + ASSERT_EQ_INT(2, target_tracker_update(&tracker, detections, 2U)); + + /* Swap input slots as the two physical trajectories cross. */ + detections[0] = detection(-200, 2000, -5, 100U); + detections[1] = detection(200, 2000, 5, 100U); + ASSERT_EQ_INT(2, target_tracker_update(&tracker, detections, 2U)); + count = target_tracker_snapshot(&tracker, snapshots, ARRAY_SIZE(snapshots)); + left_to_right = find_snapshot(snapshots, count, left_to_right_id); + right_to_left = find_snapshot(snapshots, count, right_to_left_id); + ASSERT_TRUE(left_to_right != NULL && right_to_left != NULL); + ASSERT_NEAR(200.0, left_to_right->x_mm, 0.001); + ASSERT_NEAR(-200.0, right_to_left->x_mm, 0.001); +} + +static void test_tracker_replaces_missing_track_when_capacity_is_full(void) +{ + target_tracker_t tracker; + target_tracker_config_t config = deterministic_tracker_config(); + rd03d_target_t detections[3] = { + {.x_mm = -2000, .y_mm = 2000, .speed_cm_s = 1, + .resolution_mm = 100U, .valid = true}, + {.x_mm = 0, .y_mm = 2000, .speed_cm_s = 2, + .resolution_mm = 100U, .valid = true}, + {.x_mm = 2000, .y_mm = 2000, .speed_cm_s = 3, + .resolution_mm = 100U, .valid = true}, + }; + target_snapshot_t snapshots[3]; + uint32_t departed_id = 0U; + size_t count; + + config.position_ema_alpha = 1.0F; + target_tracker_init(&tracker, &config); + ASSERT_EQ_INT(3, target_tracker_update(&tracker, detections, 3U)); + count = target_tracker_snapshot(&tracker, snapshots, ARRAY_SIZE(snapshots)); + for (size_t i = 0U; i < count; ++i) { + if (snapshots[i].x_mm == 2000.0F) { + departed_id = snapshots[i].id; + } + } + ASSERT_TRUE(departed_id != 0U); + + detections[2] = detection(6000, 2000, 4, 120U); + ASSERT_EQ_INT(3, target_tracker_update(&tracker, detections, 3U)); + count = target_tracker_snapshot(&tracker, snapshots, ARRAY_SIZE(snapshots)); + ASSERT_EQ_INT(3, count); + ASSERT_TRUE(find_snapshot(snapshots, count, departed_id) == NULL); + + bool found_new_target = false; + for (size_t i = 0U; i < count; ++i) { + if (snapshots[i].x_mm == 6000.0F) { + found_new_target = true; + ASSERT_TRUE(snapshots[i].id != departed_id); + } + } + ASSERT_TRUE(found_new_target); +} + +static void test_tracker_invalid_detection_and_reset(void) +{ + target_tracker_t tracker; + const rd03d_target_t detections[] = { + {.x_mm = 100, .y_mm = 200, .valid = false}, + {.x_mm = 300, .y_mm = 400, .speed_cm_s = 5, + .resolution_mm = 10U, .valid = true}, + }; + target_snapshot_t snapshot; + + target_tracker_init(&tracker, NULL); + ASSERT_EQ_INT(1, + target_tracker_update(&tracker, + detections, + ARRAY_SIZE(detections))); + ASSERT_EQ_INT(1, target_tracker_snapshot(&tracker, &snapshot, 1U)); + ASSERT_EQ_INT(1, snapshot.id); + target_tracker_reset(&tracker); + ASSERT_EQ_INT(0, target_tracker_snapshot(&tracker, &snapshot, 1U)); + ASSERT_EQ_INT(1, + target_tracker_update(&tracker, + detections, + ARRAY_SIZE(detections))); + ASSERT_EQ_INT(1, target_tracker_snapshot(&tracker, &snapshot, 1U)); + ASSERT_EQ_INT(1, snapshot.id); +} + +int main(void) +{ + RUN_TEST(test_sign_magnitude_boundaries); + RUN_TEST(test_decode_complete_frame_and_resolution); + RUN_TEST(test_parser_all_split_points); + RUN_TEST(test_parser_noise_corruption_and_resynchronisation); + RUN_TEST(test_parser_recovers_from_dropped_byte_and_embedded_header); + RUN_TEST(test_parser_multiple_frames_per_chunk); + RUN_TEST(test_tracker_stable_ids_slot_reorder_and_ema); + RUN_TEST(test_tracker_does_not_merge_close_targets); + RUN_TEST(test_tracker_frame_age_removal_and_reacquisition); + RUN_TEST(test_tracker_prediction_preserves_ids_through_crossing); + RUN_TEST(test_tracker_replaces_missing_track_when_capacity_is_full); + RUN_TEST(test_tracker_invalid_detection_and_reset); + + (void)printf("1..%u\n", tests_run); + return EXIT_SUCCESS; +}