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