diff --git a/.web_config.json b/.web_config.json index 22ee460..3539b7c 100644 --- a/.web_config.json +++ b/.web_config.json @@ -1,6 +1,6 @@ { - "host_ip": "192.168.0.114", - "kl630_ip": "192.168.0.201", + "host_ip": "192.168.0.105", + "kl630_ip": "192.168.0.123", "port": 8080, "docker_image": "kl630-dev" } \ No newline at end of file diff --git a/README.md b/README.md index 674cc54..1623020 100644 --- a/README.md +++ b/README.md @@ -199,12 +199,580 @@ killall kp_firmware_host_stream --- -## CLI 版本（不需要瀏覽器） +--- + +## Web Server 命令行操作指南 + +若不喜歡 GUI，可透過指令和 API 完全控制 `web_serve.py`，不需要開瀏覽器。 + +### 啟動 Web Server ```bash -python build_and_serve.py # 編譯 + 檢查 + 啟動 HTTP server -python build_and_serve.py --no-build # 跳過編譯，直接 serve -python build_and_serve.py --port 9090 +# 預設 port 8080 +python web_serve.py + +# 指定端口 +python web_serve.py --port 8080 +python web_serve.py --port 9090 +``` + +Server 啟動後可用 `curl` 調用各項功能。 + +### Web Server API 參考 + +#### 1. **查詢/保存設定** + +```bash +# 查看現在設定（Host IP、KL630 IP、HTTP Port、Docker Image） +curl http://localhost:8080/api/config + +# 修改設定 +curl -X POST http://localhost:8080/api/config \ + -H "Content-Type: application/json" \ + -d '{ + "host_ip": "192.168.3.1", + "kl630_ip": "192.168.3.10", + "port": 8080, + "docker_image": "kl630-dev" + }' +``` + +#### 2. **查看 build/ 目錄下載文件列表** + +```bash +curl http://localhost:8080/api/files +``` + +輸出示例： +```json +[ + {"name": "kp_firmware_host_stream", "size": 5242880}, + {"name": "host_stream.ini", "size": 2048}, + {"name": "demo_rtsp.sh", "size": 512}, + {"name": "deploy.sh", "size": 1024} +] +``` + +#### 3. **編譯（Server-Sent Events 串流輸出）** + +```bash +# 啟動編譯，即時輸出編譯 log +curl http://localhost:8080/api/compile/run + +# 搭配 jq 只看 error +curl http://localhost:8080/api/compile/run | grep -o '"kind":"error"' | wc -l +``` + +編譯 log 格式（SSE）： +```json +{"kind": "log", "text": "Starting cross-compile..."} +{"kind": "ok", "text": "Compile SUCCESS"} +{"kind": "error", "text": "Compile FAILED (exit 1)"} +``` + +#### 4. **部署到 KL630（Telnet）** + +```bash +# 執行完整部署流程（停止舊 firmware、下載 binary、啟動新 firmware） +curl http://localhost:8080/api/deploy/run + +# 查看部署 log（最後 100 行）可透過網頁 Terminal 或直接用 tail +``` + +部署流程等同於 Web UI 按下 **Deploy to KL630**。 + +#### 5. **ISP / Flash 一次性設定** + +```bash +# 一次性 ISP setup（DOL-HDR 參數） +curl http://localhost:8080/api/spi_setup/run + +# 或藍牙相關設定 +curl http://localhost:8080/api/bt_setup/run +``` + +#### 6. **INI 設定讀取/寫入** + +```bash +# 讀取目前 INI 設定 +curl http://localhost:8080/api/ini + +# 修改 INI 並立即套用到裝置（等同 Web UI "Apply to Device + Restart"） +curl -X POST http://localhost:8080/api/ini/apply \ + -H "Content-Type: application/json" \ + -d '{ + "fec_mode": 4, + "DrawBoxEnable": 1, + "voc_enable": 0 + }' +``` + +常見 INI 參數： +- `fec_mode`: 0=關閉, 1-5=魚眼校正模式 +- `DrawBoxEnable`: 0/1 — H.264 burn-in 偵測框 +- `voc_enable`: 0/1 — HDMI VOC 輸出 +- `ModelPath`, `ModelId`, `JobId`: 推論模型設定 + +#### 7. **RTSP 串流預覽 / 停止** + +```bash +# 啟動 RTSP 預覽（返回 MJPEG 影像串流） +curl http://localhost:8080/api/stream/video > stream.mjpeg + +# 停止 RTSP 預覽 +curl -X POST http://localhost:8080/api/stream/stop +``` + +#### 8. **STDC 推論統計** + +```bash +# 查看即時推論統計（幀率、語義分割百分比） +curl http://localhost:8080/api/stdc/stats + +# 啟動推論 +curl -X POST http://localhost:8080/api/stdc/start + +# 停止推論 +curl -X POST http://localhost:8080/api/stdc/stop +``` + +輸出示例： +```json +{ + "frame": 42, + "mov": 1, + "diff": 4.2, + "classes": { + "bunker": 0.0, + "car": 8.3, + "grass": 0.0, + "greenery": 12.1, + "person": 0.0, + "pond": 0.0, + "road": 71.4, + "tree": 8.2 + } +} +``` + +#### 9. **遠端執行指令（Telnet 到裝置）** + +```bash +# 在裝置上執行指令並取得輸出 +curl -X POST http://localhost:8080/api/terminal/exec \ + -H "Content-Type: application/json" \ + -d '{"cmd": "ps | grep firmware"}' + +# 查看 firmware log +curl -X POST http://localhost:8080/api/terminal/exec \ + -H "Content-Type: application/json" \ + -d '{"cmd": "cat /tmp/fw.log"}' + +# 查看可用模型 +curl -X POST http://localhost:8080/api/terminal/exec \ + -H "Content-Type: application/json" \ + -d '{"cmd": "ls -lh /mnt/flash/vienna/nef/"}' +``` + +#### 10. **開機自動啟動設定** + +```bash +# 查看自動啟動腳本設定 +curl http://localhost:8080/api/autostart/read + +# 寫入自動啟動腳本（`/etc/init.d/S99firmware`） +curl -X POST http://localhost:8080/api/autostart/write \ + -H "Content-Type: application/json" \ + -d '{"autostart_mode": "rtsp"}' # or "hdmi", "rtsp_hdmi" +``` + +#### 11. **模型管理** + +```bash +# 查看可用模型列表 +curl http://localhost:8080/api/model/list + +# 切換模型（需要指定 ModelId / JobId） +curl -X POST http://localhost:8080/api/model \ + -H "Content-Type: application/json" \ + -d '{ + "model_id": "100001", + "job_id": 1 + }' +``` + +### 完整工作流示例（指令行完全不用 GUI） + +```bash +# 1. 設定網路（Host IP 和 KL630 IP） +curl -X POST http://localhost:8080/api/config \ + -H "Content-Type: application/json" \ + -d '{"host_ip": "192.168.3.1", "kl630_ip": "192.168.3.10"}' + +# 2. 編譯 +echo " Compiling..." +curl http://localhost:8080/api/compile/run | grep '"kind"' | tail -1 + +# 3. 檢查生成的文件 +echo " Build files:" +curl http://localhost:8080/api/files | grep name + +# 4. 部署到裝置 +echo " Deploying to KL630..." +curl http://localhost:8080/api/deploy/run + +# 5. 查看推論統計 +echo " STDC Stats:" +curl http://localhost:8080/api/stdc/stats | python -m json.tool + +# 6. 設定魚眼校正（FEC Mode 4） +curl -X POST http://localhost:8080/api/ini/apply \ + -H "Content-Type: application/json" \ + -d '{"fec_mode": 4}' +``` + +--- + +## CLI 指令完整參考 + +完全不用瀏覽器，所有操作均可透過指令完成。 + +--- + +### 1. 編譯（交叉編譯 ARM binary） + +**方法 A — `build_and_serve.py`（推薦）** + +```bash +# 完整流程：編譯 + 檢查 binary + 啟動 HTTP server +python build_and_serve.py + +# 常用旗標 +python build_and_serve.py --no-build # 跳過編譯，直接 serve（binary 已存在時） +python build_and_serve.py --no-serve # 只編譯 + 檢查，不啟動 server +python build_and_serve.py --port 9090 # 指定 HTTP server port（預設 8080） +python build_and_serve.py --image my-kl630 # 指定 Docker image 名稱 +python build_and_serve.py --no-copy # 不複製到網路資料夾 +python build_and_serve.py --copy-dst /tmp/out # 指定複製目的地 +``` + +**方法 B — 直接 Docker** + +```bash +# 建立 Docker image（只需一次） +docker build -t kl630-dev . + +# 交叉編譯 +docker run --rm \ + -v "$(pwd):/workspace/kl630_build" \ + kl630-dev \ + bash /workspace/kl630_build/compile.sh + +# 確認輸出 +ls -lh build/kp_firmware_host_stream +``` + +--- + +### 2. 啟動 HTTP File Server（裝置 wget 用） + +```bash +# 用 build_and_serve.py（推薦，自動 copy INI + deploy.sh） +python build_and_serve.py --no-build --no-check --port 8080 + +# 或直接 Python 內建 server +cd build && python -m http.server 8080 +``` + +--- + +### 3. 部署到裝置（Telnet / 裝置端指令） + +先確認 HTTP server 在 PC 端已啟動，再 Telnet 進裝置： + +```bash +telnet 192.168.3.10 +``` + +在裝置端執行： + +```sh +# 一鍵下載並部署（最常用） +wget http://192.168.3.1:8080/deploy.sh -O /tmp/deploy.sh && sh /tmp/deploy.sh + +# 或分步驟手動執行： + +# 停止舊 firmware +killall -9 kp_firmware_host_stream 2>/dev/null; killall -9 rtsps 2>/dev/null +sleep 1; rm -f /dev/shm/* + +# 下載 binary 和 INI +wget http://192.168.3.1:8080/kp_firmware_host_stream -O /mnt/flash/vienna/kp_firmware_host_stream +chmod +x /mnt/flash/vienna/kp_firmware_host_stream +wget http://192.168.3.1:8080/host_stream.ini -O /mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin/ini/host_stream.ini + +# 下載並啟動 RTSP demo +wget http://192.168.3.1:8080/demo_rtsp.sh -O /mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin/ini/demo_rtsp.sh +chmod +x /mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin/ini/demo_rtsp.sh +cd /mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin +sh ./ini/demo_rtsp.sh +``` + +**新機器一次性 ISP 設定**（只需跑一次）： + +```sh +wget http://192.168.3.1:8080/deploy.sh -O /tmp/deploy.sh && sh /tmp/deploy.sh --setup +``` + +--- + +### 4. 裝置端常用指令 + +```sh +# 確認 firmware 是否在跑 +ps | grep firmware + +# 查看 firmware log +cat /tmp/fw.log +tail -f /tmp/fw.log # 持續監看（BusyBox 無 -f，用 watch 替代） + +# 查看 RTSP demo log +cat /tmp/rtsp_demo.log + +# 手動停止 firmware +killall -9 kp_firmware_host_stream +killall -9 rtsps + +# 直接啟動（指定模型） +BIN_DIR=/mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin +FW=/mnt/flash/vienna/kp_firmware_host_stream +LD_LIBRARY_PATH=/mnt/flash/vienna/lib $FW \ + -m nef/stdc_scnn_fp.nef \ + -i 100001 -j 1 & + +# 切換輸出模式（不用重新 deploy） +sh $BIN_DIR/ini/demo_rtsp.sh # RTSP 串流 +sh $BIN_DIR/ini/demo_hdmi.sh # HDMI 顯示 +sh $BIN_DIR/ini/demo_rtsp_hdmi.sh # RTSP + HDMI 同時 +``` + +--- + +### 5. INI 設定（指令修改） + +直接用 `sed` 修改裝置上的 INI，不需要重新 deploy： + +```sh +INI=/mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin/ini/host_stream.ini + +# 開啟 / 關閉魚眼校正 FEC +sed -i 's/^fec_mode.*/fec_mode = 4/' $INI # Mode 4（180° Two Direction） +sed -i 's/^fec_mode.*/fec_mode = 0/' $INI # 關閉 + +# 開啟 / 關閉 DrawBox（H.264 burn-in 框） +sed -i 's/^DrawBoxEnable.*/DrawBoxEnable = 1/' $INI +sed -i 's/^DrawBoxEnable.*/DrawBoxEnable = 0/' $INI + +# 開啟 HDMI / VOC 輸出 +sed -i 's/^voc_enable.*/voc_enable = 1/' $INI + +# 查看目前設定 +grep -E "fec_mode|DrawBoxEnable|voc_enable|ModelPath|ModelId" $INI +``` + +修改 INI 後需重啟 firmware 才生效： + +```sh +killall -9 kp_firmware_host_stream 2>/dev/null +sh /mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin/ini/demo_rtsp.sh +``` + +--- + +### 6. Mock Server（事件測試） + +```bash +# 啟動 mock server（port 8081） +python tools/mock_server/server.py + +# 開啟監控網頁 +# http://localhost:8081/ +``` + +用 `curl` 送測試事件（不用開網頁）： + +```bash +# Channel A — 送即時事件（HTTP） +curl -X POST http://localhost:8081/api/event \ + -H "Content-Type: application/json" \ + -d '{"response_type":"violation","content":{"id":"1","date":"2025-01-01T00:00:00+08:00","type":"road","level":2}}' + +# 常用事件類型：road / grass / hazard / person / bunker / pond / tree / car +# level：0=解除, 1=警告, 2=嚴重, 3=緊急 + +# 查詢事件列表 +curl http://localhost:8081/api/events + +# Channel C — 送 CAN 事件（測試 CAN bus TX） +curl -X POST http://localhost:8081/api/can/send \ + -H "Content-Type: application/json" \ + -d '{"type":"hazard","level":1,"can_id":256}' + +# 送油門控制指令（CAN ID 0x75） +curl -X POST http://localhost:8081/api/can/send_cmd \ + -H "Content-Type: application/json" \ + -d '{"cmd":128,"can_id":117}' # cmd: 0=關閉, 32/64/128/255=油門檔位 + +# 查詢 CAN bus 狀態 +curl http://localhost:8081/api/can/status + +# 啟動 CAN interface（等同 UI Bring Up 按鈕） +curl -X POST http://localhost:8081/api/can/bringup \ + -H "Content-Type: application/json" \ + -d '{"channel":"can0","bitrate":250000}' +``` + +--- + +## BLE 指令協議（藍牙通訊） + +KL630 透過 DX-BT24 模組（UART `/dev/ttyS1` @ 115200）與 iPad / 手機 app 溝通。 +所有訊息均為 UTF-8 JSON，無需 CRLF。 + +--- + +### Command（App → KL630） + +#### 狀態確認 +```json +{"command":"status_check"} +``` +回傳 Notify：`response_type=status_check` + +#### 查詢租借狀態 +```json +{"command":"get_rent_status"} +``` +回傳 Notify：`response_type=rent_status` + +#### 租借 +```json +{"command":"rent"} +``` +若目前 status=0 則轉為 1，其他狀態忽略。回傳 Notify：`response_type=rent_status` + +#### 歸還 +```json +{"command":"return"} +``` +若目前 status=1 則轉為 0，其他狀態忽略。回傳 Notify：`response_type=rent_status` + +--- + +### Notify（KL630 → App） + +#### 系統狀態（回應 status_check） +```json +{"response_type":"status_check","content":{"ares_x_version":"1.0.0","bluetooth_peripheral_name":"BT-24","is_intervention_cart_control":false}} +``` + +| 欄位 | 說明 | +|------|------| +| `ares_x_version` | 韌體版本，由 INI `event:ares_version` 設定 | +| `bluetooth_peripheral_name` | BLE 掃描名稱，由 INI `event:bt_name` 設定 | +| `is_intervention_cart_control` | 是否正在介入車輛控制（CAN bus 活動中）| + +#### 租借狀態（回應 rent / return / get_rent_status） +```json +{"response_type":"rent_status","content":{"status":0}} +``` + +| status | 說明 | +|--------|------| +| `0` | 未租借 | +| `1` | 租借中 | +| `2` | 管理模式 | + +#### 違規事件（由推論結果觸發） +```json +{"response_type":"violation","content":{"id":"abcd-1234-0000-0000","date":"2025-12-08T06:08:49Z","type":"lane","level":2}} +``` + +| level | 說明 | +|-------|------| +| `0` | 解除 | +| `1` | 違規發生 | +| `2` | 違規持續 6 秒 | +| `3` | 違規持續 10 秒 | + +#### 危險區域警告 +```json +{"response_type":"alert","content":{"left_level":0,"right_level":1}} +``` +左右各獨立，`0`=無危險，`1`=有危險。 + +--- + +### INI 設定（`ini/host_stream.ini` `[event]` 區段） + +```ini +bt_uart_dev = /dev/ttyS1 # UART 裝置路徑 +bt_at_probe = 0 # 0: 正常啟動；1: 首次設定（9600→115200 升速） +ares_version = 1.0.0 # 韌體版本字串 +bt_name = BT-24 # BLE 掃描名稱 +``` + +--- + +### 測試指令 + +#### 用 BLE app（nRF Connect 等）直接測試 + +複製以下 JSON 貼入 Write Characteristic 欄位送出： + +``` +{"command":"status_check"} +``` +``` +{"command":"get_rent_status"} +``` +``` +{"command":"rent"} +``` +``` +{"command":"return"} +``` + +#### 用 Mock Server 模擬裝置回傳（curl） + +```bash +# 模擬違規事件通知 +curl -X POST http://localhost:8081/api/event \ + -H "Content-Type: application/json" \ + -d '{"response_type":"violation","content":{"id":"test-001","date":"2026-04-22T00:00:00Z","type":"lane","level":1}}' + +# 模擬違規持續 6 秒 +curl -X POST http://localhost:8081/api/event \ + -H "Content-Type: application/json" \ + -d '{"response_type":"violation","content":{"id":"test-001","date":"2026-04-22T00:00:00Z","type":"lane","level":2}}' + +# 模擬違規解除 +curl -X POST http://localhost:8081/api/event \ + -H "Content-Type: application/json" \ + -d '{"response_type":"violation","content":{"id":"test-001","date":"2026-04-22T00:00:00Z","type":"lane","level":0}}' + +# 模擬危險區域警告（右側有危險） +curl -X POST http://localhost:8081/api/event \ + -H "Content-Type: application/json" \ + -d '{"response_type":"alert","content":{"left_level":0,"right_level":1}}' + +# 模擬租借狀態更新 +curl -X POST http://localhost:8081/api/event \ + -H "Content-Type: application/json" \ + -d '{"response_type":"rent_status","content":{"status":1}}' + +# 查看目前所有事件記錄 +curl http://localhost:8081/api/events ``` --- diff --git a/build_and_serve.py b/build_and_serve.py index ca1f927..96fb4c4 100644 --- a/build_and_serve.py +++ b/build_and_serve.py @@ -41,6 +41,25 @@ DEMO_RTSP_SRC = SCRIPT_DIR / "tools" / "device" / "demo_rtsp.sh" # Docker image name — change if yours is different DOCKER_IMAGE = "kl630-dev" +# ── Step clean: Remove build artefacts ─────────────────────────────────────── +def step_clean() -> None: + print(head("\n=== [clean] Clean Build Folder ===")) + removed = 0 + for pattern in ("*.o",): + for f in BUILD_DIR.glob(pattern): + f.unlink() + print(info(f" rm {f.name}")) + removed += 1 + binary = BUILD_DIR / BINARY_NAME + if binary.exists(): + binary.unlink() + print(info(f" rm {BINARY_NAME}")) + removed += 1 + if removed == 0: + print(ok(" Nothing to clean.")) + else: + print(ok(f" Removed {removed} file(s).")) + # ── Step 0: Ensure Docker image exists ─────────────────────────────────────── def step_ensure_image() -> bool: """Build the Docker image from Dockerfile if it doesn't exist yet.""" @@ -252,6 +271,10 @@ def step_serve(port: int): def main(): global DOCKER_IMAGE parser = argparse.ArgumentParser(description="KL630 build + check + serve") + parser.add_argument("--clean", action="store_true", + help="清除 build/ 下的 .o 和 binary，然後結束（不 build）") + parser.add_argument("--clean-build", action="store_true", + help="先 clean 再 build（等同 --clean 後重新執行）") parser.add_argument("--no-build", action="store_true", help="跳過 Docker build，只 check + serve") parser.add_argument("--no-check", action="store_true", @@ -276,7 +299,16 @@ def main(): print(f" Binary : {BINARY_PATH}") print(f" Port : {args.port}") print(f" Copy : {args.copy_dst}") - + + # ===== Clean only ===== + if args.clean: + step_clean() + sys.exit(0) + + # ===== Clean then build ===== + if args.clean_build: + step_clean() + # ===== Step 1: Build ===== if not args.no_build: if not step_ensure_image(): diff --git a/compile.sh b/compile.sh index b262ff6..e53217c 100644 --- a/compile.sh +++ b/compile.sh @@ -18,7 +18,11 @@ mkdir -p $BUILD_DIR CFLAGS="-DVATICS_PLATFORM -DKL630 -D_GNU_SOURCE -U_FORTIFY_SOURCE" CFLAGS="$CFLAGS -march=armv7-a -mfpu=neon -mfloat-abi=hard -Os" CFLAGS="$CFLAGS -Wall -Wno-unused-variable -Wno-unused-function" -#CFLAGS="$CFLAGS -DENABLE_DBG_LOG" + +# CAN bus: Use hardware SPI /dev/spidev1.0 (not GPIO bit-bang) +# Disabled SPI_BITBANG - using native kernel SPI driver instead +# CFLAGS="$CFLAGS -DSPI_BITBANG=1" +# CFLAGS="$CFLAGS -DSPI_GPIO_CS=1 -DSPI_GPIO_MOSI=2 -DSPI_GPIO_SCK=3 -DSPI_GPIO_MISO=4" INCLUDES="-I$WORKSPACE/include/host_stream" INCLUDES="$INCLUDES -I$WORKSPACE/include/app_flow" @@ -66,7 +70,7 @@ fi ALL_SRCS=$(ls \ $WORKSPACE/src/host_stream/*.c \ $WORKSPACE/src/app_flow/*.c \ - $WORKSPACE/src/pre_post_proc/*.c \ + $WORKSPACE/src/pre_post_proc/*.c \ 2>/dev/null) echo "" diff --git a/docs/technical_report.md b/docs/technical_report.md index b023214..c88a502 100644 --- a/docs/technical_report.md +++ b/docs/technical_report.md @@ -12,6 +12,13 @@ 8. [關鍵參數速查表](#8-關鍵參數速查表) 9. [STDC 語義分割邏輯詳解](#9-stdc-語義分割邏輯詳解) 10. [下一階段：高爾夫球車警示與煞車控制](#10-下一階段高爾夫球車警示與煞車控制) +11. [藍牙通訊：DX-BT24 BLE UART 模組](#11-藍牙通訊dx-bt24-ble-uart-模組) +12. [CAN Bus 通訊：MCP2515 SPI 控制器](#12-can-bus-通訊mcp2515-spi-控制器) +13. [事件錄製器（Event Recorder）](#13-事件錄製器event-recorder) +14. [GPIO 直接存取：gpio_devmem](#14-gpio-直接存取gpio_devmem) +15. [STDC 模型重訓練與後處理 Python→C 移植](#15-stdc-模型重訓練與後處理-pythonc-移植) +16. [快照功能：實作 / 事件觸發 / 打包上傳](#16-快照功能實作--事件觸發--打包上傳) +17. [本週進度與下週計畫（2026-04-16）](#17-本週進度與下週計畫2026-04-16) --- @@ -870,16 +877,16 @@ GPIO 煞車訊號 GPIO 警告燈 ← gpio_ctrl.c 新增 馬達控制器 / 電磁煞車 ``` -### 10.3 需新增/修改的檔案 +### 10.3 已完成 / 待完成項目 -| 檔案 | 動作 | 說明 | +| 檔案 | 狀態 | 說明 | |------|------|------| -| `src/host_stream/gpio_ctrl.c` | **新增** | GPIO 初始化、`gpio_set_warning(level)` | -| `src/host_stream/gpio_ctrl.h` | **新增** | 函式宣告、pin 定義 | -| `src/host_stream/app_header_init.c` | **修改** | STDC 結果區塊加 warn_level 計算 + 呼叫 `gpio_set_warning()` | -| `src/host_stream/kp_firmware.c` | **修改** | `main()` 加入 `gpio_ctrl_init()` | -| `ini/host_stream.ini` | **修改** | 新增 `[golf_cart]` 區段，放置 GPIO pin 號與閥值 | -| `src/host_stream/kp_firmware.c` | **修改** | `loadConfig()` 讀取 `[golf_cart]` 參數 | +| `src/host_stream/event_recorder.c` | ✅ **已實作** | 草地狀態機 + 單次碰撞事件 + JPEG 快照 + tar.gz 上傳（見第 13 節） | +| `src/host_stream/can_bus.c` | ✅ **已實作** | CAN 控制指令 `can_bus_send_control_cmd(level)`（見第 12 節） | +| `src/host_stream/gpio_devmem.c` | ✅ **已實作** | /dev/mem GPIO（SPI bit-bang 用，見第 14 節） | +| `src/host_stream/bt_uart.c` | ✅ **已實作** | BLE JSON 事件推送（見第 11 節） | +| 警告燈 / 煞車 relay GPIO | ❌ **待實作** | 需確認板上 GPIO pin 與繼電器接線後實作 `gpio_set_warning(level)` | +| `ini/host_stream.ini [golf_cart]` | ❌ **待實作** | 警告/煞車 GPIO pin 號設定 | ### 10.4 警告等級 → GPIO 輸出對應 @@ -919,5 +926,691 @@ BrakePulseMs = 0 ; 0=持續拉高；>0=觸發後維持 N ms 再釋放 --- -*報告產生日期：2026-03-24* -*適用版本：VMF Vienna SDK 2.5.6，STDC_0520.nef，Firmware Ver. 1.2.0.1203* +--- + +## 11. 藍牙通訊：DX-BT24 BLE UART 模組 + +### 11.1 模組規格 + +| 項目 | 規格 | +|------|------| +| 模組型號 | DX-BT24 | +| 通訊介面 | 透明 UART → BLE（BLE 4.2） | +| BLE 服務 | UUID `0xFFE0`；Notify `FFE1`（裝置→手機）；Write `FFE2`（手機→裝置） | +| UART 預設鮑率 | 9600 bps（出廠）；一次性升級至 115200 bps | +| KL630 UART 腳位 | `/dev/ttyS1`（J15 擴充連接器） | + +### 11.2 功能架構 + +``` +KL630 firmware + event_recorder_update() + │ + ▼ + bt_uart_send_json() ← 非阻塞：放入 FIFO 佇列即返回 + │ + ▼ + [bt_writer_thread] ← 專用執行緒，實際寫入 UART + │ + ▼ + /dev/ttyS1 → DX-BT24 → BLE → iPhone nRF Connect / 球場 iPad App +``` + +**關鍵設計**：`bt_uart_send_json()` 只做 `malloc + enqueue + cond_signal`，永不阻塞推論主迴圈。寫入執行緒在有訊息時才喚醒並寫入 UART。 + +### 11.3 JSON 事件格式 + +每次偵測到違規即送出一個 JSON 字串（無 `\r\n`，BLE 以封包為單位）： + +```json +{"class":"car","level":1} +{"class":"person","level":2} +{"class":"grass","level":1} +{"class":"boot","level":0} +``` + +| 欄位 | 說明 | +|------|------| +| `class` | 違規類型：`boot` / `road` / `grass` / `car` / `person` / `pond` / `bunker` / `tree` / `hazard` | +| `level` | 嚴重等級：0=正常/開機，1=警告，2=緊急 | + +> **注意**：不加 `\r\n`。加了 DX-BT24 會在換行後額外送出一個空的 BLE Notification，手機端看到非 UTF-8 內容。 + +### 11.4 一次性鮑率升級 + +出廠模組預設 9600 bps。升級步驟（僅需做一次）： + +1. 在 `ini/host_stream.ini` 設 `bt_at_probe = 1` +2. 啟動韌體 — `bt_uart_init()` 偵測模組當前鮑率並送出 `AT+BAUD7`（→115200）後 `AT+RESET` +3. 確認 `/tmp/fw.log` 出現 `[BT] upgrade complete` +4. 將 `bt_at_probe` 改回 `0`（避免 AT 指令被 BLE 客戶端看到） + +### 11.5 INI 參數 + +```ini +[event] +enable = 1 +bt_uart_dev = /dev/ttyS1 # UART 裝置節點 +bt_at_probe = 0 # 0: 直接以 115200 開啟（正常使用） + # 1: 執行一次性 AT 鮑率升級（出廠/重置後） +``` + +### 11.6 相關原始碼 + +| 檔案 | 說明 | +|------|------| +| `src/host_stream/bt_uart.c` | UART 驅動：FIFO 佇列 + 專用寫入執行緒 + AT 探測/升級邏輯 | +| `include/host_stream/bt_uart.h` | 公開 API：`bt_uart_init()` / `bt_uart_send_json()` / `bt_uart_close()` | +| `src/host_stream/event_recorder.c` | `fire_json_async()` 呼叫 `bt_uart_send_json()` | +| `src/host_stream/kp_firmware.c` | `loadConfig()` 中讀取 INI 並呼叫 `bt_uart_init()` | + +### 11.7 驗證方法 + +裝置端（韌體啟動後）： + +```sh +# 手動送出 JSON（stty 先設定鮑率） +stty -F /dev/ttyS1 115200 raw cs8 -parenb -cstopb -echo +printf '{"class":"test","level":0}' > /dev/ttyS1 +``` + +iPhone 端：開啟 nRF Connect → 掃描 DX-BT24 → 連線 → 訂閱 FFE1 Notify → 應看到 `{"class":"test","level":0}`（正確 UTF-8 字串）。 + +--- + +## 12. CAN Bus 通訊：MCP2515 SPI 控制器 + +### 12.1 硬體配置 + +| 項目 | 規格 | +|------|------| +| CAN 控制器 | MCP2515 | +| 介面 | SPI via J16 擴充連接器 | +| SPI 裝置節點 | `/dev/spidev1.0` | +| SPI 時脈 | 1 MHz | +| 晶振 | 8 MHz | +| 預設 CAN 速率 | 250 kbps | +| 輸出 CAN Frame ID | 0x100（11-bit Standard Frame，可 INI 設定） | + +J16 接線（依板上標示順序）： + +| J16 腳位 | MCP2515 模組 | +|---|---| +| CS | CS | +| DO | SI (MOSI) | +| CLK | SCK | +| RESERVED | SO (MISO) | + +註：若板上 `RESERVED` 版本實際未接到 `SPI_1_DI_E`，MCP2515 將無法回應。 + +### 12.2 功能架構 + +本週重構為**控制訊號模式（control-frame mode）**，移除了 MsgBroker FIFO 依賴，直接使用 CAN 控制幀進行速度控制： + +``` +KL630 firmware + event_recorder.c: grass_enter_level(level) / collision detection + │ + ▼ + msg_send() → can_bus_send_control_cmd(level) ← 直接呼叫（同步，有 retry） + │ + ▼ + send_one_frame_with_retry() ← 最多 20 次重試 + TX stuck 恢復 + │ + ▼ + mcp2515_sendMessage() ← SPI ioctl 到 /dev/spidev1.0 + │ （或 SPI_BITBANG 模式：gpio_devmem） + ▼ + MCP2515 (J16) → CAN bus → 車體馬達控制器 +``` + +> **重要改動**：`msg_send("setSpeed")` 現在直接呼叫 `can_bus_send_control_cmd()`，不再依賴 `/tmp/canbus/c0/command.fifo` MsgBroker FIFO。JSON 事件僅透過藍牙（Channel A）傳送給 iPad。 + +CAN 控制器包含 keep-alive 機制，每 200ms 自動重發最後的速度指令，確保馬達控制器不會因通訊中斷而失去控制。 + +### 12.3 CAN Frame 格式 + +#### 控制 Frame（主要輸出） + +``` +CAN ID : 0x75 (11-bit, hardcoded in s_ctl_can_id) +DLC : 8 +Data[0] : 速度控制指令（見下表） +Data[1-7]: 0x00 (reserved) +``` + +| Data[0] | 定義常數 | 觸發條件 | 說明 | +|---------|---------|---------|------| +| `10` | `SPEED_LEVEL_0` | 碰撞事件 | 緊急停車（單次觸發） | +| `10` | `SPEED_LEVEL_1` | 草地 L3（T+10s） | 嚴重違規，強制停車 | +| `10` | `SPEED_LEVEL_2` | 草地 L2（T+6s） | 持續違規，中度減速 | +| `10` | `SPEED_LEVEL_3` | 草地 L1 持續 | 持續違規，輕度減速 | +| `10` | `SPEED_LEVEL_4` | 草地 L1 進入 | 初次草地違規，警告 | +| `240` | `SPEED_LEVEL_5` | 正常狀態 / 事件結束 | 恢復正常（全速） | + +> **注意**：目前測試階段所有 SPEED_LEVEL 均設為 10（低速），正常速度為 240。生產環境可依需求調整各等級的速度值。 + +控制指令由 `event_recorder.c` 的草地狀態機和碰撞檢測觸發（詳見第 13 節）。CAN 控制器會每 200ms 自動重發最後的控制指令作為 keep-alive。 + +#### JSON 多幀（保留，目前停用） + +``` +CAN ID : 0x100 (INI can_id，可設定) +DLC : 最多 8 bytes / frame +格式 : JSON 字串分段，每幀 8 bytes，最後一幀以 0x00 補齊 +接收端 : 逐幀拼接直到遇到 null byte +``` + +此模式已保留程式碼但 `can_bus_send_json()` 目前不發送。若日後需要同時傳送 JSON 到 CAN，移除 `can_bus_send_json()` 中的 early return 即可。 + +### 12.4 初始化流程 + +```c +/* can_bus_init 內部步驟 */ +s_dev = new_mcp2515_dev(spidev); // 配置 SPI: 1MHz, 8-bit + // 或 "gpio-bitbang"（SPI_BITBANG 模式） +mcp2515_initial(s_dev); // SPI reset → 進入 CONFIG mode +mcp2515_can_speed(s_dev, CAN_250KBPS, MCP_8MHZ); // 8MHz 晶振 @ 250kbps +mcp2515_setMode(s_dev, CANCTRL_REQOP_NORMAL); // 進入 NORMAL mode + +/* 啟動迴環自測 (loopback self-test) */ +mcp2515_setMode(LOOPBACK) → sendMessage(0x321, 0xA5) → readMessage() +// 成功：log "[CAN] startup loopback self-test OK" +// 失敗：log "[CAN] startup loopback self-test failed (-N)" + +// 啟動 can_writer_thread（佇列消費用，目前主要由 can_bus_send_control_cmd 直接送） +// 啟動 can_keepalive_thread：每 200ms 重發最後控制指令 (預設 SPEED_LEVEL_5 = 240) +// 初始化完成後送出 boot 控制 frame: can_bus_send_control_cmd(SPEED_LEVEL_5) +``` + +### 12.5 TX 錯誤恢復機制 + +當 `mcp2515_sendMessage()` 失敗時觸發分層恢復： + +``` +sendMessage() 回傳 ERROR_ALLTXBUSY 或 ERROR_FAILTX + │ + ▼ +重試最多 20 次（每次 backoff 2ms） + │ + ├── 仍失敗 → 讀取 EFLG / TEC / REC 暫存器 + │ 印出 hint（TX bus-off / TX error-passive / 警告） + │ can_recover_tx_stuck(): + │ CANCTRL_ABAT → 中止掛起的重傳 + │ 清除 TXB0/1/2 TXREQ bit + │ 恢復 NORMAL mode + │ + └── 若 EFLG_TXBO 或 TEC ≥ 128 (bus-off 狀態) + can_reinit_controller_locked(): reset + 重設速率 + NORMAL + can_loopback_selftest_locked(): 確認 SPI 路徑健康 +``` + + +### 12.6 INI 參數 + +```ini +[can] +enable = 1 # 1: 啟用 CAN 輸出；0: 停用 +spidev = /dev/spidev1.0 # SPI 裝置路徑（SPI_BITBANG 模式下忽略） +speed_kbps = 250 # CAN 速率：125 / 250 / 500 / 1000 kbps +can_id = 0x100 # JSON 多幀的 11-bit CAN ID（目前停用） + # 控制 Frame 固定使用 ID 0x75 +``` + +> **本週變更**：`enable` 已從 `0` 改為 `1`（預設開啟）。 + +### 12.7 SPI Bit-bang 模式 + +當 compile.sh 加上 `-DSPI_BITBANG` 時，MCP2515 透過 gpio_devmem 以軟體模擬 SPI： + +``` +/dev/spidev1.0 不可用（被其他 kernel module 佔用）時使用此模式 + +J15 腳位 (GPIOC_0)： + GPIO1 = CS (SPI_GPIO_CS, compile.sh 定義) + GPIO2 = MOSI (SPI_GPIO_MOSI) + GPIO3 = SCK (SPI_GPIO_SCK) + GPIO4 = MISO (SPI_GPIO_MISO) + +底層實作：gpio_devmem_set(pin, 0/1) → /dev/mem → KL630 GPIO_C 0x402E0000 +``` + +詳細暫存器配置見第 14 節。 + +### 12.8 相關原始碼 + +| 檔案 | 說明 | +|------|------| +| `src/host_stream/mcp2515.c` | MCP2515 低層 SPI 驅動 | +| `include/host_stream/mcp2515.h` | MCP2515 暫存器定義、API 宣告 | +| `src/host_stream/can_bus.c` | 高層封裝：`can_bus_send_control_cmd()` + 錯誤恢復 | +| `include/host_stream/can_bus.h` | 公開 API：`can_bus_init()` / `can_bus_send_control_cmd()` / `can_bus_close()` | +| `src/host_stream/gpio_devmem.c` | SPI bit-bang 用的 GPIO /dev/mem 驅動（第 14 節） | +| `src/host_stream/kp_firmware.c` | `loadConfig()` 讀取 `[can]` INI 並呼叫 `can_bus_init()` | + +### 12.9 待確認事項 + +| 項目 | 說明 | +|------|------| +| J16 MISO 接腳 | `RESERVED` 需確認為 `SPI_1_DI_E`，否則 MCP2515 回應讀不回來 | +| 控制 Frame ID 0x75 | 與車體 ECU 協調確認不與其他 node 衝突 | +| 控制指令語意 | cmd=1/2/3 的具體油門百分比或減速邏輯由馬達控制器端定義 | +| 接收端需求 | 目前只實作發送；若需接收回授，在 can_writer_thread 加 mcp2515_readMessage() 輪詢 | +| CAN 速率匹配 | 車體 CAN bus 速率需與 `speed_kbps` 一致，否則全部 Frame 被丟棄 | + +--- + +--- + +## 13. 事件錄製器（Event Recorder） + +`event_recorder.c` 是本週新增的核心模組，負責整合 STDC 分析結果、觸發雙通道事件輸出，並將現場 JPEG 快照打包上傳。 + +### 13.1 雙通道架構 + +``` +STDC 分析結果 (stdc_analysis_t) + │ + ▼ + event_recorder_update() ← 在 recv callback（約 25fps）呼叫 + │ + ┌────┴──────────────────────────────┐ + ▼ ▼ +Channel A (BLE/iPad) Channel B (tar.gz 上傳) + bt_uart_send_json(json) VMF_SNAP → JPEG → event.json → tar.gz + {"class":"lane","level":1} → kCurl POST → /api/golf.cgi + │ │ + DX-BT24 BLE → iPad SD 卡存檔 (最多 7GB) +``` + +另外，草地事件同時觸發 CAN 控制指令（Channel C，第 12 節）： + +``` +grass_enter_level(level) + ├── fire_json_async() → BLE JSON + └── can_bus_send_control_cmd(level) → CAN 馬達控制 +``` + +### 13.2 草地違規狀態機 + +觸發條件：`on_grass == 1 AND is_moving == 1`（靜止停車在草地不觸發） + +``` +GRASS_IDLE ──── on_grass & is_moving ────► GRASS_L1 (T+0s) + │ fire BT level=1, CAN cmd=1, snap level1.jpg + │ + !on_grass 持續 2s ◄─────────────┤─── on_grass & 經過 6s ──► GRASS_L2 (T+6s) + (exit hysteresis) │ │ fire BT level=2 + │ │ │ CAN cmd=2, snap level2.jpg + ▼ │ !on_grass 持續 2s ◄─────────┤─── 經過 10s ──► GRASS_L3 (T+10s) + GRASS_DONE │ │ │ │ fire BT level=3 + fire BT level=0 │ ▼ │ │ CAN cmd=3 + CAN cmd=0 │ GRASS_DONE │ !on_grass 持續 2s + launch_upload() └─────────────────────────────── │ ──────────────► GRASS_DONE + │ + ▼ + upload_thread (detached) + delay → event.json → tar.gz → kCurl POST → SD 清理 + │ + ▼ + GRASS_IDLE +``` + +**退出遲滯（exit hysteresis）**：`GRASS_EXIT_HYSTERESIS_MS = 2000ms`。 +球場上 STDC 偶爾在路邊草地閃爍，2 秒遲滯避免誤以為事件結束。 + +### 13.3 單次碰撞事件（Single-shot） + +以下事件僅在碰撞 ROI 內偵測到，且只觸發一次（Rising edge 0→1）： + +| 類型 | 閾值 | 觸發行為 | +|------|------|---------| +| `person` | `col_person_ratio ≥ THR_PERSON_COLLISION` | BT JSON level=1 + snap + 立即上傳 | +| `bunker` | `col_bunker_ratio ≥ THR_BUNKER_COLLISION` | BT JSON level=1 + snap + 立即上傳 | +| `pond` | `col_pond_ratio ≥ THR_POND_COLLISION` | BT JSON level=1 + snap + 立即上傳 | +| `tree` | `col_tree_ratio ≥ THR_TREE_COLLISION` | BT JSON level=1 + snap + 立即上傳 | + +碰撞事件不觸發 CAN 控制指令（目前設計；日後可加）。 + +### 13.4 JSON 事件格式（BLE 輸出） + +```json +{"class":"lane","level":1} ← 草地 L1 +{"class":"lane","level":2} ← 草地 L2 +{"class":"lane","level":3} ← 草地 L3 +{"class":"lane","level":0} ← 草地結束（恢復正常） +{"class":"person","level":1} ← 行人碰撞 ROI +{"class":"bunker","level":1} ← 沙坑碰撞 ROI +``` + +> **type "lane"**：草地事件統一以 `lane`（超出車道邊界）回報，方便前端 app 分類。 + +### 13.5 JPEG 快照與 tar.gz 打包 + +快照由 `event_recorder_provide_frame()` 在 VMF 影格發送 callback 中執行： + +``` +[主執行緒: send callback] + g_snap_req.active == 1 + → VMF_SNAP_ProcessOneFrame(1920×1080, QP=75) + → save_jpeg("/tmp/ev_/level1.jpg") + → 草地事件：保留，等事件結束後統一打包 + → 碰撞事件：立即 launch_upload() +``` + +tar.gz 結構： + +``` +event__.tar.gz + ├── event.json ← id, date(UTC+8), type, max_level, duration_sec, images[] + ├── level1.jpg ← 草地 L1 快照（若存在） + ├── level2.jpg ← 草地 L2 快照（若存在） + ├── level3.jpg ← 草地 L3 快照（若存在） + └── snapshot.jpg ← 碰撞事件快照 +``` + +SD 卡清理：總大小超過 `sd_max_mb`（預設 7168 MB = 7GB）時，按 mtime 由舊到新刪除。 + +### 13.6 INI 參數 + +```ini +[event] +enable = 1 +bt_uart_dev = /dev/ttyS1 # Channel A: DX-BT24 UART 節點 +bt_at_probe = 0 # 0: 直接 115200; 1: 一次性 AT 升級 +upload_url = http://192.168.0.114:8081/api/golf.cgi # Channel B: tar.gz 上傳端點 + # Production: http://192.168.0.99/api/golf.cgi +sd_path = /tmp/sdcard/events # SD 卡存檔路徑 +sd_max_mb = 7168 # SD 卡容量上限（MB） +upload_delay_ms = 0 # 0: 事件結束後立即打包上傳 +``` + +### 13.7 相關原始碼 + +| 檔案 | 說明 | +|------|------| +| `src/host_stream/event_recorder.c` | 狀態機、快照、打包、上傳全部邏輯 | +| `include/host_stream/event_recorder.h` | 公開 API：`event_recorder_init()` / `event_recorder_update()` / `event_recorder_provide_frame()` | +| `src/host_stream/kp_firmware.c` | `loadConfig()` 讀取 `[event]` INI 並呼叫 `event_recorder_init()` | +| `src/host_stream/kp_firmware.c` | 在 recv callback 呼叫 `event_recorder_update(ana)` | + +--- + +## 14. GPIO 直接存取：gpio_devmem + +### 14.1 背景 + +KL630 J15 擴充連接器的 GPIO 腳位在部分韌體版本中已被 `dh2228fv` display driver 透過 pinmux 佔用，導致 `/sys/class/gpio` 介面無法使用。解法是透過 `/dev/mem` 直接存取實體暫存器，繞過 pinmux 衝突。 + +### 14.2 硬體對應 + +``` +KL630 GPIO_C 基底位址：0x402E0000 + +J15 連接器腳位對應（GPIOC_0 group）： + J15 Pin 1 (CS) → GPIO1 = GPIOC_0_IO_DATA_1 + J15 Pin 2 (MOSI) → GPIO2 = GPIOC_0_IO_DATA_2 + J15 Pin 3 (SCK) → GPIO3 = GPIOC_0_IO_DATA_3 + J15 Pin 4 (MISO) → GPIO4 = GPIOC_0_IO_DATA_4 +``` + +### 14.3 暫存器對應 + +| 偏移 | 暫存器 | 功能 | +|------|--------|------| +| `+0x0000` | `GPIOD_PSR` | Port Status（唯讀，反映實際 pin 狀態） | +| `+0x0004` | `GPIOD_PDDR` | Port Data Direction（bit=1 輸出，bit=0 輸入） | +| `+0x0008` | `GPIOD_PSOR` | Port Set（寫 1 → 該腳拉高，其他 pin 不影響） | +| `+0x000C` | `GPIOD_PCOR` | Port Clear（寫 1 → 該腳拉低，其他 pin 不影響） | +| `+0x0010` | `GPIOD_PTOR` | Port Toggle | +| `+0x0014` | `GPIOD_PIDR` | Port Input Disable | + +### 14.4 API + +```c +int gpio_devmem_init(void); // 開啟 /dev/mem + mmap GPIO_C +void gpio_devmem_cleanup(void); // munmap + close +int gpio_devmem_set_direction(int gpio, int out); // gpio: 1-4，out: 1=輸出 +int gpio_devmem_set(int gpio, int value); // set high/low via PSOR/PCOR +int gpio_devmem_get(int gpio); // 讀 PSR bit（0 或 1） +``` + +### 14.5 使用方式（SPI bit-bang） + +在 `compile.sh` 加入 `-DSPI_BITBANG` 後，`can_bus.c` 改用 gpio_devmem 模擬 SPI 時序： + +```bash +# compile.sh 加入的 flags： +-DSPI_BITBANG +-DSPI_GPIO_CS=1 # J15 Pin1 +-DSPI_GPIO_MOSI=2 # J15 Pin2 +-DSPI_GPIO_SCK=3 # J15 Pin3 +-DSPI_GPIO_MISO=4 # J15 Pin4 +``` + +### 14.6 注意事項 + +- 需要 root 權限（`CAP_SYS_RAWIO`，firmware 本身已以 root 執行） +- mmap 以 page（4096 bytes）為單位，base 已自動對齊，偏移在程式碼中修正 +- 若日後 pinmux 問題解決，可切回 `/dev/spidev1.0` 硬體 SPI（移除 `-DSPI_BITBANG`） + +--- + +--- + +## 15. STDC 模型重訓練與後處理 Python→C 移植 + +### 15.1 重訓練背景 + +原始 STDC 模型以公開道路資料集訓練，對高爾夫球場環境的辨識率不足： + +- 球道（fairway）被誤判為草地（grass）或道路（road） +- 沙坑（bunker）辨識率偏低 +- 球場邊緣樹木與一般道路樹木特徵差異大 + +因此收集球場現場影像，重新標注並 fine-tune 模型，提升球場環境下的分類準確度。 + +> *(訓練資料範例圖)* + +### 15.2 重訓練流程 + +``` +現場採集影像 + │ + ▼ +人工標注（8 類：道路、草地、車輛、行人、沙坑、水塘、植被、樹木） + │ + ▼ +Fine-tune STDC 模型（基於原始權重） + │ + ▼ +Python 推論驗證（stdc630inference.py） + │ + ▼ +Kneron Model Zoo 轉換 → .nef 格式（KL630 NPU 使用） + │ + ▼ +STDC_0520.nef → 部署到裝置 +``` + +> *(訓練前後準確度對比圖)* + +### 15.3 後處理 Python → C 移植 + +原先推論結果的後處理（ROI 計算、類別佔比、警告旗標）全部在 Python（`stdc630inference.py`）中完成，無法在嵌入式端即時執行。 + +**移植目標**：完全在 KL630 上執行，零 Python 依賴。 + +| 項目 | Python 原版 | C 移植版（`stdc_post_process.c`） | +|------|------------|----------------------------------| +| 分割圖解析 | numpy 陣列切片 | 直接 pointer 操作 NPU output buffer | +| 前進 ROI（梯形） | 向量化條件判斷 | `stdc_is_in_forward_roi()` 逐像素計算 | +| 碰撞 ROI | slice 計算 | 固定比例邊界，整數乘法 | +| 動態偵測（is_moving） | frame diff | `stdc_compute_motion_diff()` + `prev_roi_luma` 快取 | +| 草地持續時間計時 | time.time() | `gettimeofday()` + `consecutive_grass_frames` | +| 輸出 | JSON / 終端機 | `stdc_analysis_t` struct → event_recorder | + +**正規化對齊**：C 版輸入正規化設為 `KP_NORMALIZE_KNERON`（= `/256 − 0.5`），與 Python 訓練時的前處理一致，確保推論結果數值相同。 + +> *(Python vs C 輸出比對畫面)* + +### 15.4 相關原始碼 + +| 檔案 | 說明 | +|------|------| +| `src/pre_post_proc/stdc_post_process.c` | 後處理主體：ROI 計算、類別佔比、警告旗標、動態偵測 | +| `src/app_flow/stdc_inf_single_model.c` | NPU 推論配置：正規化、resize、post_proc callback 綁定 | +| `include/stdc_post_process.h` | `stdc_analysis_t` 結構定義、閾值常數 | + +--- + +--- + +## 16. 快照功能：實作 / 事件觸發 / 打包上傳 + +### 16.1 Snapshot function implement + +#### 架構選擇 + +KL630 VMF 提供 `VMF_SNAP`（Video Snapshot Mechanism）模組，可從現有 SSM 影像管線直接擷取 JPEG，無需額外複製 YUV 緩衝區。 + +``` +SSM Ring Buffer (vsrc_ssm_0, 1920×1080) + │ + ▼ + VMF_SNAP_Init() ← Lazy init，首次觸發時才建立（避免閒置佔資源） + opt.dwStreamIdx = 0 + opt.dwQp = 75 ← JPEG 品質 + │ + ▼ + VMF_SNAP_ProcessOneFrame() ← 在 VMF 影格 send callback 呼叫 + │ （必須在 VMF 執行緒內，不可跨執行緒） + ▼ + JPEG buffer (MemBroker, 最大 2MB) + │ + ▼ + save_jpeg("/tmp/ev_/level1.jpg") +``` + +#### 跨執行緒協調（SnapReq 機制） + +VMF_SNAP 只能在 VMF 影格 callback 執行緒中呼叫。事件判斷在 recv callback（不同執行緒）。解法： + +``` +[recv callback 執行緒] [VMF send callback 執行緒] + event 觸發 + → request_snap() → event_recorder_provide_frame() + 寫入 g_snap_req 讀取 g_snap_req.active + g_snap_req.active = 1 若 active → 執行 VMF_SNAP + （mutex 保護） 清除 active + 存檔 +``` + +若上一次快照尚未完成，新請求會被丟棄並 log（避免 race condition）。 + +> *(快照存檔後的 JPEG 範例)* + +--- + +### 16.2 Snapshot trigger by specific event + +快照由兩類事件觸發，時機與邏輯不同： + +#### 草地違規（連續事件） + +每升一個 level 拍一張，事件結束後統一打包： + +``` +GRASS_L1 進入 → snap "level1.jpg"（immediate_upload = 0） +GRASS_L2 進入 → snap "level2.jpg"（immediate_upload = 0） +GRASS_L3 進入 → snap "level3.jpg"（immediate_upload = 0） +事件結束 → launch_upload(images=["level1.jpg", "level2.jpg", ...]) +``` + +#### 碰撞 ROI 單次事件（Rising edge） + +偵測到的瞬間拍照並立即上傳： + +``` +偵測到 person / bunker / pond / tree 進入碰撞 ROI（0→1 上升緣） + → snap "snapshot.jpg"（immediate_upload = 1） + → 快照完成後立即 launch_upload()，delay_ms = 0 +``` + +| 事件類型 | 觸發條件 | 快照檔名 | 上傳時機 | +|---------|---------|---------|---------| +| 草地 L1 | on_grass & is_moving | level1.jpg | 事件結束後 | +| 草地 L2 | 草地持續 ≥ 6s | level2.jpg | 事件結束後 | +| 草地 L3 | 草地持續 ≥ 10s | level3.jpg | 事件結束後 | +| 行人碰撞 | col_person_ratio 0→1 | snapshot.jpg | 立即 | +| 沙坑碰撞 | col_bunker_ratio 0→1 | snapshot.jpg | 立即 | +| 水塘碰撞 | col_pond_ratio 0→1 | snapshot.jpg | 立即 | +| 樹木碰撞 | col_tree_ratio 0→1 | snapshot.jpg | 立即 | + +> *(各事件觸發快照的對比圖)* + +--- + +### 16.3 Zip event snapshots and send to GF cloud + +#### 打包流程 + +事件結束後由獨立 detached thread 執行，不阻塞推論主迴圈： + +``` +upload_thread (detached) + │ + ├─ [可選] sleep upload_delay_ms（目前設 0，立即執行） + │ + ├─ write_event_json("/tmp/ev_/event.json") + │ { + │ "id": "", + │ "date": "2026-04-16T10:30:00+08:00", ← 台灣時間 UTC+8 + │ "type": "grass", + │ "max_level": 2, + │ "duration_sec": 8.3, + │ "images": ["level1.jpg", "level2.jpg"] + │ } + │ + ├─ build_targz() + │ (cd /tmp/ev_ && tar cf - .) | gzip -c > /tmp/sdcard/events/event__.tar.gz + │ sync() ← SD 卡寫入確保 + │ + ├─ http_post_file() + │ kCurl --data-binary @event__.tar.gz + │ 'http://192.168.0.99/api/golf.cgi?filename=event__.tar.gz' + │ + ├─ sd_cleanup() ← 總大小超過 7GB 時刪除最舊的 .tar.gz + │ + └─ rm -rf /tmp/ev_ ← 清除暫存工作目錄 +``` + +#### tar.gz 內容結構 + +``` +event__20260416_103000.tar.gz + ├── event.json ← 事件 metadata + ├── level1.jpg ← 草地 L1 現場快照 + ├── level2.jpg ← 草地 L2 現場快照（若存在） + └── level3.jpg ← 草地 L3 現場快照（若存在） +``` + +#### 上傳端點 + +| 環境 | URL | +|------|-----| +| 開發測試 | `http://192.168.0.114:8081/api/golf.cgi` | +| 生產（GF cloud） | `http://192.168.0.99/api/golf.cgi` | + +透過 INI `upload_url` 切換，無需重新編譯。 + +#### SD 卡管理 + +- 每次上傳後執行 `sd_cleanup()` +- 掃描 `/tmp/sdcard/events/` 下所有 `.tar.gz`，超過 `sd_max_mb`（預設 7168 MB）時按 mtime 由舊到新刪除 +- 即使網路不通，tar.gz 仍保留在 SD 卡 + +> *(GF cloud 後台收到事件的截圖)* + +--- + diff --git a/include/app_flow/pre_post_proc/stdc_post_process.h b/include/app_flow/pre_post_proc/stdc_post_process.h index 2e06732..f71e242 100644 --- a/include/app_flow/pre_post_proc/stdc_post_process.h +++ b/include/app_flow/pre_post_proc/stdc_post_process.h @@ -90,6 +90,27 @@ typedef struct { uint8_t collision_risk; /* 1 if collision risk in center ROI */ uint8_t tree_approaching; /* 1 if tree ROI grows too fast */ uint8_t tree_dense; /* 1 if tree dense area */ + + /* Left ROI (x < 25% of image width) */ + float left_person_ratio; + float left_car_ratio; + float left_tree_ratio; + float left_pond_ratio; + float left_bunker_ratio; + + /* Right ROI (x > 75% of image width) */ + float right_person_ratio; + float right_car_ratio; + float right_tree_ratio; + float right_pond_ratio; + float right_bunker_ratio; + + /* Alert summary */ + uint8_t left_alert; /* 1 if any left class exceeds threshold */ + uint8_t right_alert; /* 1 if any right class exceeds threshold */ + char left_type[16]; /* "person","vehicle","tree","water_hazard","bush","" */ + char right_type[16]; + uint32_t frame_count; /* analyzed frame count */ uint32_t seg_width; /* segmentation output width */ uint32_t seg_height; /* segmentation output height */ diff --git a/include/fake/iniparser/iniparser.h b/include/fake/iniparser/iniparser.h index d49a31a..551c513 100644 --- a/include/fake/iniparser/iniparser.h +++ b/include/fake/iniparser/iniparser.h @@ -22,6 +22,13 @@ const char * iniparser_getstring(dictionary *d, const char *key, const char *def int iniparser_getboolean(dictionary *d, const char *key, int notfound); int iniparser_find_entry(dictionary *d, const char *entry); +/* Write/update one key in the dictionary (section:key format). + * Returns 0 on success, -1 on failure. */ +int iniparser_set(dictionary *d, const char *key, const char *val); + +/* Dump the dictionary back to a file in INI format. */ +void iniparser_dump_ini(const dictionary *d, FILE *f); + #ifdef __cplusplus } #endif diff --git a/include/host_stream/bt_uart.h b/include/host_stream/bt_uart.h index 596cd58..a9d85e7 100644 --- a/include/host_stream/bt_uart.h +++ b/include/host_stream/bt_uart.h @@ -7,26 +7,45 @@ * The DX-BT24 is a UART-transparent BLE module. Whatever is written to the * serial port is forwarded verbatim over BLE to the connected iOS/Android app. * - * Usage (normal operation — module already at 115200): - * bt_uart_init("/dev/ttyS1", 0) — do_at_probe=0: skip AT commands - * bt_uart_send_json(json_str) — every event, thread-safe - * bt_uart_close() — shutdown (optional) + * TX (Notify → app): + * bt_uart_send_json(json_str) — thread-safe, non-blocking queue * - * First-time / factory-reset setup (module at factory default 9600): - * bt_uart_init("/dev/ttyS1", 1) — do_at_probe=1: negotiate baud via AT - * After success, set bt_at_probe = 0 in INI — never probe again. + * RX (Command ← app) — handled automatically by the reader thread: + * {"command": "status_check"} → Notify: response_type=status_check + * {"command": "get_rent_status"} → Notify: response_type=rent_status + * {"command": "rent"} → status 0→1; Notify: rent_status + * {"command": "return"} → status 1→0; Notify: rent_status + * + * Usage: + * bt_uart_init("/dev/ttyS1", 0) + * bt_uart_set_identity("1.0.0", "BT-24") + * bt_uart_set_intervention_cb(my_fn) // optional + * bt_uart_send_json(json_str) + * bt_uart_close() */ -/* Open and configure the UART to the BT module. - * dev: device path, e.g. "/dev/ttyS1" (NULL or "" = disabled). - * do_at_probe: 0 = open directly at 115200 (normal operation, no AT sent). - * 1 = run AT baud negotiation first (one-time setup only). - * Returns 0 on success, -1 on failure / disabled. */ +/* Open UART, start TX writer + RX reader threads. + * do_at_probe=1: one-time baud upgrade 9600→115200 (factory only). */ int bt_uart_init(const char *dev, int do_at_probe); -/* Write JSON bytes to the BT module (no CRLF added). Thread-safe. No-op if not init'd. */ +/* Set firmware version and BT peripheral name broadcast in status_check replies. */ +void bt_uart_set_identity(const char *aresx_version, const char *bt_name); + +/* Write JSON bytes to the BT module. Thread-safe. No-op if not init'd. */ void bt_uart_send_json(const char *json); +/* Rent status: 0=未租借, 1=租借中, 2=管理模式. Thread-safe. */ +int bt_uart_get_rent_status(void); +void bt_uart_set_rent_status(int status); + +/* Optional callback: return 1 if cart control is currently active, else 0. + * Called when building status_check responses. */ +typedef int (*bt_intervention_fn)(void); +void bt_uart_set_intervention_cb(bt_intervention_fn fn); + +/* Reset handshake and notify peer; DX-BT24 is passthrough — no HW disconnect API. */ +void bt_uart_disconnect(void); + /* Close the UART fd and release resources. */ void bt_uart_close(void); diff --git a/include/host_stream/can_bus.h b/include/host_stream/can_bus.h new file mode 100644 index 0000000..8100727 --- /dev/null +++ b/include/host_stream/can_bus.h @@ -0,0 +1,51 @@ +/* + * can_bus.h — MCP2515 CAN bus wrapper for KL630 (J15 SPI connector) + * + * API mirrors bt_uart.h: both channels carry the same JSON payload. + * BLE channel: bt_uart_send_json(json) + * CAN channel: can_bus_send_json(json) + * + * The same JSON string {"class":"car","level":1} is transmitted on both + * channels simultaneously from fire_json_async() in event_recorder.c. + * + * MCP2515 is classic CAN (max 8 bytes per frame). Long JSON strings are + * split into sequential 8-byte frames automatically: + * Frame N: bytes [N*8 .. N*8+7] of the JSON string (padded with 0x00) + * The receiver reassembles by concatenating frames until a null byte. + * If a CAN FD controller is fitted later, switch to single-frame send. + */ + +#ifndef CAN_BUS_H +#define CAN_BUS_H + +#include + +/* + * can_bus_init — open MCP2515 via SPI and start writer thread. + * spidev : e.g. "/dev/spidev1.0" + * can_speed_kbps: 250 (typical) or 125, 500, 1000 + * can_id : 11-bit standard frame ID for outbound frames + * Returns 0 on success, -1 on error. + */ +int can_bus_init(const char *spidev, int can_speed_kbps, uint32_t can_id); + +/* + * can_bus_send_json — non-blocking: enqueue a JSON string for CAN transmission. + * Same signature as bt_uart_send_json(); call both from fire_json_async(). + * Long strings are split into multiple 8-byte CAN frames automatically. + */ +void can_bus_send_json(const char *json); + +/* + * can_bus_send_control_cmd — send one classic-CAN control frame. + * Frame format (compatible with bt_proc.c reference): + * CAN ID : 0x75 (11-bit) + * DLC : 8 + * DATA : [cmd, 0, 0, 0, 0, 0, 0, 0] + */ +void can_bus_send_control_cmd(uint8_t cmd); + +/* can_bus_close — drain queue, join writer thread, close SPI device. */ +void can_bus_close(void); + +#endif /* CAN_BUS_H */ diff --git a/include/host_stream/event_recorder.h b/include/host_stream/event_recorder.h index 2638536..6e189e6 100644 --- a/include/host_stream/event_recorder.h +++ b/include/host_stream/event_recorder.h @@ -35,4 +35,9 @@ void event_recorder_update(const stdc_analysis_t *ana); /* Take VMF_SNAP if one is pending — call from app_header_send_inference. */ void event_recorder_provide_frame(void); +/* Send collision_warning BLE notify（2.2.5） + * level=1 時 type 為觸發物件類型（"person","vehicle","tree","water_hazard","bush"） + * level=0 時 type 傳 NULL */ +void fire_collision_warning(int level, const char *type); + #endif /* EVENT_RECORDER_H */ diff --git a/include/host_stream/gpio_devmem.h b/include/host_stream/gpio_devmem.h new file mode 100644 index 0000000..3af3189 --- /dev/null +++ b/include/host_stream/gpio_devmem.h @@ -0,0 +1,65 @@ +/* + * gpio_devmem.h — Direct GPIO control via /dev/mem for KL630 + * + * Purpose: + * Bypass kernel driver pinmux conflicts by directly accessing GPIO registers + * in physical memory. Used when J15 GPIO pins are already claimed by another + * kernel module (e.g., dh2228fv display driver). + * + * Register Layout (KL630, GPIO_C base = 0x402E0000): + * +0x0000: GPIOD_PSR — Port Status Register (read-only, shows pin states) + * +0x0004: GPIOD_PDDR — Port Data Direction Register (0=input, 1=output) + * +0x0008: GPIOD_PSOR — Port Set Output Register (write 1 to set pin=1) + * +0x000C: GPIOD_PCOR — Port Clear Output Register (write 1 to set pin=0) + * +0x0010: GPIOD_PTOR — Port Toggle Output Register + * +0x0014: GPIOD_PIDR — Port Input Disable Register + * + * Usage: + * gpio_devmem_init(); + * gpio_devmem_set(1, 1); // GPIO1 = high + * gpio_devmem_get(4); // read GPIO4 + * gpio_devmem_cleanup(); + * + * Note: must run as root (CAP_SYS_RAWIO for /dev/mem access). + */ + +#ifndef GPIO_DEVMEM_H +#define GPIO_DEVMEM_H + +#include + +/* + * Initialize GPIO via /dev/mem. + * Returns: 0 on success, -1 on error. + */ +int gpio_devmem_init(void); + +/* + * Cleanup /dev/mem mapping. + */ +void gpio_devmem_cleanup(void); + +/* + * Set GPIO pin direction. + * gpio: 1-4 (J15 pins) + * output: 1 = output, 0 = input + * Returns: 0 on success, -1 on error. + */ +int gpio_devmem_set_direction(int gpio, int output); + +/* + * Set GPIO pin: 1 = high, 0 = low. + * gpio: 1-4 (J15 pins) + * value: 0 or 1 + * Returns: 0 on success, -1 on error. + */ +int gpio_devmem_set(int gpio, int value); + +/* + * Get GPIO pin state: 0 = low, 1 = high. + * gpio: 1-4 (J15 pins) + * Returns: 0, 1, or -1 on error. + */ +int gpio_devmem_get(int gpio); + +#endif /* GPIO_DEVMEM_H */ diff --git a/include/host_stream/mcp2515.h b/include/host_stream/mcp2515.h new file mode 100644 index 0000000..8e50acf --- /dev/null +++ b/include/host_stream/mcp2515.h @@ -0,0 +1,522 @@ +#ifndef _mcp2515_h_ +#define _mcp2515_h_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/***** Register Map *******/ +typedef __u32 canid_t; + +/* special address description flags for the CAN_ID */ +#define CAN_EFF_FLAG 0x80000000UL /* EFF/SFF is set in the MSB */ +#define CAN_RTR_FLAG 0x40000000UL /* remote transmission request */ +#define CAN_ERR_FLAG 0x20000000UL /* error message frame */ + +/* valid bits in CAN ID for frame formats */ +#define CAN_SFF_MASK 0x000007FFUL /* standard frame format (SFF) */ +#define CAN_EFF_MASK 0x1FFFFFFFUL /* extended frame format (EFF) */ +#define CAN_ERR_MASK 0x1FFFFFFFUL /* omit EFF, RTR, ERR flags */ + +#define CAN_MAX_DLEN 8 +/**************** mcp2515 frequency define ********************/ +/* + * Speed 8M + */ + #define MCP_8MHz_1000kBPS_CFG1 (0x00) + #define MCP_8MHz_1000kBPS_CFG2 (0x80) + #define MCP_8MHz_1000kBPS_CFG3 (0x80) + + #define MCP_8MHz_500kBPS_CFG1 (0x00) + #define MCP_8MHz_500kBPS_CFG2 (0x90) + #define MCP_8MHz_500kBPS_CFG3 (0x82) + + #define MCP_8MHz_250kBPS_CFG1 (0x00) + #define MCP_8MHz_250kBPS_CFG2 (0xB1) + #define MCP_8MHz_250kBPS_CFG3 (0x85) + + #define MCP_8MHz_200kBPS_CFG1 (0x00) + #define MCP_8MHz_200kBPS_CFG2 (0xB4) + #define MCP_8MHz_200kBPS_CFG3 (0x86) + + #define MCP_8MHz_125kBPS_CFG1 (0x01) + #define MCP_8MHz_125kBPS_CFG2 (0xB1) + #define MCP_8MHz_125kBPS_CFG3 (0x85) + + #define MCP_8MHz_100kBPS_CFG1 (0x01) + #define MCP_8MHz_100kBPS_CFG2 (0xB4) + #define MCP_8MHz_100kBPS_CFG3 (0x86) + + #define MCP_8MHz_80kBPS_CFG1 (0x01) + #define MCP_8MHz_80kBPS_CFG2 (0xBF) + #define MCP_8MHz_80kBPS_CFG3 (0x87) + + #define MCP_8MHz_50kBPS_CFG1 (0x03) + #define MCP_8MHz_50kBPS_CFG2 (0xB4) + #define MCP_8MHz_50kBPS_CFG3 (0x86) + + #define MCP_8MHz_40kBPS_CFG1 (0x03) + #define MCP_8MHz_40kBPS_CFG2 (0xBF) + #define MCP_8MHz_40kBPS_CFG3 (0x87) + + #define MCP_8MHz_33k3BPS_CFG1 (0x47) + #define MCP_8MHz_33k3BPS_CFG2 (0xE2) + #define MCP_8MHz_33k3BPS_CFG3 (0x85) + + #define MCP_8MHz_31k25BPS_CFG1 (0x07) + #define MCP_8MHz_31k25BPS_CFG2 (0xA4) + #define MCP_8MHz_31k25BPS_CFG3 (0x84) + + #define MCP_8MHz_20kBPS_CFG1 (0x07) + #define MCP_8MHz_20kBPS_CFG2 (0xBF) + #define MCP_8MHz_20kBPS_CFG3 (0x87) + + #define MCP_8MHz_10kBPS_CFG1 (0x0F) + #define MCP_8MHz_10kBPS_CFG2 (0xBF) + #define MCP_8MHz_10kBPS_CFG3 (0x87) + + #define MCP_8MHz_5kBPS_CFG1 (0x1F) + #define MCP_8MHz_5kBPS_CFG2 (0xBF) + #define MCP_8MHz_5kBPS_CFG3 (0x87) + + /* + * speed 16M + */ + #define MCP_16MHz_1000kBPS_CFG1 (0x00) + #define MCP_16MHz_1000kBPS_CFG2 (0xD0) + #define MCP_16MHz_1000kBPS_CFG3 (0x82) + + #define MCP_16MHz_500kBPS_CFG1 (0x00) + #define MCP_16MHz_500kBPS_CFG2 (0xF0) + #define MCP_16MHz_500kBPS_CFG3 (0x86) + + #define MCP_16MHz_250kBPS_CFG1 (0x41) + #define MCP_16MHz_250kBPS_CFG2 (0xF1) + #define MCP_16MHz_250kBPS_CFG3 (0x85) + + #define MCP_16MHz_200kBPS_CFG1 (0x01) + #define MCP_16MHz_200kBPS_CFG2 (0xFA) + #define MCP_16MHz_200kBPS_CFG3 (0x87) + + #define MCP_16MHz_125kBPS_CFG1 (0x03) + #define MCP_16MHz_125kBPS_CFG2 (0xF0) + #define MCP_16MHz_125kBPS_CFG3 (0x86) + + #define MCP_16MHz_100kBPS_CFG1 (0x03) + #define MCP_16MHz_100kBPS_CFG2 (0xFA) + #define MCP_16MHz_100kBPS_CFG3 (0x87) + + #define MCP_16MHz_95kBPS_CFG1 (0x03) + #define MCP_16MHz_95kBPS_CFG2 (0xAD) + #define MCP_16MHz_95kBPS_CFG3 (0x07) + + #define MCP_16MHz_83k3BPS_CFG1 (0x03) + #define MCP_16MHz_83k3BPS_CFG2 (0xBE) + #define MCP_16MHz_83k3BPS_CFG3 (0x07) + + #define MCP_16MHz_80kBPS_CFG1 (0x03) + #define MCP_16MHz_80kBPS_CFG2 (0xFF) + #define MCP_16MHz_80kBPS_CFG3 (0x87) + + #define MCP_16MHz_50kBPS_CFG1 (0x07) + #define MCP_16MHz_50kBPS_CFG2 (0xFA) + #define MCP_16MHz_50kBPS_CFG3 (0x87) + + #define MCP_16MHz_40kBPS_CFG1 (0x07) + #define MCP_16MHz_40kBPS_CFG2 (0xFF) + #define MCP_16MHz_40kBPS_CFG3 (0x87) + + #define MCP_16MHz_33k3BPS_CFG1 (0x4E) + #define MCP_16MHz_33k3BPS_CFG2 (0xF1) + #define MCP_16MHz_33k3BPS_CFG3 (0x85) + + #define MCP_16MHz_20kBPS_CFG1 (0x0F) + #define MCP_16MHz_20kBPS_CFG2 (0xFF) + #define MCP_16MHz_20kBPS_CFG3 (0x87) + + #define MCP_16MHz_10kBPS_CFG1 (0x1F) + #define MCP_16MHz_10kBPS_CFG2 (0xFF) + #define MCP_16MHz_10kBPS_CFG3 (0x87) + + #define MCP_16MHz_5kBPS_CFG1 (0x3F) + #define MCP_16MHz_5kBPS_CFG2 (0xFF) + #define MCP_16MHz_5kBPS_CFG3 (0x87) + + /* + * speed 20M + */ + #define MCP_20MHz_1000kBPS_CFG1 (0x00) + #define MCP_20MHz_1000kBPS_CFG2 (0xD9) + #define MCP_20MHz_1000kBPS_CFG3 (0x82) + + #define MCP_20MHz_500kBPS_CFG1 (0x00) + #define MCP_20MHz_500kBPS_CFG2 (0xFA) + #define MCP_20MHz_500kBPS_CFG3 (0x87) + + #define MCP_20MHz_250kBPS_CFG1 (0x41) + #define MCP_20MHz_250kBPS_CFG2 (0xFB) + #define MCP_20MHz_250kBPS_CFG3 (0x86) + + #define MCP_20MHz_200kBPS_CFG1 (0x01) + #define MCP_20MHz_200kBPS_CFG2 (0xFF) + #define MCP_20MHz_200kBPS_CFG3 (0x87) + + #define MCP_20MHz_125kBPS_CFG1 (0x03) + #define MCP_20MHz_125kBPS_CFG2 (0xFA) + #define MCP_20MHz_125kBPS_CFG3 (0x87) + + #define MCP_20MHz_100kBPS_CFG1 (0x04) + #define MCP_20MHz_100kBPS_CFG2 (0xFA) + #define MCP_20MHz_100kBPS_CFG3 (0x87) + + #define MCP_20MHz_83k3BPS_CFG1 (0x04) + #define MCP_20MHz_83k3BPS_CFG2 (0xFE) + #define MCP_20MHz_83k3BPS_CFG3 (0x87) + + #define MCP_20MHz_80kBPS_CFG1 (0x04) + #define MCP_20MHz_80kBPS_CFG2 (0xFF) + #define MCP_20MHz_80kBPS_CFG3 (0x87) + + #define MCP_20MHz_50kBPS_CFG1 (0x09) + #define MCP_20MHz_50kBPS_CFG2 (0xFA) + #define MCP_20MHz_50kBPS_CFG3 (0x87) + + #define MCP_20MHz_40kBPS_CFG1 (0x09) + #define MCP_20MHz_40kBPS_CFG2 (0xFF) + #define MCP_20MHz_40kBPS_CFG3 (0x87) + + #define MCP_20MHz_33k3BPS_CFG1 (0x0B) + #define MCP_20MHz_33k3BPS_CFG2 (0xFF) + #define MCP_20MHz_33k3BPS_CFG3 (0x87) + + +/**************** mcp2515 flag typedef ********************/ + +typedef enum{ + CANCTRL_REQOP = 0xE0, + CANCTRL_ABAT = 0x10, + CANCTRL_OSM = 0x08, + CANCTRL_CLKEN = 0x04, + CANCTRL_CLKPRE = 0x03 +}canctrl_flags; + +typedef enum { + MCP_20MHZ, + MCP_16MHZ, + MCP_8MHZ +}CAN_CLOCK; + +typedef enum { + CAN_5KBPS, + CAN_10KBPS, + CAN_20KBPS, + CAN_31K25BPS, + CAN_33KBPS, + CAN_40KBPS, + CAN_50KBPS, + CAN_80KBPS, + CAN_83K3BPS, + CAN_95KBPS, + CAN_100KBPS, + CAN_125KBPS, + CAN_200KBPS, + CAN_250KBPS, + CAN_500KBPS, + CAN_1000KBPS +}CAN_SPEED; + +typedef enum { + ERROR_OK = 0, + ERROR_FAIL = 1, + ERROR_ALLTXBUSY = 2, + ERROR_FAILINIT = 3, + ERROR_FAILTX = 4, + ERROR_NOMSG = 5 +}ERROR; + +typedef enum { + MASK0, + MASK1 +}MASK; + +typedef enum { + RXF0 = 0, + RXF1 = 1, + RXF2 = 2, + RXF3 = 3, + RXF4 = 4, + RXF5 = 5 +}RXF; + +typedef enum { + RXB0 = 0, + RXB1 = 1 +}RXBn; + +typedef enum { + TXB0 = 0, + TXB1 = 1, + TXB2 = 2 +}TXBn; + +typedef enum { + CANINTF_RX0IF = 0x01, + CANINTF_RX1IF = 0x02, + CANINTF_TX0IF = 0x04, + CANINTF_TX1IF = 0x08, + CANINTF_TX2IF = 0x10, + CANINTF_ERRIF = 0x20, + CANINTF_WAKIF = 0x40, + CANINTF_MERRF = 0x80 +}CANINTF; + +typedef enum{ + EFLG_RX1OVR = (1<<7), + EFLG_RX0OVR = (1<<6), + EFLG_TXBO = (1<<5), + EFLG_TXEP = (1<<4), + EFLG_RXEP = (1<<3), + EFLG_TXWAR = (1<<2), + EFLG_RXWAR = (1<<1), + EFLG_EWARN = (1<<0) +}EFLG; + +typedef enum{ + INSTRUCTION_WRITE = 0x02, + INSTRUCTION_READ = 0x03, + INSTRUCTION_BITMOD = 0x05, + INSTRUCTION_LOAD_TX0 = 0x40, + INSTRUCTION_LOAD_TX1 = 0x42, + INSTRUCTION_LOAD_TX2 = 0x44, + INSTRUCTION_RTS_TX0 = 0x81, + INSTRUCTION_RTS_TX1 = 0x82, + INSTRUCTION_RTS_TX2 = 0x84, + INSTRUCTION_RTS_ALL = 0x87, + INSTRUCTION_READ_RX0 = 0x90, + INSTRUCTION_READ_RX1 = 0x94, + INSTRUCTION_READ_STATUS = 0xA0, + INSTRUCTION_RX_STATUS = 0xB0, + INSTRUCTION_RESET = 0xC0 +}INSTRUCTION; + +typedef enum { + MCP_RXF0SIDH = 0x00, + MCP_RXF0SIDL = 0x01, + MCP_RXF0EID8 = 0x02, + MCP_RXF0EID0 = 0x03, + MCP_RXF1SIDH = 0x04, + MCP_RXF1SIDL = 0x05, + MCP_RXF1EID8 = 0x06, + MCP_RXF1EID0 = 0x07, + MCP_RXF2SIDH = 0x08, + MCP_RXF2SIDL = 0x09, + MCP_RXF2EID8 = 0x0A, + MCP_RXF2EID0 = 0x0B, + MCP_CANSTAT = 0x0E, + MCP_CANCTRL = 0x0F, + MCP_RXF3SIDH = 0x10, + MCP_RXF3SIDL = 0x11, + MCP_RXF3EID8 = 0x12, + MCP_RXF3EID0 = 0x13, + MCP_RXF4SIDH = 0x14, + MCP_RXF4SIDL = 0x15, + MCP_RXF4EID8 = 0x16, + MCP_RXF4EID0 = 0x17, + MCP_RXF5SIDH = 0x18, + MCP_RXF5SIDL = 0x19, + MCP_RXF5EID8 = 0x1A, + MCP_RXF5EID0 = 0x1B, + MCP_TEC = 0x1C, + MCP_REC = 0x1D, + MCP_RXM0SIDH = 0x20, + MCP_RXM0SIDL = 0x21, + MCP_RXM0EID8 = 0x22, + MCP_RXM0EID0 = 0x23, + MCP_RXM1SIDH = 0x24, + MCP_RXM1SIDL = 0x25, + MCP_RXM1EID8 = 0x26, + MCP_RXM1EID0 = 0x27, + MCP_CNF3 = 0x28, + MCP_CNF2 = 0x29, + MCP_CNF1 = 0x2A, + MCP_CANINTE = 0x2B, + MCP_CANINTF = 0x2C, + MCP_EFLG = 0x2D, + MCP_TXB0CTRL = 0x30, + MCP_TXB0SIDH = 0x31, + MCP_TXB0SIDL = 0x32, + MCP_TXB0EID8 = 0x33, + MCP_TXB0EID0 = 0x34, + MCP_TXB0DLC = 0x35, + MCP_TXB0DATA = 0x36, + MCP_TXB1CTRL = 0x40, + MCP_TXB1SIDH = 0x41, + MCP_TXB1SIDL = 0x42, + MCP_TXB1EID8 = 0x43, + MCP_TXB1EID0 = 0x44, + MCP_TXB1DLC = 0x45, + MCP_TXB1DATA = 0x46, + MCP_TXB2CTRL = 0x50, + MCP_TXB2SIDH = 0x51, + MCP_TXB2SIDL = 0x52, + MCP_TXB2EID8 = 0x53, + MCP_TXB2EID0 = 0x54, + MCP_TXB2DLC = 0x55, + MCP_TXB2DATA = 0x56, + MCP_RXB0CTRL = 0x60, + MCP_RXB0SIDH = 0x61, + MCP_RXB0SIDL = 0x62, + MCP_RXB0EID8 = 0x63, + MCP_RXB0EID0 = 0x64, + MCP_RXB0DLC = 0x65, + MCP_RXB0DATA = 0x66, + MCP_RXB1CTRL = 0x70, + MCP_RXB1SIDH = 0x71, + MCP_RXB1SIDL = 0x72, + MCP_RXB1EID8 = 0x73, + MCP_RXB1EID0 = 0x74, + MCP_RXB1DLC = 0x75, + MCP_RXB1DATA = 0x76 +}REGISTER; + +typedef enum{ + N_TXBUFFERS=3, + N_RXBUFFERS=2 +}buffers_flag; + +typedef enum{ + RXBnCTRL_RXM_STD = 0x20, + RXBnCTRL_RXM_EXT = 0x40, + RXBnCTRL_RXM_STDEXT = 0x00, + RXBnCTRL_RXM_MASK = 0x60, + RXBnCTRL_RTR = 0x08, + RXB0CTRL_BUKT = 0x04, + RXB0CTRL_FILHIT_MASK = 0x03, + RXB1CTRL_FILHIT_MASK = 0x07, + RXB0CTRL_FILHIT = 0x00, + RXB1CTRL_FILHIT = 0x01 +}RXBnCTRLFlag; + +typedef enum { + CANCTRL_REQOP_NORMAL = 0x00, + CANCTRL_REQOP_SLEEP = 0x20, + CANCTRL_REQOP_LOOPBACK = 0x40, + CANCTRL_REQOP_LISTENONLY = 0x60, + CANCTRL_REQOP_CONFIG = 0x80, + CANCTRL_REQOP_POWERUP = 0xE0 +}CANCTRL_REQOP_MODE; + +typedef enum { + TXB_EXIDE_MASK = 0x08, + DLC_MASK = 0x0F, + RTR_MASK = 0x40 +}mcp_mask_flag; + + +typedef enum { + MCP_SIDL = 1, + MCP_SIDH = 0, + MCP_EID8 = 2, + MCP_EID0 = 3, + MCP_DLC = 4, + MCP_DATA = 5, +}mcp_flag; + +typedef enum{ + CANSTAT_OPMOD = 0xE0, + CANSTAT_ICOD = 0x0E +}canstat_flag; + +struct TXBn_REGS { + REGISTER CTRL; + REGISTER SIDH; + REGISTER DATA; +}; + +struct RXBn_REGS { + REGISTER CTRL; + REGISTER SIDH; + REGISTER DATA; + CANINTF CANINTF_RXnIF; +}; + +extern const struct TXBn_REGS TXB[N_TXBUFFERS]; +extern const struct RXBn_REGS RXB[N_RXBUFFERS]; + +typedef enum { + TXB_ABTF = 0x40, + TXB_MLOA = 0x20, + TXB_TXERR = 0x10, + TXB_TXREQ = 0x08, + TXB_TXIE = 0x04, + TXB_TXP = 0x03 +}TXBnCTRL; + +typedef enum { + STAT_RX0IF = (1<<0), + STAT_RX1IF = (1<<1) +}STAT; + +typedef struct mcp2515_dev mcp2515_dev; +typedef struct spi_description spi_description; + +// defined mcp2515 feature +typedef struct mcp2515_dev +{ + spi_description* spi_dev; +}mcp2515_dev; + +// defined spi description +typedef struct spi_description +{ + char *spidev_path; + uint8_t fd; + uint8_t mode; + uint8_t bits; + uint32_t speed; +}spi_description; + +// constructor +mcp2515_dev* new_mcp2515_dev(char *); + +// Open and initial mcp2515 +int mcp2515_initial(mcp2515_dev*); + +uint8_t mcp2515_setMode(mcp2515_dev *mcp2515_device,const CANCTRL_REQOP_MODE mode); +uint8_t mcp2515_setConfigMode(mcp2515_dev *mcp2515_device); +void mcp2515_prepareId(uint8_t *buffer, const uint8_t ext, const uint32_t id); +int mcp2515_setFillter(mcp2515_dev *mcp2515_device,const RXF num, const uint8_t ext, const uint32_t ulData); +int mcp2515_setFilterMask(mcp2515_dev *mcp2515_device,const MASK mask, const uint8_t ext, const uint32_t ulData); +int mcp2515_reset(mcp2515_dev *mcp2515_device); +int mcp2515_can_speed(mcp2515_dev *mcp2515_device, const CAN_SPEED canSpeed, CAN_CLOCK canClock); + +// Send Data to MCP2515 +int mcp2515_write_register(mcp2515_dev*,uint8_t,uint8_t*,int8_t); +int mcp2515_write_register_once(mcp2515_dev *mcp2515_device, uint8_t register_address, uint8_t data); + +// Receive Data from MCP2515 +void mcp2515_read_register(mcp2515_dev *mcp2515_device, uint8_t register_address, uint8_t values[], uint8_t data_length); +uint8_t mcp2515_read_register_once(mcp2515_dev *mcp2515_device, uint8_t register_address); + +// MCP2515 mode setting +int mcp2515_set_mode(mcp2515_dev*); +uint8_t mcp2515_read_status(mcp2515_dev*); + +// mcp2515 bit modify +void mcp2515_modify_bit(mcp2515_dev*, uint8_t, uint8_t, uint8_t); + +// mcp2515 speed select + +int mcp2515_sendMessage(mcp2515_dev *mcp2515_device, uint32_t can_id, uint8_t* data, uint8_t data_length); +int mcp2515_sendMessageT(mcp2515_dev *mcp2515_device, const TXBn txbn, uint32_t can_id, uint8_t* data, uint8_t data_length); + +int mcp2515_readMessage(mcp2515_dev *mcp2515_device,canid_t* can_id, uint8_t* data, uint8_t* data_length); +int mcp2515_readMessageT(mcp2515_dev *mcp2515_device,const RXBn rxbn,canid_t* can_id, uint8_t* data, uint8_t* data_length); + +#endif \ No newline at end of file diff --git a/ini/host_stream.ini b/ini/host_stream.ini index 88be5a6..5ce55ab 100644 --- a/ini/host_stream.ini +++ b/ini/host_stream.ini @@ -9,7 +9,7 @@ initial_fec_app_type = 0 # 0: ceiling, 1: table, 2: wall eis_enable = 1 [nnm] -ModelPath = "nef/STDC04012026_models_630.nef" +ModelPath = "nef/STDC03302026_models_630.nef" ModelId = 32769 # KNERON_YOLOV5S_COCO80_640_640_3 (YOLO v5s) JobId = 200 # KDP2_INF_ID_APP_YOLO InferenceStream = 1 # Inference stream index (use stream1: 640x640) @@ -19,7 +19,7 @@ GetImageBufMode = 0 # 0: block mode 1: non-block mode RoiEnable = 0 # Enable ROI for nnm detect RoiX = 0 # ROI start x RoiY = 0 # ROI start y -DrawBoxEnable = 1 # draw object bounding box on stream0 (also enables STDC seg overlay) +DrawBoxEnable = 0 # draw object bounding box on stream0 (also enables STDC seg overlay) OnlyPerson = 1 # only draw person bounding box when DrawBoxEnable #(so far for yolo only, JobId = 11) DrawOnResize = 0; # If InferenceStream is 0. This setting needs to be enabled. The box will be drawn on all resize streams. @@ -88,12 +88,26 @@ http_url = http://192.168.0.114:8081/api/upload cooldown_ms = 1000 [event] -# Violation event recorder — two-channel: JSON (BT/iPad) + tar.gz (Allxon OOB) +# Violation event recorder ??two-channel: JSON (BT/iPad) + tar.gz (Allxon OOB) enable = 1 -bt_uart_dev = /dev/ttyS1 # Channel A: UART device for DX-BT24 BLE module → iPad +bt_uart_dev = /dev/ttyS1 # Channel A: UART device for DX-BT24 BLE module ??iPad bt_at_probe = 0 # 0: skip AT commands (normal); 1: one-time baud upgrade (factory/reset only) -upload_url = http://192.168.0.114:8081/api/golf.cgi # Channel B: tar.gz upload (OOB/cloud path) +aresx_version = 1.0.0 # Reported in status_check notify (ares_x_version) +bt_name = BT-24 # Reported in status_check notify (bluetooth_peripheral_name) +; device_id: Raw KL630 Kn chip number (e.g., 0xCE7B562C). +; Leave empty to auto-detect on first boot; value is written back automatically. +; BLE broadcast name (event:bt_name) is derived as AresX-{lower 16-bit hex}. +device_id = +upload_url = http://192.168.0.99/api/golf.cgi # Channel B: tar.gz upload (OOB/cloud path) # Production: http://192.168.0.99/api/golf.cgi sd_path = /tmp/sdcard/events # SD card event archive path -sd_max_mb = 7168 # 7 GB — delete oldest when exceeded +sd_max_mb = 7168 # 7 GB ??delete oldest when exceeded upload_delay_ms = 0 # 0: upload immediately after tar.gz built (tar is built after event ends, no extra delay needed) + +[can] +# CAN bus via MCP2515 SPI controller on J16 connector +# J16 pin order: CS -> DO -> CLK -> RESERVED(DI) +enable = 1 # 1: enable CAN bus output; 0: disabled +spidev = /dev/spidev1.0 # SPI device path for MCP2515 +speed_kbps = 250 # CAN bus speed: 125 / 250 / 500 / 1000 kbps +can_id = 0x100 # 11-bit standard frame ID for outbound event frames diff --git a/requirements.txt b/requirements.txt index 2463d3a..32b6c4c 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,2 +1,3 @@ flask>=2.3 opencv-python>=4.8 +python-can>=4.3 diff --git a/src/host_stream/app_header_init.c b/src/host_stream/app_header_init.c index ab51fa3..424752f 100644 --- a/src/host_stream/app_header_init.c +++ b/src/host_stream/app_header_init.c @@ -26,6 +26,9 @@ #define FDFR_MODEL_SIZE_COL 90 #define MAX_RESULT_BOX 80 +#define STDC_ROI_SEGMENTS 12 +#define STDC_ROI_EDGE_THICKNESS 2 + static bool _init_config_yolo_params = false; extern unsigned int g_dwDrawBoxEnable; extern unsigned int g_dwDrawBoxType; @@ -99,10 +102,10 @@ static void print_yolo_result(kp_app_yolo_result_t *yolo_data) { unsigned int i = 0; static unsigned int dwResultCounts = 0; - + if (dwResultCounts > MAX_RESULT_BOX) dwResultCounts = MAX_RESULT_BOX; - +#if 0 if (g_dwDrawBoxEnable) { for (i = 0 ; i < dwResultCounts; i++) { if (g_atDrawInfo[i].bDrawFlag && !g_dwDrawBoxType) @@ -114,7 +117,7 @@ static void print_yolo_result(kp_app_yolo_result_t *yolo_data) g_dwResultCounts = 0; for (i = 0; i < yolo_data->box_count; i++) { if (g_dwOnlyPerson) { - if (yolo_data->boxes[i].class_num != 0) + if (yolo_data->boxes[i].class_num != 0 && yolo_data->boxes[i].class_num != 2) continue; } calculate_bbox_postion(&g_atDrawInfo[i], yolo_data->boxes[i].x1, yolo_data->boxes[i].y1, @@ -130,7 +133,7 @@ static void print_yolo_result(kp_app_yolo_result_t *yolo_data) for (i = 0; i < yolo_data->box_count; i++) { if (g_dwOnlyPerson) { - if (yolo_data->boxes[i].class_num != 0) + if (yolo_data->boxes[i].class_num != 0 && yolo_data->boxes[i].class_num != 2) continue; } @@ -138,6 +141,133 @@ static void print_yolo_result(kp_app_yolo_result_t *yolo_data) yolo_data->box_count, yolo_data->boxes[i].x1, yolo_data->boxes[i].y1, yolo_data->boxes[i].x2, yolo_data->boxes[i].y2, yolo_data->boxes[i].score, yolo_data->boxes[i].class_num); } +#else + /* ── [YOLO] collision_warning（2.2.5）───────────────────────────── + * 偵測到 vehicle(class=2) 時送 BLE notify。 + * 狀態有變化（進入/離開）才送，debounce 2 秒，避免頻繁發送。 + * 將 #else 改為 #if 1 可啟用原有僅列印的邏輯。 + * ---------------------------------------------------------------- */ + { + static int s_cw_level = 0; + static char s_cw_type[16] = ""; + static struct timeval s_cw_last_sent = {0, 0}; + + int cur_level = 0; + char cur_type[16] = ""; + + if (g_dwDrawBoxEnable) { + for (i = 0 ; i < dwResultCounts; i++) { + if (g_atDrawInfo[i].bDrawFlag && !g_dwDrawBoxType) + setup_isp_privacy_mask(0, g_atDrawInfo[i].dwStartX, g_atDrawInfo[i].dwStartY, g_atDrawInfo[i].dwWidth, g_atDrawInfo[i].dwHeight); + } + memset(&g_atDrawInfo, 0, sizeof(g_atDrawInfo)); + dwResultCounts = yolo_data->box_count; + + g_dwResultCounts = 0; + for (i = 0; i < yolo_data->box_count; i++) { + if (g_dwOnlyPerson) { + if (yolo_data->boxes[i].class_num != 0 && yolo_data->boxes[i].class_num != 2) + continue; + } + calculate_bbox_postion(&g_atDrawInfo[i], yolo_data->boxes[i].x1, yolo_data->boxes[i].y1, + yolo_data->boxes[i].x2, yolo_data->boxes[i].y2, yolo_data->boxes[i].score, yolo_data->boxes[i].class_num); + g_atDrawInfo[i].bDrawFlag = true; + if(!g_dwDrawBoxType) { + setup_isp_privacy_mask(1, g_atDrawInfo[i].dwStartX, g_atDrawInfo[i].dwStartY, g_atDrawInfo[i].dwWidth, g_atDrawInfo[i].dwHeight); + } + g_dwResultCounts++; + } + if(g_dwResultCounts > MAX_RESULT_BOX) { g_dwResultCounts = MAX_RESULT_BOX; } + } + + for (i = 0; i < yolo_data->box_count; i++) { + int cls = (int)yolo_data->boxes[i].class_num; + if (cls != 2) continue; /* 只看 vehicle(2) */ + + cur_level = 1; + snprintf(cur_type, sizeof(cur_type), "vehicle"); + break; /* 找到一輛車就夠了 */ + } + + /* 狀態有變化時，檢查 debounce */ + if (cur_level != s_cw_level || + strcmp(cur_type, s_cw_type) != 0) + { + struct timeval now; + gettimeofday(&now, NULL); + + long elapsed_ms = (now.tv_sec - s_cw_last_sent.tv_sec) * 1000 + + (now.tv_usec - s_cw_last_sent.tv_usec) / 1000; + + if (s_cw_last_sent.tv_sec == 0 || elapsed_ms >= 2000) { + fire_collision_warning(cur_level, cur_level ? cur_type : NULL); + s_cw_level = cur_level; + snprintf(s_cw_type, sizeof(s_cw_type), "%s", cur_type); + s_cw_last_sent = now; + printf("[YOLO] collision_warning sent level=%d type=%s\n", + cur_level, cur_level ? cur_type : "null"); + } else { + printf("[YOLO] collision_warning debounce bypass (%ldms < 5000ms)\n", + elapsed_ms); + } + } + } +#endif +} + +static unsigned int stdc_u32_lerp(unsigned int a, unsigned int b, unsigned int i, unsigned int n) +{ + int64_t diff; + int64_t v; + + if (n == 0) + return a; + + diff = (int64_t)b - (int64_t)a; + v = (int64_t)a + (diff * (int64_t)i) / (int64_t)n; + if (v < 0) + return 0; + return (unsigned int)v; +} + +static void stdc_add_rect(DETECT_INFO *di, unsigned int *cnt, + unsigned int x, unsigned int y, + unsigned int w, unsigned int h) +{ + if (*cnt >= MAX_RESULT_BOX || w == 0 || h == 0) + return; + + di[*cnt].dwStartX = x; + di[*cnt].dwStartY = y; + di[*cnt].dwWidth = w; + di[*cnt].dwHeight = h; + di[*cnt].fScore = 0; + di[*cnt].dwClass = 0; + di[*cnt].bDrawFlag = true; + (*cnt)++; +} + +static void stdc_add_trapezoid_outline(DETECT_INFO *di, unsigned int *cnt, + unsigned int x_tl, unsigned int x_tr, + unsigned int x_bl, unsigned int x_br, + unsigned int y_top, unsigned int y_bottom) +{ + /* Top and bottom edges */ + if (x_tr > x_tl) { + stdc_add_rect(di, cnt, x_tl, y_top, x_tr - x_tl, STDC_ROI_EDGE_THICKNESS); + } + if (x_br > x_bl) { + stdc_add_rect(di, cnt, x_bl, y_bottom, x_br - x_bl, STDC_ROI_EDGE_THICKNESS); + } + + /* Left and right slanted edges as sampled points */ + for (unsigned int i = 0; i <= STDC_ROI_SEGMENTS; i++) { + unsigned int y = stdc_u32_lerp(y_top, y_bottom, i, STDC_ROI_SEGMENTS); + unsigned int xl = stdc_u32_lerp(x_tl, x_bl, i, STDC_ROI_SEGMENTS); + unsigned int xr = stdc_u32_lerp(x_tr, x_br, i, STDC_ROI_SEGMENTS); + stdc_add_rect(di, cnt, xl, y, STDC_ROI_EDGE_THICKNESS, STDC_ROI_EDGE_THICKNESS); + stdc_add_rect(di, cnt, xr, y, STDC_ROI_EDGE_THICKNESS, STDC_ROI_EDGE_THICKNESS); + } } /**************************************************************** @@ -214,7 +344,7 @@ int app_header_send_inference(uint32_t buf_addr, bool *bl_run_next_inference, vo app_yolo_header->image_format = kp_image_format; app_yolo_header->model_normalize = KP_NORMALIZE_KNERON;//KP_NORMALIZE_DISABLE; - printf("[SEND] inf_number = %d \n", inf_number); + //printf("[SEND] inf_number = %d \n", inf_number); //memcpy((void *)(buf_addr + sizeof(kdp2_ipc_app_yolo_inf_header_t)), image_buffer, image_buffer_size); ret = dma2d_copy(pInitOpt->ptDmaInfo->ptDmaHandle, pInitOpt->ptDmaInfo->ptDmaDesc, (void *)(buf_addr + sizeof(kdp2_ipc_app_yolo_inf_header_t)), (void*)image_buffer, vsrc_ssm_info); if(ret){ @@ -303,7 +433,7 @@ int app_header_recv_inference(uint32_t buf_addr, bool *bl_run_next_inference) kdp2_ipc_app_yolo_result_t *app_yolo_result = (kdp2_ipc_app_yolo_result_t *)header_stamp; kp_app_yolo_result_t *yolo_data = &app_yolo_result->yolo_data; - printf("[RECV] inf_number = %d \n", app_yolo_result->inf_number); + //printf("[RECV] inf_number = %d \n", app_yolo_result->inf_number); print_yolo_result(yolo_data); /**************************************************************** * finish application - close receive thead @@ -432,12 +562,9 @@ int app_header_recv_inference(uint32_t buf_addr, bool *bl_run_next_inference) * Requires DrawBoxEnable=1 in host_stream.ini / demo_rtsp.sh. * The draw_box thread calls draw_rect() for each entry 0..g_dwResultCounts-1. * - * Layout on 1920×1080 (stream0): - * - * [center] Collision ROI outline x=480 y=270 w=960 h=486 - * (always present; turns into a thick double-border on collision) - * [center] Forward ROI outline x=576 y=594 w=768 h=432 - * (outer bbox of trapezoid: bottom-left 30%, bottom-right 70%) + * Shared ROI trapezoid (same as web overlay polygon): + * points="45,55 55,55 70,95 30,95" + * i.e. top 45%~55%, bottom 30%~70%, y 55%~95%. * * [top-left] Warning boxes 200×55px, stride 63px (when warning active): * y=10 collision_risk y=199 grass_warning @@ -457,27 +584,28 @@ int app_header_recv_inference(uint32_t buf_addr, bool *bl_run_next_inference) if (g_dwDrawBoxEnable) { unsigned int cnt = 0; DETECT_INFO *di = g_atDrawInfo; + const unsigned int draw_w = 1920u; + const unsigned int draw_h = 1080u; - /* Collision ROI outline — always drawn */ - di[cnt].dwStartX = 480; di[cnt].dwStartY = 270; - di[cnt].dwWidth = 960; di[cnt].dwHeight = 486; - di[cnt].fScore = 0; di[cnt].dwClass = 0; di[cnt].bDrawFlag = true; - cnt++; + unsigned int x_tl = (draw_w * 45u) / 100u; + unsigned int x_tr = (draw_w * 55u) / 100u; + unsigned int x_bl = (draw_w * 30u) / 100u; + unsigned int x_br = (draw_w * 70u) / 100u; + unsigned int y_top = (draw_h * 55u) / 100u; + unsigned int y_bottom = (draw_h * 95u) / 100u; - /* When collision active: draw a second inner outline for emphasis */ - if (ana->collision_risk) { - di[cnt].dwStartX = 492; di[cnt].dwStartY = 282; - di[cnt].dwWidth = 936; di[cnt].dwHeight = 462; - di[cnt].fScore = 0; di[cnt].dwClass = 0; di[cnt].bDrawFlag = true; - cnt++; + stdc_add_trapezoid_outline(di, &cnt, + x_tl, x_tr, x_bl, x_br, + y_top, y_bottom); + + /* Collision emphasis: draw a second inner trapezoid when active. */ + if (ana->collision_risk && x_tr > x_tl + 8u && x_br > x_bl + 16u && y_bottom > y_top + 8u) { + stdc_add_trapezoid_outline(di, &cnt, + x_tl + 4u, x_tr - 4u, + x_bl + 8u, x_br - 8u, + y_top + 4u, y_bottom - 4u); } - /* Forward ROI outline — always drawn */ - di[cnt].dwStartX = 576; di[cnt].dwStartY = 594; - di[cnt].dwWidth = 768; di[cnt].dwHeight = 432; - di[cnt].fScore = 0; di[cnt].dwClass = 0; di[cnt].bDrawFlag = true; - cnt++; - /* Side warning/class panels are intentionally disabled. * Keep only central ROI overlays to avoid per-class side boxes. */ diff --git a/src/host_stream/base64.h b/src/host_stream/base64.h new file mode 100644 index 0000000..7b29c77 --- /dev/null +++ b/src/host_stream/base64.h @@ -0,0 +1,96 @@ +/* + * base64.h — Standalone RFC 4648 Base64 encode/decode, header-only (static inline). + * No external dependencies. + */ +#ifndef BASE64_H +#define BASE64_H + +#include +#include +#include + +static const char B64_ENC[] = + "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + +/* + * base64_encode - Encode src_len bytes to a NUL-terminated Base64 string. + * dst must be at least ((src_len + 2) / 3) * 4 + 1 bytes. + */ +static inline void base64_encode(const uint8_t *src, size_t src_len, char *dst) +{ + size_t i = 0, j = 0; + + while (i + 2 < src_len) { + uint32_t v = ((uint32_t)src[i] << 16) + | ((uint32_t)src[i+1] << 8) + | (uint32_t)src[i+2]; + dst[j++] = B64_ENC[(v >> 18) & 0x3F]; + dst[j++] = B64_ENC[(v >> 12) & 0x3F]; + dst[j++] = B64_ENC[(v >> 6) & 0x3F]; + dst[j++] = B64_ENC[ v & 0x3F]; + i += 3; + } + + if (i < src_len) { + uint32_t v = (uint32_t)src[i] << 16; + if (i + 1 < src_len) v |= (uint32_t)src[i+1] << 8; + dst[j++] = B64_ENC[(v >> 18) & 0x3F]; + dst[j++] = B64_ENC[(v >> 12) & 0x3F]; + dst[j++] = (i + 1 < src_len) ? B64_ENC[(v >> 6) & 0x3F] : '='; + dst[j++] = '='; + } + + dst[j] = '\0'; +} + +/* + * base64_decode - Decode a Base64 string to bytes. + * Returns number of decoded bytes, or -1 on invalid input. + * dst must be at least (strlen(src) / 4) * 3 bytes. + */ +static inline int base64_decode(const char *src, uint8_t *dst) +{ + /* Lookup table: 0-63=value, 64='=' padding, -1=invalid */ + static const int8_t LUT[256] = { + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0x00 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0x10 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 62, -1, -1, -1, 63, /* 0x20 */ + 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, -1, -1, -1, 64, -1, -1, /* 0x30 */ + -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, /* 0x40 */ + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, -1, -1, -1, -1, -1, /* 0x50 */ + -1, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, /* 0x60 */ + 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, -1, -1, -1, -1, -1, /* 0x70 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0x80 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0x90 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0xa0 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0xb0 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0xc0 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0xd0 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, /* 0xe0 */ + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 /* 0xf0 */ + }; + + int len = (int)strlen(src); + if (len % 4 != 0) return -1; + + int out = 0; + for (int i = 0; i < len; i += 4) { + int8_t a = LUT[(uint8_t)src[i]]; + int8_t b = LUT[(uint8_t)src[i+1]]; + int8_t c = LUT[(uint8_t)src[i+2]]; + int8_t d = LUT[(uint8_t)src[i+3]]; + if (a < 0 || b < 0 || c < 0 || d < 0) return -1; + + uint32_t v = ((uint32_t)a << 18) + | ((uint32_t)b << 12) + | ((c == 64) ? 0 : ((uint32_t)c << 6)) + | ((d == 64) ? 0 : (uint32_t)d); + + dst[out++] = (uint8_t)(v >> 16); + if (c != 64) dst[out++] = (uint8_t)(v >> 8); + if (d != 64) dst[out++] = (uint8_t)v; + } + return out; +} + +#endif /* BASE64_H */ diff --git a/src/host_stream/bt_uart.c b/src/host_stream/bt_uart.c index 552453b..d240868 100644 --- a/src/host_stream/bt_uart.c +++ b/src/host_stream/bt_uart.c @@ -25,13 +25,15 @@ #include #include #include +#include #include "bt_uart.h" +#include "handshake.h" /* ── UART fd ─────────────────────────────────────────────────────────────── */ static int s_bt_fd = -1; -/* ── Message queue ───────────────────────────────────────────────────────── */ +/* ── Message queue (TX) ──────────────────────────────────────────────────── */ typedef struct BtMsg { char *json; struct BtMsg *next; @@ -42,8 +44,171 @@ static BtMsg *s_q_tail = NULL; static pthread_mutex_t s_q_mtx = PTHREAD_MUTEX_INITIALIZER; static pthread_cond_t s_q_cond = PTHREAD_COND_INITIALIZER; static pthread_t s_writer_tid; +static pthread_t s_reader_tid; static volatile int s_running = 0; +/* ── Identity ────────────────────────────────────────────────────────────── */ +static char s_ares_version[32] = "1.0.0"; +static char s_bt_name[32] = "BT-24"; + +/* ── Rent status: 0=未租借 1=租借中 2=管理模式 ────────────────────────────── */ +static int s_rent_status = 0; +static pthread_mutex_t s_rent_mtx = PTHREAD_MUTEX_INITIALIZER; + +/* ── Cart-control intervention callback ──────────────────────────────────── */ +static bt_intervention_fn s_intervention_cb = NULL; + +/* ── BLE Handshake context ───────────────────────────────────────────────── */ +static handshake_ctx_t s_handshake_ctx; + +/* ── Response builders ───────────────────────────────────────────────────── */ +static void reply_status_check(void) +{ + int intervention = s_intervention_cb ? s_intervention_cb() : 0; + char json[256]; + snprintf(json, sizeof(json), + "{\"response_type\":\"status_check\",\"content\":{" + "\"ares_x_version\":\"%s\"," + "\"bluetooth_peripheral_name\":\"%s\"," + "\"is_intervention_cart_control\":%s}}", + s_ares_version, s_bt_name, + intervention ? "true" : "false"); + bt_uart_send_json(json); + printf("[BT CMD] status_check sent\n"); +} + +static void reply_rent_status(void) +{ + int st; + pthread_mutex_lock(&s_rent_mtx); + st = s_rent_status; + pthread_mutex_unlock(&s_rent_mtx); + char json[80]; + snprintf(json, sizeof(json), + "{\"response_type\":\"rent_status\",\"content\":{\"status\":%d}}", st); + bt_uart_send_json(json); + printf("[BT CMD] rent_status=%d sent\n", st); +} + +/* ── Command handler ─────────────────────────────────────────────────────── */ +static void handle_command(const char *json) +{ + printf("[BT RX] %.128s\n", json); + + /* ── Handshake state machine ─────────────────────────────────────────── */ + handshake_state_t hs_state = handshake_get_state(&s_handshake_ctx); + + if (hs_state == HS_STATE_IDLE) { + /* First message received before init sent challenge — send it now */ + char challenge_json[256]; + if (handshake_build_challenge(&s_handshake_ctx, + challenge_json, sizeof(challenge_json)) == 0) + bt_uart_send_json(challenge_json); + return; /* do not process this message until handshake completes */ + } + + if (hs_state == HS_STATE_WAIT_RESPONSE) { + char confirm_json[256]; + int result = handshake_process_response(&s_handshake_ctx, json); + if (result == 1) { + handshake_build_confirm(&s_handshake_ctx, 1, NULL, + confirm_json, sizeof(confirm_json)); + bt_uart_send_json(confirm_json); + printf("[BT] handshake SUCCESS\n"); + } else { + const char *reason = (result == -2) ? "timeout" + : (result == -1) ? "malformed" + : "invalid_response"; + handshake_build_confirm(&s_handshake_ctx, 0, reason, + confirm_json, sizeof(confirm_json)); + bt_uart_send_json(confirm_json); + printf("[BT] handshake FAILED: %s\n", reason); + bt_uart_disconnect(); + } + return; + } + + if (hs_state == HS_STATE_FAILED) { + printf("[BT] command rejected — handshake failed, waiting for reconnect\n"); + return; + } + + /* ── HS_STATE_SUCCESS: normal command processing ─────────────────────── */ + if (strstr(json, "\"status_check\"")) { + reply_status_check(); + } else if (strstr(json, "\"get_rent_status\"")) { + reply_rent_status(); + } else if (strstr(json, "\"return\"")) { + pthread_mutex_lock(&s_rent_mtx); + if (s_rent_status == 1) s_rent_status = 0; + pthread_mutex_unlock(&s_rent_mtx); + printf("[BT CMD] return processed\n"); + reply_rent_status(); + } else if (strstr(json, "\"rent\"")) { + pthread_mutex_lock(&s_rent_mtx); + if (s_rent_status == 0) s_rent_status = 1; + pthread_mutex_unlock(&s_rent_mtx); + printf("[BT CMD] rent processed\n"); + reply_rent_status(); + } else { + printf("[BT RX] unrecognised command\n"); + } +} + +/* ── Reader thread: accumulates bytes into JSON objects, calls handler ────── */ +static void *bt_reader_thread(void *arg) +{ + (void)arg; + char buf[512]; + int pos = 0, depth = 0; + int in_str = 0, escaped = 0; + + while (s_running) { + if (s_bt_fd < 0) { usleep(100000); continue; } + + fd_set fds; + struct timeval tv = {0, 500000}; /* 500 ms — lets us recheck s_running */ + FD_ZERO(&fds); + FD_SET(s_bt_fd, &fds); + if (select(s_bt_fd + 1, &fds, NULL, NULL, &tv) <= 0) + continue; + + unsigned char c; + if (read(s_bt_fd, &c, 1) != 1) continue; + + /* JSON brace / string-literal state machine */ + if (!in_str) { + if (c == '"') { in_str = 1; } + else if (c == '{') { depth++; } + else if (c == '}') { depth--; } + } else { + if (escaped) { escaped = 0; } + else if (c == '\\') { escaped = 1; } + else if (c == '"') { in_str = 0; } + } + + /* Buffer the character once we are inside a JSON object */ + if (depth > 0 || (depth == 0 && pos > 0)) { + if (pos < (int)sizeof(buf) - 1) + buf[pos++] = (char)c; + } + + /* Complete object received */ + if (depth == 0 && pos > 0) { + buf[pos] = '\0'; + handle_command(buf); + pos = 0; in_str = 0; escaped = 0; + } + + /* Discard overlong garbage */ + if (pos >= (int)sizeof(buf) - 1) { + printf("[BT RX] buffer overflow — discarding\n"); + pos = 0; depth = 0; in_str = 0; escaped = 0; + } + } + return NULL; +} + /* ── Internal: write all bytes to fd ────────────────────────────────────── */ static void uart_write_all(const char *data, size_t len) { @@ -122,6 +287,32 @@ static int open_uart(const char *dev, speed_t baud, int vmin, int vtime) return fd; } +/* ── One-time AT helpers ─────────────────────────────────────────────────── */ + +/** + * bt_set_module_name - Send AT+NAME to the DX-BT24 module. + * @fd: already-open UART fd at 115200 + * @name: BT broadcast name to set (must be non-empty) + * + * The DX-BT24 command format is: AT+NAME\r\n (no space or '=') + */ +static void bt_set_module_name(int fd, const char *name) +{ + if (!name || !*name) { + printf("[BT] WARNING: bt_set_module_name skipped — s_bt_name is empty\n"); + return; + } + char cmd[64]; + char buf[64] = {0}; + int n; + snprintf(cmd, sizeof(cmd), "AT+NAME%s\r\n", name); + write(fd, cmd, strlen(cmd)); + usleep(100000); /* 100 ms — module needs time to process */ + n = read(fd, buf, sizeof(buf) - 1); + buf[n > 0 ? n : 0] = '\0'; + printf("[BT] AT+NAME%s => %d byte(s): %s\n", name, n, n > 0 ? buf : "(none)"); +} + /* ── One-time AT baud setup (call only when bt_at_probe = 1 in INI) ─────── */ static void bt_uart_probe_and_upgrade(const char *dev) { @@ -166,17 +357,18 @@ static void bt_uart_probe_and_upgrade(const char *dev) usleep(100000); /* 100 ms: wait for ack + baud switch */ n = read(fd, buf, sizeof(buf) - 1); printf("[BT] AT+BAUD7 => %d byte(s): %s\n", n, n > 0 ? buf : "(none)"); + bt_set_module_name(fd, s_bt_name); /* set BT broadcast name before reset */ + write(fd, "AT+RESET\r\n", 10); + usleep(100000); + read(fd, buf, sizeof(buf) - 1); + close(fd); + usleep(500000); /* 500 ms: wait for module to reboot */ close(fd); usleep(300000); /* 300 ms: let module stabilise at 115200 */ /* Reopen at 115200 and reset so the new baud persists across power cycles */ fd = open_uart(dev, B115200, 0, 10); if (fd < 0) return; - write(fd, "AT+RESET\r\n", 10); - usleep(100000); - read(fd, buf, sizeof(buf) - 1); - close(fd); - usleep(500000); /* 500 ms: wait for module to reboot */ printf("[BT] upgrade complete — set bt_at_probe = 0 in INI to skip AT on next boot\n"); } @@ -206,7 +398,7 @@ int bt_uart_init(const char *dev, int do_at_probe) s_bt_fd = fd; - /* Start dedicated writer thread */ + /* Start TX writer thread */ s_running = 1; if (pthread_create(&s_writer_tid, NULL, bt_writer_thread, NULL) != 0) { perror("[BT] pthread_create writer"); @@ -217,6 +409,15 @@ int bt_uart_init(const char *dev, int do_at_probe) } pthread_setname_np(s_writer_tid, "bt_writer"); + /* Start RX reader thread */ + if (pthread_create(&s_reader_tid, NULL, bt_reader_thread, NULL) != 0) { + perror("[BT] pthread_create reader"); + /* Non-fatal: TX still works; log and continue */ + s_reader_tid = 0; + } else { + pthread_setname_np(s_reader_tid, "bt_reader"); + } + printf("[BT] bt_uart_init OK: %s @ 115200 (at_probe=%d)\n", dev, do_at_probe); /* Diagnostic ping — confirms UART→BLE channel is alive. @@ -225,6 +426,17 @@ int bt_uart_init(const char *dev, int do_at_probe) bt_uart_send_json("{\"class\":\"boot\",\"level\":0}"); printf("[BT] boot ping queued\n"); + /* Initialise handshake context and send initial challenge to peer */ + handshake_init(&s_handshake_ctx); + { + char challenge_json[256]; + if (handshake_build_challenge(&s_handshake_ctx, + challenge_json, sizeof(challenge_json)) == 0) { + bt_uart_send_json(challenge_json); + printf("[BT] handshake challenge queued\n"); + } + } + return 0; } @@ -249,17 +461,27 @@ void bt_uart_send_json(const char *json) pthread_mutex_unlock(&s_q_mtx); } +void bt_uart_disconnect(void) +{ + /* DX-BT24 is a passthrough UART module — no hardware-level BLE disconnect API. + * Reset handshake so the next peer must re-authenticate, and notify the app. */ + printf("[BT] bt_uart_disconnect: resetting handshake state\n"); + handshake_reset(&s_handshake_ctx); + bt_uart_send_json("{\"response_type\":\"disconnect\",\"content\":{}}"); +} + void bt_uart_close(void) { if (!s_running) return; - /* Signal writer thread to exit after draining remaining messages */ + /* Signal both threads to stop (reader uses select timeout to check s_running) */ pthread_mutex_lock(&s_q_mtx); s_running = 0; pthread_cond_signal(&s_q_cond); pthread_mutex_unlock(&s_q_mtx); pthread_join(s_writer_tid, NULL); + if (s_reader_tid) pthread_join(s_reader_tid, NULL); if (s_bt_fd >= 0) { close(s_bt_fd); @@ -277,3 +499,34 @@ void bt_uart_close(void) } s_q_head = s_q_tail = NULL; } + +/* ── Public: identity + rent + intervention ──────────────────────────────── */ + +void bt_uart_set_identity(const char *aresx_version, const char *bt_name) +{ + if (aresx_version && *aresx_version) + snprintf(s_ares_version, sizeof(s_ares_version), "%s", aresx_version); + if (bt_name && *bt_name) + snprintf(s_bt_name, sizeof(s_bt_name), "%s", bt_name); + printf("[BT] identity: version=%s name=%s\n", s_ares_version, s_bt_name); +} + +int bt_uart_get_rent_status(void) +{ + pthread_mutex_lock(&s_rent_mtx); + int st = s_rent_status; + pthread_mutex_unlock(&s_rent_mtx); + return st; +} + +void bt_uart_set_rent_status(int status) +{ + pthread_mutex_lock(&s_rent_mtx); + s_rent_status = status; + pthread_mutex_unlock(&s_rent_mtx); +} + +void bt_uart_set_intervention_cb(bt_intervention_fn fn) +{ + s_intervention_cb = fn; +} diff --git a/src/host_stream/can_bus.c b/src/host_stream/can_bus.c new file mode 100644 index 0000000..078d187 --- /dev/null +++ b/src/host_stream/can_bus.c @@ -0,0 +1,507 @@ +/* + * can_bus.c -- MCP2515 SPI CAN bus driver wrapper for KL630 + * + * API mirrors bt_uart.c: both channels carry the same JSON payload. + * BLE channel : bt_uart_send_json(json) + * CAN channel : can_bus_send_json(json) + * + * Both are called from fire_json_async() in event_recorder.c so the + * same {"class":"car","level":1} string goes out on both transports. + * + * MCP2515 is classic CAN (8-byte DLC max). Long JSON strings are split + * into sequential 8-byte frames: frame N carries bytes [N*8 .. N*8+7] + * of the JSON, zero-padded on the last frame. The receiver reassembles + * by concatenating until it sees a null byte (0x00 pad byte). + * When a CAN FD controller replaces MCP2515, remove the split loop. + * + * Hardware: MCP2515 CAN controller on J16 SPI connector + * J16 pins : CS / DO / CLK / RESERVED(DI) + * MCP2515 : CS / SI / SCK / SO + * SPI device : /dev/spidev1.0 (1 MHz SPI clock) + * Crystal : 8 MHz + * CAN speed : configurable via INI (default 250 kbps) + */ + +#include +#include +#include +#include +#include + +#include "mcp2515.h" +#include "can_bus.h" + +/* MCP2515 device handle */ +static mcp2515_dev *s_dev = NULL; +static uint32_t s_can_id = 0x100; +static int s_can_speed_kbps = 250; +static uint32_t s_ctl_can_id = 0x75; + +/* SPI mutex (mcp2515 is not thread-safe) */ +static pthread_mutex_t s_spi_mtx = PTHREAD_MUTEX_INITIALIZER; + +/* Keep-alive: resend s_keepalive_cmd every 200 ms so the motor controller + * never loses the current command (cmd=0 = normal speed, 1/2/3 = violation). */ +#define CAN_KEEPALIVE_INTERVAL_MS 200 + +static volatile uint8_t s_keepalive_cmd = 240; /* SPEED_LEVEL_5: send normal speed from startup */ +static pthread_mutex_t s_keepalive_mtx = PTHREAD_MUTEX_INITIALIZER; +static pthread_t s_keepalive_tid; +static volatile int s_keepalive_running = 0; + +/* Message queue -- same design as bt_uart.c */ +typedef struct CanMsg { + char *json; /* heap-allocated JSON string */ + struct CanMsg *next; +} CanMsg; + +static CanMsg *s_q_head = NULL; +static CanMsg *s_q_tail = NULL; +static pthread_mutex_t s_q_mtx = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t s_q_cond = PTHREAD_COND_INITIALIZER; +static pthread_t s_writer_tid; +static volatile int s_running = 0; + +/* Forward declaration */ +static CAN_SPEED kbps_to_enum(int kbps); + +static void can_log_eflg_hints(uint8_t eflg) +{ + if (eflg & EFLG_TXBO) printf("[CAN] hint: TX bus-off (check bitrate/termination/PCAN mode)\n"); + if (eflg & EFLG_TXEP) printf("[CAN] hint: TX error-passive (ACK likely missing or weak bus)\n"); + if (eflg & EFLG_TXWAR) printf("[CAN] hint: TX warning level reached\n"); + if (eflg & EFLG_EWARN) printf("[CAN] hint: error warning flag set\n"); +} + +static void can_recover_tx_stuck(void) +{ + /* Abort pending retransmissions, clear TXREQ on all TX buffers, + * then return to NORMAL mode. */ + mcp2515_modify_bit(s_dev, MCP_CANCTRL, CANCTRL_ABAT, CANCTRL_ABAT); + usleep(1000); + mcp2515_modify_bit(s_dev, MCP_TXB0CTRL, TXB_TXREQ, 0); + mcp2515_modify_bit(s_dev, MCP_TXB1CTRL, TXB_TXREQ, 0); + mcp2515_modify_bit(s_dev, MCP_TXB2CTRL, TXB_TXREQ, 0); + mcp2515_modify_bit(s_dev, MCP_CANCTRL, CANCTRL_ABAT, 0); + (void)mcp2515_setMode(s_dev, CANCTRL_REQOP_NORMAL); +} + +static int can_reinit_controller_locked(void) +{ + if (mcp2515_initial(s_dev) != 0) return -1; + if (mcp2515_can_speed(s_dev, kbps_to_enum(s_can_speed_kbps), MCP_8MHZ) != ERROR_OK) return -2; + if (mcp2515_setMode(s_dev, CANCTRL_REQOP_NORMAL) != ERROR_OK) return -3; + return 0; +} + +static int can_loopback_selftest_locked(void) +{ + canid_t rid = 0; + uint8_t rdata[8] = {0}; + uint8_t rlen = 0; + uint8_t payload = 0xA5; + int i; + + if (mcp2515_setMode(s_dev, CANCTRL_REQOP_LOOPBACK) != ERROR_OK) { + return -1; + } + + if (mcp2515_sendMessage(s_dev, 0x321, &payload, 1) != ERROR_OK) { + (void)mcp2515_setMode(s_dev, CANCTRL_REQOP_NORMAL); + return -2; + } + + for (i = 0; i < 20; i++) { + int rc = mcp2515_readMessage(s_dev, &rid, rdata, &rlen); + if (rc == ERROR_OK) { + (void)mcp2515_setMode(s_dev, CANCTRL_REQOP_NORMAL); + if ((rid & CAN_SFF_MASK) == 0x321 && rlen == 1 && rdata[0] == payload) { + return 0; + } + return -3; + } + usleep(1000); + } + + (void)mcp2515_setMode(s_dev, CANCTRL_REQOP_NORMAL); + return -4; +} + +/* Release MCP2515 device and owned SPI path string. */ +static void free_mcp_device(void) +{ + if (!s_dev) return; + if (s_dev->spi_dev) { + free(s_dev->spi_dev->spidev_path); + free(s_dev->spi_dev); + } + free(s_dev); + s_dev = NULL; +} + +/* Map kbps to MCP2515 CAN_SPEED enum */ +static CAN_SPEED kbps_to_enum(int kbps) +{ + switch (kbps) { + case 1000: return CAN_1000KBPS; + case 500: return CAN_500KBPS; + case 250: return CAN_250KBPS; + case 125: return CAN_125KBPS; + case 100: return CAN_100KBPS; + case 50: return CAN_50KBPS; + default: + printf("[CAN] unknown speed %d kbps, using 250\n", kbps); + return CAN_250KBPS; + } +} + +/* + * Send a JSON string as one or more 8-byte CAN frames. + * Frame layout: up to 8 bytes of JSON content, zero-padded on last frame. + * Receiver: concatenate until null byte encountered. + */ +static void send_json_frames(const char *json) +{ + size_t len = strlen(json); + size_t off = 0; + + while (off <= len) { /* include the null terminator */ + uint8_t frame[8] = {0}; + size_t chunk = (len - off + 1); /* bytes remaining incl. null */ + if (chunk > 8) chunk = 8; + + memcpy(frame, json + off, chunk); /* zero-fills remainder */ + + int rc = ERROR_FAIL; + int tries = 0; + for (tries = 0; tries < 20; tries++) { + pthread_mutex_lock(&s_spi_mtx); + rc = mcp2515_sendMessage(s_dev, s_can_id, frame, (uint8_t)chunk); + pthread_mutex_unlock(&s_spi_mtx); + + if (rc != ERROR_ALLTXBUSY) break; + usleep(2000); /* brief backoff while TX buffers drain */ + } + + if (rc != ERROR_OK) { + if (rc == ERROR_ALLTXBUSY || rc == ERROR_FAILTX) { + uint8_t eflg, tec, rec; + pthread_mutex_lock(&s_spi_mtx); + eflg = mcp2515_read_register_once(s_dev, MCP_EFLG); + tec = mcp2515_read_register_once(s_dev, MCP_TEC); + rec = mcp2515_read_register_once(s_dev, MCP_REC); + can_recover_tx_stuck(); + pthread_mutex_unlock(&s_spi_mtx); + printf("[CAN] sendMessage error %d at offset %zu after %d tries; EFLG=0x%02X TEC=%u REC=%u\n", + rc, off, tries + 1, eflg, tec, rec); + can_log_eflg_hints(eflg); + printf("[CAN] TX recovery: abort/clear TXREQ/re-enter NORMAL done\n"); + if ((eflg & EFLG_TXBO) || tec >= 128) { + int rr; + rr = can_reinit_controller_locked(); + if (rr == 0) { + printf("[CAN] controller reinit OK (reset+speed+normal)\n"); + rr = can_loopback_selftest_locked(); + if (rr == 0) { + printf("[CAN] loopback self-test OK (controller/SPI path healthy)\n"); + } else { + printf("[CAN] loopback self-test failed (%d)\n", rr); + } + } else { + printf("[CAN] controller reinit failed (%d)\n", rr); + } + } + } else { + printf("[CAN] sendMessage error %d at offset %zu\n", rc, off); + } + break; + } + off += chunk; + if (chunk < 8) break; /* last (or only) frame sent */ + } +} + +static int send_one_frame_with_retry(uint32_t can_id, const uint8_t *frame, uint8_t dlc) +{ + int rc = ERROR_FAIL; + int tries; + + for (tries = 0; tries < 20; tries++) { + pthread_mutex_lock(&s_spi_mtx); + rc = mcp2515_sendMessage(s_dev, can_id, (uint8_t *)frame, dlc); + pthread_mutex_unlock(&s_spi_mtx); + + if (rc != ERROR_ALLTXBUSY) break; + usleep(2000); + } + + if (rc == ERROR_OK) return ERROR_OK; + + if (rc == ERROR_ALLTXBUSY || rc == ERROR_FAILTX) { + uint8_t eflg, tec, rec; + pthread_mutex_lock(&s_spi_mtx); + eflg = mcp2515_read_register_once(s_dev, MCP_EFLG); + tec = mcp2515_read_register_once(s_dev, MCP_TEC); + rec = mcp2515_read_register_once(s_dev, MCP_REC); + can_recover_tx_stuck(); + pthread_mutex_unlock(&s_spi_mtx); + + printf("[CAN] sendMessage error %d id=0x%03X after %d tries; EFLG=0x%02X TEC=%u REC=%u\n", + rc, can_id, tries + 1, eflg, tec, rec); + can_log_eflg_hints(eflg); + printf("[CAN] TX recovery: abort/clear TXREQ/re-enter NORMAL done\n"); + + if ((eflg & EFLG_TXBO) || tec >= 128) { + int rr = can_reinit_controller_locked(); + if (rr == 0) { + printf("[CAN] controller reinit OK (reset+speed+normal)\n"); + rr = can_loopback_selftest_locked(); + if (rr == 0) printf("[CAN] loopback self-test OK (controller/SPI path healthy)\n"); + else printf("[CAN] loopback self-test failed (%d)\n", rr); + } else { + printf("[CAN] controller reinit failed (%d)\n", rr); + } + } + } else { + printf("[CAN] sendMessage error %d id=0x%03X\n", rc, can_id); + } + + return rc; +} + +/* Writer thread: drains queue, sends CAN frames */ +static void *can_writer_thread(void *arg) +{ + (void)arg; + while (1) { + pthread_mutex_lock(&s_q_mtx); + while (!s_q_head && s_running) + pthread_cond_wait(&s_q_cond, &s_q_mtx); + + if (!s_running && !s_q_head) { + pthread_mutex_unlock(&s_q_mtx); + break; + } + + CanMsg *msg = s_q_head; + if (msg) { + s_q_head = msg->next; + if (!s_q_head) s_q_tail = NULL; + } + pthread_mutex_unlock(&s_q_mtx); + + if (msg && s_dev) { + send_json_frames(msg->json); + free(msg->json); + free(msg); + } + } + return NULL; +} + +/* Keep-alive thread: sends s_keepalive_cmd every CAN_KEEPALIVE_INTERVAL_MS */ +static void *can_keepalive_thread(void *arg) +{ + (void)arg; + while (s_keepalive_running) { + usleep(CAN_KEEPALIVE_INTERVAL_MS * 1000); + if (!s_keepalive_running) break; + + uint8_t cmd; + pthread_mutex_lock(&s_keepalive_mtx); + cmd = s_keepalive_cmd; + pthread_mutex_unlock(&s_keepalive_mtx); + + if (!s_dev) continue; + + uint8_t frame[8] = {0}; + frame[0] = cmd; + int rc = send_one_frame_with_retry(s_ctl_can_id, frame, 8); + if (rc != ERROR_OK) + printf("[CAN-KA] keepalive tx failed (cmd=%u rc=%d)\n", cmd, rc); + } + return NULL; +} + +/* Public API */ + +int can_bus_init(const char *spidev, int can_speed_kbps, uint32_t can_id) +{ +#ifdef SPI_BITBANG + /* In bitbang mode the spidev path is unused; GPIO pins come from + * compile-time defines SPI_GPIO_CS/MOSI/SCK/MISO set in compile.sh. */ + const char *path_str = "gpio-bitbang"; +#else + if (!spidev || !*spidev) { + printf("[CAN] can_bus_init: no SPI device configured -- CAN disabled\n"); + return -1; + } +#endif + +#ifdef SPI_BITBANG + { + const char *path_str = "gpio-bitbang"; + char *path = strdup(path_str); + if (!path) return -1; + + s_dev = new_mcp2515_dev(path); + if (!s_dev) { + printf("[CAN] new_mcp2515_dev failed\n"); + free(path); + return -1; + } + + if (mcp2515_initial(s_dev) != 0) { + printf("[CAN] mcp2515_initial failed -- check SPI wiring on J15\n"); + free_mcp_device(); + return -1; + } + + if (mcp2515_can_speed(s_dev, kbps_to_enum(can_speed_kbps), MCP_8MHZ) != ERROR_OK) { + printf("[CAN] mcp2515_can_speed failed\n"); + free_mcp_device(); + return -1; + } + + if (mcp2515_setMode(s_dev, CANCTRL_REQOP_NORMAL) != ERROR_OK) { + printf("[CAN] mcp2515_setMode(NORMAL) failed\n"); + free_mcp_device(); + return -1; + } + } +#else + { + char *path = strdup(spidev); + if (!path) return -1; + + s_dev = new_mcp2515_dev(path); + if (!s_dev) { + printf("[CAN] new_mcp2515_dev failed\n"); + free(path); + return -1; + } + + if (mcp2515_initial(s_dev) != 0) { + printf("[CAN] mcp2515_initial failed\n"); + free_mcp_device(); + return -1; + } + + if (mcp2515_can_speed(s_dev, kbps_to_enum(can_speed_kbps), MCP_8MHZ) != ERROR_OK) { + printf("[CAN] mcp2515_can_speed failed\n"); + free_mcp_device(); + return -1; + } + + if (mcp2515_setMode(s_dev, CANCTRL_REQOP_NORMAL) != ERROR_OK) { + printf("[CAN] mcp2515_setMode(NORMAL) failed\n"); + free_mcp_device(); + return -1; + } + } +#endif + + s_can_speed_kbps = can_speed_kbps; + s_can_id = can_id; + + pthread_mutex_lock(&s_spi_mtx); + { + int st = can_loopback_selftest_locked(); + if (st == 0) { + printf("[CAN] startup loopback self-test OK\n"); + } else { + printf("[CAN] startup loopback self-test failed (%d)\n", st); + } + } + pthread_mutex_unlock(&s_spi_mtx); + + s_running = 1; + if (pthread_create(&s_writer_tid, NULL, can_writer_thread, NULL) != 0) { + perror("[CAN] pthread_create writer"); + free_mcp_device(); + s_running = 0; + return -1; + } + pthread_setname_np(s_writer_tid, "can_writer"); + + s_keepalive_running = 1; + if (pthread_create(&s_keepalive_tid, NULL, can_keepalive_thread, NULL) != 0) { + perror("[CAN] pthread_create keepalive"); + s_keepalive_running = 0; + /* non-fatal: keep-alive not running but CAN still works */ + } else { + pthread_setname_np(s_keepalive_tid, "can_keepalive"); + } + + printf("[CAN] can_bus_init OK: %s @ %d kbps, CAN ID=0x%03X\n", + s_dev->spi_dev->spidev_path, can_speed_kbps, can_id); + printf("[CAN] control-frame mode enabled (ID=0x%03X, DLC=8, DATA[0]=cmd)\n", s_ctl_can_id); + printf("[CAN] keep-alive thread started: cmd=%d every %d ms\n", s_keepalive_cmd, CAN_KEEPALIVE_INTERVAL_MS); + + return 0; +} + +void can_bus_send_json(const char *json) +{ + static int s_warn_once = 0; + (void)json; + if (!s_warn_once) { + printf("[CAN] can_bus_send_json ignored (control-frame mode active)\n"); + s_warn_once = 1; + } +} + +void can_bus_send_control_cmd(uint8_t cmd) +{ + uint8_t frame[8] = {0}; + int rc; + + /* Update keep-alive so it maintains this cmd until next change */ + pthread_mutex_lock(&s_keepalive_mtx); + s_keepalive_cmd = cmd; + pthread_mutex_unlock(&s_keepalive_mtx); + + if (!s_dev || !s_running) { + printf("[CAN-CTL] drop: bus not ready (cmd=%u)\n", cmd); + return; + } + + frame[0] = cmd; + rc = send_one_frame_with_retry(s_ctl_can_id, frame, 8); + if (rc == ERROR_OK) { + printf("[CAN-CTL] tx id=0x%03X dlc=8 data=[0x%02X 00 00 00 00 00 00 00]\n", + s_ctl_can_id, cmd); + } else { + printf("[CAN-CTL] tx failed (cmd=%u rc=%d)\n", cmd, rc); + } +} + +void can_bus_close(void) +{ + if (!s_running) return; + + s_keepalive_running = 0; + if (s_keepalive_tid) + pthread_join(s_keepalive_tid, NULL); + + pthread_mutex_lock(&s_q_mtx); + s_running = 0; + pthread_cond_signal(&s_q_cond); + pthread_mutex_unlock(&s_q_mtx); + + pthread_join(s_writer_tid, NULL); + + CanMsg *m = s_q_head; + while (m) { + CanMsg *next = m->next; + free(m->json); + free(m); + m = next; + } + s_q_head = s_q_tail = NULL; + + if (s_dev) { + free_mcp_device(); + } + printf("[CAN] bus closed\n"); +} diff --git a/src/host_stream/event_recorder.c b/src/host_stream/event_recorder.c index 91938de..c8f32ea 100644 --- a/src/host_stream/event_recorder.c +++ b/src/host_stream/event_recorder.c @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -35,20 +36,58 @@ #include #include #include +#include #include "event_recorder.h" #include "bt_uart.h" +#include "can_bus.h" #include "stdc_post_process.h" /* THR_*_COLLISION constants */ /* ── External (from kdp2_host_stream.c) ──────────────────────────────────── */ extern VMF_VSRC_HANDLE_T *g_ptVsrcHandle; +/* ── Speed control via MsgBroker ─────────────────────────────────────────── */ +/* Must be defined before any caller (grass state machine, single-shot). */ +static int msg_send(const char *fifo, + const char *host, + const char *cmd, + const void *data, + unsigned int data_size, + int has_response) +{ + MsgContext tMsgCtx; + + /* Speed control has moved to direct CAN control frames. + * Avoid MsgBroker FIFO dependency (/tmp/canbus/c0/command.fifo). */ + if (cmd && strcmp(cmd, "setSpeed") == 0 && data && data_size >= 1) { + can_bus_send_control_cmd(*(const uint8_t *)data); + return 0; + } + + if (!fifo || !host || !cmd) + return -1; + + memset(&tMsgCtx, 0, sizeof(tMsgCtx)); + + tMsgCtx.bHasResponse = has_response ? 1 : 0; + + tMsgCtx.pszHost = (char *)host; + tMsgCtx.dwHostLen = (unsigned int)strlen(host) + 1; + + tMsgCtx.pszCmd = (char *)cmd; + tMsgCtx.dwCmdLen = (unsigned int)strlen(cmd) + 1; + + tMsgCtx.pbyData = (unsigned char *)data; + tMsgCtx.dwDataSize = data_size; + + return MsgBroker_SendMsg(fifo, &tMsgCtx); +} + /* ── Config ──────────────────────────────────────────────────────────────── */ /* Channel A: JSON events → BT UART → iPad (via DX-BT24 BLE module) * Initialized by bt_uart_init() in kp_firmware.c. No local config needed. */ /* Channel B (tar.gz upload → OOB / cloud path) - * Same endpoint as capture JPG upload (golf.cgi), just posting tar.gz. * Simulation: http://192.168.0.114:8081/api/upload * Production: http://192.168.0.99/api/golf.cgi */ static char s_up_host[64] = "192.168.0.99"; @@ -56,10 +95,21 @@ static int s_up_port = 80; static char s_up_path[128] = "/api/golf.cgi"; static char s_sd_path[256] = "/tmp/sdcard/events"; -static long long s_sd_max_bytes = (long long)7 * 1024 * 1024 * 1024; /* 7 GB — must be long long on 32-bit ARM */ +static long long s_sd_max_bytes = (long long)7 * 1024 * 1024 * 1024; static int s_upload_delay_ms = 60000; static int s_enabled = 0; +/* Speed levels sent via MsgBroker → CAN bus process. + * To tune: adjust the numbers here only — no logic changes needed. + * SPEED_LEVEL_5 (240) is the normal/idle speed; CAN keepalive sends this every 200ms. + * Levels 0~4 are all 10 during testing; restore graduated values for production. */ +#define SPEED_LEVEL_0 10 /* collision single-shot — testing: set to 10 */ +#define SPEED_LEVEL_1 10 /* grass L3 (max severity) — testing: set to 10 */ +#define SPEED_LEVEL_2 10 /* grass L2 — testing: set to 10 */ +#define SPEED_LEVEL_3 10 /* grass L1 — testing: set to 10 */ +#define SPEED_LEVEL_4 10 /* grass L1 entry — testing: set to 10 */ +#define SPEED_LEVEL_5 240 /* normal / grass cleared (full speed) */ + /* ── VMF SNAP ────────────────────────────────────────────────────────────── */ #define SNAP_BUF_SIZE (2 * 1024 * 1024) #define SNAP_QP 75 @@ -107,15 +157,13 @@ typedef struct { static SnapReq g_snap_req; static pthread_mutex_t g_snap_mtx = PTHREAD_MUTEX_INITIALIZER; -/* ── Single-shot debounce ────────────────────────────────────────────────── */ -static int g_last_person = 0; -static int g_last_bunker = 0; -static int g_last_pond = 0; -static int g_last_tree = 0; +/* ── Collision / alert debounce state ───────────────────────────────────── */ +static int g_any_col_last = 0; /* 1 if any collision was active last frame */ +static int g_last_left_alert = 0; +static int g_last_right_alert = 0; +static int g_last_collision_warning = 0; -/* ═══════════════════════════════════════════════════════════════════════════ - * URL / network helpers (Channel B only) - * ═══════════════════════════════════════════════════════════════════════════ */ +/* *URL / network helpers (Channel B only)**/ static void parse_url_into(const char *url, char *host, size_t host_n, @@ -123,7 +171,6 @@ static void parse_url_into(const char *url, char *path, size_t path_n) { const char *p = url; - /* Default to HTTP port 80 when URL does not include :port */ if (port) *port = 80; if (strncmp(p, "http://", 7) == 0) p += 7; const char *slash = strchr(p, '/'); @@ -163,8 +210,7 @@ static int open_socket_to(const char *host, int port) return sock; } -/* ── Channel B: POST tar.gz via kCurl (same method as golf.cgi JPEG upload) ── */ -/* kCurl lives in the firmware bin directory — use absolute path so CWD doesn't matter */ +/* ── Channel B: POST tar.gz via kCurl ───────────────────────────────────── */ #define KCURL_PATH "/mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin/kCurl" static void http_post_file(const char *filepath) @@ -172,8 +218,6 @@ static void http_post_file(const char *filepath) const char *basename = strrchr(filepath, '/'); basename = basename ? basename + 1 : filepath; - /* Pass filename as query param so the server can name the file correctly. - * Format mirrors golf.cgi usage: kCurl --data-binary @file URL */ char url[320]; snprintf(url, sizeof(url), "http://%s:%d%s?filename=%s", s_up_host, s_up_port, s_up_path, basename); @@ -195,10 +239,8 @@ static void http_post_file(const char *filepath) * Helpers * ═══════════════════════════════════════════════════════════════════════════ */ -/* UTC+8 offset for Taiwan time (TZ env may not be set on embedded device) */ #define TZ_OFFSET_SEC (8 * 3600) -/* UTC time with Z suffix — reserved for future use */ __attribute__((unused)) static void now_iso_utc(char *buf, size_t n) { @@ -208,7 +250,6 @@ static void now_iso_utc(char *buf, size_t n) strftime(buf, n, "%Y-%m-%dT%H:%M:%SZ", t); } -/* Taiwan time (UTC+8) — used in tar.gz filenames and event.json for local readability */ static void now_iso(char *buf, size_t n) { struct timeval tv; @@ -232,7 +273,6 @@ static void make_work_dir(const char *event_id, char *out, size_t n) mkdir(out, 0755); } -/* ── Write event.json into work_dir ─────────────────────────────────────── */ static void write_event_json(const char *work_dir, const char *event_id, const char *type, int max_level, float duration_sec, @@ -260,7 +300,6 @@ static void write_event_json(const char *work_dir, const char *event_id, fclose(f); } -/* ── Build tar.gz on SD card ─────────────────────────────────────────────── */ static void build_targz(const char *work_dir, const char *event_id, char *out_path, size_t out_n) { @@ -272,33 +311,25 @@ static void build_targz(const char *work_dir, const char *event_id, strftime(ts, sizeof(ts), "%Y%m%d_%H%M%S", t); snprintf(out_path, out_n, "%s/event_%s_%s.tar.gz", s_sd_path, event_id, ts); - - /* Ensure SD events dir exists */ mkdir(s_sd_path, 0755); char cmd[768]; - /* BusyBox tar: pipe to gzip -c (explicit stdout mode required on some builds). - * Run cd in subshell so && pipe chain works correctly. - * stderr goes to fw.log so we can see the real error if it fails. */ snprintf(cmd, sizeof(cmd), "(cd '%s' && tar cf - . 2>>/tmp/fw.log) | gzip -c > '%s'", work_dir, out_path); int sys_rc = system(cmd); - /* FAT/SD metadata may not be flushed yet — sync before stat */ sync(); - /* Verify the file was actually created */ struct stat _st; if (stat(out_path, &_st) != 0) { printf("[EVT] tar/gzip failed (shell rc=%d) — file not created\n", sys_rc); - out_path[0] = '\0'; /* signal failure to caller */ + out_path[0] = '\0'; return; } printf("[EVT] created %s (%ld bytes)\n", out_path, (long)_st.st_size); } -/* ── SD card cleanup: delete oldest .tar.gz if total > limit ────────────── */ typedef struct { char path[320]; time_t mtime; } FileEntry; static int cmp_mtime(const void *a, const void *b) @@ -330,7 +361,6 @@ static void sd_cleanup(void) if (total <= s_sd_max_bytes) return; - /* Sort by mtime ascending (oldest first) */ qsort(files, count, sizeof(FileEntry), cmp_mtime); for (int i = 0; i < count && total > s_sd_max_bytes; i++) { struct stat st; @@ -342,7 +372,6 @@ static void sd_cleanup(void) } } -/* ── Remove work dir ─────────────────────────────────────────────────────── */ static void rm_work_dir(const char *dir) { char cmd[320]; @@ -350,27 +379,72 @@ static void rm_work_dir(const char *dir) system(cmd); } -/* ═══════════════════════════════════════════════════════════════════════════ - * JSON event fire — bt_uart_send_json() is non-blocking (internal queue + - * dedicated writer thread), so we call it directly without a wrapper thread. - * ═══════════════════════════════════════════════════════════════════════════ */ +/* * +JSON event fire — BLE only (Channel A) + **/ static void fire_json_async(const char *event_id, const char *type, int level) { - (void)event_id; /* not included in BT payload — kept for caller compatibility */ + if (!event_id) event_id = ""; + if (!type) type = "unknown"; - if (!type) type = "unknown"; + char date[32]; + now_iso(date, sizeof(date)); - char json[64]; - /* Compact format fits in a single BLE packet: - * e.g. {"class":"lane","level":1} = 26 bytes */ - snprintf(json, sizeof(json), "{\"class\":\"%s\",\"level\":%d}", type, level); - bt_uart_send_json(json); /* enqueues and returns immediately */ + char json[192]; + snprintf(json, sizeof(json), + "{\"response_type\":\"violation\"," + "\"content\":{\"id\":\"%s\",\"date\":\"%s\",\"type\":\"%s\",\"level\":%d}}", + event_id, date, type, level); + bt_uart_send_json(json); } -/* ═══════════════════════════════════════════════════════════════════════════ - * Upload thread (build tar.gz → upload → SD cleanup) - * ═══════════════════════════════════════════════════════════════════════════ */ +/* collision_warning notify (§2.2.5) */ +void fire_collision_warning(int level, const char *type) +{ + char type_str[24]; + if (level && type && type[0]) + snprintf(type_str, sizeof(type_str), "\"%s\"", type); + else + snprintf(type_str, sizeof(type_str), "null"); + char json[128]; + snprintf(json, sizeof(json), + "{\"response_type\":\"collision_warning\"," + "\"content\":{\"level\":%d,\"type\":%s}}", + level, type_str); + bt_uart_send_json(json); + printf("[EVT] collision_warning level=%d type=%s\n", level, type ? type : "null"); +} + +/* alert notify (§2.2.4) */ +static void fire_alert(int left_level, const char *left_type, + int right_level, const char *right_type) +{ + char l_str[24], r_str[24]; + if (left_level && left_type && left_type[0]) + snprintf(l_str, sizeof(l_str), "\"%s\"", left_type); + else + snprintf(l_str, sizeof(l_str), "null"); + if (right_level && right_type && right_type[0]) + snprintf(r_str, sizeof(r_str), "\"%s\"", right_type); + else + snprintf(r_str, sizeof(r_str), "null"); + char json[256]; + snprintf(json, sizeof(json), + "{\"response_type\":\"alert\"," + "\"content\":{" + "\"left\":{\"level\":%d,\"type\":%s}," + "\"right\":{\"level\":%d,\"type\":%s}}}", + left_level, l_str, right_level, r_str); + bt_uart_send_json(json); + printf("[EVT] alert left=%d(%s) right=%d(%s)\n", + left_level, left_type ? left_type : "null", + right_level, right_type ? right_type : "null"); +} + +/** +Upload thread (build tar.gz → upload → SD cleanup) + **/ typedef struct { char work_dir[256]; @@ -395,31 +469,24 @@ static void *upload_thread(void *arg) usleep((useconds_t)a->delay_ms * 1000); } - /* Write event.json */ printf("[EVT] upload: writing event.json to %s\n", a->work_dir); write_event_json(a->work_dir, a->event_id, a->event_type, a->max_level, a->duration_sec, (const char (*)[64])a->images, a->image_count); - /* Build tar.gz on SD card */ printf("[EVT] upload: building tar.gz\n"); char tgz_path[384]; build_targz(a->work_dir, a->event_id, tgz_path, sizeof(tgz_path)); - /* Upload Channel B */ if (tgz_path[0]) { printf("[EVT] upload: posting %s to %s:%d%s\n", tgz_path, s_up_host, s_up_port, s_up_path); http_post_file(tgz_path); } - /* SD card cleanup */ sd_cleanup(); - - /* Cleanup temp work dir */ rm_work_dir(a->work_dir); - /* Signal grass upload done */ pthread_mutex_lock(&g_grass_mtx); if (g_grass.upload_busy) { g_grass.upload_busy = 0; @@ -476,7 +543,7 @@ static int snap_lazy_init(void) VMF_SNAP_INITOPT_T opt; memset(&opt, 0, sizeof(opt)); opt.pszOutPinPrefix = "vsrc_ssm"; - opt.dwStreamIdx = 0; /* stream0 = 1920×1080 */ + opt.dwStreamIdx = 0; opt.pVsrcHandle = g_ptVsrcHandle; opt.dwQp = SNAP_QP; s_snap = VMF_SNAP_Init(&opt); @@ -488,7 +555,6 @@ static int snap_lazy_init(void) return 0; } -/* ── Save JPEG from snap buffer to file ──────────────────────────────────── */ static void save_jpeg(const char *path, const uint8_t *buf, int size) { FILE *f = fopen(path, "wb"); @@ -532,7 +598,6 @@ static void grass_enter_level(int level) now_iso(ts, sizeof(ts)); printf("[EVT] grass Level %d id=%s\n", level, g_grass.event_id); - /* spec: type = "lane" (超出車道邊界), not "grass" */ fire_json_async(g_grass.event_id, "lane", level); char filename[32]; @@ -541,10 +606,7 @@ static void grass_enter_level(int level) g_grass.event_id, "grass", level, 0 /* not immediate */); } -/* ═══════════════════════════════════════════════════════════════════════════ - * Public API - * ═══════════════════════════════════════════════════════════════════════════ */ - +/* * Public API**/ void event_recorder_init(const char *upload_url, const char *sd_path, int sd_max_mb, @@ -567,24 +629,17 @@ void event_recorder_init(const char *upload_url, s_sd_path, sd_max_mb, s_upload_delay_ms); } -/* ── Recv callback: drives state machine ─────────────────────────────────── */ +/* Recv callback: drives state machine */ void event_recorder_update(const stdc_analysis_t *ana) { +#if 1 if (!s_enabled) return; - /* - * Grass trigger rules (per spec: 超出車道邊界 = type "lane"): - * - Initial entry into L1 requires on_grass AND is_moving - * (prevents false trigger if cart was parked on grass at startup) - * - Once in L1/L2/L3, only on_grass is checked for hysteresis exit - * (is_moving may flicker while driving slowly → don't let it break the timer) - * - Level timers count wall-clock from t_l1 regardless of brief gaps - * - GRASS_EXIT_HYSTERESIS_MS of sustained !on_grass required to end event - */ int on_grass = ana->on_grass; int grass_trigger = on_grass && ana->is_moving; + uint8_t speed_val = 0; - /* ── Grass state machine ─────────────────────────────────────────────── */ +/* Grass state machine */ pthread_mutex_lock(&g_grass_mtx); switch (g_grass.state) { @@ -601,6 +656,9 @@ void event_recorder_update(const stdc_analysis_t *ana) g_grass.state = GRASS_L1; pthread_mutex_unlock(&g_grass_mtx); grass_enter_level(1); + speed_val = SPEED_LEVEL_4; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", + &speed_val, sizeof(speed_val), 0); return; } break; @@ -613,22 +671,28 @@ void event_recorder_update(const stdc_analysis_t *ana) g_grass.state = GRASS_L2; pthread_mutex_unlock(&g_grass_mtx); grass_enter_level(2); + speed_val = SPEED_LEVEL_3; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", + &speed_val, sizeof(speed_val), 0); + return; + } + } else { + speed_val = SPEED_LEVEL_5; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", + &speed_val, sizeof(speed_val), 0); + if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { + g_grass.state = GRASS_DONE; + gettimeofday(&g_grass.t_done, NULL); + g_grass.upload_busy = 1; + pthread_mutex_unlock(&g_grass_mtx); + fire_json_async(g_grass.event_id, "lane", 0); + float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; + const char imgs[4][64] = { "level1.jpg", "", "", "" }; + launch_upload(g_grass.work_dir, g_grass.event_id, "grass", + g_grass.max_level, dur, imgs, 1, s_upload_delay_ms); return; } - } else if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { - /* Sustained absence — event ends */ - g_grass.state = GRASS_DONE; - gettimeofday(&g_grass.t_done, NULL); - g_grass.upload_busy = 1; - pthread_mutex_unlock(&g_grass_mtx); - fire_json_async(g_grass.event_id, "lane", 0); - float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; - const char imgs[4][64] = { "level1.jpg", "", "", "" }; - launch_upload(g_grass.work_dir, g_grass.event_id, "grass", - g_grass.max_level, dur, imgs, 1, s_upload_delay_ms); - return; } - /* else: still within hysteresis window — keep L1, timer keeps running */ break; case GRASS_L2: @@ -639,78 +703,156 @@ void event_recorder_update(const stdc_analysis_t *ana) g_grass.state = GRASS_L3; pthread_mutex_unlock(&g_grass_mtx); grass_enter_level(3); + speed_val = SPEED_LEVEL_2; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", + &speed_val, sizeof(speed_val), 0); + return; + } + } else { + speed_val = SPEED_LEVEL_5; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", + &speed_val, sizeof(speed_val), 0); + if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { + g_grass.state = GRASS_DONE; + gettimeofday(&g_grass.t_done, NULL); + g_grass.upload_busy = 1; + pthread_mutex_unlock(&g_grass_mtx); + fire_json_async(g_grass.event_id, "lane", 0); + float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; + const char imgs[4][64] = { "level1.jpg", "level2.jpg", "", "" }; + launch_upload(g_grass.work_dir, g_grass.event_id, "grass", + g_grass.max_level, dur, imgs, 2, s_upload_delay_ms); return; } - } else if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { - g_grass.state = GRASS_DONE; - gettimeofday(&g_grass.t_done, NULL); - g_grass.upload_busy = 1; - pthread_mutex_unlock(&g_grass_mtx); - fire_json_async(g_grass.event_id, "lane", 0); - float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; - const char imgs[4][64] = { "level1.jpg", "level2.jpg", "", "" }; - launch_upload(g_grass.work_dir, g_grass.event_id, "grass", - g_grass.max_level, dur, imgs, 2, s_upload_delay_ms); - return; } break; case GRASS_L3: if (on_grass) { gettimeofday(&g_grass.t_last_active, NULL); - } else if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { - g_grass.state = GRASS_DONE; - gettimeofday(&g_grass.t_done, NULL); - g_grass.upload_busy = 1; - pthread_mutex_unlock(&g_grass_mtx); - fire_json_async(g_grass.event_id, "lane", 0); - float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; - const char imgs[4][64] = { "level1.jpg", "level2.jpg", "level3.jpg", "" }; - launch_upload(g_grass.work_dir, g_grass.event_id, "grass", - g_grass.max_level, dur, imgs, 3, s_upload_delay_ms); - return; + speed_val = SPEED_LEVEL_1; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", + &speed_val, sizeof(speed_val), 0); + } else { + speed_val = SPEED_LEVEL_5; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", + &speed_val, sizeof(speed_val), 0); + if (elapsed_ms_tv(&g_grass.t_last_active) >= GRASS_EXIT_HYSTERESIS_MS) { + g_grass.state = GRASS_DONE; + gettimeofday(&g_grass.t_done, NULL); + g_grass.upload_busy = 1; + pthread_mutex_unlock(&g_grass_mtx); + fire_json_async(g_grass.event_id, "lane", 0); + float dur = (float)elapsed_ms_tv(&g_grass.t_l1) / 1000.0f; + const char imgs[4][64] = { "level1.jpg", "level2.jpg", "level3.jpg", "" }; + launch_upload(g_grass.work_dir, g_grass.event_id, "grass", + g_grass.max_level, dur, imgs, 3, s_upload_delay_ms); + return; + } } break; case GRASS_DONE: - /* Upload thread will reset to IDLE when done */ + /* Upload thread will reset to IDLE when done. + * Keep sending SPEED_LEVEL_5 every frame until upload finishes + * (keepalive in can_bus.c also maintains this every 200ms). */ + speed_val = SPEED_LEVEL_5; + msg_send("/tmp/canbus/c0/command.fifo", "host_stream", "setSpeed", + &speed_val, sizeof(speed_val), 0); break; } + int grass_was_idle = (g_grass.state == GRASS_IDLE); pthread_mutex_unlock(&g_grass_mtx); - /* ── Single-shot events (Collision ROI) ─────────────────────────────── */ - /* - * Trigger only when the hazard appears inside the collision ROI - * (centre 25%–75% × 25%–70% of the frame), not from global class ratios. - * Only fire on rising edge (0→1 transition) to avoid repeated triggers. - */ - struct { int cur; int *last; const char *type; } singles[] = { - { ana->col_person_ratio >= THR_PERSON_COLLISION, &g_last_person, "person" }, - { ana->col_bunker_ratio >= THR_BUNKER_COLLISION, &g_last_bunker, "bunker" }, - { ana->col_pond_ratio >= THR_POND_COLLISION, &g_last_pond, "pond" }, - { ana->col_tree_ratio >= THR_TREE_COLLISION, &g_last_tree, "tree" }, - }; - - for (int i = 0; i < 4; i++) { - if (singles[i].cur && !*singles[i].last) { - const char *type = singles[i].type; - - char ev_id[32]; - struct timeval now; - gettimeofday(&now, NULL); - /* Include type in id to avoid work_dir collision with grass events at same second */ - snprintf(ev_id, sizeof(ev_id), "%ld_%s", now.tv_sec, type); - - char work_dir[256]; - make_work_dir(ev_id, work_dir, sizeof(work_dir)); - - printf("[EVT] single-shot: %s id=%s\n", type, ev_id); - fire_json_async(ev_id, type, 1); - request_snap("snapshot.jpg", work_dir, ev_id, type, 1, 1 /* immediate */); - } - *singles[i].last = singles[i].cur; + /* collision_warning (2.2.5)*/ + int col_now = ana->collision_risk; + if (col_now != g_last_collision_warning) { + const char *col_type = NULL; + if (ana->col_person_ratio >= THR_PERSON_COLLISION) col_type = "person"; + else if (ana->col_car_ratio >= THR_CAR_COLLISION) col_type = "vehicle"; + else if (ana->col_tree_ratio >= THR_TREE_COLLISION) col_type = "tree"; + else if (ana->col_pond_ratio >= THR_POND_COLLISION) col_type = "water_hazard"; + else if (ana->col_bunker_ratio >= THR_BUNKER_COLLISION) col_type = "bush"; + fire_collision_warning(col_now, col_now ? col_type : NULL); + g_last_collision_warning = col_now; } + + /*alert (2.2.4)*/ + int left_now = ana->left_alert; + int right_now = ana->right_alert; + if (left_now != g_last_left_alert || right_now != g_last_right_alert) { + fire_alert(left_now, left_now ? ana->left_type : NULL, + right_now, right_now ? ana->right_type : NULL); + g_last_left_alert = left_now; + g_last_right_alert = right_now; + } + + g_any_col_last = col_now; +#endif +#if 0 + /* ── [TEST] violation（2.2.2）靜態測試向量 ────────────────────────── + * 每隔 3 秒依序送出 level 1 → 2 → 3 → 0，模擬一次完整違規事件週期： + * level 1：違規發生 + * level 2：違規持續 6 秒 + * level 3：違規持續 10 秒 + * level 0：違規解除 + * 啟用：把 #if 0 改成 #if 1 + * ------------------------------------------------------------------ */ + { + static int s_viol_tick = 0; + static int s_viol_phase = 0; + static int s_viol_last = -1; + static char s_viol_id[24] = ""; + + s_viol_tick++; + if (s_viol_tick >= 75) { /* 約 3 秒（25 fps × 3）*/ + s_viol_tick = 0; + s_viol_phase = (s_viol_phase + 1) % 4; + } + + if (s_viol_phase != s_viol_last) { + s_viol_last = s_viol_phase; + if (s_viol_phase == 0) + snprintf(s_viol_id, sizeof(s_viol_id), "%ld", (long)time(NULL)); + static const int levels[] = {1, 2, 3, 0}; + int lvl = levels[s_viol_phase]; + fire_json_async(s_viol_id, "lane", lvl); + printf("[TEST] violation id=%s level=%d\n", s_viol_id, lvl); + } + } +#endif +#if 0 + /* ── [TEST] alert（2.2.4）靜態測試向量 ────────────────────────────── + * 每隔 5 秒輪流送出四種組合，驗證 iPad 端能正確顯示： + * 0: 左右都無危險 + * 1: 左側 tree，右側無危險 + * 2: 左側無危險，右側 person + * 3: 左右都有危險（tree / vehicle） + * 啟用：把 #if 0 改成 #if 1 + * ------------------------------------------------------------------ */ + { + static int s_test_tick = 0; + static int s_test_phase = 0; + static int s_last_phase = -1; + + s_test_tick++; + if (s_test_tick >= 125) { /* 約 5 秒（25 fps × 5）*/ + s_test_tick = 0; + s_test_phase = (s_test_phase + 1) % 4; + } + + if (s_test_phase != s_last_phase) { + s_last_phase = s_test_phase; + switch (s_test_phase) { + case 0: fire_alert(0, NULL, 0, NULL); printf("[TEST] alert: left=0 right=0\n"); break; + case 1: fire_alert(1, "tree", 0, NULL); printf("[TEST] alert: left=tree right=0\n"); break; + case 2: fire_alert(0, NULL, 1, "person"); printf("[TEST] alert: left=0 right=person\n"); break; + case 3: fire_alert(1, "tree", 1, "vehicle");printf("[TEST] alert: left=tree right=vehicle\n"); break; + } + } + } +#endif } /* ── Send callback: take VMF_SNAP if one is pending ─────────────────────── */ @@ -723,7 +865,6 @@ void event_recorder_provide_frame(void) pthread_mutex_unlock(&g_snap_mtx); return; } - /* Copy request locally and clear the flag */ SnapReq req = g_snap_req; g_snap_req.active = 0; pthread_mutex_unlock(&g_snap_mtx); @@ -747,8 +888,6 @@ void event_recorder_provide_frame(void) } MemBroker_FreeMemory(jpeg); - /* Single-shot: launch upload immediately (delay_ms = 0). - * Always upload even if snap failed — event.json still carries the record. */ if (req.immediate_upload) { const char imgs[4][64] = { "snapshot.jpg", "", "", "" }; int image_count = snap_ok ? 1 : 0; diff --git a/src/host_stream/gpio_devmem.c b/src/host_stream/gpio_devmem.c new file mode 100644 index 0000000..85307b7 --- /dev/null +++ b/src/host_stream/gpio_devmem.c @@ -0,0 +1,136 @@ +/* + * gpio_devmem.c — Direct GPIO control via /dev/mem for KL630 + * + * KL630 GPIO_C register base: 0x402E0000 + * J15 pinout (GPIOC_0 pins 1-4): + * Pin 1 (CS) = GPIO1 = GPIOC_0_IO_DATA_1 + * Pin 2 (MOSI) = GPIO2 = GPIOC_0_IO_DATA_2 + * Pin 3 (SCK) = GPIO3 = GPIOC_0_IO_DATA_3 + * Pin 4 (MISO) = GPIO4 = GPIOC_0_IO_DATA_4 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "gpio_devmem.h" + +/* KL630 GPIO_C base address */ +#define GPIO_C_BASE 0x402E0000 +#define PAGE_SIZE 4096 + +/* GPIO register offsets */ +#define GPIOD_PSR_OFFSET 0x0000 /* Port Status Register (read-only) */ +#define GPIOD_PDDR_OFFSET 0x0004 /* Port Data Direction Register */ +#define GPIOD_PSOR_OFFSET 0x0008 /* Port Set Output Register */ +#define GPIOD_PCOR_OFFSET 0x000C /* Port Clear Output Register */ +#define GPIOD_PTOR_OFFSET 0x0010 /* Port Toggle Output Register */ +#define GPIOD_PIDR_OFFSET 0x0014 /* Port Input Disable Register */ + +static int s_devmem_fd = -1; +static uint32_t *s_gpio_base = NULL; + +/* ──────────────────────────────────────────────────────────────────── */ +int gpio_devmem_init(void) +{ + if (s_gpio_base != NULL) return 0; /* already initialized */ + + s_devmem_fd = open("/dev/mem", O_RDWR | O_SYNC); + if (s_devmem_fd < 0) { + perror("[GPIO] open /dev/mem"); + return -1; + } + + /* Map GPIO_C registers into user space */ + s_gpio_base = (uint32_t *)mmap( + NULL, + PAGE_SIZE, + PROT_READ | PROT_WRITE, + MAP_SHARED, + s_devmem_fd, + GPIO_C_BASE & ~(PAGE_SIZE - 1) /* align to page boundary */ + ); + + if (s_gpio_base == MAP_FAILED) { + perror("[GPIO] mmap /dev/mem"); + close(s_devmem_fd); + s_devmem_fd = -1; + return -1; + } + + /* Adjust for offset within page */ + s_gpio_base = (uint32_t *)((uintptr_t)s_gpio_base + (GPIO_C_BASE & (PAGE_SIZE - 1))); + + printf("[GPIO] devmem init OK: GPIO_C at 0x%X, mapped to %p\n", GPIO_C_BASE, s_gpio_base); + return 0; +} + +/* ──────────────────────────────────────────────────────────────────── */ +void gpio_devmem_cleanup(void) +{ + if (s_gpio_base != NULL) { + s_gpio_base = (uint32_t *)((uintptr_t)s_gpio_base - (GPIO_C_BASE & (PAGE_SIZE - 1))); + munmap(s_gpio_base, PAGE_SIZE); + s_gpio_base = NULL; + } + if (s_devmem_fd >= 0) { + close(s_devmem_fd); + s_devmem_fd = -1; + } +} + +/* ──────────────────────────────────────────────────────────────────── */ +int gpio_devmem_set_direction(int gpio, int output) +{ + if (!s_gpio_base || gpio < 1 || gpio > 4) return -1; + + volatile uint32_t *pddr = (volatile uint32_t *) + ((uintptr_t)s_gpio_base + GPIOD_PDDR_OFFSET); + + if (output) { + /* Set bit for output */ + *pddr |= (1U << gpio); + } else { + /* Clear bit for input */ + *pddr &= ~(1U << gpio); + } + + return 0; +} + +/* ──────────────────────────────────────────────────────────────────── */ +int gpio_devmem_set(int gpio, int value) +{ + if (!s_gpio_base || gpio < 1 || gpio > 4) return -1; + + volatile uint32_t *psor = (volatile uint32_t *) + ((uintptr_t)s_gpio_base + GPIOD_PSOR_OFFSET); + volatile uint32_t *pcor = (volatile uint32_t *) + ((uintptr_t)s_gpio_base + GPIOD_PCOR_OFFSET); + + if (value) { + /* Write 1 to PSOR to set output high */ + *psor = (1U << gpio); + } else { + /* Write 1 to PCOR to set output low */ + *pcor = (1U << gpio); + } + + return 0; +} + +/* ──────────────────────────────────────────────────────────────────── */ +int gpio_devmem_get(int gpio) +{ + if (!s_gpio_base || gpio < 1 || gpio > 4) return -1; + + volatile uint32_t *psr = (volatile uint32_t *) + ((uintptr_t)s_gpio_base + GPIOD_PSR_OFFSET); + + uint32_t val = *psr; + return (val >> gpio) & 1; +} diff --git a/src/host_stream/handshake.c b/src/host_stream/handshake.c new file mode 100644 index 0000000..2166dd4 --- /dev/null +++ b/src/host_stream/handshake.c @@ -0,0 +1,204 @@ +/* + * handshake.c — BLE Challenge-Response Authentication implementation + * + * Key: SHA256("29C310F5")[0..15] XOR 0x5A + * Challenge: R = 4-byte timestamp (big-endian) + 12 random bytes + * Verify: base64(SHA256(R || Key)) == received_response + */ +#include +#include +#include +#include +#include +#include +#include + +#include "sha256.h" +#include "base64.h" +#include "handshake.h" + +/* Timestamp when challenge was sent (for timeout check) */ +static time_t s_challenge_send_time = 0; + +/* ── Key derivation ─────────────────────────────────────────────────────── */ + +void handshake_derive_key(uint8_t out_key[HANDSHAKE_KEY_LEN]) +{ + uint8_t digest[32]; + sha256((const uint8_t *)HANDSHAKE_KN_STR, strlen(HANDSHAKE_KN_STR), digest); + for (int i = 0; i < HANDSHAKE_KEY_LEN; i++) + out_key[i] = digest[i] ^ 0x5A; +} + +void handshake_print_key(const uint8_t key[HANDSHAKE_KEY_LEN]) +{ + for (int i = 0; i < HANDSHAKE_KEY_LEN; i++) + printf("%02x%s", key[i], (i < HANDSHAKE_KEY_LEN - 1) ? " " : "\n"); +} + +/* ── Init / Reset ────────────────────────────────────────────────────────── */ + +void handshake_init(handshake_ctx_t *ctx) +{ + memset(ctx, 0, sizeof(*ctx)); + ctx->state = HS_STATE_IDLE; + handshake_derive_key(ctx->key); +} + +void handshake_reset(handshake_ctx_t *ctx) +{ + ctx->state = HS_STATE_IDLE; + memset(ctx->R, 0, sizeof(ctx->R)); + s_challenge_send_time = 0; +} + +handshake_state_t handshake_get_state(const handshake_ctx_t *ctx) +{ + return ctx->state; +} + +int handshake_check_timeout(handshake_ctx_t *ctx) +{ + if (ctx->state != HS_STATE_WAIT_RESPONSE) + return 0; + return (time(NULL) - s_challenge_send_time >= HANDSHAKE_TIMEOUT_SEC) ? 1 : 0; +} + +/* ── Challenge generation ────────────────────────────────────────────────── */ + +int handshake_build_challenge(handshake_ctx_t *ctx, + char *out_json_buf, size_t buf_size) +{ +#if 0 /* ── TEST VECTOR (Protocol 5.3): change to #if 0 to restore random R ── */ + /* R = 6a4636a0a1b2c3d4e5f60718293a4b5c + * Expected challenge: akY2oKGyw9Tl9gcYKTpLXA== + * Expected response: NISSISwhM59eitBMS/stTnJMSLIU7YWZG7bd+NkQBAU= */ + static const uint8_t TEST_R[HANDSHAKE_R_LEN] = { + 0x6a, 0x46, 0x36, 0xa0, 0xa1, 0xb2, 0xc3, 0xd4, + 0xe5, 0xf6, 0x07, 0x18, 0x29, 0x3a, 0x4b, 0x5c + }; + memcpy(ctx->R, TEST_R, HANDSHAKE_R_LEN); + time_t now = time(NULL); /* still needed for s_challenge_send_time */ +#else /* ── Normal operation: random R ────────────────────────────────────── */ + /* R[0..3] = current time big-endian */ + time_t now = time(NULL); + ctx->R[0] = (uint8_t)((now >> 24) & 0xFF); + ctx->R[1] = (uint8_t)((now >> 16) & 0xFF); + ctx->R[2] = (uint8_t)((now >> 8) & 0xFF); + ctx->R[3] = (uint8_t)( now & 0xFF); + + /* R[4..15] = 12 random bytes from /dev/urandom */ + int fd = open("/dev/urandom", O_RDONLY); + if (fd >= 0) { + if (read(fd, ctx->R + 4, 12) != 12) { + /* partial read fallback */ + for (int i = 4; i < 16; i++) ctx->R[i] ^= (uint8_t)(rand() ^ i); + } + close(fd); + } else { + /* /dev/urandom unavailable — simple fallback */ + srand((unsigned int)now); + for (int i = 4; i < 16; i++) + ctx->R[i] = (uint8_t)(rand() ^ i ^ (int)now); + } +#endif + + /* Base64-encode R (16 bytes → 24 chars + NUL) */ + char r_b64[28]; /* ceil(16/3)*4+1 = 25 bytes, round up */ + base64_encode(ctx->R, HANDSHAKE_R_LEN, r_b64); + + /* Build JSON */ + int n = snprintf(out_json_buf, buf_size, + "{\"response_type\":\"handshake_challenge\"," + "\"content\":{\"challenge\":\"%s\"}}", + r_b64); + if (n < 0 || (size_t)n >= buf_size) + return -1; + + /* Record send time and advance state */ + s_challenge_send_time = now; + ctx->state = HS_STATE_WAIT_RESPONSE; + return 0; +} + +/* ── Response validation ─────────────────────────────────────────────────── */ + +int handshake_process_response(handshake_ctx_t *ctx, const char *json_str) +{ + /* 1. Timeout check */ + if (handshake_check_timeout(ctx)) { + ctx->state = HS_STATE_FAILED; + return -2; + } + + /* 2. Parse "response" field value from JSON + * Looks for: "response":"" */ + const char *p = strstr(json_str, "\"response\""); + if (!p) return -1; + p = strchr(p, ':'); + if (!p) return -1; + /* skip whitespace and colon */ + while (*p == ':' || *p == ' ' || *p == '\t') p++; + if (*p != '"') return -1; + p++; /* skip opening quote */ + const char *end = strchr(p, '"'); + if (!end) return -1; + size_t val_len = (size_t)(end - p); + if (val_len == 0 || val_len > 64) return -1; + + char received_b64[68]; + memcpy(received_b64, p, val_len); + received_b64[val_len] = '\0'; + + /* 3. Compute expected = base64(SHA256(R || Key)) */ + uint8_t input[HANDSHAKE_R_LEN + HANDSHAKE_KEY_LEN]; /* 32 bytes */ + memcpy(input, ctx->R, HANDSHAKE_R_LEN); + memcpy(input + HANDSHAKE_R_LEN, ctx->key, HANDSHAKE_KEY_LEN); + + uint8_t digest[32]; + sha256(input, HANDSHAKE_R_LEN + HANDSHAKE_KEY_LEN, digest); + + char expected_b64[48]; /* 32 bytes → 44 chars + NUL */ + base64_encode(digest, 32, expected_b64); + + printf("[HS DEBUG] R : "); + for (int i = 0; i < HANDSHAKE_R_LEN; i++) printf("%02x", ctx->R[i]); + printf("\n"); + + printf("[HS DEBUG] Key : "); + for (int i = 0; i < HANDSHAKE_KEY_LEN; i++) printf("%02x", ctx->key[i]); + printf("\n"); + + printf("[HS DEBUG] expected : %s\n", expected_b64); + printf("[HS DEBUG] received : %s\n", received_b64); + + /* 4. Constant-time compare (simple for embedded) */ + if (strcmp(received_b64, expected_b64) == 0) { + ctx->state = HS_STATE_SUCCESS; + return 1; + } + + ctx->state = HS_STATE_FAILED; + return 0; +} + +/* ── Confirm message ─────────────────────────────────────────────────────── */ + +int handshake_build_confirm(handshake_ctx_t *ctx, int success, + const char *reason_str, + char *out_json_buf, size_t buf_size) +{ + (void)ctx; + if (success) { + snprintf(out_json_buf, buf_size, + "{\"response_type\":\"handshake_confirm\"," + "\"content\":{\"status\":\"success\"}}"); + } else { + const char *reason = reason_str ? reason_str : "invalid_response"; + snprintf(out_json_buf, buf_size, + "{\"response_type\":\"handshake_confirm\"," + "\"content\":{\"status\":\"failed\",\"reason\":\"%s\"}}", + reason); + } + return 0; +} diff --git a/src/host_stream/handshake.h b/src/host_stream/handshake.h new file mode 100644 index 0000000..5061562 --- /dev/null +++ b/src/host_stream/handshake.h @@ -0,0 +1,72 @@ +/* + * handshake.h — BLE Challenge-Response Authentication Protocol + * + * Protocol flow: + * 1. Device sends: {"response_type":"handshake_challenge","content":{"challenge":""}} + * 2. iPad computes: SHA256(R || Key) and replies with base64 of the digest + * 3. Device verifies and sends: {"response_type":"handshake_confirm","content":{"status":"success"|"failed"}} + * + * Key derivation: SHA256("29C310F5") → first 16 bytes XOR 0x5A + */ +#ifndef HANDSHAKE_H +#define HANDSHAKE_H + +#include +#include + +#define HANDSHAKE_KEY_LEN 16 +#define HANDSHAKE_KN_STR "29C310F5"//"48872dc21ede" /* test vector; restore to "29C310F5" after verification */ +#define HANDSHAKE_R_LEN 16 +#define HANDSHAKE_TIMEOUT_SEC 5 + +typedef enum { + HS_STATE_IDLE = 0, + HS_STATE_WAIT_RESPONSE, + HS_STATE_SUCCESS, + HS_STATE_FAILED +} handshake_state_t; + +typedef struct { + handshake_state_t state; + uint8_t key[HANDSHAKE_KEY_LEN]; + uint8_t R[HANDSHAKE_R_LEN]; +} handshake_ctx_t; + +/* Derive the 16-byte handshake key: SHA256(HANDSHAKE_KN_STR)[0..15] XOR 0x5A */ +void handshake_derive_key(uint8_t out_key[HANDSHAKE_KEY_LEN]); + +/* Print key bytes to stdout as space-separated hex (e.g., "58 48 c8 ff ...") */ +void handshake_print_key(const uint8_t key[HANDSHAKE_KEY_LEN]); + +/* Initialise ctx: derive key, set state=IDLE, zero R */ +void handshake_init(handshake_ctx_t *ctx); + +/* Generate random R, set state=WAIT_RESPONSE, write JSON to out_json_buf. + * Returns 0 on success, -1 on error. */ +int handshake_build_challenge(handshake_ctx_t *ctx, + char *out_json_buf, size_t buf_size); + +/* Validate iPad response JSON (must contain "response" field with base64 value). + * Returns 1: success (state→SUCCESS) + * 0: wrong answer (state→FAILED) + * -1: malformed JSON + * -2: timeout */ +int handshake_process_response(handshake_ctx_t *ctx, const char *json_str); + +/* Build confirm JSON. success=1 → {"status":"success"}; success=0 → {"status":"failed","reason":"..."} + * reason_str may be NULL (defaults to "invalid_response"). + * Returns 0. */ +int handshake_build_confirm(handshake_ctx_t *ctx, int success, + const char *reason_str, + char *out_json_buf, size_t buf_size); + +/* Reset state to IDLE, zero R (key is retained) */ +void handshake_reset(handshake_ctx_t *ctx); + +/* Return current state */ +handshake_state_t handshake_get_state(const handshake_ctx_t *ctx); + +/* Return 1 if WAIT_RESPONSE and timeout expired, else 0 */ +int handshake_check_timeout(handshake_ctx_t *ctx); + +#endif /* HANDSHAKE_H */ diff --git a/src/host_stream/kdp2_host_stream.c b/src/host_stream/kdp2_host_stream.c index 7817615..3b2984b 100644 --- a/src/host_stream/kdp2_host_stream.c +++ b/src/host_stream/kdp2_host_stream.c @@ -440,8 +440,8 @@ static int init_video_source(HOST_STREAM_INIT_OPT_T* pHostStreamInit) } } else { /* FEC passthrough (no lens correction). Load FEC config for gFecDefValue defaults only. - * Pass NULL for ptFrontConfig — skips lens curve node loading (not needed for passthrough). - * Do NOT set ptFecConfig here — SDK sets only eFecMethod=GTR (below) for this case. */ + * Pass NULL for ptFrontConfig ??skips lens curve node loading (not needed for passthrough). + * Do NOT set ptFecConfig here ??SDK sets only eFecMethod=GTR (below) for this case. */ if(loadFECConfig(pHostStreamInit, NULL) == -1){ printf("[%s] No fec config file, using defaults\n", __func__); } @@ -534,15 +534,24 @@ void set_data_to_yuv(unsigned char* pucYBuff, unsigned int dwWidth, unsigned int dwOffsetU = dwWidth * dwHeight + (((tDrawPoint.dwY >> 1) * dwWidth) >> 1) + (tDrawPoint.dwX >> 1); dwOffsetV = dwOffsetU + dwPlaneSize ; - if(iColor){ - pucYBuff[dwOffsetY] = 0x00; - pucYBuff[dwOffsetU] = 0x00; - pucYBuff[dwOffsetV] = 0xFF; - }else{ - pucYBuff[dwOffsetY] = 0xFF; - pucYBuff[dwOffsetU] = 0x00; - pucYBuff[dwOffsetV] = 0x00; - } + /* YCbCr colour table indexed by YOLO class number (BT.601 full-range). + * class=0 person ??green (Y=149, Cb= 43, Cr= 21) + * class=1 (other) ??white (Y=235, Cb=128, Cr=128) + * class=2 vehicle ??red (Y= 76, Cb= 84, Cr=255) + * fallback ??white */ + static const unsigned char CLASS_YUV[][3] = { + {149, 43, 21}, /* 0: person ??green */ + {235, 128, 128}, /* 1: other ??white */ + { 76, 84, 255}, /* 2: vehicle ??red */ + }; + const unsigned char *yuv; + if (iColor >= 0 && iColor < (int)(sizeof(CLASS_YUV)/sizeof(CLASS_YUV[0]))) + yuv = CLASS_YUV[iColor]; + else + yuv = CLASS_YUV[1]; /* default white */ + pucYBuff[dwOffsetY] = yuv[0]; + pucYBuff[dwOffsetU] = yuv[1]; + pucYBuff[dwOffsetV] = yuv[2]; } @@ -613,6 +622,10 @@ void draw_rect(YUV_BUFF_INFO_T* yuvBuffInfo, DETECT_INFO *ptDetInfo, int iColor) int iLineEndX = 0, iLineEndY = 0; int iStartX = 0, iStartY = 0, iWidth = 0, iHeight = 0; + /* Use class number as colour index so each class gets a distinct colour. + * iColor parameter is kept for API compatibility but ignored. */ + iColor = (int)ptDetInfo->dwClass; + iStartX = (int)ptDetInfo->dwStartX; iStartY = (int)ptDetInfo->dwStartY; iWidth = (int)ptDetInfo->dwWidth; @@ -656,7 +669,7 @@ void draw_rect(YUV_BUFF_INFO_T* yuvBuffInfo, DETECT_INFO *ptDetInfo, int iColor) * Per-class YCbCr colours for STDC segmentation overlay (BT.601 full-range). * Order matches STDC_CLASS_* indices: * 0=bunker 1=car 2=grass 3=greenery 4=person 5=pond 6=road 7=tree - * road (class 6) is left transparent — it occupies most of the frame and adds + * road (class 6) is left transparent ??it occupies most of the frame and adds * visual noise rather than information. */ static const uint8_t s_stdc_yuv[STDC_NUM_CLASSES][3] = { @@ -666,7 +679,7 @@ static const uint8_t s_stdc_yuv[STDC_NUM_CLASSES][3] = { {137, 104, 55}, /* greenery #22c55e */ {144, 59, 203}, /* person #f97316 */ {154, 182, 87}, /* pond #60a5fa */ - { 0, 0, 0}, /* road — transparent (unused) */ + { 0, 0, 0}, /* road ??transparent (unused) */ { 88, 113, 80}, /* tree #15803d */ }; @@ -697,7 +710,7 @@ static void stdc_paint_seg_overlay(unsigned char *base_buf, uint32_t y_stride = info->dwYStride; uint32_t uv_stride = info->dwYStride >> 1; - /* Pre-compute column mapping: output x → seg col index */ + /* Pre-compute column mapping: output x ??seg col index */ uint8_t col_lut[1920]; uint32_t fx_max = (frame_w < 1920) ? frame_w : 1920; for (uint32_t fx = 0; fx < fx_max; fx++) @@ -708,7 +721,7 @@ static void stdc_paint_seg_overlay(unsigned char *base_buf, const uint8_t *seg_row = local_map + seg_r * seg_w; uint8_t *y_row = y_buf + fy * y_stride; - /* Y plane — every pixel */ + /* Y plane ??every pixel */ for (uint32_t fx = 0; fx < fx_max; fx++) { uint8_t cls = seg_row[col_lut[fx]]; if (cls == STDC_CLASS_ROAD || cls >= STDC_NUM_CLASSES) continue; @@ -716,7 +729,7 @@ static void stdc_paint_seg_overlay(unsigned char *base_buf, y_row[fx] = (uint8_t)(((uint16_t)y_row[fx] + col[0]) >> 1); } - /* UV plane — every other row, every other column */ + /* UV plane ??every other row, every other column */ if ((fy & 1) == 0) { uint8_t *cb_row = cb_buf + (fy >> 1) * uv_stride; uint8_t *cr_row = cr_buf + (fy >> 1) * uv_stride; @@ -1530,8 +1543,8 @@ void *kdp2_host_voc_thread(void *arg) if (vsrc_ssm_info.dwOffset[0] != 0 && vsrc_ssm_info.dwOffset[1] != 0) { abuf[q_idx].apdwData[0] = ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[0]; abuf[q_idx].apdwData[1] = ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[1]; - /* NV12 (semi-planar): offset[2]==0 means no separate Cr plane → NULL. - * YM12 (planar): offset[2]!=0 → Cr plane pointer. */ + /* NV12 (semi-planar): offset[2]==0 means no separate Cr plane ??NULL. + * YM12 (planar): offset[2]!=0 ??Cr plane pointer. */ abuf[q_idx].apdwData[2] = vsrc_ssm_info.dwOffset[2] ? (ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[2]) : NULL; } else { @@ -1659,7 +1672,7 @@ void *kdp2_host_stream_image_thread(void *arg) * VOC thread unblocks on g_dwInitBind and immediately checks g_dwDrawBoxType * to decide which SSM pin to read from. If draw box is enabled on stream 0, * it must see g_dwDrawBoxType=1 so it reads from VENC_VSRC_B_PIN (overlay - * output) rather than the raw ISP SSM — fixes intermittent HDMI no-overlay. */ + * output) rather than the raw ISP SSM ??fixes intermittent HDMI no-overlay. */ if (pHostStreamInit->bDrawBoxEnable && pHostStreamInit->dwEncodeStreamCount > 0) g_dwDrawBoxType = 1; g_dwInitBind = 1; diff --git a/src/host_stream/kp_firmware.c b/src/host_stream/kp_firmware.c index ed311ec..0f41381 100644 --- a/src/host_stream/kp_firmware.c +++ b/src/host_stream/kp_firmware.c @@ -42,6 +42,8 @@ #include "fec_api.h" #include "event_recorder.h" #include "bt_uart.h" +#include "can_bus.h" +#include "handshake.h" //fifo queue buffer setting #define IMAGE_BUFFER_COUNT 3 @@ -224,17 +226,78 @@ int loadConfig(HOST_STREAM_INIT_OPT_T* pHostStreamInit) printf("[NNM] Model: %s dwModelId: %d dwJobId: %d \n", pHostStreamInit->pszModelPath, pHostStreamInit->dwModelId, pHostStreamInit->dwJobId); /* --- [event] section: violation event recording + upload --- */ { - int ev_enable = iniparser_getint(ini, "event:enable", 0); - const char *bt_dev = iniparser_getstring(ini, "event:bt_uart_dev", "/dev/ttyS1"); - int bt_at_probe = iniparser_getint(ini, "event:bt_at_probe", 0); - const char *ev_up = iniparser_getstring(ini, "event:upload_url", "http://192.168.0.114:8081/api/upload"); - const char *ev_sd = iniparser_getstring(ini, "event:sd_path", "/tmp/sdcard/events"); - int ev_max_mb = iniparser_getint(ini, "event:sd_max_mb", 7168); - int ev_delay = iniparser_getint(ini, "event:upload_delay_ms", 60000); + int ev_enable = iniparser_getint(ini, "event:enable", 0); + const char *bt_dev = iniparser_getstring(ini, "event:bt_uart_dev", "/dev/ttyS1"); + int bt_at_probe = iniparser_getint(ini, "event:bt_at_probe", 0); + const char *aresx_ver = iniparser_getstring(ini, "event:aresx_version", "1.0.0"); + const char *ev_up = iniparser_getstring(ini, "event:upload_url", "http://192.168.0.114:8081/api/upload"); + const char *ev_sd = iniparser_getstring(ini, "event:sd_path", "/tmp/sdcard/events"); + int ev_max_mb = iniparser_getint(ini, "event:sd_max_mb", 7168); + int ev_delay = iniparser_getint(ini, "event:upload_delay_ms", 60000); + + /* device_id (event:device_id): raw Kn chip number string, e.g. "0xCE7B562C". + * bt_name (event:bt_name): BLE broadcast name derived from kn, e.g. "AresX-562C". + * + * First-boot (device_id empty): get kn, build both strings, mark need_ini_save. + * AT-probe flow (bt_at_probe==1): bt_set_module_name runs inside bt_uart_init(); + * after it returns, persist bt_name + device_id and set bt_at_probe=0. */ + const char *device_id_ini = iniparser_getstring(ini, "event:device_id", ""); + char device_id[32]; /* raw kn number, e.g. "0xCE7B562C" */ + char bt_name[32]; /* derived BLE name, e.g. "AresX-562C" */ + int need_ini_save = 0; + + if (!device_id_ini || !*device_id_ini) { + uint32_t kn = VMF_NNM_Get_Kn_Number(); + snprintf(device_id, sizeof(device_id), "0x%08X", kn); + snprintf(bt_name, sizeof(bt_name), "AresX-%04X", (unsigned)(kn & 0xFFFF)); + printf("[BT] auto device_id=%s bt_name=%s\n", device_id, bt_name); + iniparser_set(ini, "event:device_id", device_id); + iniparser_set(ini, "event:bt_name", bt_name); + need_ini_save = 1; + } else { + snprintf(device_id, sizeof(device_id), "%s", device_id_ini); + const char *bt_name_ini = iniparser_getstring(ini, "event:bt_name", "BT-24"); + snprintf(bt_name, sizeof(bt_name), "%s", bt_name_ini); + printf("[BT] device_id=%s bt_name=%s (from INI)\n", device_id, bt_name); + } + + /* Order matters: set_identity BEFORE init so s_bt_name is ready + * when bt_uart_probe_and_upgrade() calls bt_set_module_name(). */ + bt_uart_set_identity(aresx_ver, bt_name); bt_uart_init(bt_dev, bt_at_probe); + + /* After AT probe+upgrade: persist bt_name, disable at_probe for next boot. */ + if (bt_at_probe) { + iniparser_set(ini, "event:bt_name", bt_name); + iniparser_set(ini, "event:bt_at_probe", "0"); + need_ini_save = 1; + } + + if (need_ini_save) { + FILE *fini = fopen(HOST_STREAM_CONFIG_PATH, "w"); + if (fini) { + iniparser_dump_ini(ini, fini); + fclose(fini); + printf("[BT] INI persisted: bt_name=%s device_id=%s bt_at_probe=%d\n", + bt_name, device_id, bt_at_probe ? 0 : bt_at_probe); + } else { + printf("[BT] WARNING: cannot write INI back (%s)\n", HOST_STREAM_CONFIG_PATH); + } + } + event_recorder_init(ev_up, ev_sd, ev_max_mb, ev_delay, ev_enable); } + /* --- [can] section: MCP2515 CAN bus via J15 SPI connector --- */ + { + int can_enable = iniparser_getint(ini, "can:enable", 0); + const char *can_spidev = iniparser_getstring(ini, "can:spidev", "/dev/spidev1.0"); + int can_speed = iniparser_getint(ini, "can:speed_kbps", 250); + int can_id_raw = iniparser_getint(ini, "can:can_id", 0x100); + if (can_enable) + can_bus_init(can_spidev, can_speed, (uint32_t)can_id_raw); + } + iniparser_freedict(ini); return 0; } @@ -379,14 +442,24 @@ int main (int argc, char* argv[]) pthread_t thread_f = 0; VMF_NNM_Get_Version(&major, &minor, &patch, &build); + uint32_t kn_number = VMF_NNM_Get_Kn_Number(); + + /* Derive and print BLE handshake key for startup verification. + * Expected: 58 48 c8 ff b2 eb b1 b8 69 92 b5 20 48 8d 1d 61 */ + { + uint8_t hs_key[HANDSHAKE_KEY_LEN]; + handshake_derive_key(hs_key); + printf("[INIT] handshake key: "); + handshake_print_key(hs_key); + } printf("\n\n**********************************************************\n"); printf("Kneron Firmware\n"); printf("Ver. %d.%d.%d.%d\n", major, minor, patch, build); printf("Build Time: %s %s\n", __DATE__, __TIME__); + printf("Kn Number = 0x%08x\n",kn_number); printf("**********************************************************\n"); printf("HOST STREAM mode \n"); - //pthread_t task_infcb_handle; //kmdw_inference_result_handler_callback_thread; pthread_t task_inf_data_handle; //VMF_NNM_Inference_Image_Dispatcher_Thread; pthread_t task_stream_image_handle; // <-> kdp2_host_stream_image_thread; //pthread_t task_usb_recv_handle; // <-> kdp2_usb_companion_image_thread; diff --git a/src/host_stream/mcp2515.c b/src/host_stream/mcp2515.c new file mode 100644 index 0000000..06b1554 --- /dev/null +++ b/src/host_stream/mcp2515.c @@ -0,0 +1,1174 @@ +#include "mcp2515.h" +#include "gpio_devmem.h" +#include +#include +#include + +#ifndef SPI_GPIO_CS +#define SPI_GPIO_CS 1 +#endif + +#ifndef SPI_HW_MANUAL_CS +#define SPI_HW_MANUAL_CS 1 +#endif + +/* ====================================================================== + * GPIO bit-bang SPI — compiled when -DSPI_BITBANG=1 + * + * Compile flags (set in compile.sh): + * -DSPI_BITBANG=1 + * -DSPI_GPIO_CS=1 (J15 pin, active-low chip select) + * -DSPI_GPIO_MOSI=2 (J15 pin, master out) + * -DSPI_GPIO_SCK=3 (J15 pin, clock) + * -DSPI_GPIO_MISO=4 (J15 pin, master in) + * + * SPI Mode 0 (CPOL=0 CPHA=0): clock idles low, data sampled on rising edge. + * + * Implementation: Direct /dev/mem GPIO access via gpio_devmem module. + * Bypasses kernel driver pinmux conflicts (e.g., dh2228fv display driver). + * ====================================================================== */ +#ifdef SPI_BITBANG + +#ifndef SPI_GPIO_CS +# define SPI_GPIO_CS 1 +# define SPI_GPIO_MOSI 2 +# define SPI_GPIO_SCK 3 +# define SPI_GPIO_MISO 4 +#endif + +static inline void bb_set(int gpio, int v) +{ + gpio_devmem_set(gpio, v); +} + +static inline int bb_get(int gpio) +{ + return gpio_devmem_get(gpio); +} + +static uint8_t bb_byte(uint8_t tx) +{ + uint8_t rx = 0; + int i; + for (i = 7; i >= 0; i--) { + bb_set(SPI_GPIO_MOSI, (tx >> i) & 1); + usleep(2); /* Setup time: MOSI stable before SCK rise */ + bb_set(SPI_GPIO_SCK, 1); /* Clock high */ + usleep(3); /* Hold time: MISO stable after SCK rise */ + rx = (uint8_t)((rx << 1) | bb_get(SPI_GPIO_MISO)); + bb_set(SPI_GPIO_SCK, 0); /* Clock low */ + usleep(2); /* Clock period: ~200 kHz effective SPI clock */ + } + return rx; +} + +int spi_initial(mcp2515_dev *mcp2515_device) +{ + /* Initialize /dev/mem GPIO access */ + if (gpio_devmem_init() != 0) { + printf("[SPI-BB] gpio_devmem_init failed (must run as root)\n"); + return -1; + } + + /* Configure GPIO directions */ + gpio_devmem_set_direction(SPI_GPIO_CS, 1); /* CS output */ + gpio_devmem_set_direction(SPI_GPIO_MOSI, 1); /* MOSI output */ + gpio_devmem_set_direction(SPI_GPIO_SCK, 1); /* SCK output */ + gpio_devmem_set_direction(SPI_GPIO_MISO, 0); /* MISO input */ + + /* Initialize pins: CS=high (deselected), SCK=low */ + bb_set(SPI_GPIO_CS, 1); + bb_set(SPI_GPIO_SCK, 0); + + mcp2515_device->spi_dev->fd = 1; /* non-zero = initialised */ + printf("[SPI-BB] spi open success (devmem bitbang CS=%d MOSI=%d SCK=%d MISO=%d)\n", + SPI_GPIO_CS, SPI_GPIO_MOSI, SPI_GPIO_SCK, SPI_GPIO_MISO); + return 0; +} + +uint8_t* spi_send(mcp2515_dev *mcp2515_device, uint8_t *data, int length) +{ + uint8_t *rx = (uint8_t *)malloc(sizeof(uint8_t) * length); + if (!rx) return NULL; + int i; + bb_set(SPI_GPIO_CS, 0); /* CS low: select chip */ + usleep(1); /* CS setup time */ + for (i = 0; i < length; i++) + rx[i] = bb_byte(data[i]); + bb_set(SPI_GPIO_CS, 1); /* CS high: deselect */ + usleep(10); /* CS hold + processing time */ + return rx; +} + +#else /* ---- hardware spidev ioctl path -------------------------------- */ + +#if SPI_HW_MANUAL_CS +static int s_hw_manual_cs_ready = 0; +static int s_hw_manual_cs_active = 0; + +static void hw_cs_set(int value) +{ + if (!s_hw_manual_cs_ready || !s_hw_manual_cs_active) return; + gpio_devmem_set(SPI_GPIO_CS, value ? 1 : 0); +} +#endif + +#ifndef SPI_DIAG_LOG +#define SPI_DIAG_LOG 0 +#endif + +/* Forward declaration: helpers below call spi_send before its definition. */ +uint8_t* spi_send(mcp2515_dev *mcp2515_device, uint8_t *data, int length); + +static int write_sysfs_text(const char *path, const char *text) +{ + int fd = open(path, O_WRONLY); + ssize_t wr; + if (fd < 0) return -1; + wr = write(fd, text, strlen(text)); + close(fd); + if (wr < 0) return -2; + return 0; +} + +static int read_sysfs_text(const char *path, char *buf, size_t buflen) +{ + int fd = open(path, O_RDONLY); + ssize_t rd; + if (fd < 0) return -1; + rd = read(fd, buf, buflen - 1); + close(fd); + if (rd < 0) return -2; + buf[rd] = '\0'; + return 0; +} + +static int spidev_path_to_spi_node(const char *spidev_path, char *node, size_t node_sz) +{ + const char *base = strrchr(spidev_path, '/'); + const char *name = base ? (base + 1) : spidev_path; + if (strncmp(name, "spidev", 6) != 0) return -1; + if (snprintf(node, node_sz, "spi%s", name + 6) >= (int)node_sz) return -2; + return 0; +} + +static int ensure_spidev_binding(const char *spidev_path) +{ + char spi_node[32] = {0}; + char path[128] = {0}; + char modalias[128] = {0}; + char driver_link[128] = {0}; + char driver_real[512] = {0}; + char unbind_path[512] = {0}; + + if (spidev_path_to_spi_node(spidev_path, spi_node, sizeof(spi_node)) != 0) { + return 0; + } + + snprintf(path, sizeof(path), "/sys/bus/spi/devices/%s/modalias", spi_node); + if (read_sysfs_text(path, modalias, sizeof(modalias)) != 0) { + printf("[SPI] cannot read %s\n", path); + return 0; + } + + /* trim trailing newline */ + { + size_t n = strlen(modalias); + while (n > 0 && (modalias[n - 1] == '\n' || modalias[n - 1] == '\r')) { + modalias[n - 1] = '\0'; + n--; + } + } + + printf("[SPI] %s modalias=%s\n", spi_node, modalias); + + if (strstr(modalias, "dh2228fv") == NULL) { + return 0; + } + + printf("[SPI] rebinding %s: dh2228fv -> spidev\n", spi_node); + + snprintf(driver_link, sizeof(driver_link), "/sys/bus/spi/devices/%s/driver", spi_node); + if (realpath(driver_link, driver_real) != NULL) { + snprintf(unbind_path, sizeof(unbind_path), "%s/unbind", driver_real); + if (write_sysfs_text(unbind_path, spi_node) != 0) { + printf("[SPI] warn: unbind via %s failed for %s (errno=%d)\n", unbind_path, spi_node, errno); + } + } else { + printf("[SPI] warn: resolve driver path failed for %s (errno=%d)\n", driver_link, errno); + } + + snprintf(path, sizeof(path), "/sys/bus/spi/devices/%s/driver_override", spi_node); + if (write_sysfs_text(path, "spidev") != 0) { + printf("[SPI] warn: write driver_override failed for %s (errno=%d)\n", spi_node, errno); + } + + if (write_sysfs_text("/sys/bus/spi/drivers/spidev/bind", spi_node) != 0) { + printf("[SPI] warn: bind spidev failed for %s (errno=%d)\n", spi_node, errno); + } + + if (read_sysfs_text(path, modalias, sizeof(modalias)) == 0) { + size_t n = strlen(modalias); + while (n > 0 && (modalias[n - 1] == '\n' || modalias[n - 1] == '\r')) { + modalias[n - 1] = '\0'; + n--; + } + printf("[SPI] %s modalias(after rebind)=%s\n", spi_node, modalias); + if (strstr(modalias, "dh2228fv") != NULL) { + printf("[SPI] ERROR: %s still bound to dh2228fv; platform DTS/driver binding must be changed\n", spi_node); + return -1; + } + } + + return 0; +} + +static int spi_apply_mode_speed(mcp2515_dev *mcp2515_device, uint8_t mode, uint32_t speed) +{ + int fd = mcp2515_device->spi_dev->fd; + int ret; + + mcp2515_device->spi_dev->mode = mode; + mcp2515_device->spi_dev->speed = speed; + + ret = ioctl(fd, SPI_IOC_WR_MODE, &(mcp2515_device->spi_dev->mode)); + if (ret == -1) return -1; + ret = ioctl(fd, SPI_IOC_RD_MODE, &(mcp2515_device->spi_dev->mode)); + if (ret == -1) return -2; + + ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &(mcp2515_device->spi_dev->speed)); + if (ret == -1) return -3; + ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &(mcp2515_device->spi_dev->speed)); + if (ret == -1) return -4; + + return 0; +} + +static int spi_probe_link_once(mcp2515_dev *mcp2515_device, uint8_t *out_canstat) +{ + uint8_t reset_cmd = INSTRUCTION_RESET; + uint8_t read_canstat[3] = {INSTRUCTION_READ, MCP_CANSTAT, 0x00}; + uint8_t *rx = NULL; + + rx = spi_send(mcp2515_device, &reset_cmd, 1); + if (!rx) return -1; + free(rx); + + usleep(10000); + rx = spi_send(mcp2515_device, read_canstat, 3); + if (!rx) return -2; + + *out_canstat = rx[2]; + free(rx); + return 0; +} + +static int spi_autoprobe_phase(mcp2515_dev *mcp2515_device, const char *tag, int use_manual_cs) +{ + static const uint8_t modes[] = {0, 1, 2, 3}; + static const uint32_t speeds[] = {1000000, 500000, 250000, 125000}; + size_t i, j; + +#if SPI_HW_MANUAL_CS + if (use_manual_cs && !s_hw_manual_cs_ready) { + if (SPI_DIAG_LOG) { + printf("[SPI-PROBE] phase=%s skipped (manual CS not available)\n", tag); + } + return 0; + } + s_hw_manual_cs_active = use_manual_cs ? 1 : 0; + if (s_hw_manual_cs_active) hw_cs_set(1); +#else + (void)use_manual_cs; +#endif + + if (SPI_DIAG_LOG) { + printf("[SPI-PROBE] phase=%s start\n", tag); + } + + for (i = 0; i < sizeof(modes) / sizeof(modes[0]); i++) { + for (j = 0; j < sizeof(speeds) / sizeof(speeds[0]); j++) { + uint8_t canstat = 0; + int rc = spi_apply_mode_speed(mcp2515_device, modes[i], speeds[j]); + if (rc != 0) { + if (SPI_DIAG_LOG) { + printf("[SPI-PROBE] phase=%s mode=%u speed=%u apply failed (%d, errno=%d)\n", + tag, modes[i], speeds[j], rc, errno); + } + continue; + } + + rc = spi_probe_link_once(mcp2515_device, &canstat); + if (rc != 0) { + if (SPI_DIAG_LOG) { + printf("[SPI-PROBE] phase=%s mode=%u speed=%u probe IO failed (%d)\n", tag, modes[i], speeds[j], rc); + } + continue; + } + + if (SPI_DIAG_LOG) { + printf("[SPI-PROBE] phase=%s mode=%u speed=%u -> CANSTAT=0x%02X\n", tag, modes[i], speeds[j], canstat); + } + if (canstat == 0x80) { + if (SPI_DIAG_LOG) { + printf("[SPI-PROBE] selected phase=%s mode=%u speed=%u (CONFIG mode ok)\n", tag, modes[i], speeds[j]); + } + return 1; + } + } + } + + return 0; +} + +static void spi_autoprobe(mcp2515_dev *mcp2515_device) +{ + if (spi_autoprobe_phase(mcp2515_device, "kernel-cs", 0)) return; +#if SPI_HW_MANUAL_CS + if (spi_autoprobe_phase(mcp2515_device, "manual-cs", 1)) return; +#endif + + spi_apply_mode_speed(mcp2515_device, 0, 1000000); + if (SPI_DIAG_LOG) { + printf("[SPI-PROBE] no valid combo found in any phase, fallback mode=0 speed=1000000\n"); + } +} + +int spi_initial(mcp2515_dev *mcp2515_device) +{ + int ret = 0; + int bind_ok = 0; + + bind_ok = ensure_spidev_binding(mcp2515_device->spi_dev->spidev_path); + if (bind_ok < 0) { + return -8; + } + + int fd = open(mcp2515_device->spi_dev->spidev_path, O_RDWR); + if (fd < 0) { + printf("[SPI] failed to open %s (errno=%d)\n", mcp2515_device->spi_dev->spidev_path, errno); + return -1; + } + mcp2515_device->spi_dev->fd = (uint8_t)fd; + + ret = ioctl(fd, SPI_IOC_WR_MODE, &(mcp2515_device->spi_dev->mode)); + if (ret == -1) { printf("[SPI] WR_MODE failed (errno=%d)\n", errno); return -2; } + ret = ioctl(fd, SPI_IOC_RD_MODE, &(mcp2515_device->spi_dev->mode)); + if (ret == -1) { printf("[SPI] RD_MODE failed (errno=%d)\n", errno); return -3; } + ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &(mcp2515_device->spi_dev->bits)); + if (ret == -1) { printf("[SPI] WR_BITS failed (errno=%d)\n", errno); return -4; } + ret = ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &(mcp2515_device->spi_dev->bits)); + if (ret == -1) { printf("[SPI] RD_BITS failed (errno=%d)\n", errno); return -5; } + ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &(mcp2515_device->spi_dev->speed)); + if (ret == -1) { printf("[SPI] WR_SPEED failed (errno=%d)\n", errno); return -6; } + ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &(mcp2515_device->spi_dev->speed)); + if (ret == -1) { printf("[SPI] RD_SPEED failed (errno=%d)\n", errno); return -7; } + +#if SPI_HW_MANUAL_CS + if (gpio_devmem_init() == 0) { + gpio_devmem_set_direction(SPI_GPIO_CS, 1); + s_hw_manual_cs_ready = 1; + s_hw_manual_cs_active = 1; + hw_cs_set(1); + s_hw_manual_cs_active = 0; + printf("[SPI] manual CS enabled on GPIO%d\n", SPI_GPIO_CS); + } else { + printf("[SPI] manual CS unavailable; fallback to kernel CS\n"); + } +#endif + + spi_autoprobe(mcp2515_device); + + printf("[SPI] open success: %s (fd=%d, mode=%d, bits=%d, speed=%u Hz)\n", + mcp2515_device->spi_dev->spidev_path, fd, mcp2515_device->spi_dev->mode, + mcp2515_device->spi_dev->bits, mcp2515_device->spi_dev->speed); + return 0; +} + +uint8_t* spi_send(mcp2515_dev *mcp2515_device, uint8_t *data, int length) +{ + int ret = 0; + uint8_t *tx_buffer = (uint8_t *)malloc(sizeof(uint8_t) * length); + memcpy(tx_buffer, data, sizeof(uint8_t) * length); + uint8_t *rx_buffer = (uint8_t *)malloc(sizeof(uint8_t) * length); + memset(rx_buffer, 0, sizeof(uint8_t) * length); + + struct spi_ioc_transfer tr = { + .tx_buf = (unsigned long)tx_buffer, + .rx_buf = (unsigned long)rx_buffer, + .len = length, + .delay_usecs = 200, + .speed_hz = mcp2515_device->spi_dev->speed, + .bits_per_word = mcp2515_device->spi_dev->bits, + }; + +#if SPI_HW_MANUAL_CS + hw_cs_set(0); + usleep(2); +#endif + + ret = ioctl(mcp2515_device->spi_dev->fd, SPI_IOC_MESSAGE(1), &tr); + +#if SPI_HW_MANUAL_CS + usleep(2); + hw_cs_set(1); +#endif + + if (ret < 1) { + printf("[SPI] ioctl failed: ret=%d, errno=%d, fd=%d, len=%d\n", ret, errno, mcp2515_device->spi_dev->fd, length); + exit(0); + } + free(tx_buffer); + return rx_buffer; +} + +#endif /* SPI_BITBANG */ + +/* ======================================================================== */ + +const struct TXBn_REGS TXB [N_TXBUFFERS] = { + {MCP_TXB0CTRL, MCP_TXB0SIDH, MCP_TXB0DATA}, + {MCP_TXB1CTRL, MCP_TXB1SIDH, MCP_TXB1DATA}, + {MCP_TXB2CTRL, MCP_TXB2SIDH, MCP_TXB2DATA} +}; + +const struct RXBn_REGS RXB [N_RXBUFFERS] = { + {MCP_RXB0CTRL, MCP_RXB0SIDH, MCP_RXB0DATA, CANINTF_RX0IF}, + {MCP_RXB1CTRL, MCP_RXB1SIDH, MCP_RXB1DATA, CANINTF_RX1IF} +}; + +mcp2515_dev* new_mcp2515_dev(char *spidev_path) +{ + mcp2515_dev* mcp2515_device = (mcp2515_dev*)malloc(sizeof(mcp2515_dev)); + mcp2515_device->spi_dev = (spi_description*)malloc(sizeof(spi_description)); + mcp2515_device->spi_dev->spidev_path = spidev_path; + mcp2515_device->spi_dev->mode = 0; + mcp2515_device->spi_dev->bits = 8; + mcp2515_device->spi_dev->speed = 1000000; + return mcp2515_device; +} +int mcp2515_initial(mcp2515_dev *mcp2515_device) +{ + int ret = 0; + // uint8_t data = 0; + ret = spi_initial(mcp2515_device); + if(ret!=0)return ret; + // Send RESET SPI cmd + mcp2515_reset(mcp2515_device); + return 0; +} + +uint8_t mcp2515_setMode(mcp2515_dev *mcp2515_device,const CANCTRL_REQOP_MODE mode) +{ + mcp2515_modify_bit(mcp2515_device,MCP_CANCTRL, CANCTRL_REQOP, mode); + usleep(10000); /* 10 ms: BITMOD instruction processing time */ + + uint8_t time_count = 0; + uint8_t modeMatch = 0; + const char *mode_str = "unknown"; + if (mode == CANCTRL_REQOP_CONFIG) mode_str = "CONFIG"; + else if (mode == CANCTRL_REQOP_LISTENONLY) mode_str = "LISTEN"; + else if (mode == CANCTRL_REQOP_LOOPBACK) mode_str = "LOOPBACK"; + else if (mode == CANCTRL_REQOP_NORMAL) mode_str = "NORMAL"; + + while (time_count < 10) { + uint8_t canstat_full = mcp2515_read_register_once(mcp2515_device,MCP_CANSTAT); + uint8_t newmode = canstat_full & CANSTAT_OPMOD; + + modeMatch = newmode == mode; + if (time_count == 0 || time_count == 9 || modeMatch) { + printf("[MCP] setMode(%s/0x%02X): attempt %d, CANSTAT=0x%02X (mode bits=0x%02X), expect=0x%02X %s\n", + mode_str, mode, time_count, canstat_full, newmode, mode, modeMatch ? "✓" : ""); + } + + if (modeMatch) { + break; + } + usleep(1000); /* 1 ms retry delay */ + time_count++; + } + if (!modeMatch) { + printf("[MCP] ERROR: setMode(%s) failed after %d attempts\n", mode_str, time_count); + } + return modeMatch ? ERROR_OK : ERROR_FAIL; + +} + +uint8_t mcp2515_setConfigMode(mcp2515_dev *mcp2515_device) +{ + return mcp2515_setMode(mcp2515_device,CANCTRL_REQOP_CONFIG); +} + +void mcp2515_prepareId(uint8_t *buffer, const uint8_t ext, const uint32_t id) +{ + uint16_t canid = (uint16_t)(id & 0x0FFFF); + + if (ext > 0) { + buffer[MCP_EID0] = (uint8_t) (canid & 0xFF); + buffer[MCP_EID8] = (uint8_t) (canid >> 8); + canid = (uint16_t)(id >> 16); + buffer[MCP_SIDL] = (uint8_t) (canid & 0x03); + buffer[MCP_SIDL] += (uint8_t) ((canid & 0x1C) << 3); + buffer[MCP_SIDL] |= TXB_EXIDE_MASK; + buffer[MCP_SIDH] = (uint8_t) (canid >> 5); + } else { + buffer[MCP_SIDH] = (uint8_t) (canid >> 3); + buffer[MCP_SIDL] = (uint8_t) ((canid & 0x07 ) << 5); + buffer[MCP_EID0] = 0; + buffer[MCP_EID8] = 0; + } +} + +int mcp2515_setFillter(mcp2515_dev *mcp2515_device,const RXF num, const uint8_t ext, const uint32_t ulData) +{ + ERROR res = mcp2515_setConfigMode(mcp2515_device); + if (res != ERROR_OK) { + return res; + } + + REGISTER reg; + + switch (num) { + case RXF0: reg = MCP_RXF0SIDH; break; + case RXF1: reg = MCP_RXF1SIDH; break; + case RXF2: reg = MCP_RXF2SIDH; break; + case RXF3: reg = MCP_RXF3SIDH; break; + case RXF4: reg = MCP_RXF4SIDH; break; + case RXF5: reg = MCP_RXF5SIDH; break; + default: + return ERROR_FAIL; + } + + uint8_t tbufdata[4]; + mcp2515_prepareId(tbufdata, ext, ulData); + mcp2515_write_register(mcp2515_device, reg, tbufdata, 4); + + return ERROR_OK; +} + +int mcp2515_setFilterMask(mcp2515_dev *mcp2515_device,const MASK mask, const uint8_t ext, const uint32_t ulData) +{ + ERROR res = mcp2515_setConfigMode(mcp2515_device); + if (res != ERROR_OK) { + return res; + } + + uint8_t tbufdata[4]; + mcp2515_prepareId(tbufdata, ext, ulData); + + uint8_t reg; + switch (mask) { + case MASK0: reg = MCP_RXM0SIDH; break; + case MASK1: reg = MCP_RXM1SIDH; break; + default: + return ERROR_FAIL; + } + + mcp2515_write_register(mcp2515_device, reg, tbufdata, 4); + + return ERROR_OK; +} + +int mcp2515_reset(mcp2515_dev *mcp2515_device) +{ + uint8_t data = 0; + // Send RESET SPI cmd + data = INSTRUCTION_RESET; + printf("[MCP] Sending RESET instruction (0x%02X)\n", INSTRUCTION_RESET); + uint8_t *test_ret = spi_send(mcp2515_device, &data, sizeof(data)/sizeof(uint8_t)); + if (test_ret) { + printf("[MCP] RESET response: rx[0]=0x%02X\n", test_ret[0]); + free(test_ret); + } + + usleep(100000); /* 100ms: MCP2515 reset + initialization time */ + printf("[MCP] After RESET: waiting for chip to stabilize...\n"); + + // Set to configuration mode + uint8_t zeros[14]; + memset(zeros, 0, sizeof(zeros)); + printf("mcp2515 clean MCP_TXBXCTRL\n"); + mcp2515_write_register(mcp2515_device,MCP_TXB0CTRL, zeros, 14); + mcp2515_write_register(mcp2515_device,MCP_TXB1CTRL, zeros, 14); + mcp2515_write_register(mcp2515_device,MCP_TXB2CTRL, zeros, 14); + printf("mcp2515 clean MCP_RXBXCTRL\n"); + mcp2515_write_register_once(mcp2515_device,MCP_RXB0CTRL, 0); + mcp2515_write_register_once(mcp2515_device,MCP_RXB1CTRL, 0); + printf("mcp2515 clean MCP_CANINTE\n"); + mcp2515_write_register_once(mcp2515_device, MCP_CANINTE, + CANINTF_RX0IF | CANINTF_RX1IF | CANINTF_ERRIF | CANINTF_MERRF); + printf("mcp2515 clean MCP_RXB0CTRL\n"); + mcp2515_modify_bit(mcp2515_device, MCP_RXB0CTRL, + RXBnCTRL_RXM_MASK | RXB0CTRL_BUKT | RXB0CTRL_FILHIT_MASK, + RXBnCTRL_RXM_STDEXT | RXB0CTRL_BUKT | RXB0CTRL_FILHIT); + printf("mcp2515 clean MCP_RXB1CTRL\n"); + mcp2515_modify_bit(mcp2515_device,MCP_RXB1CTRL, + RXBnCTRL_RXM_MASK | RXB1CTRL_FILHIT_MASK, + RXBnCTRL_RXM_STDEXT | RXB1CTRL_FILHIT); + // clear filters and masks + // do not filter any standard frames for RXF0 used by RXB0 + // do not filter any extended frames for RXF1 used by RXB1 + printf("mcp2515 clean RXF\n"); + RXF filters[] = {RXF0, RXF1, RXF2, RXF3, RXF4, RXF5}; + int i=0; + for (i=0; i<6; i++) { + uint8_t ext = 0; + if(i == 1){ + ext = 1; + } + else{ + ext = 0; + } + + ERROR result = mcp2515_setFillter(mcp2515_device,filters[i], ext, 0); + if (result != 0) { + return result; + } + } + + printf("mcp2515 clean mask\n"); + MASK masks[] = {MASK0, MASK1}; + for (i=0; i<2; i++) { + ERROR result = mcp2515_setFilterMask(mcp2515_device,masks[i], 1, 0); + if (result != 0) { + return result; + } + } + printf("mcp2515 reset success\n"); + return 0; +} + + +int mcp2515_can_speed(mcp2515_dev *mcp2515_device, const CAN_SPEED canSpeed, CAN_CLOCK canClock) +{ + ERROR error = mcp2515_setConfigMode(mcp2515_device); + if (error != ERROR_OK) { + return error; + } + + uint8_t set, cfg1, cfg2, cfg3; + set = 1; + + switch (canClock) + { + case (MCP_8MHZ): + switch (canSpeed) + { + case (CAN_5KBPS): // 5KBPS + cfg1 = MCP_8MHz_5kBPS_CFG1; + cfg2 = MCP_8MHz_5kBPS_CFG2; + cfg3 = MCP_8MHz_5kBPS_CFG3; + break; + + case (CAN_10KBPS): // 10KBPS + cfg1 = MCP_8MHz_10kBPS_CFG1; + cfg2 = MCP_8MHz_10kBPS_CFG2; + cfg3 = MCP_8MHz_10kBPS_CFG3; + break; + + case (CAN_20KBPS): // 20KBPS + cfg1 = MCP_8MHz_20kBPS_CFG1; + cfg2 = MCP_8MHz_20kBPS_CFG2; + cfg3 = MCP_8MHz_20kBPS_CFG3; + break; + + case (CAN_31K25BPS): // 31.25KBPS + cfg1 = MCP_8MHz_31k25BPS_CFG1; + cfg2 = MCP_8MHz_31k25BPS_CFG2; + cfg3 = MCP_8MHz_31k25BPS_CFG3; + break; + + case (CAN_33KBPS): // 33.333KBPS + cfg1 = MCP_8MHz_33k3BPS_CFG1; + cfg2 = MCP_8MHz_33k3BPS_CFG2; + cfg3 = MCP_8MHz_33k3BPS_CFG3; + break; + + case (CAN_40KBPS): // 40Kbps + cfg1 = MCP_8MHz_40kBPS_CFG1; + cfg2 = MCP_8MHz_40kBPS_CFG2; + cfg3 = MCP_8MHz_40kBPS_CFG3; + break; + + case (CAN_50KBPS): // 50Kbps + cfg1 = MCP_8MHz_50kBPS_CFG1; + cfg2 = MCP_8MHz_50kBPS_CFG2; + cfg3 = MCP_8MHz_50kBPS_CFG3; + break; + + case (CAN_80KBPS): // 80Kbps + cfg1 = MCP_8MHz_80kBPS_CFG1; + cfg2 = MCP_8MHz_80kBPS_CFG2; + cfg3 = MCP_8MHz_80kBPS_CFG3; + break; + + case (CAN_100KBPS): // 100Kbps + cfg1 = MCP_8MHz_100kBPS_CFG1; + cfg2 = MCP_8MHz_100kBPS_CFG2; + cfg3 = MCP_8MHz_100kBPS_CFG3; + break; + + case (CAN_125KBPS): // 125Kbps + cfg1 = MCP_8MHz_125kBPS_CFG1; + cfg2 = MCP_8MHz_125kBPS_CFG2; + cfg3 = MCP_8MHz_125kBPS_CFG3; + break; + + case (CAN_200KBPS): // 200Kbps + cfg1 = MCP_8MHz_200kBPS_CFG1; + cfg2 = MCP_8MHz_200kBPS_CFG2; + cfg3 = MCP_8MHz_200kBPS_CFG3; + break; + + case (CAN_250KBPS): // 250Kbps + cfg1 = MCP_8MHz_250kBPS_CFG1; + cfg2 = MCP_8MHz_250kBPS_CFG2; + cfg3 = MCP_8MHz_250kBPS_CFG3; + break; + + case (CAN_500KBPS): // 500Kbps + cfg1 = MCP_8MHz_500kBPS_CFG1; + cfg2 = MCP_8MHz_500kBPS_CFG2; + cfg3 = MCP_8MHz_500kBPS_CFG3; + break; + + case (CAN_1000KBPS): // 1Mbps + cfg1 = MCP_8MHz_1000kBPS_CFG1; + cfg2 = MCP_8MHz_1000kBPS_CFG2; + cfg3 = MCP_8MHz_1000kBPS_CFG3; + break; + + default: + set = 0; + break; + } + break; + + case (MCP_16MHZ): + switch (canSpeed) + { + case (CAN_5KBPS): // 5Kbps + cfg1 = MCP_16MHz_5kBPS_CFG1; + cfg2 = MCP_16MHz_5kBPS_CFG2; + cfg3 = MCP_16MHz_5kBPS_CFG3; + break; + + case (CAN_10KBPS): // 10Kbps + cfg1 = MCP_16MHz_10kBPS_CFG1; + cfg2 = MCP_16MHz_10kBPS_CFG2; + cfg3 = MCP_16MHz_10kBPS_CFG3; + break; + + case (CAN_20KBPS): // 20Kbps + cfg1 = MCP_16MHz_20kBPS_CFG1; + cfg2 = MCP_16MHz_20kBPS_CFG2; + cfg3 = MCP_16MHz_20kBPS_CFG3; + break; + + case (CAN_33KBPS): // 33.333Kbps + cfg1 = MCP_16MHz_33k3BPS_CFG1; + cfg2 = MCP_16MHz_33k3BPS_CFG2; + cfg3 = MCP_16MHz_33k3BPS_CFG3; + break; + + case (CAN_40KBPS): // 40Kbps + cfg1 = MCP_16MHz_40kBPS_CFG1; + cfg2 = MCP_16MHz_40kBPS_CFG2; + cfg3 = MCP_16MHz_40kBPS_CFG3; + break; + + case (CAN_50KBPS): // 50Kbps + cfg1 = MCP_16MHz_50kBPS_CFG1; + cfg2 = MCP_16MHz_50kBPS_CFG2; + cfg3 = MCP_16MHz_50kBPS_CFG3; + break; + + case (CAN_80KBPS): // 80Kbps + cfg1 = MCP_16MHz_80kBPS_CFG1; + cfg2 = MCP_16MHz_80kBPS_CFG2; + cfg3 = MCP_16MHz_80kBPS_CFG3; + break; + + case (CAN_83K3BPS): // 83.333Kbps + cfg1 = MCP_16MHz_83k3BPS_CFG1; + cfg2 = MCP_16MHz_83k3BPS_CFG2; + cfg3 = MCP_16MHz_83k3BPS_CFG3; + break; + + case (CAN_95KBPS): // 95Kbps + cfg1 = MCP_16MHz_95kBPS_CFG1; + cfg2 = MCP_16MHz_95kBPS_CFG2; + cfg3 = MCP_16MHz_95kBPS_CFG3; + break; + + case (CAN_100KBPS): // 100Kbps + cfg1 = MCP_16MHz_100kBPS_CFG1; + cfg2 = MCP_16MHz_100kBPS_CFG2; + cfg3 = MCP_16MHz_100kBPS_CFG3; + break; + + case (CAN_125KBPS): // 125Kbps + cfg1 = MCP_16MHz_125kBPS_CFG1; + cfg2 = MCP_16MHz_125kBPS_CFG2; + cfg3 = MCP_16MHz_125kBPS_CFG3; + break; + + case (CAN_200KBPS): // 200Kbps + cfg1 = MCP_16MHz_200kBPS_CFG1; + cfg2 = MCP_16MHz_200kBPS_CFG2; + cfg3 = MCP_16MHz_200kBPS_CFG3; + break; + + case (CAN_250KBPS): // 250Kbps + cfg1 = MCP_16MHz_250kBPS_CFG1; + cfg2 = MCP_16MHz_250kBPS_CFG2; + cfg3 = MCP_16MHz_250kBPS_CFG3; + break; + + case (CAN_500KBPS): // 500Kbps + cfg1 = MCP_16MHz_500kBPS_CFG1; + cfg2 = MCP_16MHz_500kBPS_CFG2; + cfg3 = MCP_16MHz_500kBPS_CFG3; + break; + + case (CAN_1000KBPS): // 1Mbps + cfg1 = MCP_16MHz_1000kBPS_CFG1; + cfg2 = MCP_16MHz_1000kBPS_CFG2; + cfg3 = MCP_16MHz_1000kBPS_CFG3; + break; + + default: + set = 0; + break; + } + break; + + case (MCP_20MHZ): + switch (canSpeed) + { + case (CAN_33KBPS): // 33.333Kbps + cfg1 = MCP_20MHz_33k3BPS_CFG1; + cfg2 = MCP_20MHz_33k3BPS_CFG2; + cfg3 = MCP_20MHz_33k3BPS_CFG3; + break; + + case (CAN_40KBPS): // 40Kbps + cfg1 = MCP_20MHz_40kBPS_CFG1; + cfg2 = MCP_20MHz_40kBPS_CFG2; + cfg3 = MCP_20MHz_40kBPS_CFG3; + break; + + case (CAN_50KBPS): // 50Kbps + cfg1 = MCP_20MHz_50kBPS_CFG1; + cfg2 = MCP_20MHz_50kBPS_CFG2; + cfg3 = MCP_20MHz_50kBPS_CFG3; + break; + + case (CAN_80KBPS): // 80Kbps + cfg1 = MCP_20MHz_80kBPS_CFG1; + cfg2 = MCP_20MHz_80kBPS_CFG2; + cfg3 = MCP_20MHz_80kBPS_CFG3; + break; + + case (CAN_83K3BPS): // 83.333Kbps + cfg1 = MCP_20MHz_83k3BPS_CFG1; + cfg2 = MCP_20MHz_83k3BPS_CFG2; + cfg3 = MCP_20MHz_83k3BPS_CFG3; + break; + + case (CAN_100KBPS): // 100Kbps + cfg1 = MCP_20MHz_100kBPS_CFG1; + cfg2 = MCP_20MHz_100kBPS_CFG2; + cfg3 = MCP_20MHz_100kBPS_CFG3; + break; + + case (CAN_125KBPS): // 125Kbps + cfg1 = MCP_20MHz_125kBPS_CFG1; + cfg2 = MCP_20MHz_125kBPS_CFG2; + cfg3 = MCP_20MHz_125kBPS_CFG3; + break; + + case (CAN_200KBPS): // 200Kbps + cfg1 = MCP_20MHz_200kBPS_CFG1; + cfg2 = MCP_20MHz_200kBPS_CFG2; + cfg3 = MCP_20MHz_200kBPS_CFG3; + break; + + case (CAN_250KBPS): // 250Kbps + cfg1 = MCP_20MHz_250kBPS_CFG1; + cfg2 = MCP_20MHz_250kBPS_CFG2; + cfg3 = MCP_20MHz_250kBPS_CFG3; + break; + + case (CAN_500KBPS): // 500Kbps + cfg1 = MCP_20MHz_500kBPS_CFG1; + cfg2 = MCP_20MHz_500kBPS_CFG2; + cfg3 = MCP_20MHz_500kBPS_CFG3; + break; + + case (CAN_1000KBPS): // 1Mbps + cfg1 = MCP_20MHz_1000kBPS_CFG1; + cfg2 = MCP_20MHz_1000kBPS_CFG2; + cfg3 = MCP_20MHz_1000kBPS_CFG3; + break; + + default: + set = 0; + break; + } + break; + + default: + set = 0; + break; + } + if (set) { + mcp2515_write_register_once(mcp2515_device, MCP_CNF1, cfg1); + mcp2515_write_register_once(mcp2515_device, MCP_CNF2, cfg2); + mcp2515_write_register_once(mcp2515_device, MCP_CNF3, cfg3); + return ERROR_OK; + } + else { + return ERROR_FAIL; + } +} + +int mcp2515_write_register_once(mcp2515_dev *mcp2515_device, uint8_t register_address, uint8_t data) +{ + // tx_buffer length = write_cmd + register_address + 1 + uint8_t *tx_buffer = (uint8_t*)malloc(sizeof(uint8_t) * 3); + tx_buffer[0] = INSTRUCTION_WRITE; + tx_buffer[1] = register_address; + tx_buffer[2] = data; + // send + uint8_t *spi_ret = spi_send(mcp2515_device,tx_buffer,3); + free(spi_ret); + free(tx_buffer); + return 0; +} + +int mcp2515_write_register(mcp2515_dev *mcp2515_device, uint8_t register_address, uint8_t *data, int8_t data_length) +{ + // tx_buffer length = write_cmd + register_address + data_length + uint8_t *tx_buffer = (uint8_t*)malloc(sizeof(uint8_t) * (data_length + 2)); + tx_buffer[0] = INSTRUCTION_WRITE; + tx_buffer[1] = register_address; + memcpy(tx_buffer + 2, data, sizeof(uint8_t) * data_length); + // send + uint8_t *spi_ret = spi_send(mcp2515_device,tx_buffer,data_length + 2); + free(spi_ret); + free(tx_buffer); + return 0; +} + +void mcp2515_read_register(mcp2515_dev *mcp2515_device, uint8_t register_address, uint8_t values[], uint8_t data_length) +{ + // tx buffer + // uint8_t result = 0; + uint8_t *tx_buffer = (uint8_t*)malloc(sizeof(uint8_t)*(data_length + 2)); + tx_buffer[0] = INSTRUCTION_READ; + tx_buffer[1] = register_address; + // rx + // uint8_t *rx_buffer = (uint8_t*)malloc(sizeof(uint8_t)*(data_length + 2)); + // + uint8_t *spi_ret = spi_send(mcp2515_device,tx_buffer,(data_length + 2)); + memcpy(values, spi_ret + 2, sizeof(uint8_t) * data_length); + // free(rx_buffer); + free(tx_buffer); + // result = spi_ret[2]; + free(spi_ret); + // return 0; +} + +uint8_t mcp2515_read_register_once(mcp2515_dev *mcp2515_device, uint8_t register_address) +{ + // tx buffer + uint8_t result = 0; + uint8_t *tx_buffer = (uint8_t*)malloc(sizeof(uint8_t)*3); + tx_buffer[0] = INSTRUCTION_READ; + tx_buffer[1] = register_address; + tx_buffer[2] = 0x00; + + uint8_t *spi_ret = spi_send(mcp2515_device, tx_buffer, 3); + free(tx_buffer); + + if (spi_ret) { + result = spi_ret[2]; + if (SPI_DIAG_LOG && register_address == MCP_CANSTAT) { + printf("[SPI-READ] reg=0x%02X: rx[0]=0x%02X rx[1]=0x%02X rx[2]=0x%02X (result=0x%02X)\n", + register_address, spi_ret[0], spi_ret[1], spi_ret[2], result); + } + free(spi_ret); + } + return result; +} + +uint8_t mcp2515_read_status(mcp2515_dev *mcp2515_device) +{ + uint8_t read_status[2] = {INSTRUCTION_READ_STATUS,0x00}; + uint8_t *spi_ret = spi_send(mcp2515_device, read_status, 2); + uint8_t result = spi_ret[1]; + free(spi_ret); + return result; +} + +void mcp2515_modify_bit(mcp2515_dev *mcp2515_device, uint8_t register_address, uint8_t mask, uint8_t data) +{ + uint8_t *tx_buffer = (uint8_t*)malloc(sizeof(uint8_t)*4); + tx_buffer[0] = INSTRUCTION_BITMOD; + tx_buffer[1] = register_address; + tx_buffer[2] = mask; + tx_buffer[3] = data; + uint8_t *rx_data = spi_send(mcp2515_device,tx_buffer,4); + free(tx_buffer); + free(rx_data); +} +int mcp2515_sendMessage(mcp2515_dev *mcp2515_device, uint32_t can_id, uint8_t* data, uint8_t data_length) +{ +//printf("%s: func:%s,line:%d \n", __FILE__, __func__,__LINE__); + if (data_length > 8) { + return ERROR_FAILTX; + } + TXBn txBuffers[N_TXBUFFERS] = {TXB0, TXB1, TXB2}; + int i=0; + for (i=0; iCTRL); + if ( (ctrlval & TXB_TXREQ) == 0 ) { + printf("Send Message: %x\n",txbuf->CTRL); + return mcp2515_sendMessageT(mcp2515_device,txBuffers[i], can_id, data, data_length); + } + } + //printf("%s: func:%s,line:%d \n", __FILE__, __func__,__LINE__); + return ERROR_ALLTXBUSY; +} + +int mcp2515_sendMessageT(mcp2515_dev *mcp2515_device, const TXBn txbn, uint32_t can_id, uint8_t* data, uint8_t data_length) +{ + if (data_length > 8) { + return ERROR_FAILTX; + } + printf("%x %x ",can_id,data_length); + int i=0; + for(i=0;iSIDH, data1, 5 + data_length); + + mcp2515_modify_bit(mcp2515_device,txbuf->CTRL, TXB_TXREQ, TXB_TXREQ); + + /* Wait for this TX buffer to complete, otherwise burst sends can keep + * all three TXBn in-flight and repeatedly report ERROR_ALLTXBUSY. */ + { + int retry; + uint8_t saw_txerr = 0; + for (retry = 0; retry < 50; retry++) { + uint8_t ctrl = mcp2515_read_register_once(mcp2515_device,txbuf->CTRL); + + if ((ctrl & (TXB_ABTF | TXB_MLOA)) != 0) { + printf("ERROR\n"); + return ERROR_FAILTX; + } + + if (ctrl & TXB_TXERR) { + saw_txerr = 1; + } + + if ((ctrl & TXB_TXREQ) == 0) { + if (saw_txerr) { + printf("ERROR\n"); + return ERROR_FAILTX; + } + printf("OK\n"); + return ERROR_OK; + } + usleep(1000); + } + } + + /* Timed out waiting TXREQ clear; treat as busy so caller can retry. */ + return ERROR_ALLTXBUSY; +} + +int mcp2515_readMessage(mcp2515_dev *mcp2515_device,canid_t *can_id, uint8_t* data, uint8_t *data_length) +{ + ERROR rc; + uint8_t stat = mcp2515_read_status(mcp2515_device); + + if ( stat & STAT_RX0IF ) { + rc = mcp2515_readMessageT(mcp2515_device,RXB0, can_id, data, data_length); + } else if ( stat & STAT_RX1IF ) { + rc = mcp2515_readMessageT(mcp2515_device,RXB1, can_id, data, data_length); + } else { + rc = ERROR_NOMSG; + } + + return rc; +} + +int mcp2515_readMessageT(mcp2515_dev *mcp2515_device,const RXBn rxbn,canid_t *can_id, uint8_t* data, uint8_t *data_length) +{ + const struct RXBn_REGS *rxb = &RXB[rxbn]; + + uint8_t tbufdata[5]; + + mcp2515_read_register(mcp2515_device,rxb->SIDH, tbufdata, 5); + // printf("%x %x %x %x %x \n",tbufdata[MCP_SIDH],tbufdata[MCP_SIDL],tbufdata[MCP_EID8],tbufdata[MCP_EID0],tbufdata[MCP_DLC]); + uint32_t id = (tbufdata[MCP_SIDH]<<3) | (tbufdata[MCP_SIDL]>>5); + // printf("id1: %x\n",id); + if ( (tbufdata[MCP_SIDL] & TXB_EXIDE_MASK) == TXB_EXIDE_MASK ) { + id = (id<<2) + (tbufdata[MCP_SIDL] & 0x03); + id = (id<<8) + tbufdata[MCP_EID8]; + id = (id<<8) + tbufdata[MCP_EID0]; + id |= CAN_EFF_FLAG; + } + // printf("id2: %x\n",id); + uint8_t dlc = (tbufdata[MCP_DLC] & DLC_MASK); + if (dlc > CAN_MAX_DLEN) { + return ERROR_FAIL; + } + + uint8_t ctrl = mcp2515_read_register_once(mcp2515_device,rxb->CTRL); + if (ctrl & RXBnCTRL_RTR) { + id |= CAN_RTR_FLAG; + } + // printf("id3: %x\n",id); + *can_id = id & CAN_SFF_MASK; // CAN_SFF_MASK = 0x7FF (11bit mask) + // *can_id = id; + *data_length = dlc; + + mcp2515_read_register(mcp2515_device,rxb->DATA, data, dlc); + + mcp2515_modify_bit(mcp2515_device,MCP_CANINTF, rxb->CANINTF_RXnIF, 0); + + return ERROR_OK; +} \ No newline at end of file diff --git a/src/host_stream/sha256.h b/src/host_stream/sha256.h new file mode 100644 index 0000000..7a4301c --- /dev/null +++ b/src/host_stream/sha256.h @@ -0,0 +1,159 @@ +/* + * sha256.h — Standalone RFC 6234 SHA-256, header-only (static inline). + * No external dependencies beyond and . + * + * Test vector: + * sha256("abc", 3, d) → + * ba7816bf 8f01cfea 414140de 5dae2ec7 3b00361b bde327b6 0b82c10f 46850c58 + */ +#ifndef SHA256_H +#define SHA256_H + +#include +#include +#include + +typedef struct { + uint32_t state[8]; + uint64_t count; /* total bits processed */ + uint8_t buf[64]; /* partial block */ +} sha256_ctx; + +/* ── Round constants (first 32 bits of cube roots of first 64 primes) ── */ +static const uint32_t SHA256_K[64] = { + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, + 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, + 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, + 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, + 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, + 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, + 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, + 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, + 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, + 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, + 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, + 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 +}; + +/* ── Bit operations ── */ +#define SHA256_ROTR(x, n) (((x) >> (n)) | ((x) << (32 - (n)))) +#define SHA256_CH(e, f, g) (((e) & (f)) ^ (~(e) & (g))) +#define SHA256_MAJ(a, b, c) (((a) & (b)) ^ ((a) & (c)) ^ ((b) & (c))) +#define SHA256_EP0(a) (SHA256_ROTR(a, 2) ^ SHA256_ROTR(a, 13) ^ SHA256_ROTR(a, 22)) +#define SHA256_EP1(e) (SHA256_ROTR(e, 6) ^ SHA256_ROTR(e, 11) ^ SHA256_ROTR(e, 25)) +#define SHA256_SIG0(w)(SHA256_ROTR(w, 7) ^ SHA256_ROTR(w, 18) ^ ((w) >> 3)) +#define SHA256_SIG1(w)(SHA256_ROTR(w, 17) ^ SHA256_ROTR(w, 19) ^ ((w) >> 10)) + +/* ── Process one 64-byte block ── */ +static inline void sha256_transform(sha256_ctx *ctx, const uint8_t blk[64]) +{ + uint32_t w[64]; + uint32_t a, b, c, d, e, f, g, h, t1, t2; + int i; + + for (i = 0; i < 16; i++) + w[i] = ((uint32_t)blk[i*4 ] << 24) | ((uint32_t)blk[i*4+1] << 16) + | ((uint32_t)blk[i*4+2] << 8) | (uint32_t)blk[i*4+3]; + for (i = 16; i < 64; i++) + w[i] = SHA256_SIG1(w[i-2]) + w[i-7] + SHA256_SIG0(w[i-15]) + w[i-16]; + + a = ctx->state[0]; b = ctx->state[1]; c = ctx->state[2]; d = ctx->state[3]; + e = ctx->state[4]; f = ctx->state[5]; g = ctx->state[6]; h = ctx->state[7]; + + for (i = 0; i < 64; i++) { + t1 = h + SHA256_EP1(e) + SHA256_CH(e, f, g) + SHA256_K[i] + w[i]; + t2 = SHA256_EP0(a) + SHA256_MAJ(a, b, c); + h = g; g = f; f = e; e = d + t1; + d = c; c = b; b = a; a = t1 + t2; + } + + ctx->state[0] += a; ctx->state[1] += b; ctx->state[2] += c; ctx->state[3] += d; + ctx->state[4] += e; ctx->state[5] += f; ctx->state[6] += g; ctx->state[7] += h; +} + +/* ── Public API ── */ +static inline void sha256_init(sha256_ctx *ctx) +{ + ctx->state[0] = 0x6a09e667; ctx->state[1] = 0xbb67ae85; + ctx->state[2] = 0x3c6ef372; ctx->state[3] = 0xa54ff53a; + ctx->state[4] = 0x510e527f; ctx->state[5] = 0x9b05688c; + ctx->state[6] = 0x1f83d9ab; ctx->state[7] = 0x5be0cd19; + ctx->count = 0; + memset(ctx->buf, 0, sizeof(ctx->buf)); +} + +static inline void sha256_update(sha256_ctx *ctx, const uint8_t *data, size_t len) +{ + /* bytes already in partial block */ + size_t used = (size_t)((ctx->count / 8) % 64); + ctx->count += (uint64_t)len * 8; + + if (used) { + size_t fill = 64 - used; + if (len < fill) { + memcpy(ctx->buf + used, data, len); + return; + } + memcpy(ctx->buf + used, data, fill); + sha256_transform(ctx, ctx->buf); + data += fill; + len -= fill; + } + + while (len >= 64) { + sha256_transform(ctx, data); + data += 64; + len -= 64; + } + + if (len) + memcpy(ctx->buf, data, len); +} + +static inline void sha256_final(sha256_ctx *ctx, uint8_t digest[32]) +{ + /* Padding: append 0x80, zeros, then 64-bit big-endian bit count */ + uint64_t bits = ctx->count; + size_t used = (size_t)((ctx->count / 8) % 64); + uint8_t pad[64]; + int i; + + memset(pad, 0, sizeof(pad)); + pad[0] = 0x80; + + if (used < 56) { + /* Enough room in current block: pad to byte 56 */ + sha256_update(ctx, pad, 56 - used); + } else { + /* Need an extra block: fill to end of current, then 56 zero bytes */ + sha256_update(ctx, pad, 64 - used + 56); + } + + /* Append 64-bit bit-length (big-endian) */ + for (i = 0; i < 8; i++) + pad[i] = (uint8_t)(bits >> (56 - 8 * i)); + sha256_update(ctx, pad, 8); + + /* Extract digest from state */ + for (i = 0; i < 8; i++) { + digest[i*4 + 0] = (uint8_t)(ctx->state[i] >> 24); + digest[i*4 + 1] = (uint8_t)(ctx->state[i] >> 16); + digest[i*4 + 2] = (uint8_t)(ctx->state[i] >> 8); + digest[i*4 + 3] = (uint8_t)(ctx->state[i] ); + } +} + +/* One-shot convenience: init + update + final */ +static inline void sha256(const uint8_t *data, size_t len, uint8_t digest[32]) +{ + sha256_ctx ctx; + sha256_init(&ctx); + sha256_update(&ctx, data, len); + sha256_final(&ctx, digest); +} + +#endif /* SHA256_H */ diff --git a/src/pre_post_proc/stdc_post_process.c b/src/pre_post_proc/stdc_post_process.c index d6dafb6..6f2402d 100644 --- a/src/pre_post_proc/stdc_post_process.c +++ b/src/pre_post_proc/stdc_post_process.c @@ -141,15 +141,12 @@ int stdc_post_process(int model_id, struct kdp_image_s *image_p) uint32_t fwd_total = 0, fwd_grass = 0, fwd_tree = 0; uint32_t col_total = 0; uint32_t col_count[STDC_NUM_CLASSES] = {0}; + uint32_t left_total = 0, right_total = 0; + uint32_t left_count[STDC_NUM_CLASSES] = {0}; + uint32_t right_count[STDC_NUM_CLASSES] = {0}; float motion_diff; uint8_t is_moving = 1; - /* Collision ROI boundaries in pixel coords */ - uint32_t col_r0 = (uint32_t)(COL_ROI_LEFT * num_col); - uint32_t col_r1 = (uint32_t)(COL_ROI_RIGHT * num_col); - uint32_t col_c0 = (uint32_t)(COL_ROI_TOP * num_row); - uint32_t col_c1 = (uint32_t)(COL_ROI_BOTTOM * num_row); - motion_diff = stdc_compute_motion_diff(image_p, &is_moving); for (uint32_t r = 0; r < num_row; r++) { @@ -172,8 +169,10 @@ int stdc_post_process(int model_id, struct kdp_image_s *image_p) if (seg_idx < STDC_SEG_MAP_MAX) result->seg_map[seg_idx] = (uint8_t)best_cls; - /* Forward ROI */ - if (stdc_is_in_forward_roi(r, c, num_row, num_col)) { + int in_trapezoid_roi = stdc_is_in_forward_roi(r, c, num_row, num_col); + + /* Forward ROI (web trapezoid) */ + if (in_trapezoid_roi) { fwd_total++; if (best_cls == STDC_CLASS_GRASS) fwd_grass++; @@ -181,11 +180,22 @@ int stdc_post_process(int model_id, struct kdp_image_s *image_p) fwd_tree++; } - /* Collision ROI */ - if (r >= col_c0 && r < col_c1 && c >= col_r0 && c < col_r1) { + /* Collision ROI: align with web stream trapezoid ROI */ + if (in_trapezoid_roi) { col_total++; col_count[best_cls]++; } + + /* Left ROI: x < 25% of image width */ + if (c < (uint32_t)((float)num_col * 0.25f)) { + left_total++; + left_count[best_cls]++; + } + /* Right ROI: x > 75% of image width */ + if (c > (uint32_t)((float)num_col * 0.75f)) { + right_total++; + right_count[best_cls]++; + } } } @@ -219,6 +229,67 @@ int stdc_post_process(int model_id, struct kdp_image_s *image_p) ana->col_bunker_ratio = (float)col_count[STDC_CLASS_BUNKER] * inv_col; ana->col_pond_ratio = (float)col_count[STDC_CLASS_POND] * inv_col; + /* Left / Right ROI ratios */ + float inv_left = (left_total > 0) ? 1.0f / (float)left_total : 0.0f; + float inv_right = (right_total > 0) ? 1.0f / (float)right_total : 0.0f; + ana->left_person_ratio = (float)left_count[STDC_CLASS_PERSON] * inv_left; + ana->left_car_ratio = (float)left_count[STDC_CLASS_CAR] * inv_left; + ana->left_tree_ratio = (float)left_count[STDC_CLASS_TREE] * inv_left; + ana->left_pond_ratio = (float)left_count[STDC_CLASS_POND] * inv_left; + ana->left_bunker_ratio = (float)left_count[STDC_CLASS_BUNKER] * inv_left; + + ana->right_person_ratio = (float)right_count[STDC_CLASS_PERSON] * inv_right; + ana->right_car_ratio = (float)right_count[STDC_CLASS_CAR] * inv_right; + ana->right_tree_ratio = (float)right_count[STDC_CLASS_TREE] * inv_right; + ana->right_pond_ratio = (float)right_count[STDC_CLASS_POND] * inv_right; + ana->right_bunker_ratio = (float)right_count[STDC_CLASS_BUNKER] * inv_right; + + /* Left alert: highest-ratio class that exceeds its collision threshold */ + ana->left_alert = 0; + ana->left_type[0] = '\0'; + { + struct { float ratio; float thr; const char *name; } lc[] = { + { ana->left_person_ratio, THR_PERSON_COLLISION, "person" }, + { ana->left_car_ratio, THR_CAR_COLLISION, "vehicle" }, + { ana->left_tree_ratio, THR_TREE_COLLISION, "tree" }, + { ana->left_pond_ratio, THR_POND_COLLISION, "water_hazard" }, + { ana->left_bunker_ratio, THR_BUNKER_COLLISION, "bush" }, + }; + float best = 0.0f; + for (int i = 0; i < 5; i++) { + if (lc[i].ratio >= lc[i].thr) { + ana->left_alert = 1; + if (lc[i].ratio > best) { + best = lc[i].ratio; + snprintf(ana->left_type, sizeof(ana->left_type), "%s", lc[i].name); + } + } + } + } + + /* Right alert: same logic */ + ana->right_alert = 0; + ana->right_type[0] = '\0'; + { + struct { float ratio; float thr; const char *name; } rc[] = { + { ana->right_person_ratio, THR_PERSON_COLLISION, "person" }, + { ana->right_car_ratio, THR_CAR_COLLISION, "vehicle" }, + { ana->right_tree_ratio, THR_TREE_COLLISION, "tree" }, + { ana->right_pond_ratio, THR_POND_COLLISION, "water_hazard" }, + { ana->right_bunker_ratio, THR_BUNKER_COLLISION, "bush" }, + }; + float best = 0.0f; + for (int i = 0; i < 5; i++) { + if (rc[i].ratio >= rc[i].thr) { + ana->right_alert = 1; + if (rc[i].ratio > best) { + best = rc[i].ratio; + snprintf(ana->right_type, sizeof(ana->right_type), "%s", rc[i].name); + } + } + } + } + /* ------------------------------------------------------- * 3. Warning flags * ------------------------------------------------------- */ diff --git a/tools/device/deploy.sh b/tools/device/deploy.sh index 3812ade..cac59d3 100644 --- a/tools/device/deploy.sh +++ b/tools/device/deploy.sh @@ -13,7 +13,7 @@ set -e -HOST_URL="http://192.168.0.114:8080" +HOST_URL="http://192.168.0.105:8080" BIN_DIR=/mnt/flash/plus/kp_firmware/kp_firmware_0/kp_firmware/bin FW=/mnt/flash/vienna/kp_firmware_host_stream INI=$BIN_DIR/ini/host_stream.ini diff --git a/tools/mock_server/server.py b/tools/mock_server/server.py index f5ee4e7..83f98bb 100644 --- a/tools/mock_server/server.py +++ b/tools/mock_server/server.py @@ -6,6 +6,10 @@ Two independent channels: Channel A (iPad / BLE path): POST /api/event → real-time violation JSON Channel B (OOB / Cloud path): POST /api/upload → tar.gz event archive +Channel C (CAN bus): + POST /api/can/send → send a CAN frame via SocketCAN (PCAN-USB FD) + GET /api/can/status → {available, channel, bitrate, last_error} + Plus: GET /api/time → provides UTC time to KL630 (no NTP needed) GET / → web dashboard @@ -21,6 +25,299 @@ import threading from http.server import HTTPServer, BaseHTTPRequestHandler from urllib.parse import urlparse, parse_qs +# ── CAN bus support (python-can, optional) ─────────────────────────────────── +try: + import can + _CAN_AVAILABLE = True +except ImportError: + _CAN_AVAILABLE = False + +# Event type → CAN Data[0] mapping (must match can_bus.h on KL630) +_CAN_EVT = { + 'boot': 0x00, + 'road': 0x01, + 'grass': 0x02, + 'car': 0x03, + 'person': 0x04, + 'pond': 0x05, + 'bunker': 0x06, + 'tree': 0x07, + 'hazard': 0x08, +} + +class CanBus: + """Thread-safe SocketCAN wrapper around python-can Bus. + + Sends CAN frames (test buttons) AND receives frames from the KL630 + device, reassembling multi-frame JSON strings and appending them to + the shared events list (same as Channel A). + """ + DEFAULT_CHANNEL = 'can0' + DEFAULT_BITRATE = 250000 # 250 kbps — must match device INI + + def __init__(self): + self._lock = threading.Lock() + self._bus = None + self._channel = self.DEFAULT_CHANNEL + self._bitrate = self.DEFAULT_BITRATE + self._last_error = '' + self._available = False + self._rx_thread = None + self._rx_buf = {} # per-arbitration-id reassembly buffer + self._rx_count = 0 + self._tx_count = 0 + self._tx_fail = 0 + self._last_rx_ts = 0.0 + self._last_rx_id = None + self._last_rx_text = '' + self._last_tx_error = '' + self._open() + + def _open(self): + if not _CAN_AVAILABLE: + self._last_error = 'python-can not installed (pip install python-can)' + return + # Try CAN FD first (PCAN-USB FD), fall back to classic CAN + for fd_mode in (True, False): + try: + kwargs = dict(channel=self._channel, interface='socketcan', + bitrate=self._bitrate) + if fd_mode: + kwargs['fd'] = True + self._bus = can.interface.Bus(**kwargs) + self._available = True + self._last_error = '' + mode_str = 'FD' if fd_mode else 'classic' + print(f'[CAN] opened {self._channel} @ {self._bitrate//1000} kbps ({mode_str})') + # Start receive thread + self._rx_buf = {} + t = threading.Thread(target=self._rx_loop, daemon=True, + name='can_rx') + t.start() + self._rx_thread = t + return + except Exception as e: + if fd_mode: + continue # try classic CAN + self._last_error = str(e) + self._available = False + print(f'[CAN] open failed: {e}') + print(f'[CAN] hint: sudo ip link set {self._channel} up type can bitrate {self._bitrate}') + + def _rx_loop(self): + """Background thread: receive CAN frames, reassemble JSON, log event.""" + buf = {} # arbitration_id → accumulated bytes + while self._available and self._bus: + try: + msg = self._bus.recv(timeout=1.0) + if msg is None: + continue + aid = msg.arbitration_id + chunk = bytes(msg.data) + + # Special handling for control frames (ID=0x75): process immediately as raw + if aid == 0x75: + self._on_raw_received(aid, chunk) + continue + + # Accumulate bytes for this CAN ID (for JSON reassembly) + buf.setdefault(aid, b'') + buf[aid] += chunk + + # Check for null terminator (end of JSON string) + if b'\x00' in buf[aid]: + raw = buf[aid].split(b'\x00', 1)[0] + buf[aid] = b'' + try: + text = raw.decode('utf-8').strip() + if text: + self._on_json_received(aid, text) + except Exception: + self._on_raw_received(aid, raw) + # Safety: discard if buffer grows too large (no null after 256 bytes) + elif len(buf[aid]) > 256: + self._on_raw_received(aid, buf[aid]) + buf[aid] = b'' + except Exception: + break + + def _on_json_received(self, can_id, text): + """Called when a complete JSON string arrives from CAN bus.""" + try: + data = json.loads(text) + except Exception: + # Not valid JSON — might be a single-frame message without null pad + # Try treating raw text as-is if it looks like our format + data = {'raw': text} + + evt_class = data.get('class', data.get('type', '?')) + evt_level = data.get('level', '?') + ts = tw_str() + print(f' [CHANNEL-C RX] CAN id=0x{can_id:03X} {text}') + + entry = { + 'server_time': ts, + 'channel': 'CAN', + 'source': 'can', + 'can_id': f'0x{can_id:03X}', + 'response_type': 'violation', + 'content': { + 'id': str(int(time.time() * 1000)), + 'date': ts, + 'type': evt_class, + 'level': evt_level, + } + } + self._rx_count += 1 + self._last_rx_ts = time.time() + self._last_rx_id = can_id + self._last_rx_text = text[:160] + with events_lock: + events.append(entry) + if len(events) > 200: + del events[:-200] + + def _on_raw_received(self, can_id, raw: bytes): + hx = ' '.join(f'{b:02X}' for b in raw[:32]) + ts = tw_str() + + # Control frame detection (ID=0x75): throttle command — add to event list for display + if can_id == 0x75 and len(raw) >= 1: + level = raw[0] + status = '关闭油门' if level == 0 else f'油门={level}' + display = f'CAN id=0x{can_id:03X} {status}' + print(f' [CHANNEL-C RX] {display}') + self._rx_count += 1 + self._last_rx_ts = time.time() + self._last_rx_id = can_id + self._last_rx_text = display + + # Add to event list for display + entry = { + 'server_time': ts, + 'channel': 'CAN', + 'source': 'can', + 'can_id': f'0x{can_id:03X}', + 'response_type': 'throttle', + 'content': { + 'id': str(int(time.time() * 1000)), + 'date': ts, + 'type': 'throttle', + 'level': level, + 'status': status, + } + } + with events_lock: + events.append(entry) + if len(events) > 200: + del events[:-200] + return + + # Other CAN IDs: log as raw data, also add to event list + display = f'RAW [{hx}]' + print(f' [CHANNEL-C RX-RAW] CAN id=0x{can_id:03X} {display}') + + self._rx_count += 1 + self._last_rx_ts = time.time() + self._last_rx_id = can_id + self._last_rx_text = display + + entry = { + 'server_time': ts, + 'channel': 'CAN', + 'source': 'can', + 'can_id': f'0x{can_id:03X}', + 'response_type': 'can_raw', + 'content': { + 'id': str(int(time.time() * 1000)), + 'date': ts, + 'type': 'can_raw', + 'level': 0, + 'hex': hx, + } + } + with events_lock: + events.append(entry) + if len(events) > 200: + del events[:-200] + + def reopen(self, channel=None, bitrate=None): + with self._lock: + if self._bus: + try: self._bus.shutdown() + except: pass + self._bus = None + if channel: self._channel = channel + if bitrate: self._bitrate = int(bitrate) + self._open() + return self.status() + + def send(self, can_id: int, data: bytes) -> dict: + with self._lock: + if not self._available or not self._bus: + return {'ok': False, 'error': self._last_error or 'CAN not available'} + try: + # Keep TX compatible with classic CAN: split long payloads into + # 8-byte frames (same framing idea as device-side JSON TX). + frames = 0 + off = 0 + while off < len(data): + chunk = data[off:off+8] + msg = can.Message( + arbitration_id=can_id, + data=chunk, + is_extended_id=False, + is_fd=False, + ) + self._bus.send(msg) + frames += 1 + off += len(chunk) + if len(data) > 8: + time.sleep(0.001) + + self._tx_count += frames + self._last_tx_error = '' + return {'ok': True, 'frames': frames} + except Exception as e: + self._last_error = str(e) + self._last_tx_error = str(e) + self._tx_fail += 1 + return {'ok': False, 'error': str(e)} + + def check(self, can_id: int = 0x120) -> dict: + # Short probe payload so status checks are valid on classic CAN too. + payload = b'\xA5' + tx = self.send(can_id, payload) + st = self.status() + st.update({ + 'tx_probe_ok': bool(tx.get('ok')), + 'tx_probe_error': tx.get('error', ''), + }) + return st + + def status(self) -> dict: + now = time.time() + last_rx_age = None + if self._last_rx_ts > 0: + last_rx_age = round(now - self._last_rx_ts, 3) + return { + 'available': self._available, + 'channel': self._channel, + 'bitrate': self._bitrate, + 'last_error': self._last_error, + 'lib': _CAN_AVAILABLE, + 'rx_count': self._rx_count, + 'tx_count': self._tx_count, + 'tx_fail': self._tx_fail, + 'last_rx_age_sec': last_rx_age, + 'last_rx_id': (f'0x{self._last_rx_id:03X}' if self._last_rx_id is not None else ''), + 'last_rx_text': self._last_rx_text, + 'last_tx_error': self._last_tx_error, + } + + +_can_bus = CanBus() + TZ_TW = datetime.timezone(datetime.timedelta(hours=8)) def now_tw(): return datetime.datetime.now(TZ_TW) def tw_str(dt=None): @@ -64,6 +361,14 @@ class Handler(BaseHTTPRequestHandler): with events_lock: self._json(list(events)) + elif path == '/api/can/status': + self._json(_can_bus.status()) + + elif path == '/api/can/check': + qs = parse_qs(urlparse(self.path).query) + can_id = int((qs.get('can_id') or ['0x120'])[0], 0) + self._json(_can_bus.check(can_id=can_id)) + elif path == '/api/files': files = [] for name in sorted(os.listdir(UPLOAD_DIR), reverse=True): @@ -169,8 +474,15 @@ class Handler(BaseHTTPRequestHandler): data = json.loads(body.decode('utf-8')) entry = { 'server_time': tw_str(), + 'source': 'http', + 'channel': 'HTTP', } entry.update(data) + # Normalize source/channel even if caller passes custom fields. + if not entry.get('source'): + entry['source'] = 'http' + if not entry.get('channel'): + entry['channel'] = 'HTTP' with events_lock: events.append(entry) if len(events) > 200: @@ -183,6 +495,89 @@ class Handler(BaseHTTPRequestHandler): print(f" [CHANNEL-A] parse error: {e}") self.send_error(400, str(e)) + # ── Channel C: CAN bus frame send ──────────────────────────── + elif path == '/api/can/send': + try: + req = json.loads(body.decode('utf-8')) + evt_type = req.get('type', 'hazard') + evt_level = int(req.get('level', 0)) + can_id = int(req.get('can_id', 0x100)) + # Same JSON payload as BLE/iPad channel + payload = f'{{"class":"{evt_type}","level":{evt_level}}}' + result = _can_bus.send(can_id, payload.encode('utf-8')) + if result['ok']: + print(f" [CHANNEL-C] CAN id=0x{can_id:03X} {payload}") + else: + print(f" [CHANNEL-C] CAN FAIL: {result['error']}") + self._json(result) + except Exception as e: + self._json({'ok': False, 'error': str(e)}) + + elif path == '/api/can/send_cmd': + try: + req = json.loads(body.decode('utf-8')) + cmd = int(req.get('cmd', 0)) & 0xFF + can_id = int(req.get('can_id', 0x75)) + payload = bytes([cmd, 0, 0, 0, 0, 0, 0, 0]) + result = _can_bus.send(can_id, payload) + if result.get('ok'): + print(f' [CHANNEL-C TX-CMD] CAN id=0x{can_id:03X} cmd=0x{cmd:02X}') + ts = tw_str() + entry = { + 'server_time': ts, + 'channel': 'CAN', + 'source': 'can', + 'can_id': f'0x{can_id:03X}', + 'response_type': 'can_tx_cmd', + 'content': { + 'id': str(int(time.time() * 1000)), + 'date': ts, + 'type': 'can_tx_cmd', + 'level': cmd, + 'hex': f'{cmd:02X} 00 00 00 00 00 00 00', + } + } + with events_lock: + events.append(entry) + if len(events) > 200: + del events[:-200] + self._json(result) + except Exception as e: + self._json({'ok': False, 'error': str(e)}) + + elif path == '/api/can/config': + try: + req = json.loads(body.decode('utf-8')) + result = _can_bus.reopen( + channel=req.get('channel'), + bitrate=req.get('bitrate'), + ) + self._json(result) + except Exception as e: + self._json({'ok': False, 'error': str(e)}) + + elif path == '/api/can/bringup': + import subprocess + try: + req = json.loads(body.decode('utf-8')) + ch = req.get('channel', 'can0') + br = int(req.get('bitrate', 250000)) + # Bring the interface down first, then up with the requested bitrate + subprocess.run(['sudo', 'ip', 'link', 'set', ch, 'down'], check=False) + r = subprocess.run( + ['sudo', 'ip', 'link', 'set', ch, 'up', 'type', 'can', 'bitrate', str(br)], + capture_output=True, text=True + ) + if r.returncode == 0: + print(f'[CAN] brought up {ch} @ {br//1000} kbps') + self._json({'ok': True}) + else: + err = (r.stderr or r.stdout).strip() + print(f'[CAN] bring-up failed: {err}') + self._json({'ok': False, 'error': err}) + except Exception as e: + self._json({'ok': False, 'error': str(e)}) + # ── Channel B: tar.gz archive upload ───────────────────────── elif path in ('/api/upload', '/api/golf.cgi'): # 1) kCurl sends filename as query param: /api/upload?filename=xxx @@ -235,6 +630,10 @@ class Handler(BaseHTTPRequestHandler): self.send_response(200) self.send_header('Content-Type', content_type + '; charset=utf-8') self.send_header('Content-Length', str(len(data))) + # Prevent browser from caching HTML so edits are always picked up + self.send_header('Cache-Control', 'no-store, no-cache, must-revalidate') + self.send_header('Pragma', 'no-cache') + self._cors() self.end_headers() self.wfile.write(data) @@ -251,7 +650,16 @@ if __name__ == '__main__': print(f" Event API : POST http://localhost:{PORT}/api/event") print(f" Upload API : POST http://localhost:{PORT}/api/upload") print(f" Golf API : POST http://localhost:{PORT}/api/golf.cgi") + print(f" CAN status : GET http://localhost:{PORT}/api/can/status") + print(f" CAN send : POST http://localhost:{PORT}/api/can/send") print(f" Uploads : {UPLOAD_DIR}") + st = _can_bus.status() + if st['available']: + print(f" CAN bus : {st['channel']} @ {st['bitrate']//1000} kbps [OK]") + else: + print(f" CAN bus : {st.get('last_error','unavailable')} [OFFLINE]") + if st['lib']: + print(f" fix: sudo ip link set can0 up type can bitrate 250000") print("=" * 55) try: server.serve_forever() diff --git a/tools/mock_server/static/index.html b/tools/mock_server/static/index.html index 4dd59ed..f3290d4 100644 --- a/tools/mock_server/static/index.html +++ b/tools/mock_server/static/index.html @@ -17,6 +17,14 @@ header { .dot-live { width: 8px; height: 8px; border-radius: 50%; background: #4ade80; box-shadow: 0 0 8px #4ade80; flex-shrink: 0; } header h1 { font-size: 1rem; font-weight: 600; color: #fff; } #utc-clock { margin-left: auto; font-size: 0.78rem; color: #555d7a; font-family: monospace; } +#can-status-badge { + font-size: 0.68rem; font-weight: 700; padding: 2px 9px; + border-radius: 10px; letter-spacing: 0.04em; cursor: pointer; + border: none; background: #1e2130; color: #555d7a; + transition: background 0.2s; +} +#can-status-badge.ok { background: #14312a; color: #4ade80; } +#can-status-badge.err { background: #2a1a1a; color: #f87171; } /* ── Layout ── */ .layout { display: grid; grid-template-columns: 1fr 1fr; height: calc(100vh - 49px); } @@ -104,6 +112,7 @@ header h1 { font-size: 1rem; font-weight: 600; color: #fff; } .log-row.lv2 { border-color: #fb923c; } .log-row.lv3 { border-color: #f87171; } .log-row.single { border-color: #a78bfa; } +.log-row.throttle { border-color: #22d3ee; } .log-time { color: #555d7a; font-family: monospace; white-space: nowrap; flex-shrink: 0; } .log-type { font-weight: 600; color: #dde1f0; } .log-sub { color: #555d7a; } @@ -112,6 +121,7 @@ header h1 { font-size: 1rem; font-weight: 600; color: #fff; } .test-bar { border-top: 1px solid #1e2130; padding: 8px 12px; background: #0e1018; flex-shrink: 0; + max-height: 45vh; overflow-y: auto; } details summary { font-size: 0.68rem; color: #3a3f55; cursor: pointer; user-select: none; @@ -133,6 +143,31 @@ details[open] summary::before { transform: rotate(90deg); } .b-hz { background:#60a5fa;color:#000; } .b-pe { background:#a78bfa;color:#fff; } .b-up { background:#1e2130;color:#60a5fa;border:1px solid #2a3050; } +.b-can { background:#1a2b2f;color:#22d3ee;border:1px solid #164e63; } + +/* ── CAN config row ── */ +.can-cfg { + display: flex; align-items: center; gap: 8px; margin-top: 8px; flex-wrap: wrap; +} +.can-cfg label { font-size: 0.68rem; color: #555d7a; white-space: nowrap; } +.can-cfg input { + background: #1a1d29; border: 1px solid #252836; border-radius: 4px; + color: #dde1f0; font-size: 0.72rem; padding: 3px 6px; width: 80px; +} +.can-toggle { + display: flex; align-items: center; gap: 6px; margin-top: 8px; +} +.can-toggle input[type=checkbox] { accent-color: #22d3ee; width: 14px; height: 14px; cursor: pointer; } +.can-toggle label { font-size: 0.72rem; color: #8891b0; cursor: pointer; } +.can-diag { + margin-top: 8px; padding: 8px; + border: 1px solid #1e2130; border-radius: 6px; + background: #10131d; font-size: 0.68rem; color: #8ea0bf; + font-family: ui-monospace, SFMono-Regular, Menlo, Consolas, monospace; + line-height: 1.45; +} +.can-diag .k { color: #5f7093; } +.can-diag .v { color: #c7d5ef; } /* ── File list (Channel B) ── */ .file-scroll { flex: 1; overflow-y: auto; padding: 12px; } @@ -194,6 +229,7 @@ details[open] summary::before { transform: rotate(90deg); }

KL630 Golf Event Monitor

+
—
@@ -237,18 +273,67 @@ details[open] summary::before { transform: rotate(90deg); }
-
- 測試送出 +
+ 指令
- - - - - - - - + + + +
+
+ + + + +
+
+ + + + +
+
+ + + + + +
+
+ + + + + +
+
+ +
+ CAN 設定 + + +
+ + + + + + + + + + +
+ +
+
status: -
+
rx_count: 0   tx_count: 0   tx_fail: 0
+
last_rx: -
+
last_tx_err: -
+
+ +
@@ -264,15 +349,6 @@ details[open] summary::before { transform: rotate(90deg); }
尚未收到上傳
- -
-
- 測試送出 -
- -
-
-
@@ -286,7 +362,7 @@ details[open] summary::before { transform: rotate(90deg); } diff --git a/web_serve.py b/web_serve.py index a86dfe9..7833b3e 100644 --- a/web_serve.py +++ b/web_serve.py @@ -413,6 +413,55 @@ def _verify_firmware_running(tn): ok = any("FW PID:" in ln for ln in lines) return ok, lines +def _spi_setup_on_device(tn, spi_node="spi1.0"): + """Try to switch SPI node from display driver to spidev and report status.""" + steps = [ + ("SPI preflight: listing SPI devices...", + "ls -l /sys/bus/spi/devices 2>/dev/null || true", 10), + ("SPI preflight: current modalias...", + f"cat /sys/bus/spi/devices/{spi_node}/modalias 2>/dev/null || echo 'modalias missing'", 10), + ("SPI preflight: current driver link...", + f"readlink /sys/bus/spi/devices/{spi_node}/driver 2>/dev/null || echo 'driver link missing'", 10), + ("SPI setup: unbind current driver...", + f"DRV=$(basename $(readlink /sys/bus/spi/devices/{spi_node}/driver 2>/dev/null) 2>/dev/null || true); " + f"echo DRIVER=$DRV; " + f"[ -n \"$DRV\" ] && echo {spi_node} > /sys/bus/spi/drivers/$DRV/unbind 2>/dev/null || true; " + "echo 'unbind done'", 12), + ("SPI setup: set driver_override=spidev (if supported)...", + f"if [ -e /sys/bus/spi/devices/{spi_node}/driver_override ]; then " + f"echo spidev > /sys/bus/spi/devices/{spi_node}/driver_override && echo 'override OK'; " + "else echo 'driver_override missing'; fi", 10), + ("SPI setup: bind spidev...", + f"echo {spi_node} > /sys/bus/spi/drivers/spidev/bind 2>/dev/null || true; echo 'bind done'", 10), + ("SPI verify: modalias + driver + /dev/spidev*", + f"echo 'modalias:'; cat /sys/bus/spi/devices/{spi_node}/modalias 2>/dev/null || true; " + f"echo 'driver:'; readlink /sys/bus/spi/devices/{spi_node}/driver 2>/dev/null || true; " + "echo '/dev:'; ls -l /dev/spidev* 2>/dev/null || echo 'no /dev/spidev'", 12), + ] + + out = [] + for label, cmd, timeout in steps: + out.append(("log", label)) + out.append(("prompt", f"$ {cmd}")) + for ln in _telnet_run(tn, cmd, timeout=timeout): + kind = "ok" + low = ln.lower() + if "missing" in low or "no /dev/spidev" in low: + kind = "warn" + out.append((kind, ln)) + + # Final health hint + final = _telnet_run( + tn, + f"M=$(cat /sys/bus/spi/devices/{spi_node}/modalias 2>/dev/null || true); " + "echo FINAL_MODALIAS=$M; " + "echo $M | grep -q dh2228fv && echo 'SPI_BINDING_BLOCKED' || echo 'SPI_BINDING_OK'", + timeout=10, + ) + for ln in final: + out.append(("warn" if "BLOCKED" in ln else "ok", ln)) + return out + # ── API: deploy via Telnet (SSE) ────────────────────────────────────────────── @app.route("/api/deploy/run") def api_deploy_run(): @@ -422,6 +471,7 @@ def api_deploy_run(): port = int(request.args.get("port", cfg["port"])) out_rtsp = request.args.get("out_rtsp", "1") == "1" out_hdmi = request.args.get("out_hdmi", "0") == "1" + spi_fix = request.args.get("spi_fix", "1") == "1" base = f"http://{h_ip}:{port}" bd = BIN_DIR_DEVICE fw = FW_PATH_DEVICE @@ -484,6 +534,12 @@ def api_deploy_run(): tn = _telnet_connect(kl_ip) _drain_shell(tn) yield sse(f"Connected to {kl_ip}", "ok") + + if spi_fix: + yield sse("Running SPI setup preflight (spi1.0 -> spidev)...") + for kind, text in _spi_setup_on_device(tn, "spi1.0"): + yield sse(text, kind) + for label, cmd, bg, timeout in steps: yield sse(label) yield sse(f"$ {cmd}", "prompt") @@ -506,6 +562,30 @@ def api_deploy_run(): return Response(generate(), mimetype="text/event-stream", headers={"Cache-Control": "no-cache", "X-Accel-Buffering": "no"}) +# ── API: SPI setup via Telnet (SSE) ───────────────────────────────────────── +@app.route("/api/spi_setup/run") +def api_spi_setup_run(): + cfg = load_config() + kl_ip = request.args.get("ip", cfg["kl630_ip"]) + spi_node = request.args.get("node", "spi1.0") + + def generate(): + yield sse(f"Connecting to {kl_ip}:23 via Telnet...") + try: + tn = _telnet_connect(kl_ip) + _drain_shell(tn) + yield sse("Connected.", "ok") + for kind, text in _spi_setup_on_device(tn, spi_node): + yield sse(text, kind) + tn.close() + yield sse("SPI setup flow complete.", "ok") + except Exception as e: + yield sse(f"Telnet error: {e}", "error") + yield sse_done() + + return Response(generate(), mimetype="text/event-stream", + headers={"Cache-Control": "no-cache", "X-Accel-Buffering": "no"}) + # ── API: BT UART one-time baud setup via Telnet (SSE) ──────────────────────── @app.route("/api/bt_setup/run") def api_bt_setup_run(): @@ -1617,6 +1697,10 @@ input:checked+.slider:before{transform:translateX(16px);background:#fff} BT 初始化 +