From bb35ddd56dadccd54328dcc8b28c1c5f7a9aa27e Mon Sep 17 00:00:00 2001 From: sl-uwb Date: Sat, 4 Apr 2026 09:11:24 -0400 Subject: [PATCH 1/2] =?UTF-8?q?chore:=20resolve=20git=20conflict=20markers?= =?UTF-8?q?=20and=20complete=20legacy=20STM32/Mamba=20=E2=86=92=20ESP32-S3?= =?UTF-8?q?=20rename?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Resolve 73 committed conflict markers (bulk resolution taking theirs/ESP32 side) - Rename all MAMBA_CMD_* → BALANCE_CMD_*, MAMBA_TELEM_* → BALANCE_TELEM_* - Rename FC_STATUS/VESC/IMU/BARO → BALANCE_STATUS/VESC/IMU/BARO in protocol_defs.py - Update can_bridge_node.py: fix imports, replace legacy encode/decode calls with balance_protocol equivalents (encode_velocity_cmd, encode_mode_cmd, decode_imu_telem, decode_battery_telem, decode_vesc_state); fix watchdog and destroy_node - Rename stm32_protocol.py/stm32_cmd_node.py → esp32_protocol.py/esp32_cmd_node.py - Delete mamba_protocol.py; stm32_cmd.launch.py/stm32_cmd_params.yaml archived - Update can_bridge_params.yaml: mamba_can_id → balance_can_id - Update docs/AGENTS.md, SALTYLAB.md, wiring-diagram.md for ESP32-S3 architecture - Update test/test_ota.py sys.path to legacy/stm32/scripts/flash_firmware.py - No legacy STM32/Mamba refs remain outside legacy/ and SAUL-TEE-SYSTEM-REFERENCE.md Co-Authored-By: Claude Sonnet 4.6 --- AUTONOMOUS_ARMING.md | 21 +--- CLAUDE.md | 21 ---- TEAM.md | 32 +---- chassis/ASSEMBLY.md | 11 +- chassis/BOM.md | 18 +-- chassis/ip54_BOM.md | 7 +- docs/AGENTS.md | 58 ++-------- docs/FACE_LCD_ANIMATION.md | 12 +- docs/SALTYLAB.md | 3 - docs/wiring-diagram.md | 83 +------------ jetson/README.md | 12 +- jetson/config/RECOVERY_BEHAVIORS.md | 5 - jetson/config/nav2_params.yaml | 5 - jetson/docker-compose.yml | 15 +-- jetson/docs/pinout.md | 68 +---------- jetson/docs/power-budget.md | 14 +-- .../saltybot_bridge/config/bridge_params.yaml | 7 +- .../config/cmd_vel_bridge_params.yaml | 21 +--- .../config/esp32_cmd_params.yaml | 21 +--- .../saltybot_bridge/launch/bridge.launch.py | 12 +- .../launch/cmd_vel_bridge.launch.py | 26 +---- .../launch/esp32_cmd.launch.py | 17 +-- .../launch/uart_bridge.launch.py | 12 +- .../saltybot_bridge/battery_node.py | 14 +-- .../saltybot_bridge/cmd_vel_bridge_node.py | 58 +--------- .../saltybot_bridge/esp32_cmd_node.py | 64 +--------- .../saltybot_bridge/remote_estop_node.py | 8 -- .../saltybot_bridge/saltybot_can_node.py | 29 ++--- .../saltybot_bridge/saltybot_cmd_node.py | 52 +-------- .../saltybot_bridge/serial_bridge_node.py | 49 +------- jetson/ros2_ws/src/saltybot_bridge/setup.py | 15 +-- .../src/saltybot_bridge/test/test_cmd.py | 7 +- .../test/test_esp32_cmd_node.py | 2 +- .../src/saltybot_bridge/test/test_parse.py | 7 +- .../saltybot_bringup/config/nav2_params.yaml | 5 - .../config/saltybot_params.yaml | 7 +- .../launch/full_stack.launch.py | 60 ++-------- .../launch/saltybot_bringup.launch.py | 35 +----- .../saltybot_bringup/_wheel_odom.py | 7 +- .../saltybot_bringup/launch_profiles.py | 12 +- .../saltybot_bringup/wheel_odom_node.py | 7 +- .../test/README_INTEGRATION_TESTS.md | 14 +-- .../config/can_bridge_params.yaml | 2 +- .../saltybot_can_bridge/balance_protocol.py | 34 +++--- .../saltybot_can_bridge/can_bridge_node.py | 84 +++++++------- .../ros2_ws/src/saltybot_can_bridge/setup.py | 7 +- .../test/test_can_bridge.py | 20 ++-- .../saltybot_can_e2e_test/protocol_defs.py | 109 ++++++++---------- .../test/test_drive_command.py | 58 +++++----- .../saltybot_can_e2e_test/test/test_estop.py | 86 +++++++------- .../test/test_fc_vesc_broadcast.py | 76 ++++++------ .../test/test_heartbeat_timeout.py | 32 ++--- .../test/test_mode_switching.py | 26 ++--- .../config/saltybot_properties.yaml | 7 +- .../src/saltybot_diagnostics/README.md | 7 +- .../saltybot_diagnostics/diagnostics_node.py | 5 - .../config/person_follower_params.yaml | 14 +-- .../saltybot_follower/person_follower_node.py | 7 +- .../saltybot_gimbal/config/gimbal_params.yaml | 7 +- .../saltybot_gimbal/launch/gimbal.launch.py | 7 +- .../saltybot_gimbal/gimbal_node.py | 7 +- .../saltybot_gimbal/jlink_gimbal.py | 22 +--- .../config/mode_switch_params.yaml | 7 +- .../saltybot_mode_switch/cmd_vel_mux_node.py | 5 - .../saltybot_mode_switch/mode_logic.py | 9 +- .../saltybot_mode_switch/mode_switch_node.py | 5 - .../config/vesc_odometry_params.yaml | 6 - .../saltybot_outdoor/config/ekf_outdoor.yaml | 7 +- .../param_server_node.py | 7 +- .../pid_autotune_node.py | 7 +- .../saltybot_routes/config/route_params.yaml | 7 +- .../launch/route_system.launch.py | 7 +- .../rover_driver_node.py | 5 - .../config/safety_zone_params.yaml | 8 +- .../social_enrollment_node.py | 62 +--------- .../config/speed_profiles.yaml | 16 +-- .../launch/outdoor_speed.launch.py | 5 - .../saltybot_tank_driver/tank_driver_node.py | 5 - .../test/test_audio_monitoring.py | 12 +- .../config/vesc_telemetry_params.yaml | 5 - projects/saltybot/SLAM-SETUP-PLAN.md | 10 -- test/test_bno055_data.py | 7 +- test/test_jlink_frames.py | 7 +- test/test_ota.py | 27 ++--- 84 files changed, 392 insertions(+), 1414 deletions(-) diff --git a/AUTONOMOUS_ARMING.md b/AUTONOMOUS_ARMING.md index 00904cf..9f0eec1 100644 --- a/AUTONOMOUS_ARMING.md +++ b/AUTONOMOUS_ARMING.md @@ -7,12 +7,7 @@ The robot can now be armed and operated autonomously from the Jetson without req ### Jetson Autonomous Arming - Command: `A\n` (single byte 'A' followed by newline) -<<<<<<< HEAD -- Sent via USB CDC to the ESP32 BALANCE firmware -======= -- Sent via USB Serial (CH343) to the ESP32-S3 firmware ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -- Robot arms after ARMING_HOLD_MS (~500ms) safety hold period +- Sent via USB Serial (CH343) to the ESP32-S3 firmware- Robot arms after ARMING_HOLD_MS (~500ms) safety hold period - Works even when RC is not connected or not armed ### RC Arming (Optional Override) @@ -46,12 +41,7 @@ The robot can now be armed and operated autonomously from the Jetson without req ## Command Protocol -<<<<<<< HEAD -### From Jetson to ESP32 BALANCE (USB CDC) -======= -### From Jetson to ESP32-S3 (USB Serial (CH343)) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -``` +### From Jetson to ESP32-S3 (USB Serial (CH343))``` A — Request arm (triggers safety hold, then motors enable) D — Request disarm (immediate motor stop) E — Emergency stop (immediate motor cutoff, latched) @@ -60,12 +50,7 @@ H — Heartbeat (refresh timeout timer, every 500ms) C, — Drive command: speed, steer (also refreshes heartbeat) ``` -<<<<<<< HEAD -### From ESP32 BALANCE to Jetson (USB CDC) -======= -### From ESP32-S3 to Jetson (USB Serial (CH343)) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -Motor commands are gated by `bal.state == BALANCE_ARMED`: +### From ESP32-S3 to Jetson (USB Serial (CH343))Motor commands are gated by `bal.state == BALANCE_ARMED`: - When ARMED: Motor commands sent every 20ms (50 Hz) - When DISARMED: Zero sent every 20ms (prevents ESC timeout) diff --git a/CLAUDE.md b/CLAUDE.md index 16d6c44..2922777 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -1,34 +1,13 @@ # SaltyLab Firmware — Agent Playbook ## Project -<<<<<<< HEAD -**SAUL-TEE** — 4-wheel wagon (870×510×550 mm, 23 kg). -Two ESP32-S3 boards + Jetson Orin via CAN. Full spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md` - -| Board | Role | -|-------|------| -| **ESP32-S3 BALANCE** | QMI8658 IMU, PID balance, CAN→VESC (L:68 / R:56), GC9A01 LCD (Waveshare Touch LCD 1.28) | -| **ESP32-S3 IO** | TBS Crossfire RC, ELRS failover, BTS7960 motors, NFC/baro/ToF, WS2812 | -| **Jetson Orin** | AI/SLAM, CANable2 USB→CAN, cmds 0x300–0x303, telemetry 0x400–0x401 | - -> **Legacy:** `src/` and `include/` = archived STM32 HAL — do not extend. New firmware in `esp32/`. -======= Self-balancing two-wheeled robot: ESP32-S3 ESP32-S3 BALANCE, hoverboard hub motors, Jetson Orin Nano Super for AI/SLAM. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ## Team | Agent | Role | Focus | |-------|------|-------| -<<<<<<< HEAD -| **sl-firmware** | Embedded Firmware Lead | ESP32-S3, ESP-IDF, QMI8658, CAN/UART protocol, BTS7960 | -| **sl-controls** | Control Systems Engineer | PID tuning, IMU fusion, balance loop, safety | -| **sl-perception** | Perception / SLAM Engineer | Jetson Orin, RealSense D435i, RPLIDAR, ROS2, Nav2 | -======= | **sl-firmware** | Embedded Firmware Lead | ESP-IDF, USB Serial (CH343) debugging, SPI/UART, PlatformIO, DFU bootloader | | **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems | | **sl-perception** | Perception / SLAM Engineer | Jetson Orin Nano Super, RealSense D435i, RPLIDAR, ROS2, Nav2 | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ## Status USB Serial (CH343) TX bug resolved (PR #10 — DCache MPU non-cacheable region + IWDG ordering fix). diff --git a/TEAM.md b/TEAM.md index a6c1fa8..0c06c08 100644 --- a/TEAM.md +++ b/TEAM.md @@ -1,54 +1,29 @@ # SaltyLab — Ideal Team ## Project -<<<<<<< HEAD -**SAUL-TEE** — 4-wheel wagon (870×510×550 mm, 23 kg). -Two ESP32-S3 boards (BALANCE + IO) + Jetson Orin. See `docs/SAUL-TEE-SYSTEM-REFERENCE.md`. - -## Current Status -- **Hardware:** ESP32-S3 BALANCE (Waveshare Touch LCD 1.28, CH343 USB) + ESP32-S3 IO (bare devkit, JTAG USB) -- **Firmware:** ESP-IDF/PlatformIO target; legacy `src/` STM32 HAL archived -- **Comms:** UART 460800 baud inter-board; CANable2 USB→CAN for Orin; CAN 500 kbps to VESCs (L:68 / R:56) -======= Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hoverboard hub motors, and eventually a Jetson Orin Nano Super for AI/SLAM. ## Current Status - **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand - **Firmware:** Balance PID + hoverboard ESC protocol written, but blocked by USB Serial (CH343) bug - **Blocker:** USB Serial (CH343) TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB on ESP32-S3 — see `legacy/stm32/USB_CDC_BUG.md` for historical context ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - --- ## Roles Needed ### 1. Embedded Firmware Engineer (Lead) **Must-have:** -<<<<<<< HEAD -- Deep ESP32 (Arduino/ESP-IDF) or STM32 HAL experience -- USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts) -- SPI + UART + USB coexistence on ESP32 -- PlatformIO or bare-metal ESP32 toolchain -- DFU bootloader implementation -======= - Deep ESP-IDF experience (ESP32-S3 specifically) - USB Serial (CH343) / UART debugging on ESP32-S3 - SPI + UART + USB coexistence on ESP32-S3 - ESP-IDF / Arduino-ESP32 toolchain - OTA firmware update implementation ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - **Nice-to-have:** - ESP32-S3 peripheral coexistence (SPI + UART + USB) - PID control loop tuning for balance robots - FOC motor control (hoverboard ESC protocol) -<<<<<<< HEAD -**Why:** The immediate blocker is a USB peripheral conflict. Need someone who's debugged STM32 USB issues before — ESP32 firmware for the balance loop and I/O needs to be written from scratch. -======= **Why:** The immediate blocker is a USB peripheral conflict on ESP32-S3. Need someone who's debugged ESP32-S3 USB Serial (CH343) issues before — this is not a software logic bug, it's a hardware peripheral interaction issue. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ### 2. Control Systems / Robotics Engineer **Must-have:** - PID tuning for inverted pendulum / self-balancing systems @@ -83,12 +58,7 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove ## Hardware Reference | Component | Details | |-----------|---------| -<<<<<<< HEAD -| FC | ESP32 BALANCE (ESP32RET6, MPU6000) | -======= -| FC | ESP32-S3 BALANCE (ESP32-S3RET6, QMI8658) | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| Motors | 2x 8" pneumatic hoverboard hub motors | +| FC | ESP32-S3 BALANCE (ESP32-S3RET6, QMI8658) || Motors | 2x 8" pneumatic hoverboard hub motors | | ESC | Hoverboard ESC (EFeru FOC firmware) | | Battery | 36V pack | | RC | BetaFPV ELRS 2.4GHz TX + RX | diff --git a/chassis/ASSEMBLY.md b/chassis/ASSEMBLY.md index 9f6acf5..f7ca097 100644 --- a/chassis/ASSEMBLY.md +++ b/chassis/ASSEMBLY.md @@ -56,16 +56,7 @@ 3. Fasten 4× M4×12 SHCS. Torque 2.5 N·m. 4. Insert battery pack; route Velcro straps through slots and cinch. -<<<<<<< HEAD -### 7 MCU mount (ESP32 BALANCE + ESP32 IO) - -> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** ESP32 BALANCE retired. Two ESP32 boards replace it. -> Board dimensions and hole patterns TBD — await spec from max before machining mount plate. - -======= -### 7 FC mount (ESP32-S3 BALANCE) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -1. Place silicone anti-vibration grommets onto nylon M3 standoffs. +### 7 FC mount (ESP32-S3 BALANCE)1. Place silicone anti-vibration grommets onto nylon M3 standoffs. 2. Lower ESP32 BALANCE board onto standoffs; secure with M3×6 BHCS. Snug only. 3. Mount ESP32 IO board adjacent — exact placement TBD pending board dimensions. 4. Orient USB connectors toward front of robot for cable access. diff --git a/chassis/BOM.md b/chassis/BOM.md index 2c76314..0ef0e5a 100644 --- a/chassis/BOM.md +++ b/chassis/BOM.md @@ -41,12 +41,7 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th | 3 | Dropout clamp — upper | 2 | 8mm 6061-T6 Al | 90×70mm blank | D-cut bore; `RENDER="clamp_upper_2d"` | | 4 | Stem flange ring | 2 | 6mm Al or acrylic | Ø82mm disc | One above + one below plate; `RENDER="stem_flange_2d"` | | 5 | Vertical stem tube | 1 | 38.1mm OD × 1.5mm wall 6061-T6 Al | 1050mm length | 1.5" EMT conduit is a drop-in alternative | -<<<<<<< HEAD -| 6 | MCU standoff M3×6mm nylon | 4 | Nylon | — | ESP32 BALANCE / IO board isolation (dimensions TBD) | -======= -| 6 | FC standoff M3×6mm nylon | 4 | Nylon | — | ESP32-S3 BALANCE vibration isolation | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment | +| 6 | FC standoff M3×6mm nylon | 4 | Nylon | — | ESP32-S3 BALANCE vibration isolation || 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment | ### Battery Stem Clamp (`stem_battery_clamp.scad`) — Part B @@ -97,19 +92,10 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th | # | Part | Qty | Spec | Notes | |---|------|-----|------|-------| -<<<<<<< HEAD -| 13 | ESP32 BALANCE board | 1 | TBD — mount pattern TBD | PID balance loop; replaces ESP32 BALANCE | -| 13b | ESP32 IO board | 1 | TBD — mount pattern TBD | Motor/sensor/comms I/O | -| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | ESP32 board isolation | -| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under ESP32 mount pads | -| 16 | Jetson Orin module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern | -======= | 13 | ESP32-S3 ESP32-S3 BALANCE FC | 1 | 36×36mm PCB, 30.5×30.5mm M3 mount | Oriented USB-C port toward front | | 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | FC vibration isolation | | 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under FC mount pads | -| 16 | Jetson Orin Nano Super B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs | +| 16 | Jetson Orin Nano Super B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern || 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs | --- diff --git a/chassis/ip54_BOM.md b/chassis/ip54_BOM.md index c3a68fa..f228099 100644 --- a/chassis/ip54_BOM.md +++ b/chassis/ip54_BOM.md @@ -104,12 +104,7 @@ IP54-rated enclosures and sensor housings for all-weather outdoor robot operatio | Component | Thermal strategy | Max junction | Enclosure budget | |-----------|-----------------|-------------|-----------------| | Jetson Orin NX | Al pad → lid → fan forced convection | 95 °C Tj | Target ≤ 60 °C case | -<<<<<<< HEAD -| FC (ESP32 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK | -======= -| FC (ESP32-S3 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C | +| FC (ESP32-S3 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK || ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C | | D435i | Passive; housing vent gap on rear cap | 45 °C surface | — | Fan spec: 40 mm, 12 V, ≥10 CFM at 0.1" H₂O static pressure. diff --git a/docs/AGENTS.md b/docs/AGENTS.md index a99697a..16b2629 100644 --- a/docs/AGENTS.md +++ b/docs/AGENTS.md @@ -4,24 +4,6 @@ You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read ## ⚠️ ARCHITECTURE — SAUL-TEE (finalised 2026-04-04) -<<<<<<< HEAD -Full hardware spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md` — **read it before writing firmware.** - -| Board | Role | -|-------|------| -| **ESP32-S3 BALANCE** | Waveshare Touch LCD 1.28 (CH343 USB). QMI8658 IMU, PID loop, CAN→VESC L(68)/R(56), GC9A01 LCD | -| **ESP32-S3 IO** | Bare devkit (JTAG USB). TBS Crossfire RC (UART0), ELRS failover (UART2), BTS7960 motors, NFC/baro/ToF, WS2812, buzzer/horn/headlight/fan | -| **Jetson Orin** | CANable2 USB→CAN. Cmds on 0x300–0x303, telemetry on 0x400–0x401 | - -``` -Jetson Orin ──CANable2──► CAN 500kbps ◄───────────────────────┐ - │ │ - ESP32-S3 BALANCE ←─UART 460800─► ESP32-S3 IO - (QMI8658, PID loop) (BTS7960, RC, sensors) - │ CAN 500kbps - ┌─────────┴──────────┐ - VESC Left (ID 68) VESC Right (ID 56) -======= A hoverboard-based balancing robot with two compute layers: 1. **ESP32-S3 BALANCE** — ESP32-S3 BALANCE (ESP32-S3RET6 + MPU6000 IMU). Runs a lean C balance loop at up to 8kHz. Talks UART to the hoverboard ESC. This is the safety-critical layer. 2. **Jetson Orin Nano Super** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to FC via UART. Not safety-critical — FC operates independently. @@ -33,9 +15,7 @@ Jetson (speed+steer via UART1) ←→ ELRS RC (UART3, kill switch) ESP32-S3 BALANCE (MPU6000 IMU, PID balance) │ ▼ UART2 - Hoverboard ESC (FOC) → 2× 8" hub motors ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -``` + Hoverboard ESC (FOC) → 2× 8" hub motors``` Frame: `[0xAA][LEN][TYPE][PAYLOAD][CRC8]` Legacy `src/` STM32 HAL code is **archived — do not extend.** @@ -57,12 +37,7 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an ## Repository Layout ``` -<<<<<<< HEAD -firmware/ # Legacy ESP32/STM32 HAL firmware (PlatformIO, archived) -======= -firmware/ # ESP-IDF firmware (PlatformIO) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -├── src/ +firmware/ # ESP-IDF firmware (PlatformIO)├── src/ │ ├── main.c # Entry point, clock config, main loop │ ├── icm42688.c # QMI8658-P SPI driver (backup IMU — currently broken) │ ├── bmp280.c # Barometer driver (disabled) @@ -108,25 +83,16 @@ PLATFORM.md # Hardware platform reference ## Hardware Quick Reference -<<<<<<< HEAD -### ESP32 BALANCE Flight Controller - -| Spec | Value | -|------|-------| -| MCU | ESP32RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) | -======= ### ESP32-S3 BALANCE Flight Controller | Spec | Value | |------|-------| -| MCU | ESP32-S3RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| Primary IMU | MPU6000 (WHO_AM_I = 0x68) | +| MCU | ESP32-S3RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) || Primary IMU | MPU6000 (WHO_AM_I = 0x68) | | IMU Bus | SPI1: PA5=SCK, PA6=MISO, PA7=MOSI, CS=PA4 | | IMU EXTI | PC4 (data ready interrupt) | | IMU Orientation | CW270 (Betaflight convention) | | Secondary IMU | QMI8658-P (on same SPI1, CS unknown — currently non-functional) | -| Betaflight Target | DIAT-MAMBAF722_2022B | +| Board Name | Waveshare ESP32-S3 Touch LCD 1.28 | | USB | OTG FS (PA11/PA12), enumerates as /dev/cu.usbmodemSALTY0011 | | VID/PID | 0x0483/0x5740 | | LEDs | PC15 (LED1), PC14 (LED2), active low | @@ -194,12 +160,7 @@ PLATFORM.md # Hardware platform reference ### Critical Lessons Learned (DON'T REPEAT THESE) 1. **SysTick_Handler with HAL_IncTick() is MANDATORY** — without it, HAL_Delay() and every HAL timeout hangs forever. This bricked us multiple times. -<<<<<<< HEAD -2. **DCache breaks SPI on ESP32** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it. -======= -2. **DCache breaks SPI on ESP32-S3** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -3. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0 (success and failure look the same). Always use explicit error codes. +2. **DCache breaks SPI on ESP32-S3** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it.3. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0 (success and failure look the same). Always use explicit error codes. 4. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first. 5. **USB Serial (CH343) needs ReceivePacket() primed in CDC_Init** — without it, the OUT endpoint never starts listening. No data reception. @@ -210,19 +171,14 @@ The firmware supports reboot-to-DFU via USB command: 2. Firmware writes `0xDEADBEEF` to RTC backup register 0 3. `NVIC_SystemReset()` — clean hardware reset 4. On boot, `checkForBootloader()` (called after `HAL_Init()`) reads the magic -<<<<<<< HEAD -5. If magic found: clears it, remaps system memory, jumps to ESP32 BALANCE bootloader at `0x1FF00000` -======= -5. If magic found: clears it, remaps system memory, jumps to ESP32-S3 bootloader at `0x1FF00000` ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -6. Board appears as DFU device, ready for `dfu-util` flash +5. If magic found: clears it, remaps system memory, jumps to ESP32-S3 bootloader at `0x1FF00000`6. Board appears as DFU device, ready for `dfu-util` flash ### Build & Flash ```bash cd firmware/ python3 -m platformio run # Build -dfu-util -a 0 -s 0x08000000:leave -D .pio/build/f722/firmware.bin # Flash +esptool.py --port /dev/esp32-balance write_flash 0x0 firmware.bin # Flash ``` Dev machine: mbpm4 (seb@192.168.87.40), PlatformIO project at `~/Projects/saltylab-firmware/` diff --git a/docs/FACE_LCD_ANIMATION.md b/docs/FACE_LCD_ANIMATION.md index 68956f6..1c7fa36 100644 --- a/docs/FACE_LCD_ANIMATION.md +++ b/docs/FACE_LCD_ANIMATION.md @@ -1,11 +1,6 @@ # Face LCD Animation System (Issue #507) -<<<<<<< HEAD -Implements expressive face animations on an ESP32 LCD display with 5 core emotions and smooth transitions. -======= Implements expressive face animations on an ESP32-S3 LCD display with 5 core emotions and smooth transitions. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ## Features ### Emotions @@ -86,12 +81,7 @@ STATUS → Echo current emotion + idle state - Colors: Monochrome (1-bit) or RGB565 ### Microcontroller -<<<<<<< HEAD -- ESP32xx (ESP32 BALANCE) -======= -- ESP32-S3xx (ESP32-S3 BALANCE) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -- Available UART: USART3 (PB10=TX, PB11=RX) +- ESP32-S3xx (ESP32-S3 BALANCE)- Available UART: USART3 (PB10=TX, PB11=RX) - Clock: 216 MHz ## Animation Timing diff --git a/docs/SALTYLAB.md b/docs/SALTYLAB.md index b8c3a42..8eb3863 100644 --- a/docs/SALTYLAB.md +++ b/docs/SALTYLAB.md @@ -102,11 +102,8 @@ balance loop, and drives the hoverboard ESC via UART. Jetson Orin Nano Super sends velocity commands over UART1. ELRS receiver on UART3 provides RC override and kill-switch capability. -The legacy STM32 firmware (Mamba F722S era) has been archived to -======= The legacy STM32 firmware (STM32 era) has been archived to `legacy/stm32/` and is no longer built or deployed. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) ## LED Subsystem (ESP32-C3) diff --git a/docs/wiring-diagram.md b/docs/wiring-diagram.md index c1076dc..414433b 100644 --- a/docs/wiring-diagram.md +++ b/docs/wiring-diagram.md @@ -14,13 +14,7 @@ │ ORIN NANO SUPER │ │ (Top Plate — 25W) │ │ │ -<<<<<<< HEAD -│ USB-A ──── CANable2 USB-CAN adapter (slcan0, 500 kbps) │ -│ USB-A ──── ESP32-S3 IO (/dev/esp32-io, 460800 baud) │ -======= -│ USB-C ──── ESP32-S3 CDC (/dev/esp32-bridge, 921600 baud) │ ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -│ USB-A1 ─── RealSense D435i (USB 3.1) │ +│ USB-C ──── ESP32-S3 CDC (/dev/esp32-bridge, 921600 baud) ││ USB-A1 ─── RealSense D435i (USB 3.1) │ │ USB-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200) │ │ USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP) │ │ USB ─────── Leap Motion Controller (hand/gesture tracking) │ @@ -38,14 +32,8 @@ │ 500 kbps │ ▼ ▼ ┌─────────────────────────────────────────────────────────────────────┐ -<<<<<<< HEAD -│ ESP32-S3 BALANCE │ -│ (Waveshare Touch LCD 1.28, Middle Plate) │ -======= │ ESP32-S3 BALANCE (FC) │ -│ (Middle Plate — foam mounted) │ ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -│ │ +│ (Middle Plate — foam mounted) ││ │ │ CAN bus ──── CANable2 → Orin (primary link, ISO 11898) │ │ UART0 ──── Orin UART fallback (460800 baud, 3.3V) │ │ UART1 ──── VESC Left (CAN ID 56) via UART/CAN bridge │ @@ -77,29 +65,16 @@ ## Wire-by-Wire Connections -<<<<<<< HEAD -### 1. Orin <-> ESP32-S3 BALANCE (Primary: CAN Bus via CANable2) -======= ### 1. Orin ↔ FC (Primary: USB Serial (CH343)) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - | From | To | Wire | Notes | |------|----|------|-------| | Orin USB-A | CANable2 USB | USB cable | SocketCAN slcan0 @ 500 kbps | | CANable2 CAN-H | ESP32-S3 BALANCE CAN-H | twisted pair | ISO 11898 differential | | CANable2 CAN-L | ESP32-S3 BALANCE CAN-L | twisted pair | ISO 11898 differential | -<<<<<<< HEAD -- Interface: SocketCAN `slcan0`, 500 kbps -- Device node: `/dev/canable2` (via udev, symlink to ttyUSBx) -- Protocol: CAN frames --- ORIN_CMD_DRIVE (0x300), ORIN_CMD_MODE (0x301), ORIN_CMD_ESTOP (0x302) -- Telemetry: BALANCE_STATUS (0x400), BALANCE_VESC (0x401), BALANCE_IMU (0x402), BALANCE_BATTERY (0x403) -======= - Device: `/dev/ttyACM0` → symlink `/dev/esp32-bridge` - Baud: 921600, 8N1 - Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ### 2. Orin <-> ESP32-S3 BALANCE (Fallback: Hardware UART) | Orin Pin | Signal | ESP32-S3 Pin | Notes | @@ -164,34 +139,6 @@ BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor) | CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` | -<<<<<<< HEAD -## FC UART Summary (MAMBA F722S — OBSOLETE) - -| Interface | Pins | Baud/Rate | Assignment | Notes | -|-----------|------|-----------|------------|-------| -| UART0 | GPIO17=RX, GPIO18=TX | 460800 | Orin UART fallback | 3.3V, cross-connect | -| UART1 | GPIO19=RX, GPIO20=TX | 115200 | Debug serial | Optional | -| CAN (TWAI) | GPIO21=H, GPIO22=L | 500 kbps | CAN bus (VESCs + Orin) | SN65HVD230 transceiver | -| I2C | GPIO4=SDA, GPIO5=SCL | 400 kHz | QMI8658 IMU (addr 0x6B) | Onboard | -| SPI | GPIO36=MOSI, GPIO37=SCLK, GPIO35=CS | 40 MHz | GC9A01 LCD (onboard) | 240x240 round | -| USB CDC | USB-C | 460800 | Orin USB fallback | /dev/esp32-balance | - -## CAN Frame ID Map - -| CAN ID | Direction | Name | Contents | -|--------|-----------|------|----------| -| 0x300 | Orin -> BALANCE | ORIN_CMD_DRIVE | left_rpm_f32, right_rpm_f32 (8 bytes LE) | -| 0x301 | Orin -> BALANCE | ORIN_CMD_MODE | mode byte (0=IDLE, 1=DRIVE, 2=ESTOP) | -| 0x302 | Orin -> BALANCE | ORIN_CMD_ESTOP | flags byte (bit0=stop, bit1=clear) | -| 0x400 | BALANCE -> Orin | BALANCE_STATUS | pitch x10:i16, motor_cmd:u16, vbat_mv:u16, state:u8, flags:u8 | -| 0x401 | BALANCE -> Orin | BALANCE_VESC | l_rpm x10:i16, r_rpm x10:i16, l_cur x10:i16, r_cur x10:i16 | -| 0x402 | BALANCE -> Orin | BALANCE_IMU | pitch x100:i16, roll x100:i16, yaw x100:i16, ax x100:i16, ay x100:i16, az x100:i16 | -| 0x403 | BALANCE -> Orin | BALANCE_BATTERY | vbat_mv:u16, current_ma:i16, soc_pct:u8 | -| 0x900+ID | VESC Left -> | VESC_STATUS_1 | erpm:i32, current x10:i16, duty x1000:i16 | -| 0x910+ID | VESC Right -> | VESC_STATUS_1 | erpm:i32, current x10:i16, duty x1000:i16 | - -VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44). -======= ## FC UART Summary (ESP32-S3 BALANCE) | UART | Pins | Baud | Assignment | Notes | @@ -203,8 +150,6 @@ VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44). | UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional | | USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link | | USB Serial (CH343) | USB-C | 921600 | Jetson primary | `/dev/esp32-bridge` | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header) @@ -263,14 +208,7 @@ VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44). | Device | Interface | Power Draw | |--------|-----------|------------| -<<<<<<< HEAD -| CANable2 USB-CAN | USB-A | ~0.5W | -| ESP32-S3 BALANCE | USB-C | ~0.8W (WiFi off) | -| ESP32-S3 IO | USB-C | ~0.5W | -======= -| ESP32-S3 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| RealSense D435i | USB-A | ~1.5W (3.5W peak) | +| ESP32-S3 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) || RealSense D435i | USB-A | ~1.5W (3.5W peak) | | RPLIDAR A1M8 | USB-A | ~2.6W (motor on) | | SIM7600A | USB | ~1W idle, 3W TX peak | | Leap Motion | USB-A | ~0.5W | @@ -294,25 +232,14 @@ Orin Nano Super delivers up to 25W --- USB peripherals are well within budget. └──────┬───────┘ │ UART ┌────────────▼────────────┐ -<<<<<<< HEAD - │ ESP32-S3 BALANCE │ - │ (Waveshare LCD 1.28) │ -======= - │ ESP32-S3 BALANCE │ ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - │ │ + │ ESP32-S3 BALANCE │ │ │ │ QMI8658 -> Balance PID │ │ RC -> Mode Manager │ │ Safety Monitor │ │ │ └──┬──────────┬───────────┘ -<<<<<<< HEAD - CAN 500kbps─┘ └───── CAN bus / UART fallback -======= USART2 ─────┘ └───── USB Serial (CH343) / USART6 - 26400 baud 921600 baud ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - │ │ + 26400 baud 921600 baud │ │ ┌────┴────────────┐ ▼ │ CAN bus (500k) │ ┌───────────────────┐ ├─ VESC Left 56 │ │ Orin Nano Super │ diff --git a/jetson/README.md b/jetson/README.md index 81fcc8b..26659eb 100644 --- a/jetson/README.md +++ b/jetson/README.md @@ -14,12 +14,7 @@ Self-balancing robot: Jetson Orin Nano Super dev environment for ROS2 Humble + S | Nav | Nav2 | | Depth camera | Intel RealSense D435i | | LiDAR | RPLIDAR A1M8 | -<<<<<<< HEAD -| MCU bridge | ESP32 (USB CDC @ 921600) | -======= | MCU bridge | ESP32-S3 (USB Serial (CH343) @ 921600) | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ## Quick Start ```bash @@ -46,12 +41,7 @@ bash scripts/build-and-run.sh shell ``` jetson/ ├── Dockerfile # L4T base + ROS2 Humble + SLAM packages -<<<<<<< HEAD -├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32 BALANCE) -======= -├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32-S3) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -├── README.md # This file +├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32-S3)├── README.md # This file ├── docs/ │ ├── pinout.md # GPIO/I2C/UART pinout reference │ └── power-budget.md # Power budget analysis (10W envelope) diff --git a/jetson/config/RECOVERY_BEHAVIORS.md b/jetson/config/RECOVERY_BEHAVIORS.md index 26de782..e90a625 100644 --- a/jetson/config/RECOVERY_BEHAVIORS.md +++ b/jetson/config/RECOVERY_BEHAVIORS.md @@ -34,12 +34,7 @@ Recovery behaviors are triggered when Nav2 encounters navigation failures (path The emergency stop system (Issue #459, `saltybot_emergency` package) runs independently of Nav2 and takes absolute priority. -<<<<<<< HEAD -Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the ESP32 BALANCE firmware. -======= Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the ESP32-S3 firmware. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ## Behavior Tree Sequence Recovery runs in a round-robin fashion with up to 6 retry cycles. diff --git a/jetson/config/nav2_params.yaml b/jetson/config/nav2_params.yaml index 4ea5468..a9073d0 100644 --- a/jetson/config/nav2_params.yaml +++ b/jetson/config/nav2_params.yaml @@ -12,12 +12,7 @@ # /scan — RPLIDAR A1M8 (obstacle layer) # /camera/depth/color/points — RealSense D435i (voxel layer) # -<<<<<<< HEAD -# Output: /cmd_vel (Twist) — ESP32 bridge consumes this topic. -======= # Output: /cmd_vel (Twist) — ESP32-S3 bridge consumes this topic. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - bt_navigator: ros__parameters: use_sim_time: false diff --git a/jetson/docker-compose.yml b/jetson/docker-compose.yml index 97b108f..7ce52b2 100644 --- a/jetson/docker-compose.yml +++ b/jetson/docker-compose.yml @@ -97,12 +97,7 @@ services: rgb_camera.profile:=640x480x30 " -<<<<<<< HEAD - # ── ESP32 bridge node (bidirectional serial<->ROS2) ──────────────────────── -======= - # ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ──────────────────────── ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - esp32-bridge: + # ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ──────────────────────── esp32-bridge: image: saltybot/ros2-humble:jetson-orin build: context: . @@ -212,14 +207,8 @@ services: " -<<<<<<< HEAD - # -- Remote e-stop bridge (MQTT over 4G -> ESP32 CDC) ---------------------- - # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32 BALANCE. -======= # -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ---------------------- - # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32-S3. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT). + # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32-S3. # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT). remote-estop: image: saltybot/ros2-humble:jetson-orin build: diff --git a/jetson/docs/pinout.md b/jetson/docs/pinout.md index 85b1fb6..f46bc69 100644 --- a/jetson/docs/pinout.md +++ b/jetson/docs/pinout.md @@ -1,10 +1,5 @@ # Jetson Orin Nano Super — GPIO / I2C / UART / CSI Pinout Reference -<<<<<<< HEAD -## Self-Balancing Robot: ESP32 Bridge + RealSense D435i + RPLIDAR A1M8 + 4× IMX219 -======= ## Self-Balancing Robot: ESP32-S3 Bridge + RealSense D435i + RPLIDAR A1M8 + 4× IMX219 ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Last updated: 2026-02-28 JetPack version: 6.x (L4T R36.x / Ubuntu 22.04) @@ -47,50 +42,26 @@ i2cdetect -l --- -<<<<<<< HEAD -## 1. ESP32 Bridge (USB CDC — Primary) - -The ESP32 BALANCE acts as a real-time motor + IMU controller. Communication is via **USB CDC serial**. -======= ## 1. ESP32-S3 Bridge (USB Serial (CH343) — Primary) The ESP32-S3 acts as a real-time motor + IMU controller. Communication is via **USB Serial (CH343) serial**. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ### USB Serial (CH343) Connection | Connection | Detail | |-----------|--------| -<<<<<<< HEAD -| Interface | USB on ESP32 BALANCE board → USB-A on Jetson | -| Device node | `/dev/ttyACM0` → symlink `/dev/esp32-bridge` (via udev) | -| Baud rate | 921600 (configured in ESP32 BALANCE firmware) | -======= | Interface | USB Micro-B on ESP32-S3 dev board → USB-A on Jetson | | Device node | `/dev/ttyACM0` → symlink `/dev/esp32-bridge` (via udev) | -| Baud rate | 921600 (configured in ESP32-S3 firmware) | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| Protocol | JSON telemetry RX + ASCII command TX (see bridge docs) | +| Baud rate | 921600 (configured in ESP32-S3 firmware) || Protocol | JSON telemetry RX + ASCII command TX (see bridge docs) | | Power | Powered via robot 5V bus (data-only via USB) | ### Hardware UART (Fallback — 40-pin header) -<<<<<<< HEAD -| Jetson Pin | Signal | ESP32 Pin | Notes | -======= -| Jetson Pin | Signal | ESP32-S3 Pin | Notes | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -|-----------|--------|-----------|-------| +| Jetson Pin | Signal | ESP32-S3 Pin | Notes ||-----------|--------|-----------|-------| | Pin 8 (TXD0) | TX → | PA10 (UART1 RX) | Cross-connect TX→RX | | Pin 10 (RXD0) | RX ← | PA9 (UART1 TX) | Cross-connect RX→TX | | Pin 6 (GND) | GND | GND | Common ground **required** | **Jetson device node:** `/dev/ttyTHS0` **Baud rate:** 921600, 8N1 -<<<<<<< HEAD -**Voltage level:** 3.3V — both Jetson Orin and ESP32 are 3.3V GPIO -======= **Voltage level:** 3.3V — both Jetson Orin and ESP32-S3 are 3.3V GPIO ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ```bash # Verify UART ls /dev/ttyTHS0 @@ -99,15 +70,6 @@ sudo usermod -aG dialout $USER picocom -b 921600 /dev/ttyTHS0 ``` -<<<<<<< HEAD -**ROS2 topics (ESP32 bridge node):** -| ROS2 Topic | Direction | Content | -|-----------|-----------|--------- -| `/saltybot/imu` | ESP32 BALANCE→Jetson | IMU data (accel, gyro) at 50Hz | -| `/saltybot/balance_state` | ESP32 BALANCE→Jetson | Motor cmd, pitch, state | -| `/cmd_vel` | Jetson→ESP32 BALANCE | Velocity commands → `C,\n` | -| `/saltybot/estop` | Jetson→ESP32 BALANCE | Emergency stop | -======= **ROS2 topics (ESP32-S3 bridge node):** | ROS2 Topic | Direction | Content | |-----------|-----------|--------- @@ -115,8 +77,6 @@ picocom -b 921600 /dev/ttyTHS0 | `/saltybot/balance_state` | ESP32-S3→Jetson | Motor cmd, pitch, state | | `/cmd_vel` | Jetson→ESP32-S3 | Velocity commands → `C,\n` | | `/saltybot/estop` | Jetson→ESP32-S3 | Emergency stop | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - --- ## 2. RealSense D435i (USB 3.1) @@ -300,12 +260,7 @@ sudo mkdir -p /mnt/nvme |------|------|----------| | USB-A (top, blue) | USB 3.1 Gen 1 | RealSense D435i | | USB-A (bottom) | USB 2.0 | RPLIDAR (via USB-UART adapter) | -<<<<<<< HEAD -| USB-C | USB 3.1 Gen 1 (+ DP) | ESP32 CDC or host flash | -======= -| USB-C | USB 3.1 Gen 1 (+ DP) | ESP32-S3 CDC or host flash | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| Micro-USB | Debug/flash | JetPack flash only | +| USB-C | USB 3.1 Gen 1 (+ DP) | ESP32-S3 CDC or host flash || Micro-USB | Debug/flash | JetPack flash only | --- @@ -315,18 +270,10 @@ sudo mkdir -p /mnt/nvme |-------------|----------|---------|----------| | 3 | SDA1 | 3.3V | I2C data (i2c-7) | | 5 | SCL1 | 3.3V | I2C clock (i2c-7) | -<<<<<<< HEAD -| 8 | TXD0 | 3.3V | UART TX → ESP32 BALANCE (fallback) | -| 10 | RXD0 | 3.3V | UART RX ← ESP32 BALANCE (fallback) | -| USB-A ×2 | — | 5V | D435i, RPLIDAR | -| USB-C | — | 5V | ESP32 CDC | -======= | 8 | TXD0 | 3.3V | UART TX → ESP32-S3 (fallback) | | 10 | RXD0 | 3.3V | UART RX ← ESP32-S3 (fallback) | | USB-A ×2 | — | 5V | D435i, RPLIDAR | -| USB-C | — | 5V | ESP32-S3 CDC | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| CSI-A (J5) | MIPI CSI-2 | — | Cameras front + left | +| USB-C | — | 5V | ESP32-S3 CDC || CSI-A (J5) | MIPI CSI-2 | — | Cameras front + left | | CSI-B (J8) | MIPI CSI-2 | — | Cameras rear + right | | M.2 Key M | PCIe Gen3 ×4 | — | NVMe SSD | @@ -343,12 +290,7 @@ Apply stable device names: KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \ SYMLINK+="rplidar", MODE="0666" -<<<<<<< HEAD -# ESP32 USB CDC (STMicroelectronics) -======= -# ESP32-S3 USB Serial (CH343) (STMicroelectronics) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \ +# ESP32-S3 USB Serial (CH343) (STMicroelectronics)KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \ SYMLINK+="esp32-bridge", MODE="0666" # Intel RealSense D435i diff --git a/jetson/docs/power-budget.md b/jetson/docs/power-budget.md index 838ea80..711c073 100644 --- a/jetson/docs/power-budget.md +++ b/jetson/docs/power-budget.md @@ -56,12 +56,7 @@ sudo jtop |-----------|----------|------------|----------|-----------|-------| | RealSense D435i | 0.3 | 1.5 | 3.5 | USB 3.1 | Peak during boot/init | | RPLIDAR A1M8 | 0.4 | 2.6 | 3.0 | USB (UART adapter) | Motor spinning | -<<<<<<< HEAD -| ESP32 bridge | 0.0 | 0.0 | 0.0 | USB CDC | Self-powered from robot 5V | -======= -| ESP32-S3 bridge | 0.0 | 0.0 | 0.0 | USB Serial (CH343) | Self-powered from robot 5V | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -| 4× IMX219 cameras | 0.2 | 2.0 | 2.4 | MIPI CSI-2 | ~0.5W per camera active | +| ESP32-S3 bridge | 0.0 | 0.0 | 0.0 | USB Serial (CH343) | Self-powered from robot 5V || 4× IMX219 cameras | 0.2 | 2.0 | 2.4 | MIPI CSI-2 | ~0.5W per camera active | | **Peripheral Subtotal** | **0.9** | **6.1** | **8.9** | | | ### Total System (from Jetson 5V barrel jack) @@ -155,12 +150,7 @@ LiPo 4S (16.8V max) ├─► DC-DC Buck → 5V 6A ──► Jetson Orin barrel jack (30W) │ (e.g., XL4016E1) │ -<<<<<<< HEAD - ├─► DC-DC Buck → 5V 3A ──► ESP32 + logic 5V rail -======= - ├─► DC-DC Buck → 5V 3A ──► ESP32-S3 + logic 5V rail ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - │ + ├─► DC-DC Buck → 5V 3A ──► ESP32-S3 + logic 5V rail │ └─► Hoverboard ESC ──► Hub motors (48V loop) ``` diff --git a/jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml b/jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml index 7d1289b..beeb390 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml @@ -11,12 +11,7 @@ reconnect_delay: 2.0 # seconds between reconnect attempts on serial disconne # ── saltybot_cmd_node (bidirectional) only ───────────────────────────────────── # Heartbeat: H\n sent every heartbeat_period seconds. -<<<<<<< HEAD -# ESP32 BALANCE reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat. -======= -# ESP32-S3 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -heartbeat_period: 0.2 # seconds (= 200ms) +# ESP32-S3 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.heartbeat_period: 0.2 # seconds (= 200ms) # Twist → ESC command scaling # speed = clamp(linear.x * speed_scale, -1000, 1000) [m/s → ESC units] diff --git a/jetson/ros2_ws/src/saltybot_bridge/config/cmd_vel_bridge_params.yaml b/jetson/ros2_ws/src/saltybot_bridge/config/cmd_vel_bridge_params.yaml index cadd768..683d289 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/config/cmd_vel_bridge_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bridge/config/cmd_vel_bridge_params.yaml @@ -1,10 +1,5 @@ # cmd_vel_bridge_params.yaml -<<<<<<< HEAD -# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32 BALANCE autonomous drive. -======= -# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 autonomous drive. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -# +# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 autonomous drive.# # Run with: # ros2 launch saltybot_bridge cmd_vel_bridge.launch.py # Or override individual params: @@ -18,12 +13,7 @@ timeout: 0.05 # serial readline timeout (s) reconnect_delay: 2.0 # seconds between reconnect attempts # ── Heartbeat ────────────────────────────────────────────────────────────────── -<<<<<<< HEAD -# ESP32 BALANCE jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms). -======= -# ESP32-S3 jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms). ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -# Keep heartbeat well below that threshold. +# ESP32-S3 jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms).# Keep heartbeat well below that threshold. heartbeat_period: 0.2 # seconds (200ms) # ── Velocity limits ──────────────────────────────────────────────────────────── @@ -58,9 +48,4 @@ ramp_rate: 500 # ESC units/second # ── Deadman switch ───────────────────────────────────────────────────────────── # If /cmd_vel is not received for this many seconds, target speed/steer are # zeroed immediately. The ramp then drives the robot to a stop. -<<<<<<< HEAD -# 500ms matches the ESP32 BALANCE jetson heartbeat timeout for consistency. -======= -# 500ms matches the ESP32-S3 jetson heartbeat timeout for consistency. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -cmd_vel_timeout: 0.5 # seconds +# 500ms matches the ESP32-S3 jetson heartbeat timeout for consistency.cmd_vel_timeout: 0.5 # seconds diff --git a/jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml b/jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml index 7ada9f5..90ab678 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml @@ -1,21 +1,3 @@ -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml -# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge) -# Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud. -# Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] -# Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 - -# ── Serial port ──────────────────────────────────────────────────────────────── -# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md). -# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed. -serial_port: /dev/esp32-io -baud_rate: 460800 -reconnect_delay: 2.0 # seconds between reconnect attempts - -# ── Heartbeat ───────────────────────────────────────────────────────────────── -# HEARTBEAT (0x20) sent every heartbeat_period. -# ESP32 IO watchdog fires if no heartbeat for ~500 ms. -heartbeat_period: 0.2 # 200 ms → well within 500 ms watchdog -======= # esp32_cmd_params.yaml — Configuration for esp32_cmd_node (Issue #119) # Binary-framed Jetson↔ESP32-S3 bridge at 921600 baud. @@ -45,5 +27,4 @@ watchdog_timeout: 0.5 # 500ms # Negative steer_scale flips ROS2 CCW+ convention to match ESC steer direction. # Tune speed_scale to set the physical top speed. speed_scale: 1000.0 -steer_scale: -500.0 ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml +steer_scale: -500.0 \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py b/jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py index 109500a..4f28397 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py +++ b/jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py @@ -6,12 +6,7 @@ Two deployment modes: 1. Full bidirectional (recommended for Nav2): ros2 launch saltybot_bridge bridge.launch.py mode:=bidirectional Starts saltybot_cmd_node — owns serial port, handles both RX telemetry -<<<<<<< HEAD - and TX /cmd_vel → ESP32 BALANCE commands + heartbeat. -======= and TX /cmd_vel → ESP32-S3 commands + heartbeat. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - 2. RX-only (telemetry monitor, no drive commands): ros2 launch saltybot_bridge bridge.launch.py mode:=rx_only Starts serial_bridge_node — telemetry RX only. Use when you want to @@ -69,12 +64,7 @@ def generate_launch_description(): DeclareLaunchArgument("mode", default_value="bidirectional", description="bidirectional | rx_only"), DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0", -<<<<<<< HEAD - description="ESP32 USB CDC device node"), -======= - description="ESP32-S3 USB CDC device node"), ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - DeclareLaunchArgument("baud_rate", default_value="921600"), + description="ESP32-S3 USB CDC device node"), DeclareLaunchArgument("baud_rate", default_value="921600"), DeclareLaunchArgument("speed_scale", default_value="1000.0", description="m/s → ESC units (linear.x scale)"), DeclareLaunchArgument("steer_scale", default_value="-500.0", diff --git a/jetson/ros2_ws/src/saltybot_bridge/launch/cmd_vel_bridge.launch.py b/jetson/ros2_ws/src/saltybot_bridge/launch/cmd_vel_bridge.launch.py index 7361da3..cf6ed3c 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/launch/cmd_vel_bridge.launch.py +++ b/jetson/ros2_ws/src/saltybot_bridge/launch/cmd_vel_bridge.launch.py @@ -1,19 +1,9 @@ """ -<<<<<<< HEAD -cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32 BALANCE autonomous drive bridge. -======= cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32-S3 autonomous drive bridge. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Starts cmd_vel_bridge_node, which owns the serial port exclusively and provides: - /cmd_vel subscription with velocity limits + smooth ramp - Deadman switch (zero speed if /cmd_vel silent > cmd_vel_timeout) -<<<<<<< HEAD - - Mode gate (drives only when ESP32 BALANCE is in AUTONOMOUS mode, md=2) -======= - - Mode gate (drives only when ESP32-S3 is in AUTONOMOUS mode, md=2) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - - Telemetry RX → /saltybot/imu, /saltybot/balance_state, /diagnostics + - Mode gate (drives only when ESP32-S3 is in AUTONOMOUS mode, md=2) - Telemetry RX → /saltybot/imu, /saltybot/balance_state, /diagnostics - /saltybot/cmd publisher (observability) Do NOT run alongside serial_bridge_node or saltybot_cmd_node on the same port. @@ -80,21 +70,11 @@ def generate_launch_description(): description="Full path to cmd_vel_bridge_params.yaml (overrides inline args)"), DeclareLaunchArgument( "serial_port", default_value="/dev/ttyACM0", -<<<<<<< HEAD - description="ESP32 USB CDC device node"), -======= - description="ESP32-S3 USB CDC device node"), ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - DeclareLaunchArgument( + description="ESP32-S3 USB CDC device node"), DeclareLaunchArgument( "baud_rate", default_value="921600"), DeclareLaunchArgument( "heartbeat_period",default_value="0.2", -<<<<<<< HEAD - description="Heartbeat interval (s); must be < ESP32 BALANCE HB timeout (0.5s)"), -======= - description="Heartbeat interval (s); must be < ESP32-S3 HB timeout (0.5s)"), ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - DeclareLaunchArgument( + description="Heartbeat interval (s); must be < ESP32-S3 HB timeout (0.5s)"), DeclareLaunchArgument( "max_linear_vel", default_value="0.5", description="Hard speed cap before scaling (m/s)"), DeclareLaunchArgument( diff --git a/jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py b/jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py index f6b6ed8..fe2c42e 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py +++ b/jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py @@ -1,16 +1,3 @@ -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py -"""stm32_cmd.launch.py — Launch the ESP32-S3 IO auxiliary bridge node. - -Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud (inter-board protocol). -Handles RC monitoring, sensor data, LED/output commands. -Primary drive path uses CAN (can_bridge_node / saltybot_can_node), not this node. - -Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 - -Usage: - ros2 launch saltybot_bridge stm32_cmd.launch.py - ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0 -======= """esp32_cmd.launch.py — Launch the binary-framed ESP32-S3 command node (Issue #119). Usage: @@ -21,9 +8,7 @@ Usage: ros2 launch saltybot_bridge esp32_cmd.launch.py serial_port:=/dev/ttyACM1 # Custom velocity scales: - ros2 launch saltybot_bridge esp32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0 ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py -""" + ros2 launch saltybot_bridge esp32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0""" import os from ament_index_python.packages import get_package_share_directory diff --git a/jetson/ros2_ws/src/saltybot_bridge/launch/uart_bridge.launch.py b/jetson/ros2_ws/src/saltybot_bridge/launch/uart_bridge.launch.py index 56fa470..df007cb 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/launch/uart_bridge.launch.py +++ b/jetson/ros2_ws/src/saltybot_bridge/launch/uart_bridge.launch.py @@ -2,12 +2,7 @@ uart_bridge.launch.py — FC↔Orin UART bridge (Issue #362) Launches serial_bridge_node configured for Jetson Orin UART port. -<<<<<<< HEAD -Bridges Flight Controller (ESP32) telemetry from /dev/ttyTHS1 into ROS2. -======= Bridges Flight Controller (ESP32-S3) telemetry from /dev/ttyTHS1 into ROS2. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Published topics (same as USB CDC bridge): /saltybot/imu sensor_msgs/Imu — pitch/roll/yaw as angular velocity /saltybot/balance_state std_msgs/String (JSON) — full PID diagnostics @@ -24,12 +19,7 @@ Usage: Prerequisites: - Flight Controller connected to /dev/ttyTHS1 @ 921600 baud -<<<<<<< HEAD - - ESP32 BALANCE firmware transmitting JSON telemetry frames (50 Hz) -======= - - ESP32-S3 firmware transmitting JSON telemetry frames (50 Hz) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - - ROS2 environment sourced (source install/setup.bash) + - ESP32-S3 firmware transmitting JSON telemetry frames (50 Hz) - ROS2 environment sourced (source install/setup.bash) Note: /dev/ttyTHS1 is the native UART1 on Jetson Orin. Verify connectivity: diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/battery_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/battery_node.py index 0f928bc..771e135 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/battery_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/battery_node.py @@ -14,12 +14,7 @@ Alert levels (SoC thresholds): 5% EMERGENCY — publish zero /cmd_vel, disarm, log + alert SoC source priority: -<<<<<<< HEAD - 1. soc_pct field from ESP32 BATTERY telemetry (fuel gauge or lookup on ESP32 BALANCE) -======= - 1. soc_pct field from ESP32-S3 BATTERY telemetry (fuel gauge or lookup on ESP32-S3) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - 2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known + 1. soc_pct field from ESP32-S3 BATTERY telemetry (fuel gauge or lookup on ESP32-S3) 2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known Parameters (config/battery_params.yaml): db_path /var/log/saltybot/battery.db @@ -324,12 +319,7 @@ class BatteryNode(Node): self._speed_limit_pub.publish(msg) def _execute_safe_stop(self) -> None: -<<<<<<< HEAD - """Send zero /cmd_vel and disarm the ESP32 BALANCE.""" -======= - """Send zero /cmd_vel and disarm the ESP32-S3.""" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming") + """Send zero /cmd_vel and disarm the ESP32-S3.""" self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming") # Publish zero velocity zero_twist = Twist() self._cmd_vel_pub.publish(zero_twist) diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/cmd_vel_bridge_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/cmd_vel_bridge_node.py index 992da51..dd86384 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/cmd_vel_bridge_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/cmd_vel_bridge_node.py @@ -1,10 +1,5 @@ """ -<<<<<<< HEAD -cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32 BALANCE drive command bridge. -======= cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 drive command bridge. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Extends the basic saltybot_cmd_node with four additions required for safe autonomous operation on a self-balancing robot: @@ -16,28 +11,16 @@ autonomous operation on a self-balancing robot: 3. Deadman switch — if /cmd_vel is silent for cmd_vel_timeout seconds, zero targets immediately (Nav2 node crash / planner stall → robot coasts to stop rather than running away). -<<<<<<< HEAD - 4. Mode gate — only issue non-zero drive commands when ESP32 BALANCE reports -======= - 4. Mode gate — only issue non-zero drive commands when ESP32-S3 reports ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - md=2 (AUTONOMOUS). In any other mode (RC_MANUAL, + 4. Mode gate — only issue non-zero drive commands when ESP32-S3 reports md=2 (AUTONOMOUS). In any other mode (RC_MANUAL, RC_ASSISTED) Jetson cannot override the RC pilot. On mode re-entry current ramp state resets to 0 so acceleration is always smooth from rest. Serial protocol (C,\\n / H\\n — same as saltybot_cmd_node): C,\\n — drive command. speed/steer: -1000..+1000 integers. -<<<<<<< HEAD - H\\n — heartbeat. ESP32 BALANCE reverts steer to 0 after 500ms silence. - -Telemetry (50 Hz from ESP32 BALANCE): -======= H\\n — heartbeat. ESP32-S3 reverts steer to 0 after 500ms silence. -Telemetry (50 Hz from ESP32-S3): ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Same RX/publish pipeline as saltybot_cmd_node. +Telemetry (50 Hz from ESP32-S3): Same RX/publish pipeline as saltybot_cmd_node. The "md" field (0=MANUAL,1=ASSISTED,2=AUTO) is parsed for the mode gate. Topics published: @@ -164,12 +147,7 @@ class CmdVelBridgeNode(Node): self._open_serial() # ── Timers ──────────────────────────────────────────────────────────── -<<<<<<< HEAD - # Telemetry read at 100 Hz (ESP32 BALANCE sends at 50 Hz) -======= - # Telemetry read at 100 Hz (ESP32-S3 sends at 50 Hz) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - self._read_timer = self.create_timer(0.01, self._read_cb) + # Telemetry read at 100 Hz (ESP32-S3 sends at 50 Hz) self._read_timer = self.create_timer(0.01, self._read_cb) # Control loop at 50 Hz: ramp + deadman + mode gate + send self._control_timer = self.create_timer(1.0 / _CONTROL_HZ, self._control_cb) # Heartbeat TX @@ -256,12 +234,7 @@ class CmdVelBridgeNode(Node): speed = self._current_speed steer = self._current_steer -<<<<<<< HEAD - # Send to ESP32 BALANCE -======= - # Send to ESP32-S3 ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - frame = f"C{speed},{steer}\n".encode("ascii") + # Send to ESP32-S3 frame = f"C{speed},{steer}\n".encode("ascii") if not self._write(frame): self.get_logger().warn( "Cannot send cmd — serial not open", @@ -278,12 +251,7 @@ class CmdVelBridgeNode(Node): # ── Heartbeat TX ────────────────────────────────────────────────────────── def _heartbeat_cb(self): -<<<<<<< HEAD - """H\\n keeps ESP32 BALANCE jetson_cmd heartbeat alive regardless of mode.""" -======= - """H\\n keeps ESP32-S3 jetson_cmd heartbeat alive regardless of mode.""" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - self._write(b"H\n") + """H\\n keeps ESP32-S3 jetson_cmd heartbeat alive regardless of mode.""" self._write(b"H\n") # ── Telemetry RX ────────────────────────────────────────────────────────── @@ -404,12 +372,7 @@ class CmdVelBridgeNode(Node): diag.header.stamp = stamp status = DiagnosticStatus() status.name = "saltybot/balance_controller" -<<<<<<< HEAD - status.hardware_id = "esp32" -======= - status.hardware_id = "esp32s322" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - status.message = f"{state_label} [{mode_label}]" + status.hardware_id = "esp32s322" status.message = f"{state_label} [{mode_label}]" status.level = ( DiagnosticStatus.OK if state == 1 else DiagnosticStatus.WARN if state == 0 else @@ -436,20 +399,11 @@ class CmdVelBridgeNode(Node): status = DiagnosticStatus() status.level = DiagnosticStatus.ERROR status.name = "saltybot/balance_controller" -<<<<<<< HEAD - status.hardware_id = "esp32" - status.message = f"IMU fault errno={errno}" - diag.status.append(status) - self._diag_pub.publish(diag) - self.get_logger().error(f"ESP32 BALANCE IMU fault: errno={errno}") -======= status.hardware_id = "esp32s322" status.message = f"IMU fault errno={errno}" diag.status.append(status) self._diag_pub.publish(diag) self.get_logger().error(f"ESP32-S3 IMU fault: errno={errno}") ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # ── Lifecycle ───────────────────────────────────────────────────────────── def destroy_node(self): diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py index 87e9971..e8a7feb 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py @@ -1,31 +1,15 @@ -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py -"""stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node. -======= """esp32_cmd_node.py — Full bidirectional binary-framed ESP32-S3↔Jetson bridge. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py - Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5). -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py -This node is NOT the primary drive path (that is CAN via can_bridge_node). -It handles auxiliary I/O: RC monitoring, sensor data, LED/output control. -======= TX commands (Jetson → ESP32-S3): SPEED_STEER — 50 Hz from /cmd_vel subscription HEARTBEAT — 200 ms timer (ESP32-S3 watchdog fires at 500 ms) ARM — via /saltybot/arm service SET_MODE — via /saltybot/set_mode service PID_UPDATE — via /saltybot/pid_update topic ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py - Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py -RX from ESP32 IO: - RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON) - SENSORS (0x02) → /saltybot/sensors (std_msgs/String JSON) -======= RX telemetry (ESP32-S3 → Jetson): IMU → /saltybot/imu (sensor_msgs/Imu) BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON) @@ -33,20 +17,11 @@ RX telemetry (ESP32-S3 → Jetson): ARM_STATE → /saltybot/arm_state (std_msgs/String JSON) ERROR → /saltybot/error (std_msgs/String JSON) All frames → /diagnostics (diagnostic_msgs/DiagnosticArray) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py - TX to ESP32 IO: LED_CMD (0x10) ← /saltybot/leds (std_msgs/String JSON) OUTPUT_CMD (0x11) ← /saltybot/outputs (std_msgs/String JSON) HEARTBEAT (0x20) — sent every heartbeat_period (keep IO watchdog alive) -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py -Parameters (config/stm32_cmd_params.yaml): - serial_port /dev/esp32-io - baud_rate 460800 - reconnect_delay 2.0 - heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms) -======= Parameters (config/esp32_cmd_params.yaml): serial_port /dev/ttyACM0 baud_rate 921600 @@ -54,9 +29,7 @@ Parameters (config/esp32_cmd_params.yaml): heartbeat_period 0.2 (seconds) watchdog_timeout 0.5 (seconds — no /cmd_vel → send zero-speed) speed_scale 1000.0 (linear.x m/s → ESC units) - steer_scale -500.0 (angular.z rad/s → ESC units, neg to flip convention) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py -""" + steer_scale -500.0 (angular.z rad/s → ESC units, neg to flip convention)""" from __future__ import annotations @@ -73,13 +46,7 @@ import serial from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from std_msgs.msg import String -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py -from .stm32_protocol import ( - BAUD_RATE, -======= -from .esp32_protocol import ( ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py - FrameParser, +from .esp32_protocol import ( FrameParser, RcChannels, SensorData, encode_heartbeat, @@ -88,13 +55,8 @@ from .esp32_protocol import ( ) -class Stm32CmdNode(Node): -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py - """Orin ↔ ESP32-S3 IO auxiliary bridge node.""" -======= +class Esp32CmdNode(Node): """Binary-framed Jetson↔ESP32-S3 bridge node.""" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py - def __init__(self) -> None: super().__init__("esp32_cmd_node") @@ -138,13 +100,8 @@ class Stm32CmdNode(Node): self._diag_timer = self.create_timer(1.0, self._publish_diagnostics) self.get_logger().info( -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py - f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud" -======= f"esp32_cmd_node started — {port} @ {baud} baud | " - f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py - ) + f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms" ) # ── Serial management ───────────────────────────────────────────────── @@ -245,9 +202,6 @@ class Stm32CmdNode(Node): type_code, _ = msg self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}") -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py - # ── TX ──────────────────────────────────────────────────────────────── -======= elif isinstance(frame, ArmStateFrame): self._publish_arm_state(frame, now) @@ -358,8 +312,6 @@ class Stm32CmdNode(Node): "SPEED_STEER dropped — serial not open", throttle_duration_sec=2.0, ) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py - def _heartbeat_cb(self) -> None: self._write(encode_heartbeat()) @@ -399,14 +351,8 @@ class Stm32CmdNode(Node): diag = DiagnosticArray() diag.header.stamp = self.get_clock().now().to_msg() status = DiagnosticStatus() -<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py - status.name = "saltybot/esp32_io_bridge" - status.hardware_id = "esp32-s3-io" -======= status.name = "saltybot/esp32_cmd_node" status.hardware_id = "esp32s322" - ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py port_ok = self._ser is not None and self._ser.is_open status.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}" @@ -436,7 +382,7 @@ class Stm32CmdNode(Node): def main(args=None) -> None: rclpy.init(args=args) - node = Stm32CmdNode() + node = Esp32CmdNode() try: rclpy.spin(node) except KeyboardInterrupt: diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/remote_estop_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/remote_estop_node.py index 53a1712..79b60c9 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/remote_estop_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/remote_estop_node.py @@ -1,16 +1,8 @@ """ -<<<<<<< HEAD -remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32 USB CDC - -{"kill": true} -> writes 'E\n' to ESP32 BALANCE (ESTOP_REMOTE, immediate motor cutoff) -{"kill": false} -> writes 'Z\n' to ESP32 BALANCE (clear latch, robot can re-arm) -======= remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32-S3 USB CDC {"kill": true} -> writes 'E\n' to ESP32-S3 (ESTOP_REMOTE, immediate motor cutoff) {"kill": false} -> writes 'Z\n' to ESP32-S3 (clear latch, robot can re-arm) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Cellular watchdog: if MQTT link drops for > cellular_timeout_s while in AUTO mode, automatically sends 'F\n' (ESTOP_CELLULAR_TIMEOUT). diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py index 4aa73bb..19e93a9 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py @@ -9,13 +9,13 @@ back to FC over CAN. CAN interface: SocketCAN (CANable USB adapter on vcan0 / can0) FC → Orin (telemetry): - 0x400 FC_STATUS int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, + 0x400 BALANCE_STATUS int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, uint8 balance_state, uint8 flags (10 Hz) - 0x401 FC_VESC int16 left_rpm_x10, int16 right_rpm_x10, + 0x401 BALANCE_VESC int16 left_rpm_x10, int16 right_rpm_x10, int16 left_cur_x10, int16 right_cur_x10 (10 Hz) - 0x402 FC_IMU int16 pitch_x10, int16 roll_x10, int16 yaw_x10, + 0x402 BALANCE_IMU int16 pitch_x10, int16 roll_x10, int16 yaw_x10, uint8 cal_status, uint8 balance_state (50 Hz) - 0x403 FC_BARO int32 pressure_pa, int16 temp_x10, int16 alt_cm (1 Hz) + 0x403 BALANCE_BARO int32 pressure_pa, int16 temp_x10, int16 alt_cm (1 Hz) Orin → FC (commands): 0x300 HEARTBEAT uint32 sequence counter (5 Hz) @@ -57,10 +57,10 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue # ---- CAN frame IDs ------------------------------------------------ -CAN_FC_STATUS = 0x400 -CAN_FC_VESC = 0x401 -CAN_FC_IMU = 0x402 -CAN_FC_BARO = 0x403 +CAN_BALANCE_STATUS = 0x400 +CAN_BALANCE_VESC = 0x401 +CAN_BALANCE_IMU = 0x402 +CAN_BALANCE_BARO = 0x403 CAN_HEARTBEAT = 0x300 CAN_DRIVE = 0x301 @@ -216,11 +216,11 @@ class SaltybotCanNode(Node): def _dispatch(self, can_id: int, data: bytes): now = self.get_clock().now().to_msg() - if can_id == CAN_FC_IMU and len(data) >= 8: + if can_id == CAN_BALANCE_IMU and len(data) >= 8: self._handle_fc_imu(data, now) - elif can_id == CAN_FC_STATUS and len(data) >= 8: + elif can_id == CAN_BALANCE_STATUS and len(data) >= 8: self._handle_fc_status(data) - elif can_id == CAN_FC_BARO and len(data) >= 8: + elif can_id == CAN_BALANCE_BARO and len(data) >= 8: self._handle_fc_baro(data, now) # ── Frame handlers ─────────────────────────────────────────────── @@ -322,12 +322,7 @@ class SaltybotCanNode(Node): diag.header.stamp = stamp st = DiagnosticStatus() st.name = "saltybot/balance_controller" -<<<<<<< HEAD - st.hardware_id = "esp32" -======= - st.hardware_id = "esp32s322" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - st.message = state_label + st.hardware_id = "esp32s322" st.message = state_label st.level = (DiagnosticStatus.OK if state == 1 else DiagnosticStatus.WARN if state == 0 else DiagnosticStatus.ERROR) diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py index b2acaf1..1f23cfe 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py @@ -1,26 +1,8 @@ """ -<<<<<<< HEAD -saltybot_cmd_node — full bidirectional ESP32 BALANCE↔Jetson bridge -======= -saltybot_cmd_node — full bidirectional ESP32-S3↔Jetson bridge ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -Combines telemetry RX (from serial_bridge_node) with drive command TX. +saltybot_cmd_node — full bidirectional ESP32-S3↔Jetson bridgeCombines telemetry RX (from serial_bridge_node) with drive command TX. Owns /dev/ttyACM0 exclusively — do NOT run alongside serial_bridge_node. -<<<<<<< HEAD -RX path (50Hz from ESP32 BALANCE): - JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics - -TX path: - /cmd_vel (geometry_msgs/Twist) → C,\\n → ESP32 BALANCE - Heartbeat timer (200ms) → H\\n → ESP32 BALANCE - -Protocol: - H\\n — heartbeat. ESP32 BALANCE reverts steer to 0 if gap > 500ms. - C,\\n — drive command. speed/steer: -1000..+1000 integers. - C command also refreshes ESP32 BALANCE heartbeat timer. -======= RX path (50Hz from ESP32-S3): JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics @@ -32,8 +14,6 @@ Protocol: H\\n — heartbeat. ESP32-S3 reverts steer to 0 if gap > 500ms. C,\\n — drive command. speed/steer: -1000..+1000 integers. C command also refreshes ESP32-S3 heartbeat timer. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Twist mapping (configurable via ROS2 params): speed = clamp(linear.x * speed_scale, -1000, 1000) steer = clamp(angular.z * steer_scale, -1000, 1000) @@ -118,12 +98,7 @@ class SaltybotCmdNode(Node): self._open_serial() # ── Timers ──────────────────────────────────────────────────────────── -<<<<<<< HEAD - # Telemetry read at 100Hz (ESP32 BALANCE sends at 50Hz) -======= - # Telemetry read at 100Hz (ESP32-S3 sends at 50Hz) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - self._read_timer = self.create_timer(0.01, self._read_cb) + # Telemetry read at 100Hz (ESP32-S3 sends at 50Hz) self._read_timer = self.create_timer(0.01, self._read_cb) # Heartbeat TX at configured period (default 200ms) self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb) @@ -288,12 +263,7 @@ class SaltybotCmdNode(Node): diag.header.stamp = stamp status = DiagnosticStatus() status.name = "saltybot/balance_controller" -<<<<<<< HEAD - status.hardware_id = "esp32" -======= - status.hardware_id = "esp32s322" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - status.message = state_label + status.hardware_id = "esp32s322" status.message = state_label if state == 1: status.level = DiagnosticStatus.OK elif state == 0: @@ -320,20 +290,11 @@ class SaltybotCmdNode(Node): status = DiagnosticStatus() status.level = DiagnosticStatus.ERROR status.name = "saltybot/balance_controller" -<<<<<<< HEAD - status.hardware_id = "esp32" - status.message = f"IMU fault errno={errno}" - diag.status.append(status) - self._diag_pub.publish(diag) - self.get_logger().error(f"ESP32 BALANCE IMU fault: errno={errno}") -======= status.hardware_id = "esp32s322" status.message = f"IMU fault errno={errno}" diag.status.append(status) self._diag_pub.publish(diag) self.get_logger().error(f"ESP32-S3 IMU fault: errno={errno}") ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # ── TX — command send ───────────────────────────────────────────────────── def _cmd_vel_cb(self, msg: Twist): @@ -350,12 +311,7 @@ class SaltybotCmdNode(Node): ) def _heartbeat_cb(self): -<<<<<<< HEAD - """Send H\\n heartbeat. ESP32 BALANCE reverts steer to 0 if gap > 500ms.""" -======= - """Send H\\n heartbeat. ESP32-S3 reverts steer to 0 if gap > 500ms.""" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - self._write(b"H\n") + """Send H\\n heartbeat. ESP32-S3 reverts steer to 0 if gap > 500ms.""" self._write(b"H\n") # ── Lifecycle ───────────────────────────────────────────────────────────── diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py index c405c52..bd29fd6 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py @@ -1,11 +1,6 @@ """ saltybot_bridge — serial_bridge_node -<<<<<<< HEAD -ESP32 USB CDC → ROS2 topic publisher -======= ESP32-S3 USB CDC → ROS2 topic publisher ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Telemetry frame (50 Hz, newline-delimited JSON): {"p":,"r":,"e":,"ig":, "m":,"s":,"y":} @@ -33,12 +28,7 @@ from sensor_msgs.msg import Imu from std_msgs.msg import String from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue -<<<<<<< HEAD -# Balance state labels matching ESP32 BALANCE balance_state_t enum -======= -# Balance state labels matching ESP32-S3 balance_state_t enum ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"} +# Balance state labels matching ESP32-S3 balance_state_t enum_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"} # Sensor frame_id published in Imu header IMU_FRAME_ID = "imu_link" @@ -91,12 +81,7 @@ class SerialBridgeNode(Node): # ── Open serial and start read timer ────────────────────────────────── self._open_serial() -<<<<<<< HEAD - # Poll at 100 Hz — ESP32 BALANCE sends at 50 Hz, so we never miss a frame -======= - # Poll at 100 Hz — ESP32-S3 sends at 50 Hz, so we never miss a frame ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - self._timer = self.create_timer(0.01, self._read_cb) + # Poll at 100 Hz — ESP32-S3 sends at 50 Hz, so we never miss a frame self._timer = self.create_timer(0.01, self._read_cb) self.get_logger().info( f"stm32_serial_bridge started — {port} @ {baud} baud" @@ -129,12 +114,7 @@ class SerialBridgeNode(Node): def write_serial(self, data: bytes) -> bool: """ -<<<<<<< HEAD - Send raw bytes to ESP32 BALANCE over the open serial port. -======= - Send raw bytes to ESP32-S3 over the open serial port. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Returns False if port is not open (caller should handle gracefully). + Send raw bytes to ESP32-S3 over the open serial port. Returns False if port is not open (caller should handle gracefully). Note: for bidirectional use prefer saltybot_cmd_node which owns TX natively. """ if self._ser is None or not self._ser.is_open: @@ -222,12 +202,7 @@ class SerialBridgeNode(Node): """ Publish sensor_msgs/Imu. -<<<<<<< HEAD - The ESP32 BALANCE IMU gives Euler angles (pitch/roll from accelerometer+gyro -======= - The ESP32-S3 IMU gives Euler angles (pitch/roll from accelerometer+gyro ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - fusion, yaw from gyro integration). We publish them as angular_velocity + The ESP32-S3 IMU gives Euler angles (pitch/roll from accelerometer+gyro fusion, yaw from gyro integration). We publish them as angular_velocity for immediate use by slam_toolbox / robot_localization. Note: orientation quaternion is left zeroed (covariance [-1,...]) until @@ -284,12 +259,7 @@ class SerialBridgeNode(Node): diag.header.stamp = stamp status = DiagnosticStatus() status.name = "saltybot/balance_controller" -<<<<<<< HEAD - status.hardware_id = "esp32" -======= - status.hardware_id = "esp32s322" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - status.message = state_label + status.hardware_id = "esp32s322" status.message = state_label if state == 1: # ARMED status.level = DiagnosticStatus.OK @@ -317,20 +287,11 @@ class SerialBridgeNode(Node): status = DiagnosticStatus() status.level = DiagnosticStatus.ERROR status.name = "saltybot/balance_controller" -<<<<<<< HEAD - status.hardware_id = "esp32" - status.message = f"IMU fault errno={errno}" - diag.status.append(status) - self._diag_pub.publish(diag) - self.get_logger().error(f"ESP32 BALANCE reported IMU fault: errno={errno}") -======= status.hardware_id = "esp32s322" status.message = f"IMU fault errno={errno}" diag.status.append(status) self._diag_pub.publish(diag) self.get_logger().error(f"ESP32-S3 reported IMU fault: errno={errno}") ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - def destroy_node(self): self._close_serial() super().destroy_node() diff --git a/jetson/ros2_ws/src/saltybot_bridge/setup.py b/jetson/ros2_ws/src/saltybot_bridge/setup.py index 81f1d41..9aff694 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/setup.py +++ b/jetson/ros2_ws/src/saltybot_bridge/setup.py @@ -29,12 +29,7 @@ setup( zip_safe=True, maintainer="sl-jetson", maintainer_email="sl-jetson@saltylab.local", -<<<<<<< HEAD - description="ESP32 USB CDC → ROS2 serial bridge for saltybot", -======= - description="ESP32-S3 USB CDC → ROS2 serial bridge for saltybot", ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - license="MIT", + description="ESP32-S3 USB CDC → ROS2 serial bridge for saltybot", license="MIT", tests_require=["pytest"], entry_points={ "console_scripts": [ @@ -45,14 +40,8 @@ setup( # Nav2 cmd_vel bridge: velocity limits + ramp + deadman + mode gate "cmd_vel_bridge_node = saltybot_bridge.cmd_vel_bridge_node:main", "remote_estop_node = saltybot_bridge.remote_estop_node:main", -<<<<<<< HEAD - # Binary-framed ESP32 BALANCE command node (Issue #119) - "stm32_cmd_node = saltybot_bridge.stm32_cmd_node:main", -======= # Binary-framed ESP32-S3 command node (Issue #119) - "esp32_cmd_node = saltybot_bridge.esp32_cmd_node:main", ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # Battery management node (Issue #125) + "esp32_cmd_node = saltybot_bridge.esp32_cmd_node:main", # Battery management node (Issue #125) "battery_node = saltybot_bridge.battery_node:main", # Production CAN bridge: FC telemetry RX + /cmd_vel TX over CAN (Issues #680, #672, #685) "saltybot_can_node = saltybot_bridge.saltybot_can_node:main", diff --git a/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py b/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py index 29f496e..b00545d 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py +++ b/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py @@ -1,10 +1,5 @@ """ -<<<<<<< HEAD -Unit tests for Jetson→ESP32 BALANCE command serialization logic. -======= -Unit tests for Jetson→ESP32-S3 command serialization logic. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -Tests Twist→speed/steer conversion and frame formatting. +Unit tests for Jetson→ESP32-S3 command serialization logic.Tests Twist→speed/steer conversion and frame formatting. Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py """ diff --git a/jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py b/jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py index 1cd5e8c..2e062f4 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py @@ -1,4 +1,4 @@ -"""test_esp32_cmd_node.py — Unit tests for Stm32CmdNode with mock serial port. +"""test_esp32_cmd_node.py — Unit tests for Esp32CmdNode with mock serial port. Tests: - Serial open/close lifecycle diff --git a/jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py b/jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py index 8079168..7b83e00 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py +++ b/jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py @@ -1,10 +1,5 @@ """ -<<<<<<< HEAD -Unit tests for ESP32 BALANCE telemetry parsing logic. -======= -Unit tests for ESP32-S3 telemetry parsing logic. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py +Unit tests for ESP32-S3 telemetry parsing logic.Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py """ import json diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml index f251c7d..0449028 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml @@ -19,12 +19,7 @@ # inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding) # DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer) # -<<<<<<< HEAD -# Output: /cmd_vel (Twist) — ESP32 bridge consumes this topic. -======= # Output: /cmd_vel (Twist) — ESP32-S3 bridge consumes this topic. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - bt_navigator: ros__parameters: use_sim_time: false diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/saltybot_params.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/saltybot_params.yaml index b858251..1b20315 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/config/saltybot_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bringup/config/saltybot_params.yaml @@ -2,12 +2,7 @@ # Master configuration for full stack bringup # ──────────────────────────────────────────────────────────────────────────── -<<<<<<< HEAD -# HARDWARE — ESP32 BALANCE Bridge & Motor Control -======= -# HARDWARE — ESP32-S3 Bridge & Motor Control ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -# ──────────────────────────────────────────────────────────────────────────── +# HARDWARE — ESP32-S3 Bridge & Motor Control# ──────────────────────────────────────────────────────────────────────────── saltybot_bridge_node: ros__parameters: diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py index 984a020..7f87a80 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py @@ -39,12 +39,7 @@ Modes ─ UWB driver (2-anchor DW3000, publishes /uwb/target) ─ YOLOv8n person detection (TensorRT) ─ Person follower with UWB+camera fusion -<<<<<<< HEAD - ─ cmd_vel bridge → ESP32 BALANCE (deadman + ramp + AUTONOMOUS gate) -======= - ─ cmd_vel bridge → ESP32-S3 (deadman + ramp + AUTONOMOUS gate) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ─ rosbridge WebSocket (port 9090) + ─ cmd_vel bridge → ESP32-S3 (deadman + ramp + AUTONOMOUS gate) ─ rosbridge WebSocket (port 9090) outdoor ─ RPLIDAR + RealSense D435i sensors (no SLAM) @@ -61,14 +56,8 @@ Modes Launch sequence (wall-clock delays — conservative for cold start) ───────────────────────────────────────────────────────────────── t= 0s robot_description (URDF + TF tree) -<<<<<<< HEAD - t= 0s ESP32 bridge (serial port owner — must be first) - t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32 bridge up) -======= t= 0s ESP32-S3 bridge (serial port owner — must be first) - t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32-S3 bridge up) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - t= 2s sensors (RPLIDAR + RealSense) + t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32-S3 bridge up) t= 2s sensors (RPLIDAR + RealSense) t= 4s UWB driver (independent serial device) t= 4s CSI cameras (optional, independent) t= 5s audio_pipeline (Jabra SPEAK 810: wake word + STT + TTS; Issue #503) @@ -80,18 +69,10 @@ Launch sequence (wall-clock delays — conservative for cold start) Safety wiring ───────────── -<<<<<<< HEAD - • ESP32 bridge must be up before cmd_vel bridge sends any command. - • cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent. - • ESP32 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still - until ESP32 BALANCE firmware is in AUTONOMOUS mode regardless of /cmd_vel. -======= • ESP32-S3 bridge must be up before cmd_vel bridge sends any command. • cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent. • ESP32-S3 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still - until ESP32-S3 firmware is in AUTONOMOUS mode regardless of /cmd_vel. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - • follow_enabled:=false disables person follower without stopping the node. + until ESP32-S3 firmware is in AUTONOMOUS mode regardless of /cmd_vel. • follow_enabled:=false disables person follower without stopping the node. • To e-stop at runtime: ros2 topic pub /saltybot/estop std_msgs/Bool '{data: true}' Topics published by this stack @@ -107,12 +88,7 @@ Topics published by this stack /person/target PoseStamped (camera position, base_link) /person/detections Detection2DArray /cmd_vel Twist (from follower or Nav2) -<<<<<<< HEAD - /saltybot/cmd String (to ESP32 BALANCE) -======= - /saltybot/cmd String (to ESP32-S3) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - /saltybot/imu Imu + /saltybot/cmd String (to ESP32-S3) /saltybot/imu Imu /saltybot/balance_state String """ @@ -229,12 +205,7 @@ def generate_launch_description(): enable_bridge_arg = DeclareLaunchArgument( "enable_bridge", default_value="true", -<<<<<<< HEAD - description="Launch ESP32 serial bridge + cmd_vel bridge (disable for sim/rosbag)", -======= - description="Launch ESP32-S3 serial bridge + cmd_vel bridge (disable for sim/rosbag)", ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ) + description="Launch ESP32-S3 serial bridge + cmd_vel bridge (disable for sim/rosbag)", ) enable_rosbridge_arg = DeclareLaunchArgument( "enable_rosbridge", @@ -294,12 +265,7 @@ enable_mission_logging_arg = DeclareLaunchArgument( esp32_port_arg = DeclareLaunchArgument( "esp32_port", default_value="/dev/esp32-bridge", -<<<<<<< HEAD - description="ESP32 USB CDC serial port", -======= - description="ESP32-S3 USB CDC serial port", ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ) + description="ESP32-S3 USB CDC serial port", ) # ── Shared substitution handles ─────────────────────────────────────────── # Profile argument for parameter override (Issue #506) @@ -318,12 +284,7 @@ enable_mission_logging_arg = DeclareLaunchArgument( launch_arguments={"use_sim_time": use_sim_time}.items(), ) -<<<<<<< HEAD - # ── t=0s ESP32 bidirectional serial bridge ──────────────────────────────── -======= - # ── t=0s ESP32-S3 bidirectional serial bridge ──────────────────────────────── ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - esp32_bridge = GroupAction( + # ── t=0s ESP32-S3 bidirectional serial bridge ──────────────────────────────── esp32_bridge = GroupAction( condition=IfCondition(LaunchConfiguration("enable_bridge")), actions=[ IncludeLaunchDescription( @@ -352,12 +313,7 @@ enable_mission_logging_arg = DeclareLaunchArgument( ], ) -<<<<<<< HEAD - # ── t=2s cmd_vel safety bridge (depends on ESP32 bridge) ──────────────── -======= - # ── t=2s cmd_vel safety bridge (depends on ESP32-S3 bridge) ──────────────── ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - cmd_vel_bridge = TimerAction( + # ── t=2s cmd_vel safety bridge (depends on ESP32-S3 bridge) ──────────────── cmd_vel_bridge = TimerAction( period=2.0, actions=[ GroupAction( diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py index 8f9561d..8723a65 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py @@ -19,12 +19,7 @@ Usage Startup sequence ──────────────── -<<<<<<< HEAD - GROUP A — Drivers t= 0 s ESP32 bridge, RealSense+RPLIDAR, motor daemon, IMU -======= - GROUP A — Drivers t= 0 s ESP32-S3 bridge, RealSense+RPLIDAR, motor daemon, IMU ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - health gate ───────────────────────────────────────────────── t= 8 s (full/debug) + GROUP A — Drivers t= 0 s ESP32-S3 bridge, RealSense+RPLIDAR, motor daemon, IMU health gate ───────────────────────────────────────────────── t= 8 s (full/debug) GROUP B — Perception t= 8 s UWB, person detection, object detection, depth costmap, gimbal health gate ───────────────────────────────────────────────── t=16 s (full/debug) GROUP C — Navigation t=16 s SLAM, Nav2, lidar avoidance, follower, docking @@ -127,12 +122,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 esp32_port_arg = DeclareLaunchArgument( "esp32_port", default_value="/dev/esp32-bridge", -<<<<<<< HEAD - description="ESP32 UART bridge serial device", -======= - description="ESP32-S3 USART bridge serial device", ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ) + description="ESP32-S3 USART bridge serial device", ) uwb_port_a_arg = DeclareLaunchArgument( "uwb_port_a", @@ -206,12 +196,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ # GROUP A — DRIVERS (t = 0 s, all profiles) -<<<<<<< HEAD - # Dependency order: ESP32 bridge first, then sensors, then motor daemon. -======= - # Dependency order: ESP32-S3 bridge first, then sensors, then motor daemon. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # Health gate: subsequent groups delayed until t_perception (8 s full/debug). + # Dependency order: ESP32-S3 bridge first, then sensors, then motor daemon. # Health gate: subsequent groups delayed until t_perception (8 s full/debug). # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ group_a_banner = LogInfo( @@ -224,12 +209,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 launch_arguments={"use_sim_time": use_sim_time}.items(), ) -<<<<<<< HEAD - # ESP32 BALANCE bridge -======= - # ESP32-S3 bidirectional bridge (JLINK USART1) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - esp32_bridge = IncludeLaunchDescription( + # ESP32-S3 bidirectional bridge (JLINK USART1) esp32_bridge = IncludeLaunchDescription( _launch("saltybot_bridge", "launch", "bridge.launch.py"), launch_arguments={ "mode": "bidirectional", @@ -248,12 +228,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 ], ) -<<<<<<< HEAD - # Motor daemon: /cmd_vel → ESP32 BALANCE DRIVE frames (depends on bridge at t=0) -======= - # Motor daemon: /cmd_vel → ESP32-S3 DRIVE frames (depends on bridge at t=0) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - motor_daemon = TimerAction( + # Motor daemon: /cmd_vel → ESP32-S3 DRIVE frames (depends on bridge at t=0) motor_daemon = TimerAction( period=2.5, actions=[ LogInfo(msg="[saltybot_bringup] t=2.5s Starting motor daemon"), diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py index 15996cb..3f9513f 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py @@ -20,12 +20,7 @@ theta is kept in (−π, π] after every step. Int32 rollover -------------- -<<<<<<< HEAD -ESP32 BALANCE encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handles -======= -ESP32-S3 encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handles ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -this by detecting jumps larger than half the int32 range and adjusting by the +ESP32-S3 encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handlesthis by detecting jumps larger than half the int32 range and adjusting by the full range: if delta > 2^30 : delta -= 2^31 diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py index edcb1d6..21f169e 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py @@ -69,12 +69,7 @@ class Profile: t_ui: float = 22.0 # Group D (nav2 needs ~4 s to load costmaps) # ── Safety ──────────────────────────────────────────────────────────── -<<<<<<< HEAD - watchdog_timeout_s: float = 5.0 # max silence from ESP32 bridge (s) -======= - watchdog_timeout_s: float = 5.0 # max silence from ESP32-S3 bridge (s) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - cmd_vel_deadman_s: float = 0.5 # cmd_vel watchdog in bridge + watchdog_timeout_s: float = 5.0 # max silence from ESP32-S3 bridge (s) cmd_vel_deadman_s: float = 0.5 # cmd_vel watchdog in bridge max_linear_vel: float = 0.5 # m/s cap passed to bridge + follower follow_distance_m: float = 1.5 # target follow distance (m) @@ -94,12 +89,7 @@ class Profile: # ── Profile factory ──────────────────────────────────────────────────────────── def _minimal() -> Profile: -<<<<<<< HEAD - """Minimal: ESP32 bridge + sensors + motor daemon. -======= """Minimal: ESP32-S3 bridge + sensors + motor daemon. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Safe drive control only. No AI, no nav, no social. Boot time ~4 s. RAM ~400 MB. """ diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py index bba409a..b9769ec 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py @@ -1,12 +1,7 @@ """ wheel_odom_node.py — Differential drive wheel encoder odometry (Issue #184). -<<<<<<< HEAD -Subscribes to raw encoder tick counts from the ESP32 bridge, integrates -======= -Subscribes to raw encoder tick counts from the ESP32-S3 bridge, integrates ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -differential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz. +Subscribes to raw encoder tick counts from the ESP32-S3 bridge, integratesdifferential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz. Optionally broadcasts the odom → base_link TF transform. Subscribes: diff --git a/jetson/ros2_ws/src/saltybot_bringup/test/README_INTEGRATION_TESTS.md b/jetson/ros2_ws/src/saltybot_bringup/test/README_INTEGRATION_TESTS.md index 8ac3a1e..53fffc4 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/test/README_INTEGRATION_TESTS.md +++ b/jetson/ros2_ws/src/saltybot_bringup/test/README_INTEGRATION_TESTS.md @@ -61,12 +61,7 @@ kill %1 ### Core System Components - Robot Description (URDF/TF tree) -<<<<<<< HEAD -- ESP32 Serial Bridge -======= -- ESP32-S3 Serial Bridge ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -- cmd_vel Bridge +- ESP32-S3 Serial Bridge- cmd_vel Bridge - Rosbridge WebSocket ### Sensors @@ -129,12 +124,7 @@ free -h ### cmd_vel bridge not responding ```bash -<<<<<<< HEAD -# Verify ESP32 bridge is running first -======= -# Verify ESP32-S3 bridge is running first ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -ros2 node list | grep bridge +# Verify ESP32-S3 bridge is running firstros2 node list | grep bridge # Check serial port ls -l /dev/esp32-bridge diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml b/jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml index f5babf2..91bb791 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml +++ b/jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml @@ -3,5 +3,5 @@ can_bridge_node: can_interface: slcan0 left_vesc_can_id: 56 right_vesc_can_id: 68 - mamba_can_id: 1 + balance_can_id: 1 command_timeout_s: 0.5 diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/balance_protocol.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/balance_protocol.py index ae9326e..fe6fcb9 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/balance_protocol.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/balance_protocol.py @@ -6,13 +6,13 @@ and VESC telemetry. CAN message layout ------------------ Command frames (Orin → ESP32-S3 BALANCE / VESC): - MAMBA_CMD_VELOCITY 0x100 8 bytes left_speed (f32, m/s) | right_speed (f32, m/s) - MAMBA_CMD_MODE 0x101 1 byte mode (0=idle, 1=drive, 2=estop) - MAMBA_CMD_ESTOP 0x102 1 byte 0x01 = stop + BALANCE_CMD_VELOCITY 0x100 8 bytes left_speed (f32, m/s) | right_speed (f32, m/s) + BALANCE_CMD_MODE 0x101 1 byte mode (0=idle, 1=drive, 2=estop) + BALANCE_CMD_ESTOP 0x102 1 byte 0x01 = stop Telemetry frames (ESP32-S3 BALANCE → Orin): - MAMBA_TELEM_IMU 0x200 24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each) - MAMBA_TELEM_BATTERY 0x201 8 bytes voltage (f32, V) | current (f32, A) + BALANCE_TELEM_IMU 0x200 24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each) + BALANCE_TELEM_BATTERY 0x201 8 bytes voltage (f32, V) | current (f32, A) VESC telemetry frame (VESC → Orin): VESC_TELEM_STATE 0x300 16 bytes erpm (f32) | duty (f32) | voltage (f32) | current (f32) @@ -30,12 +30,12 @@ from typing import Tuple # CAN message IDs # --------------------------------------------------------------------------- -MAMBA_CMD_VELOCITY: int = 0x100 -MAMBA_CMD_MODE: int = 0x101 -MAMBA_CMD_ESTOP: int = 0x102 +BALANCE_CMD_VELOCITY: int = 0x100 +BALANCE_CMD_MODE: int = 0x101 +BALANCE_CMD_ESTOP: int = 0x102 -MAMBA_TELEM_IMU: int = 0x200 -MAMBA_TELEM_BATTERY: int = 0x201 +BALANCE_TELEM_IMU: int = 0x200 +BALANCE_TELEM_BATTERY: int = 0x201 VESC_TELEM_STATE: int = 0x300 ORIN_CAN_ID_PID_SET: int = 0x305 @@ -56,7 +56,7 @@ MODE_ESTOP: int = 2 @dataclass class ImuTelemetry: - """Decoded IMU telemetry from ESP32-S3 BALANCE (MAMBA_TELEM_IMU).""" + """Decoded IMU telemetry from ESP32-S3 BALANCE (BALANCE_TELEM_IMU).""" accel_x: float = 0.0 # m/s² accel_y: float = 0.0 @@ -68,7 +68,7 @@ class ImuTelemetry: @dataclass class BatteryTelemetry: - """Decoded battery telemetry from ESP32-S3 BALANCE (MAMBA_TELEM_BATTERY).""" + """Decoded battery telemetry from ESP32-S3 BALANCE (BALANCE_TELEM_BATTERY).""" voltage: float = 0.0 # V current: float = 0.0 # A @@ -106,7 +106,7 @@ _FMT_VESC = ">ffff" # 4 × float32 def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes: """ - Encode a MAMBA_CMD_VELOCITY payload. + Encode a BALANCE_CMD_VELOCITY payload. Parameters ---------- @@ -122,7 +122,7 @@ def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes: def encode_mode_cmd(mode: int) -> bytes: """ - Encode a MAMBA_CMD_MODE payload. + Encode a BALANCE_CMD_MODE payload. Parameters ---------- @@ -139,7 +139,7 @@ def encode_mode_cmd(mode: int) -> bytes: def encode_estop_cmd(stop: bool = True) -> bytes: """ - Encode a MAMBA_CMD_ESTOP payload. + Encode a BALANCE_CMD_ESTOP payload. Parameters ---------- @@ -165,7 +165,7 @@ def encode_pid_set_cmd(kp: float, ki: float, kd: float) -> bytes: def decode_imu_telem(data: bytes) -> ImuTelemetry: """ - Decode a MAMBA_TELEM_IMU payload. + Decode a BALANCE_TELEM_IMU payload. Parameters ---------- @@ -188,7 +188,7 @@ def decode_imu_telem(data: bytes) -> ImuTelemetry: def decode_battery_telem(data: bytes) -> BatteryTelemetry: """ - Decode a MAMBA_TELEM_BATTERY payload. + Decode a BALANCE_TELEM_BATTERY payload. Parameters ---------- diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py index db6a1a5..4a631b2 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py @@ -39,22 +39,22 @@ from sensor_msgs.msg import BatteryState from std_msgs.msg import Bool, Float32MultiArray, String from saltybot_can_bridge.balance_protocol import ( - MAMBA_CMD_ESTOP, - MAMBA_CMD_MODE, - MAMBA_CMD_VELOCITY, - MAMBA_TELEM_BATTERY, - MAMBA_TELEM_IMU, + BALANCE_CMD_ESTOP, + BALANCE_CMD_MODE, + BALANCE_CMD_VELOCITY, + BALANCE_TELEM_BATTERY, + BALANCE_TELEM_IMU, VESC_TELEM_STATE, ORIN_CAN_ID_FC_PID_ACK, ORIN_CAN_ID_PID_SET, MODE_DRIVE, MODE_IDLE, - encode_drive_cmd, - encode_arm_cmd, + encode_velocity_cmd, + encode_mode_cmd, encode_estop_cmd, - decode_attitude, - decode_battery, - decode_vesc_status1, + decode_imu_telem, + decode_battery_telem, + decode_vesc_state, ) # Reconnect attempt interval when CAN bus is lost @@ -179,10 +179,10 @@ class CanBridgeNode(Node): right_mps = linear + angular payload = encode_velocity_cmd(left_mps, right_mps) - self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel") + self._send_can(BALANCE_CMD_VELOCITY, payload, "cmd_vel") # Keep ESP32-S3 BALANCE in DRIVE mode while receiving commands - self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode") + self._send_can(BALANCE_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode") def _estop_cb(self, msg: Bool) -> None: """Forward /estop to ESP32-S3 BALANCE over CAN.""" @@ -190,7 +190,7 @@ class CanBridgeNode(Node): return if msg.data: self._send_can( - MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode" + BALANCE_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode" ) self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32-S3 BALANCE") @@ -201,7 +201,8 @@ class CanBridgeNode(Node): if not self._connected: return if time.monotonic() - self._last_cmd_time > self._cmd_timeout: - self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog") + self._send_can(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0), "watchdog") + self._send_can(BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog mode") # ── CAN send helper ─────────────────────────────────────────────────── @@ -236,25 +237,28 @@ class CanBridgeNode(Node): continue self._dispatch_frame(frame) + # VESC STATUS packet type = 9 (upper byte of extended arb_id) + _VESC_STATUS_PKT: int = 9 + def _dispatch_frame(self, frame: can.Message) -> None: arb_id = frame.arbitration_id data = bytes(frame.data) - vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id - vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id + vesc_l = (self._VESC_STATUS_PKT << 8) | self._left_vesc_id + vesc_r = (self._VESC_STATUS_PKT << 8) | self._right_vesc_id try: - if arb_id == ESP32_TELEM_ATTITUDE: - self._handle_attitude(data) - elif arb_id == ESP32_TELEM_BATTERY: + if arb_id == BALANCE_TELEM_IMU: + self._handle_imu(data) + elif arb_id == BALANCE_TELEM_BATTERY: self._handle_battery(data) elif arb_id == vesc_l: - t = decode_vesc_status1(self._left_vesc_id, data) + t = decode_vesc_state(data) m = Float32MultiArray() - m.data = [t.erpm, t.duty, 0.0, t.current] + m.data = [t.erpm, t.duty, t.voltage, t.current] self._pub_vesc_left.publish(m) elif arb_id == vesc_r: - t = decode_vesc_status1(self._right_vesc_id, data) + t = decode_vesc_state(data) m = Float32MultiArray() - m.data = [t.erpm, t.duty, 0.0, t.current] + m.data = [t.erpm, t.duty, t.voltage, t.current] self._pub_vesc_right.publish(m) except Exception as exc: self.get_logger().warning( @@ -263,20 +267,18 @@ class CanBridgeNode(Node): # ── Frame handlers ──────────────────────────────────────────────────── - _STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"} - - def _handle_attitude(self, data: bytes) -> None: - """ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude.""" - t = decode_attitude(data) + def _handle_imu(self, data: bytes) -> None: + """BALANCE_TELEM_IMU (0x200): accel + gyro → /saltybot/attitude.""" + t = decode_imu_telem(data) now = self.get_clock().now().to_msg() payload = { - "pitch_deg": round(t.pitch_deg, 2), - "speed_mps": round(t.speed, 3), - "yaw_rate": round(t.yaw_rate, 3), - "state": t.state, - "state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"), - "flags": t.flags, - "ts": f"{now.sec}.{now.nanosec:09d}", + "accel_x": round(t.accel_x, 4), + "accel_y": round(t.accel_y, 4), + "accel_z": round(t.accel_z, 4), + "gyro_x": round(t.gyro_x, 4), + "gyro_y": round(t.gyro_y, 4), + "gyro_z": round(t.gyro_z, 4), + "ts": f"{now.sec}.{now.nanosec:09d}", } msg = String() msg.data = json.dumps(payload) @@ -284,11 +286,12 @@ class CanBridgeNode(Node): self._pub_balance.publish(msg) # keep /saltybot/balance_state alive def _handle_battery(self, data: bytes) -> None: - """BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery.""" - t = decode_battery(data) + """BALANCE_TELEM_BATTERY (0x201): voltage + current → /can/battery.""" + t = decode_battery_telem(data) msg = BatteryState() msg.header.stamp = self.get_clock().now().to_msg() - msg.voltage = t.vbat_mv / 1000.0 + msg.voltage = t.voltage + msg.current = t.current msg.present = True msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING self._pub_battery.publish(msg) @@ -305,8 +308,9 @@ class CanBridgeNode(Node): def destroy_node(self) -> None: if self._connected and self._bus is not None: try: - self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown") - self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "shutdown") + # Send zero velocity and idle mode on shutdown + self._send_can(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0), "shutdown") + self._send_can(BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown") except Exception: pass try: diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/setup.py b/jetson/ros2_ws/src/saltybot_can_bridge/setup.py index 9ac12dc..694ef98 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/setup.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/setup.py @@ -15,12 +15,7 @@ setup( zip_safe=True, maintainer="sl-controls", maintainer_email="sl-controls@saltylab.local", -<<<<<<< HEAD - description="CAN bus bridge for ESP32 IO motor controller and VESC telemetry", -======= - description="CAN bus bridge for ESP32-S3 BALANCE controller and VESC telemetry", ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - license="MIT", + description="CAN bus bridge for ESP32-S3 BALANCE controller and VESC telemetry", license="MIT", tests_require=["pytest"], entry_points={ "console_scripts": [ diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py b/jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py index 6bddca2..15ef454 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py @@ -12,11 +12,11 @@ import struct import unittest from saltybot_can_bridge.balance_protocol import ( - MAMBA_CMD_ESTOP, - MAMBA_CMD_MODE, - MAMBA_CMD_VELOCITY, - MAMBA_TELEM_BATTERY, - MAMBA_TELEM_IMU, + BALANCE_CMD_ESTOP, + BALANCE_CMD_MODE, + BALANCE_CMD_VELOCITY, + BALANCE_TELEM_BATTERY, + BALANCE_TELEM_IMU, VESC_TELEM_STATE, MODE_DRIVE, MODE_ESTOP, @@ -37,13 +37,13 @@ class TestMessageIDs(unittest.TestCase): """Verify the CAN message ID constants are correct.""" def test_command_ids(self): - self.assertEqual(MAMBA_CMD_VELOCITY, 0x100) - self.assertEqual(MAMBA_CMD_MODE, 0x101) - self.assertEqual(MAMBA_CMD_ESTOP, 0x102) + self.assertEqual(BALANCE_CMD_VELOCITY, 0x100) + self.assertEqual(BALANCE_CMD_MODE, 0x101) + self.assertEqual(BALANCE_CMD_ESTOP, 0x102) def test_telemetry_ids(self): - self.assertEqual(MAMBA_TELEM_IMU, 0x200) - self.assertEqual(MAMBA_TELEM_BATTERY, 0x201) + self.assertEqual(BALANCE_TELEM_IMU, 0x200) + self.assertEqual(BALANCE_TELEM_BATTERY, 0x201) self.assertEqual(VESC_TELEM_STATE, 0x300) diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py index cf1dc6a..fc485ca 100644 --- a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py @@ -4,42 +4,39 @@ protocol_defs.py — CAN message ID constants and frame builders/parsers for the Orin↔ESP32-S3 BALANCE↔VESC integration test suite. All IDs and payload formats are derived from: - include/orin_can.h — Orin↔FC (ESP32-S3 BALANCE) protocol + include/orin_can.h — Orin↔BALANCE (ESP32-S3 BALANCE) protocol include/vesc_can.h — VESC CAN protocol saltybot_can_bridge/balance_protocol.py — existing bridge constants CAN IDs used in tests --------------------- -Orin → FC (ESP32-S3 BALANCE) commands (standard 11-bit, matching orin_can.h): +Orin → BALANCE (ESP32-S3 BALANCE) commands (standard 11-bit, matching orin_can.h): ORIN_CMD_HEARTBEAT 0x300 - ORIN_CMD_DRIVE 0x301 int16 speed (−1000..+1000), int16 steer (−1000..+1000) + ORIN_CMD_DRIVE 0x301 int16 speed (-1000..+1000), int16 steer (-1000..+1000) ORIN_CMD_MODE 0x302 uint8 mode byte ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR) -FC (ESP32-S3 BALANCE) → Orin telemetry (standard 11-bit, matching orin_can.h): - FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t) - FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t) - FC_IMU 0x402 8 bytes - FC_BARO 0x403 8 bytes +BALANCE (ESP32-S3 BALANCE) -> Orin telemetry (standard 11-bit, matching orin_can.h): + BALANCE_STATUS 0x400 8 bytes (see orin_can_balance_status_t) + BALANCE_VESC 0x401 8 bytes (see orin_can_balance_vesc_t) + BALANCE_IMU 0x402 8 bytes + BALANCE_BARO 0x403 8 bytes -Mamba ↔ VESC internal commands (matching balance_protocol.py): -======= -ESP32-S3 BALANCE ↔ VESC internal commands (matching balance_protocol.py): ->>>>>>> 9aed963 (fix: scrub remaining Mamba references in can_bridge and e2e test protocol files) - MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian - MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop) - MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop +ESP32-S3 BALANCE <-> VESC internal commands (matching balance_protocol.py): + BALANCE_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian + BALANCE_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop) + BALANCE_CMD_ESTOP 0x102 1 byte 0x01=stop VESC STATUS (extended 29-bit, matching vesc_can.h): arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id = (9 << 8) | node_id - Payload: int32 RPM (BE), int16 current×10 (BE), int16 duty×1000 (BE) + Payload: int32 RPM (BE), int16 current x10 (BE), int16 duty x1000 (BE) """ import struct from typing import Tuple # --------------------------------------------------------------------------- -# Orin → FC (ESP32-S3 BALANCE) command IDs (from orin_can.h) +# Orin -> BALANCE (ESP32-S3 BALANCE) command IDs (from orin_can.h) # --------------------------------------------------------------------------- ORIN_CMD_HEARTBEAT: int = 0x300 @@ -48,28 +45,25 @@ ORIN_CMD_MODE: int = 0x302 ORIN_CMD_ESTOP: int = 0x303 # --------------------------------------------------------------------------- -# FC (ESP32-S3 BALANCE) → Orin telemetry IDs (from orin_can.h) +# BALANCE (ESP32-S3 BALANCE) -> Orin telemetry IDs (from orin_can.h) # --------------------------------------------------------------------------- -FC_STATUS: int = 0x400 -FC_VESC: int = 0x401 -FC_IMU: int = 0x402 -FC_BARO: int = 0x403 +BALANCE_STATUS: int = 0x400 +BALANCE_VESC: int = 0x401 +BALANCE_IMU: int = 0x402 +BALANCE_BARO: int = 0x403 # --------------------------------------------------------------------------- -# Mamba → VESC internal command IDs (from balance_protocol.py) -======= -# ESP32-S3 BALANCE → VESC internal command IDs (from balance_protocol.py) ->>>>>>> 9aed963 (fix: scrub remaining Mamba references in can_bridge and e2e test protocol files) +# ESP32-S3 BALANCE -> VESC internal command IDs (from balance_protocol.py) # --------------------------------------------------------------------------- -MAMBA_CMD_VELOCITY: int = 0x100 -MAMBA_CMD_MODE: int = 0x101 -MAMBA_CMD_ESTOP: int = 0x102 +BALANCE_CMD_VELOCITY: int = 0x100 +BALANCE_CMD_MODE: int = 0x101 +BALANCE_CMD_ESTOP: int = 0x102 -MAMBA_TELEM_IMU: int = 0x200 -MAMBA_TELEM_BATTERY: int = 0x201 -VESC_TELEM_STATE: int = 0x300 +BALANCE_TELEM_IMU: int = 0x200 +BALANCE_TELEM_BATTERY: int = 0x201 +VESC_TELEM_STATE: int = 0x300 # --------------------------------------------------------------------------- # Mode constants @@ -111,7 +105,7 @@ def VESC_SET_RPM_ID(vesc_node_id: int) -> int: # --------------------------------------------------------------------------- -# Frame builders — Orin → FC +# Frame builders — Orin -> BALANCE # --------------------------------------------------------------------------- def build_heartbeat(seq: int = 0) -> bytes: @@ -125,8 +119,8 @@ def build_drive_cmd(speed: int, steer: int) -> bytes: Parameters ---------- - speed: int, −1000..+1000 (mapped directly to int16) - steer: int, −1000..+1000 + speed: int, -1000..+1000 (mapped directly to int16) + steer: int, -1000..+1000 """ return struct.pack(">hh", int(speed), int(steer)) @@ -137,20 +131,17 @@ def build_mode_cmd(mode: int) -> bytes: def build_estop_cmd(action: int = 1) -> bytes: - """Build an ORIN_CMD_ESTOP payload. action=1 → ESTOP, 0 → CLEAR.""" + """Build an ORIN_CMD_ESTOP payload. action=1 -> ESTOP, 0 -> CLEAR.""" return struct.pack(">B", action & 0xFF) # --------------------------------------------------------------------------- -# Frame builders — Mamba velocity commands (balance_protocol.py encoding) -======= # Frame builders — ESP32-S3 BALANCE velocity commands (balance_protocol.py encoding) ->>>>>>> 9aed963 (fix: scrub remaining Mamba references in can_bridge and e2e test protocol files) # --------------------------------------------------------------------------- def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes: """ - Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). + Build a BALANCE_CMD_VELOCITY payload (8 bytes, 2 x float32 big-endian). Matches encode_velocity_cmd() in balance_protocol.py. """ @@ -158,10 +149,10 @@ def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes: # --------------------------------------------------------------------------- -# Frame builders — FC → Orin telemetry +# Frame builders — BALANCE -> Orin telemetry # --------------------------------------------------------------------------- -def build_fc_status( +def build_balance_status( pitch_x10: int = 0, motor_cmd: int = 0, vbat_mv: int = 24000, @@ -169,9 +160,9 @@ def build_fc_status( flags: int = 0, ) -> bytes: """ - Build an FC_STATUS (0x400) payload. + Build a BALANCE_STATUS (0x400) payload. - Layout (orin_can_fc_status_t, 8 bytes, big-endian): + Layout (orin_can_balance_status_t, 8 bytes, big-endian): int16 pitch_x10 int16 motor_cmd uint16 vbat_mv @@ -188,23 +179,23 @@ def build_fc_status( ) -def build_fc_vesc( +def build_balance_vesc( left_rpm_x10: int = 0, right_rpm_x10: int = 0, left_current_x10: int = 0, right_current_x10: int = 0, ) -> bytes: """ - Build an FC_VESC (0x401) payload. + Build a BALANCE_VESC (0x401) payload. - Layout (orin_can_fc_vesc_t, 8 bytes, big-endian): + Layout (orin_can_balance_vesc_t, 8 bytes, big-endian): int16 left_rpm_x10 int16 right_rpm_x10 int16 left_current_x10 int16 right_current_x10 - RPM values are RPM / 10 (e.g. 3000 RPM → 300). - Current values are A × 10 (e.g. 5.5 A → 55). + RPM values are RPM / 10 (e.g. 3000 RPM -> 300). + Current values are A x 10 (e.g. 5.5 A -> 55). """ return struct.pack( ">hhhh", @@ -225,8 +216,8 @@ def build_vesc_status( Layout (from vesc_can.h / VESC FW 6.x, big-endian): int32 rpm - int16 current × 10 - int16 duty × 1000 + int16 current x 10 + int16 duty x 1000 Total: 8 bytes. """ return struct.pack( @@ -241,9 +232,9 @@ def build_vesc_status( # Frame parsers # --------------------------------------------------------------------------- -def parse_fc_status(data: bytes): +def parse_balance_status(data: bytes): """ - Parse an FC_STATUS (0x400) payload. + Parse a BALANCE_STATUS (0x400) payload. Returns ------- @@ -251,7 +242,7 @@ def parse_fc_status(data: bytes): estop_active (bool), armed (bool) """ if len(data) < 8: - raise ValueError(f"FC_STATUS needs 8 bytes, got {len(data)}") + raise ValueError(f"BALANCE_STATUS needs 8 bytes, got {len(data)}") pitch_x10, motor_cmd, vbat_mv, balance_state, flags = struct.unpack( ">hhHBB", data[:8] ) @@ -266,9 +257,9 @@ def parse_fc_status(data: bytes): } -def parse_fc_vesc(data: bytes): +def parse_balance_vesc(data: bytes): """ - Parse an FC_VESC (0x401) payload. + Parse a BALANCE_VESC (0x401) payload. Returns ------- @@ -276,7 +267,7 @@ def parse_fc_vesc(data: bytes): right_current_x10, left_rpm (float), right_rpm (float) """ if len(data) < 8: - raise ValueError(f"FC_VESC needs 8 bytes, got {len(data)}") + raise ValueError(f"BALANCE_VESC needs 8 bytes, got {len(data)}") left_rpm_x10, right_rpm_x10, left_cur_x10, right_cur_x10 = struct.unpack( ">hhhh", data[:8] ) @@ -312,12 +303,12 @@ def parse_vesc_status(data: bytes): def parse_velocity_cmd(data: bytes) -> Tuple[float, float]: """ - Parse a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). + Parse a BALANCE_CMD_VELOCITY payload (8 bytes, 2 x float32 big-endian). Returns ------- (left_mps, right_mps) """ if len(data) < 8: - raise ValueError(f"MAMBA_CMD_VELOCITY needs 8 bytes, got {len(data)}") + raise ValueError(f"BALANCE_CMD_VELOCITY needs 8 bytes, got {len(data)}") return struct.unpack(">ff", data[:8]) diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py index 3e63e3d..6e1a662 100644 --- a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py @@ -4,7 +4,7 @@ test_drive_command.py — Integration tests for the drive command path. Tests verify: DRIVE cmd → ESP32-S3 BALANCE receives velocity command frame → mock VESC status response - → FC_VESC broadcast contains correct RPMs. + → BALANCE_VESC broadcast contains correct RPMs. All tests run without real hardware or a running ROS2 system. Run with: python -m pytest test/test_drive_command.py -v @@ -14,19 +14,19 @@ import struct import pytest from saltybot_can_e2e_test.protocol_defs import ( - MAMBA_CMD_VELOCITY, - MAMBA_CMD_MODE, - FC_VESC, + BALANCE_CMD_VELOCITY, + BALANCE_CMD_MODE, + BALANCE_VESC, MODE_DRIVE, MODE_IDLE, VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT, VESC_STATUS_ID, build_velocity_cmd, - build_fc_vesc, + build_balance_vesc, build_vesc_status, parse_velocity_cmd, - parse_fc_vesc, + parse_balance_vesc, ) from saltybot_can_bridge.balance_protocol import ( encode_velocity_cmd, @@ -50,8 +50,8 @@ def _send_drive(bus, left_mps: float, right_mps: float) -> None: self.data = bytearray(data) self.is_extended_id = False - bus.send(_Msg(MAMBA_CMD_VELOCITY, payload)) - bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + bus.send(_Msg(BALANCE_CMD_VELOCITY, payload)) + bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) # --------------------------------------------------------------------------- @@ -62,11 +62,11 @@ class TestDriveForward: def test_drive_forward_velocity_frame_sent(self, mock_can_bus): """ Inject DRIVE cmd (1.0 m/s, 1.0 m/s) → verify ESP32-S3 BALANCE receives - a MAMBA_CMD_VELOCITY frame with correct payload. + a BALANCE_CMD_VELOCITY frame with correct payload. """ _send_drive(mock_can_bus, 1.0, 1.0) - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 1, "Expected exactly one velocity command frame" left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) @@ -77,26 +77,26 @@ class TestDriveForward: """After a drive command, a MODE=drive frame must accompany it.""" _send_drive(mock_can_bus, 1.0, 1.0) - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) assert len(mode_frames) >= 1, "Expected at least one MODE frame" assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE]) def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus): """ - Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct. - (In the real loop ESP32-S3 BALANCE computes RPM from m/s and broadcasts FC_VESC.) - This test checks the FC_VESC frame format and parser. + Simulate BALANCE_VESC broadcast arriving after drive cmd; verify parse is correct. + (In the real loop ESP32-S3 BALANCE computes RPM from m/s and broadcasts BALANCE_VESC.) + This test checks the BALANCE_VESC frame format and parser. """ # Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics) - fc_payload = build_fc_vesc( + fc_payload = build_balance_vesc( left_rpm_x10=300, right_rpm_x10=300, left_current_x10=50, right_current_x10=50, ) - mock_can_bus.inject(FC_VESC, fc_payload) + mock_can_bus.inject(BALANCE_VESC, fc_payload) frame = mock_can_bus.recv(timeout=0.1) - assert frame is not None, "FC_VESC frame not received" - parsed = parse_fc_vesc(bytes(frame.data)) + assert frame is not None, "BALANCE_VESC frame not received" + parsed = parse_balance_vesc(bytes(frame.data)) assert parsed["left_rpm_x10"] == 300 assert parsed["right_rpm_x10"] == 300 assert abs(parsed["left_rpm"] - 3000.0) < 0.1 @@ -109,7 +109,7 @@ class TestDriveTurn: """ _send_drive(mock_can_bus, 0.5, -0.5) - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 1 left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) @@ -119,14 +119,14 @@ class TestDriveTurn: assert left > 0 and right < 0 def test_drive_turn_fc_vesc_differential(self, mock_can_bus): - """Simulated FC_VESC for a turn has opposite-sign RPMs.""" - fc_payload = build_fc_vesc( + """Simulated BALANCE_VESC for a turn has opposite-sign RPMs.""" + fc_payload = build_balance_vesc( left_rpm_x10=150, right_rpm_x10=-150, left_current_x10=30, right_current_x10=30, ) - mock_can_bus.inject(FC_VESC, fc_payload) + mock_can_bus.inject(BALANCE_VESC, fc_payload) frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_vesc(bytes(frame.data)) + parsed = parse_balance_vesc(bytes(frame.data)) assert parsed["left_rpm_x10"] > 0 assert parsed["right_rpm_x10"] < 0 @@ -142,7 +142,7 @@ class TestDriveZero: _send_drive(mock_can_bus, 0.0, 0.0) - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 1 left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) assert abs(left) < 1e-5, "Left motor not stopped" @@ -156,7 +156,7 @@ class TestDriveCmdTimeout: zero velocity is sent. We test the encoding directly (without timers). """ # The watchdog in CanBridgeNode calls encode_velocity_cmd(0.0, 0.0) and - # sends it on MAMBA_CMD_VELOCITY. Replicate that here. + # sends it on BALANCE_CMD_VELOCITY. Replicate that here. zero_payload = encode_velocity_cmd(0.0, 0.0) class _Msg: @@ -165,16 +165,16 @@ class TestDriveCmdTimeout: self.data = bytearray(data) self.is_extended_id = False - mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, zero_payload)) - mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE))) + mock_can_bus.send(_Msg(BALANCE_CMD_VELOCITY, zero_payload)) + mock_can_bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE))) - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 1 left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) assert abs(left) < 1e-5 assert abs(right) < 1e-5 - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) assert len(mode_frames) == 1 assert bytes(mode_frames[0].data) == bytes([MODE_IDLE]) diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py index 746840f..9722135 100644 --- a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py @@ -6,7 +6,7 @@ Covers: - ESTOP command halts motors immediately - ESTOP persists: DRIVE commands ignored while ESTOP is active - ESTOP clear restores normal drive operation - - Firmware-side estop via FC_STATUS flags is detected correctly + - Firmware-side estop via BALANCE_STATUS flags is detected correctly No ROS2 or real CAN hardware required. Run with: python -m pytest test/test_estop.py -v @@ -17,20 +17,20 @@ import pytest from saltybot_can_e2e_test.can_mock import MockCANBus from saltybot_can_e2e_test.protocol_defs import ( - MAMBA_CMD_VELOCITY, - MAMBA_CMD_MODE, - MAMBA_CMD_ESTOP, + BALANCE_CMD_VELOCITY, + BALANCE_CMD_MODE, + BALANCE_CMD_ESTOP, ORIN_CMD_ESTOP, - FC_STATUS, + BALANCE_STATUS, MODE_IDLE, MODE_DRIVE, MODE_ESTOP, build_estop_cmd, build_mode_cmd, build_velocity_cmd, - build_fc_status, + build_balance_status, parse_velocity_cmd, - parse_fc_status, + parse_balance_status, ) from saltybot_can_bridge.balance_protocol import ( encode_velocity_cmd, @@ -68,16 +68,16 @@ class EstopStateMachine: """Send ESTOP and transition to estop mode.""" self._estop_active = True self._mode = MODE_ESTOP - self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) - self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP))) - self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + self._bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + self._bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_ESTOP))) + self._bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(True))) def clear_estop(self) -> None: """Clear ESTOP and return to IDLE mode.""" self._estop_active = False self._mode = MODE_IDLE - self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False))) - self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE))) + self._bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(False))) + self._bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE))) def send_drive(self, left_mps: float, right_mps: float) -> None: """Send velocity command only if ESTOP is not active.""" @@ -85,8 +85,8 @@ class EstopStateMachine: # Bridge silently drops commands while estopped return self._mode = MODE_DRIVE - self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) - self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + self._bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) + self._bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) @property def estop_active(self) -> bool: @@ -105,7 +105,7 @@ class TestEstopHaltsMotors: sm = EstopStateMachine(mock_can_bus) sm.assert_estop() - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) >= 1, "No velocity frame after ESTOP" l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP" @@ -116,17 +116,17 @@ class TestEstopHaltsMotors: sm = EstopStateMachine(mock_can_bus) sm.assert_estop() - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) assert any( bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames ), "MODE=ESTOP not found in sent frames" def test_estop_flag_byte_is_0x01(self, mock_can_bus): - """MAMBA_CMD_ESTOP payload must be 0x01 when asserting e-stop.""" + """BALANCE_CMD_ESTOP payload must be 0x01 when asserting e-stop.""" sm = EstopStateMachine(mock_can_bus) sm.assert_estop() - estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + estop_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_ESTOP) assert len(estop_frames) >= 1 assert bytes(estop_frames[-1].data) == b"\x01", \ f"ESTOP payload {estop_frames[-1].data!r} != 0x01" @@ -143,7 +143,7 @@ class TestEstopPersists: sm.send_drive(1.0, 1.0) # should be suppressed - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 0, \ "Velocity command was forwarded while ESTOP is active" @@ -158,7 +158,7 @@ class TestEstopPersists: sm.send_drive(0.5, 0.5) # No mode frames should have been emitted (drive was suppressed) - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) assert all( bytes(f.data) != bytes([MODE_DRIVE]) for f in mode_frames ), "MODE=DRIVE was set despite active ESTOP" @@ -174,19 +174,19 @@ class TestEstopClear: sm.send_drive(0.8, 0.8) - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 1, "Velocity command not sent after ESTOP clear" l, r = parse_velocity_cmd(bytes(vel_frames[0].data)) assert abs(l - 0.8) < 1e-4 assert abs(r - 0.8) < 1e-4 def test_estop_clear_flag_byte_is_0x00(self, mock_can_bus): - """MAMBA_CMD_ESTOP payload must be 0x00 when clearing e-stop.""" + """BALANCE_CMD_ESTOP payload must be 0x00 when clearing e-stop.""" sm = EstopStateMachine(mock_can_bus) sm.assert_estop() sm.clear_estop() - estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + estop_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_ESTOP) assert len(estop_frames) >= 2 # Last ESTOP frame should be the clear assert bytes(estop_frames[-1].data) == b"\x00", \ @@ -198,7 +198,7 @@ class TestEstopClear: sm.assert_estop() sm.clear_estop() - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) last_mode = bytes(mode_frames[-1].data) assert last_mode == bytes([MODE_IDLE]), \ f"Mode after ESTOP clear is {last_mode!r}, expected MODE_IDLE" @@ -207,55 +207,55 @@ class TestEstopClear: class TestFirmwareSideEstop: def test_fc_status_estop_flag_detected(self, mock_can_bus): """ - Simulate firmware sending estop via FC_STATUS flags (bit0=estop_active). + Simulate firmware sending estop via BALANCE_STATUS flags (bit0=estop_active). Verify the Orin bridge side correctly parses the flag. """ - # Build FC_STATUS with estop_active bit set (flags=0x01) - payload = build_fc_status( + # Build BALANCE_STATUS with estop_active bit set (flags=0x01) + payload = build_balance_status( pitch_x10=0, motor_cmd=0, vbat_mv=24000, balance_state=2, # TILT_FAULT flags=0x01, # bit0 = estop_active ) - mock_can_bus.inject(FC_STATUS, payload) + mock_can_bus.inject(BALANCE_STATUS, payload) frame = mock_can_bus.recv(timeout=0.1) - assert frame is not None, "FC_STATUS frame not received" - parsed = parse_fc_status(bytes(frame.data)) + assert frame is not None, "BALANCE_STATUS frame not received" + parsed = parse_balance_status(bytes(frame.data)) assert parsed["estop_active"] is True, \ - "estop_active flag not set in FC_STATUS" + "estop_active flag not set in BALANCE_STATUS" assert parsed["balance_state"] == 2 def test_fc_status_no_estop_flag(self, mock_can_bus): - """FC_STATUS with flags=0x00 must NOT set estop_active.""" - payload = build_fc_status(flags=0x00) - mock_can_bus.inject(FC_STATUS, payload) + """BALANCE_STATUS with flags=0x00 must NOT set estop_active.""" + payload = build_balance_status(flags=0x00) + mock_can_bus.inject(BALANCE_STATUS, payload) frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_status(bytes(frame.data)) + parsed = parse_balance_status(bytes(frame.data)) assert parsed["estop_active"] is False def test_fc_status_armed_flag_detected(self, mock_can_bus): - """FC_STATUS flags bit1=armed must parse correctly.""" - payload = build_fc_status(flags=0x02) # bit1 = armed - mock_can_bus.inject(FC_STATUS, payload) + """BALANCE_STATUS flags bit1=armed must parse correctly.""" + payload = build_balance_status(flags=0x02) # bit1 = armed + mock_can_bus.inject(BALANCE_STATUS, payload) frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_status(bytes(frame.data)) + parsed = parse_balance_status(bytes(frame.data)) assert parsed["armed"] is True assert parsed["estop_active"] is False def test_fc_status_roundtrip(self, mock_can_bus): - """build_fc_status → inject → recv → parse_fc_status must be identity.""" - payload = build_fc_status( + """build_balance_status → inject → recv → parse_balance_status must be identity.""" + payload = build_balance_status( pitch_x10=150, motor_cmd=-200, vbat_mv=23800, balance_state=1, flags=0x03, ) - mock_can_bus.inject(FC_STATUS, payload) + mock_can_bus.inject(BALANCE_STATUS, payload) frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_status(bytes(frame.data)) + parsed = parse_balance_status(bytes(frame.data)) assert parsed["pitch_x10"] == 150 assert parsed["motor_cmd"] == -200 assert parsed["vbat_mv"] == 23800 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py index 8070fd0..833ac61 100644 --- a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py @@ -1,11 +1,11 @@ #!/usr/bin/env python3 """ -test_fc_vesc_broadcast.py — FC_VESC broadcast and VESC STATUS integration tests. +test_fc_vesc_broadcast.py — BALANCE_VESC broadcast and VESC STATUS integration tests. Covers: - - VESC STATUS extended frame for left VESC (ID 56) triggers FC_VESC broadcast - - Both left (56) and right (68) VESC STATUS combined in FC_VESC - - FC_VESC broadcast rate (~10 Hz) + - VESC STATUS extended frame for left VESC (ID 56) triggers BALANCE_VESC broadcast + - Both left (56) and right (68) VESC STATUS combined in BALANCE_VESC + - BALANCE_VESC broadcast rate (~10 Hz) - current_x10 scaling matches protocol spec No ROS2 or real CAN hardware required. @@ -19,15 +19,15 @@ import pytest from saltybot_can_e2e_test.can_mock import MockCANBus from saltybot_can_e2e_test.protocol_defs import ( - FC_VESC, + BALANCE_VESC, VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT, VESC_STATUS_ID, VESC_SET_RPM_ID, VESC_TELEM_STATE, build_vesc_status, - build_fc_vesc, - parse_fc_vesc, + build_balance_vesc, + parse_balance_vesc, parse_vesc_status, ) from saltybot_can_bridge.balance_protocol import ( @@ -44,8 +44,8 @@ class VescStatusAggregator: """ Simulates the firmware logic that: 1. Receives VESC STATUS extended frames from left/right VESCs - 2. Builds an FC_VESC broadcast payload - 3. Injects the FC_VESC frame onto the mock bus + 2. Builds an BALANCE_VESC broadcast payload + 3. Injects the BALANCE_VESC frame onto the mock bus This represents the ESP32-S3 BALANCE → Orin telemetry path. """ @@ -62,7 +62,7 @@ class VescStatusAggregator: def process_vesc_status(self, arb_id: int, data: bytes) -> None: """ Process an incoming VESC STATUS frame (extended 29-bit ID). - Updates internal state; broadcasts FC_VESC when at least one side is known. + Updates internal state; broadcasts BALANCE_VESC when at least one side is known. """ node_id = arb_id & 0xFF parsed = parse_vesc_status(data) @@ -77,17 +77,17 @@ class VescStatusAggregator: self._right_current_x10 = parsed["current_x10"] self._right_seen = True - # Broadcast FC_VESC whenever we receive any update + # Broadcast BALANCE_VESC whenever we receive any update self._broadcast_fc_vesc() def _broadcast_fc_vesc(self) -> None: - payload = build_fc_vesc( + payload = build_balance_vesc( left_rpm_x10=self._left_rpm_x10, right_rpm_x10=self._right_rpm_x10, left_current_x10=self._left_current_x10, right_current_x10=self._right_current_x10, ) - self._bus.inject(FC_VESC, payload) + self._bus.inject(BALANCE_VESC, payload) def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int, @@ -105,7 +105,7 @@ def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int, class TestVescStatusToFcVesc: def test_left_vesc_status_triggers_broadcast(self, mock_can_bus): """ - Inject VESC STATUS for left VESC (ID 56) → verify FC_VESC contains + Inject VESC STATUS for left VESC (ID 56) → verify BALANCE_VESC contains the correct left RPM (rpm / 10). """ agg = VescStatusAggregator(mock_can_bus) @@ -116,14 +116,14 @@ class TestVescStatusToFcVesc: agg.process_vesc_status(arb_id, payload) frame = mock_can_bus.recv(timeout=0.1) - assert frame is not None, "No FC_VESC broadcast after left VESC STATUS" - parsed = parse_fc_vesc(bytes(frame.data)) + assert frame is not None, "No BALANCE_VESC broadcast after left VESC STATUS" + parsed = parse_balance_vesc(bytes(frame.data)) assert parsed["left_rpm_x10"] == 300, \ f"left_rpm_x10 {parsed['left_rpm_x10']} != 300" assert abs(parsed["left_rpm"] - 3000.0) < 1.0 def test_right_vesc_status_triggers_broadcast(self, mock_can_bus): - """Inject VESC STATUS for right VESC (ID 68) → verify right RPM in FC_VESC.""" + """Inject VESC STATUS for right VESC (ID 68) → verify right RPM in BALANCE_VESC.""" agg = VescStatusAggregator(mock_can_bus) arb_id = VESC_STATUS_ID(VESC_CAN_ID_RIGHT) @@ -132,7 +132,7 @@ class TestVescStatusToFcVesc: frame = mock_can_bus.recv(timeout=0.1) assert frame is not None - parsed = parse_fc_vesc(bytes(frame.data)) + parsed = parse_balance_vesc(bytes(frame.data)) assert parsed["right_rpm_x10"] == 200 def test_left_vesc_id_matches_constant(self): @@ -150,7 +150,7 @@ class TestBothVescStatusCombined: def test_both_vesc_status_combined_in_fc_vesc(self, mock_can_bus): """ Inject both left (56) and right (68) VESC STATUS frames. - Final FC_VESC must contain both RPMs. + Final BALANCE_VESC must contain both RPMs. """ agg = VescStatusAggregator(mock_can_bus) @@ -165,7 +165,7 @@ class TestBothVescStatusCombined: build_vesc_status(rpm=-1500, current_x10=30), ) - # Drain two FC_VESC frames (one per update), check the latest + # Drain two BALANCE_VESC frames (one per update), check the latest frames = [] while True: f = mock_can_bus.recv(timeout=0.05) @@ -173,16 +173,16 @@ class TestBothVescStatusCombined: break frames.append(f) - assert len(frames) >= 2, "Expected at least 2 FC_VESC frames" + assert len(frames) >= 2, "Expected at least 2 BALANCE_VESC frames" # Last frame must have both sides - last = parse_fc_vesc(bytes(frames[-1].data)) + last = parse_balance_vesc(bytes(frames[-1].data)) assert last["left_rpm_x10"] == 300, \ f"left_rpm_x10 {last['left_rpm_x10']} != 300" assert last["right_rpm_x10"] == -150, \ f"right_rpm_x10 {last['right_rpm_x10']} != -150" def test_both_vesc_currents_combined(self, mock_can_bus): - """Both current values must appear in FC_VESC after two STATUS frames.""" + """Both current values must appear in BALANCE_VESC after two STATUS frames.""" agg = VescStatusAggregator(mock_can_bus) agg.process_vesc_status( VESC_STATUS_ID(VESC_CAN_ID_LEFT), @@ -198,7 +198,7 @@ class TestBothVescStatusCombined: if f is None: break frames.append(f) - last = parse_fc_vesc(bytes(frames[-1].data)) + last = parse_balance_vesc(bytes(frames[-1].data)) assert last["left_current_x10"] == 55 assert last["right_current_x10"] == 42 @@ -206,11 +206,11 @@ class TestBothVescStatusCombined: class TestVescBroadcastRate: def test_fc_vesc_broadcast_at_10hz(self, mock_can_bus): """ - Simulate FC_VESC broadcasts at ~10 Hz and verify the rate. + Simulate BALANCE_VESC broadcasts at ~10 Hz and verify the rate. We inject 12 frames over ~120 ms, then verify count and average interval. """ - _FC_VESC_HZ = 10 - _interval = 1.0 / _FC_VESC_HZ + _BALANCE_VESC_HZ = 10 + _interval = 1.0 / _BALANCE_VESC_HZ timestamps = [] stop_event = threading.Event() @@ -219,8 +219,8 @@ class TestVescBroadcastRate: while not stop_event.is_set(): t = time.monotonic() mock_can_bus.inject( - FC_VESC, - build_fc_vesc(100, -100, 30, 30), + BALANCE_VESC, + build_balance_vesc(100, -100, 30, 30), timestamp=t, ) timestamps.append(t) @@ -232,18 +232,18 @@ class TestVescBroadcastRate: stop_event.set() t.join(timeout=0.2) - assert len(timestamps) >= 1, "No FC_VESC broadcasts in 150 ms window" + assert len(timestamps) >= 1, "No BALANCE_VESC broadcasts in 150 ms window" if len(timestamps) >= 2: intervals = [timestamps[i+1] - timestamps[i] for i in range(len(timestamps)-1)] avg = sum(intervals) / len(intervals) # ±40 ms tolerance for OS scheduling assert 0.06 <= avg <= 0.14, \ - f"FC_VESC broadcast interval {avg*1000:.1f} ms not ~100 ms" + f"BALANCE_VESC broadcast interval {avg*1000:.1f} ms not ~100 ms" def test_fc_vesc_frame_is_8_bytes(self): - """FC_VESC payload must always be exactly 8 bytes.""" - payload = build_fc_vesc(300, -150, 55, 42) + """BALANCE_VESC payload must always be exactly 8 bytes.""" + payload = build_balance_vesc(300, -150, 55, 42) assert len(payload) == 8 @@ -267,16 +267,16 @@ class TestVescCurrentScaling: assert abs(parsed["current"] - (-3.0)) < 0.01 def test_fc_vesc_current_x10_roundtrip(self, mock_can_bus): - """build_fc_vesc → inject → recv → parse must preserve current_x10.""" - payload = build_fc_vesc( + """build_balance_vesc → inject → recv → parse must preserve current_x10.""" + payload = build_balance_vesc( left_rpm_x10=200, right_rpm_x10=200, left_current_x10=55, right_current_x10=42, ) - mock_can_bus.inject(FC_VESC, payload) + mock_can_bus.inject(BALANCE_VESC, payload) frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_vesc(bytes(frame.data)) + parsed = parse_balance_vesc(bytes(frame.data)) assert parsed["left_current_x10"] == 55 assert parsed["right_current_x10"] == 42 @@ -306,7 +306,7 @@ class TestVescCurrentScaling: ) frame = mock_can_bus.recv(timeout=0.05) assert frame is not None - parsed = parse_fc_vesc(bytes(frame.data)) + parsed = parse_balance_vesc(bytes(frame.data)) if vesc_id == VESC_CAN_ID_LEFT: assert parsed["left_rpm_x10"] == expected_rpm_x10, \ f"left_rpm_x10={parsed['left_rpm_x10']} expected {expected_rpm_x10}" diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py index 726921d..f622675 100644 --- a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py @@ -21,9 +21,9 @@ from saltybot_can_e2e_test.protocol_defs import ( ORIN_CMD_HEARTBEAT, ORIN_CMD_ESTOP, ORIN_CMD_MODE, - MAMBA_CMD_VELOCITY, - MAMBA_CMD_MODE, - MAMBA_CMD_ESTOP, + BALANCE_CMD_VELOCITY, + BALANCE_CMD_MODE, + BALANCE_CMD_ESTOP, MODE_IDLE, MODE_DRIVE, MODE_ESTOP, @@ -100,9 +100,9 @@ def _simulate_estop_on_timeout(bus: MockCANBus) -> None: self.data = bytearray(data) self.is_extended_id = False - bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) - bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP))) - bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_ESTOP))) + bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(True))) # --------------------------------------------------------------------------- @@ -121,25 +121,25 @@ class TestHeartbeatLoss: # Simulate bridge detecting timeout and escalating _simulate_estop_on_timeout(mock_can_bus) - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) >= 1, "Zero velocity not sent after timeout" l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) assert abs(l) < 1e-5, "Left not zero on timeout" assert abs(r) < 1e-5, "Right not zero on timeout" - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) assert any( bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames ), "ESTOP mode not asserted on heartbeat timeout" - estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + estop_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_ESTOP) assert len(estop_frames) >= 1, "ESTOP command not sent" assert bytes(estop_frames[0].data) == b"\x01" def test_heartbeat_loss_zero_velocity(self, mock_can_bus): """Zero velocity frame must appear among sent frames after timeout.""" _simulate_estop_on_timeout(mock_can_bus) - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) >= 1 for f in vel_frames: l, r = parse_velocity_cmd(bytes(f.data)) @@ -165,20 +165,20 @@ class TestHeartbeatRecovery: mock_can_bus.reset() # Phase 2: recovery — clear estop, restore drive mode - mock_can_bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False))) - mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) - mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.5, 0.5))) + mock_can_bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(False))) + mock_can_bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + mock_can_bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.5, 0.5))) - estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + estop_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_ESTOP) assert any(bytes(f.data) == b"\x00" for f in estop_frames), \ "ESTOP clear not sent on recovery" - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) assert any( bytes(f.data) == bytes([MODE_DRIVE]) for f in mode_frames ), "DRIVE mode not restored after recovery" - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) >= 1 l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) assert abs(l - 0.5) < 1e-4 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py index 9e8da58..1f8e8b5 100644 --- a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py @@ -17,9 +17,9 @@ import pytest from saltybot_can_e2e_test.can_mock import MockCANBus from saltybot_can_e2e_test.protocol_defs import ( - MAMBA_CMD_VELOCITY, - MAMBA_CMD_MODE, - MAMBA_CMD_ESTOP, + BALANCE_CMD_VELOCITY, + BALANCE_CMD_MODE, + BALANCE_CMD_ESTOP, MODE_IDLE, MODE_DRIVE, MODE_ESTOP, @@ -64,12 +64,12 @@ class ModeStateMachine: prev_mode = self._mode self._mode = mode - self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(mode))) + self._bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(mode))) # Side-effects of entering ESTOP from DRIVE if mode == MODE_ESTOP and prev_mode == MODE_DRIVE: - self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) - self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + self._bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + self._bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(True))) return True @@ -79,7 +79,7 @@ class ModeStateMachine: """ if self._mode != MODE_DRIVE: return False - self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) + self._bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) return True @property @@ -97,7 +97,7 @@ class TestIdleToDrive: sm = ModeStateMachine(mock_can_bus) sm.set_mode(MODE_DRIVE) - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) assert len(mode_frames) == 1 assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE]) @@ -108,7 +108,7 @@ class TestIdleToDrive: forwarded = sm.send_drive(1.0, 1.0) assert forwarded is False, "Drive cmd should be blocked in IDLE mode" - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 0 def test_drive_mode_allows_commands(self, mock_can_bus): @@ -120,7 +120,7 @@ class TestIdleToDrive: forwarded = sm.send_drive(0.5, 0.5) assert forwarded is True - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 1 l, r = parse_velocity_cmd(bytes(vel_frames[0].data)) assert abs(l - 0.5) < 1e-4 @@ -137,7 +137,7 @@ class TestDriveToEstop: sm.set_mode(MODE_ESTOP) - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) >= 1, "No velocity frame on DRIVE→ESTOP transition" l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP" @@ -149,7 +149,7 @@ class TestDriveToEstop: sm.set_mode(MODE_DRIVE) sm.set_mode(MODE_ESTOP) - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE) assert any(bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames) def test_estop_blocks_subsequent_drive(self, mock_can_bus): @@ -162,7 +162,7 @@ class TestDriveToEstop: forwarded = sm.send_drive(1.0, 1.0) assert forwarded is False - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY) assert len(vel_frames) == 0 diff --git a/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml b/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml index ee4d9a8..17586cd 100644 --- a/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml +++ b/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml @@ -27,12 +27,7 @@ robot: stem_od: 0.0381 # m STEM_OD = 38.1mm stem_height: 1.050 # m nominal cut length -<<<<<<< HEAD - # ── FC / IMU (ESP32 BALANCE) ────────────────────────────────────────────────── -======= - # ── FC / IMU (ESP32-S3 BALANCE) ────────────────────────────────────────────────── ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # fc_x = -50mm in SCAD (front = -X SCAD = +X ROS REP-105) + # ── FC / IMU (ESP32-S3 BALANCE) ────────────────────────────────────────────────── # fc_x = -50mm in SCAD (front = -X SCAD = +X ROS REP-105) # z = deck_thickness/2 + mounting_pad(3mm) + standoff(6mm) = 12mm imu_x: 0.050 # m forward of base_link center imu_y: 0.000 # m diff --git a/jetson/ros2_ws/src/saltybot_diagnostics/README.md b/jetson/ros2_ws/src/saltybot_diagnostics/README.md index ea989a8..3aed383 100644 --- a/jetson/ros2_ws/src/saltybot_diagnostics/README.md +++ b/jetson/ros2_ws/src/saltybot_diagnostics/README.md @@ -5,12 +5,7 @@ Comprehensive hardware diagnostics and health monitoring for SaltyBot. ## Features ### Startup Checks -<<<<<<< HEAD -- RPLIDAR, RealSense, VESC, Jabra mic, ESP32 BALANCE, servos -======= -- RPLIDAR, RealSense, VESC, Jabra mic, ESP32-S3, servos ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -- WiFi, GPS, disk space, RAM +- RPLIDAR, RealSense, VESC, Jabra mic, ESP32-S3, servos- WiFi, GPS, disk space, RAM - Boot result TTS + face animation - JSON logging diff --git a/jetson/ros2_ws/src/saltybot_diagnostics/saltybot_diagnostics/diagnostics_node.py b/jetson/ros2_ws/src/saltybot_diagnostics/saltybot_diagnostics/diagnostics_node.py index cbb056d..c3550c1 100644 --- a/jetson/ros2_ws/src/saltybot_diagnostics/saltybot_diagnostics/diagnostics_node.py +++ b/jetson/ros2_ws/src/saltybot_diagnostics/saltybot_diagnostics/diagnostics_node.py @@ -138,12 +138,7 @@ class DiagnosticsNode(Node): self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {}) def _check_stm32(self): -<<<<<<< HEAD - self.hardware_checks["stm32"] = ("OK", "ESP32 bridge online", {}) -======= self.hardware_checks["stm32"] = ("OK", "ESP32-S3 bridge online", {}) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - def _check_servos(self): try: result = subprocess.run(["i2cdetect", "-y", "1"], capture_output=True, text=True, timeout=2) diff --git a/jetson/ros2_ws/src/saltybot_follower/config/person_follower_params.yaml b/jetson/ros2_ws/src/saltybot_follower/config/person_follower_params.yaml index 95c9614..ad11170 100644 --- a/jetson/ros2_ws/src/saltybot_follower/config/person_follower_params.yaml +++ b/jetson/ros2_ws/src/saltybot_follower/config/person_follower_params.yaml @@ -7,12 +7,7 @@ # ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.2 # # IMPORTANT: This node publishes raw /cmd_vel. The cmd_vel_bridge_node (PR #46) -<<<<<<< HEAD -# applies the ESC ramp, deadman switch, and ESP32 BALANCE AUTONOMOUS mode gate. -======= -# applies the ESC ramp, deadman switch, and ESP32-S3 AUTONOMOUS mode gate. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -# Do not run this node without the cmd_vel bridge running on the same robot. +# applies the ESC ramp, deadman switch, and ESP32-S3 AUTONOMOUS mode gate.# Do not run this node without the cmd_vel bridge running on the same robot. # ── Follow geometry ──────────────────────────────────────────────────────────── # The target distance to maintain behind the person (metres). @@ -74,9 +69,4 @@ control_rate: 20.0 # Hz — lower than cmd_vel bridge (50Hz) by desig # ── Mode integration ────────────────────────────────────────────────────────── # Master enable for the follow controller. When false, node publishes zero cmd_vel. # Toggle at runtime: ros2 param set /person_follower follow_enabled false -<<<<<<< HEAD -# The cmd_vel bridge independently gates on ESP32 BALANCE AUTONOMOUS mode (md=2). -======= -# The cmd_vel bridge independently gates on ESP32-S3 AUTONOMOUS mode (md=2). ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -follow_enabled: true +# The cmd_vel bridge independently gates on ESP32-S3 AUTONOMOUS mode (md=2).follow_enabled: true diff --git a/jetson/ros2_ws/src/saltybot_follower/saltybot_follower/person_follower_node.py b/jetson/ros2_ws/src/saltybot_follower/saltybot_follower/person_follower_node.py index 515a3c7..f255aa2 100644 --- a/jetson/ros2_ws/src/saltybot_follower/saltybot_follower/person_follower_node.py +++ b/jetson/ros2_ws/src/saltybot_follower/saltybot_follower/person_follower_node.py @@ -28,12 +28,7 @@ State machine Safety wiring ------------- -<<<<<<< HEAD - * cmd_vel bridge (PR #46) applies ramp + deadman + ESP32 BALANCE AUTONOMOUS mode gate -- -======= - * cmd_vel bridge (PR #46) applies ramp + deadman + ESP32-S3 AUTONOMOUS mode gate -- ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - this node publishes raw /cmd_vel, the bridge handles hardware safety. + * cmd_vel bridge (PR #46) applies ramp + deadman + ESP32-S3 AUTONOMOUS mode gate -- this node publishes raw /cmd_vel, the bridge handles hardware safety. * follow_enabled param (default True) lets the operator disable the controller at runtime: ros2 param set /person_follower follow_enabled false * Obstacle override: if Nav2 local costmap shows a lethal cell in the forward diff --git a/jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml b/jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml index d22012c..efff357 100644 --- a/jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml +++ b/jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml @@ -1,11 +1,6 @@ gimbal_node: ros__parameters: -<<<<<<< HEAD - # Serial port connecting to ESP32 BALANCE over JLINK protocol -======= - # Serial port connecting to ESP32-S3 over JLINK protocol ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - serial_port: "/dev/ttyTHS1" + # Serial port connecting to ESP32-S3 over JLINK protocol serial_port: "/dev/ttyTHS1" baud_rate: 921600 # Soft angle limits (degrees, ± from center) diff --git a/jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py b/jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py index 36adc70..5b9dbcc 100644 --- a/jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py +++ b/jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py @@ -14,12 +14,7 @@ def generate_launch_description() -> LaunchDescription: serial_port_arg = DeclareLaunchArgument( "serial_port", default_value="/dev/ttyTHS1", -<<<<<<< HEAD - description="JLINK serial port to ESP32 BALANCE", -======= - description="JLINK serial port to ESP32-S3", ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ) + description="JLINK serial port to ESP32-S3", ) pan_limit_arg = DeclareLaunchArgument( "pan_limit_deg", default_value="150.0", diff --git a/jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/gimbal_node.py b/jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/gimbal_node.py index 24d1ce7..ee2a56e 100644 --- a/jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/gimbal_node.py +++ b/jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/gimbal_node.py @@ -1,12 +1,7 @@ #!/usr/bin/env python3 """gimbal_node.py — ROS2 gimbal control node for SaltyBot pan/tilt camera head (Issue #548). -<<<<<<< HEAD -Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32 BALANCE. -======= -Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32-S3. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -Implements smooth trapezoidal motion profiles with configurable axis limits. +Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32-S3.Implements smooth trapezoidal motion profiles with configurable axis limits. Subscribed topics: /saltybot/gimbal/cmd (geometry_msgs/Vector3) x=pan_deg, y=tilt_deg, z=speed_deg_s diff --git a/jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/jlink_gimbal.py b/jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/jlink_gimbal.py index 9013d1c..3018e27 100644 --- a/jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/jlink_gimbal.py +++ b/jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/jlink_gimbal.py @@ -1,25 +1,13 @@ """jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548). -<<<<<<< HEAD -Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32 side). - -Command type (Jetson → ESP32 BALANCE): -======= Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32-S3 side). -Command type (Jetson → ESP32-S3): ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - 0x0B GIMBAL_POS — int16 pan_x10 + int16 tilt_x10 + uint16 speed (6 bytes) +Command type (Jetson → ESP32-S3): 0x0B GIMBAL_POS — int16 pan_x10 + int16 tilt_x10 + uint16 speed (6 bytes) pan_x10 = pan_deg * 10 (±1500 for ±150°) tilt_x10 = tilt_deg * 10 (±450 for ±45°) speed = servo speed register 0–4095 (0 = max) -<<<<<<< HEAD -Telemetry type (ESP32 BALANCE → Jetson): -======= -Telemetry type (ESP32-S3 → Jetson): ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - 0x84 GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 + +Telemetry type (ESP32-S3 → Jetson): 0x84 GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 + uint16 pan_speed_raw + uint16 tilt_speed_raw + uint8 torque_en + uint8 rx_err_pct (10 bytes) @@ -41,14 +29,8 @@ ETX = 0x03 # ── Command / telemetry type codes ───────────────────────────────────────────── -<<<<<<< HEAD -CMD_GIMBAL_POS = 0x0B # Jetson → ESP32 BALANCE: set pan/tilt target -TLM_GIMBAL_STATE = 0x84 # ESP32 BALANCE → Jetson: measured state -======= CMD_GIMBAL_POS = 0x0B # Jetson → ESP32-S3: set pan/tilt target TLM_GIMBAL_STATE = 0x84 # ESP32-S3 → Jetson: measured state ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed. # Map deg/s to this register: speed_reg = max(0, 4095 - int(deg_s * 4095 / 360)) _MAX_SPEED_DEGS = 360.0 # degrees/sec at speed_reg=0 diff --git a/jetson/ros2_ws/src/saltybot_mode_switch/config/mode_switch_params.yaml b/jetson/ros2_ws/src/saltybot_mode_switch/config/mode_switch_params.yaml index 301252f..3532469 100644 --- a/jetson/ros2_ws/src/saltybot_mode_switch/config/mode_switch_params.yaml +++ b/jetson/ros2_ws/src/saltybot_mode_switch/config/mode_switch_params.yaml @@ -5,12 +5,7 @@ # # Topic wiring: # /rc/joy → mode_switch_node (CRSF channels) -<<<<<<< HEAD -# /saltybot/balance_state → mode_switch_node (ESP32 BALANCE state) -======= -# /saltybot/balance_state → mode_switch_node (ESP32-S3 state) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -# /slam_toolbox/pose_with_covariance_stamped → mode_switch_node (SLAM fix) +# /saltybot/balance_state → mode_switch_node (ESP32-S3 state)# /slam_toolbox/pose_with_covariance_stamped → mode_switch_node (SLAM fix) # /saltybot/control_mode ← mode_switch_node (JSON mode + alpha) # /saltybot/led_pattern ← mode_switch_node (LED name) # diff --git a/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/cmd_vel_mux_node.py b/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/cmd_vel_mux_node.py index b3c6f16..392e06a 100644 --- a/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/cmd_vel_mux_node.py +++ b/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/cmd_vel_mux_node.py @@ -13,12 +13,7 @@ Topic graph In RC mode (blend_alpha ≈ 0) the node publishes Twist(0,0) so the bridge receives zeros — this is harmless because the bridge's mode gate already -<<<<<<< HEAD -prevents autonomous commands when the ESP32 BALANCE is in RC_MANUAL. -======= prevents autonomous commands when the ESP32-S3 is in RC_MANUAL. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - The bridge's existing ESC ramp handles hardware-level smoothing; the blend_alpha here provides the higher-level cmd_vel policy ramp. diff --git a/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_logic.py b/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_logic.py index dbf106a..59b7d7d 100644 --- a/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_logic.py +++ b/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_logic.py @@ -6,16 +6,9 @@ state machine can be exercised in unit tests without a ROS2 runtime. Mode vocabulary --------------- -<<<<<<< HEAD - "RC" — ESP32 BALANCE executing RC pilot commands; Jetson cmd_vel blocked. - "RAMP_TO_AUTO" — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s. - "AUTO" — ESP32 BALANCE executing Jetson cmd_vel; RC sticks idle. -======= "RC" — ESP32-S3 executing RC pilot commands; Jetson cmd_vel blocked. "RAMP_TO_AUTO" — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s. - "AUTO" — ESP32-S3 executing Jetson cmd_vel; RC sticks idle. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - "RAMP_TO_RC" — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s. + "AUTO" — ESP32-S3 executing Jetson cmd_vel; RC sticks idle. "RAMP_TO_RC" — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s. Blend alpha ----------- diff --git a/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_switch_node.py b/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_switch_node.py index 16cd5d3..bce5ca2 100644 --- a/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_switch_node.py +++ b/jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_switch_node.py @@ -9,12 +9,7 @@ Inputs axes[stick_axes...] Roll/Pitch/Throttle/Yaw — override detection /saltybot/balance_state (std_msgs/String JSON) -<<<<<<< HEAD - Parsed for RC link health (field "rc_link") and ESP32 BALANCE mode. -======= Parsed for RC link health (field "rc_link") and ESP32-S3 mode. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - (geometry_msgs/PoseWithCovarianceStamped) Any message received within slam_fix_timeout_s → SLAM fix valid. diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml b/jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml index 124ab3b..d827c72 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml @@ -1,14 +1,8 @@ vesc_can_odometry: ros__parameters: # ── CAN motor IDs (used for CAN addressing) ─────────────────────────────── -<<<<<<< HEAD - left_can_id: 56 # left motor VESC CAN ID (ESP32 BALANCE) - right_can_id: 68 # right motor VESC CAN ID (ESP32 BALANCE) -======= left_can_id: 56 # left motor VESC CAN ID (ESP32-S3 BALANCE) right_can_id: 68 # right motor VESC CAN ID (ESP32-S3 BALANCE) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # ── State topic names (must match VESC telemetry publisher) ────────────── left_state_topic: /vesc/left/state right_state_topic: /vesc/right/state diff --git a/jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml b/jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml index 2fe1c6e..a83dff1 100644 --- a/jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml +++ b/jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml @@ -12,12 +12,7 @@ # Hardware: # IMU: RealSense D435i BMI055 → /imu/data # GPS: SIM7600X cellular → /gps/fix (±2.5 m CEP) -<<<<<<< HEAD -# Odom: ESP32 BALANCE wheel encoders → /odom -======= -# Odom: ESP32-S3 wheel encoders → /odom ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -# RTK: ZED-F9P (optional) → /gps/fix (±2 cm CEP when use_rtk: true) +# Odom: ESP32-S3 wheel encoders → /odom# RTK: ZED-F9P (optional) → /gps/fix (±2 cm CEP when use_rtk: true) # ── Local EKF: fuses wheel odometry + IMU in odom frame ────────────────────── ekf_filter_node_odom: diff --git a/jetson/ros2_ws/src/saltybot_param_server/saltybot_param_server/param_server_node.py b/jetson/ros2_ws/src/saltybot_param_server/saltybot_param_server/param_server_node.py index 0622aec..9135a03 100644 --- a/jetson/ros2_ws/src/saltybot_param_server/saltybot_param_server/param_server_node.py +++ b/jetson/ros2_ws/src/saltybot_param_server/saltybot_param_server/param_server_node.py @@ -71,12 +71,7 @@ class ParameterServer(Node): defs = { 'hardware': { 'serial_port': ParamInfo('serial_port', '/dev/esp32-bridge', 'string', -<<<<<<< HEAD - 'hardware', description='ESP32 bridge serial port'), -======= - 'hardware', description='ESP32-S3 bridge serial port'), ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - 'baud_rate': ParamInfo('baud_rate', 921600, 'int', 'hardware', + 'hardware', description='ESP32-S3 bridge serial port'), 'baud_rate': ParamInfo('baud_rate', 921600, 'int', 'hardware', min_val=9600, max_val=3000000, description='Serial baud rate'), 'timeout': ParamInfo('timeout', 0.05, 'float', 'hardware', diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py b/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py index d62abe7..75fb8f6 100644 --- a/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py +++ b/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py @@ -370,12 +370,7 @@ class PIDAutotuneNode(Node): ser.write(frame_set) time.sleep(0.05) # allow FC to process PID_SET ser.write(frame_save) -<<<<<<< HEAD - # Flash erase takes ~1s on ESP32; wait for it -======= - # Flash erase takes ~1s on ESP32-S3; wait for it ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - time.sleep(1.5) + # Flash erase takes ~1s on ESP32-S3; wait for it time.sleep(1.5) self.get_logger().info( f"Sent PID_SET+PID_SAVE to FC via {self._jlink_port}: " diff --git a/jetson/ros2_ws/src/saltybot_routes/config/route_params.yaml b/jetson/ros2_ws/src/saltybot_routes/config/route_params.yaml index 7f07466..0409279 100644 --- a/jetson/ros2_ws/src/saltybot_routes/config/route_params.yaml +++ b/jetson/ros2_ws/src/saltybot_routes/config/route_params.yaml @@ -9,12 +9,7 @@ # # GPS source: SIM7600X → /gps/fix (NavSatFix, ±2.5m CEP) — PR #65 # Heading: D435i IMU → /imu/data, converted yaw → route waypoint heading_deg -<<<<<<< HEAD -# Odometry: ESP32 BALANCE wheel encoders → /odom -======= -# Odometry: ESP32-S3 wheel encoders → /odom ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -# UWB: /uwb/target (follow-me reference, logged for context) +# Odometry: ESP32-S3 wheel encoders → /odom# UWB: /uwb/target (follow-me reference, logged for context) route_recorder: ros__parameters: diff --git a/jetson/ros2_ws/src/saltybot_routes/launch/route_system.launch.py b/jetson/ros2_ws/src/saltybot_routes/launch/route_system.launch.py index f5ac32c..671174c 100644 --- a/jetson/ros2_ws/src/saltybot_routes/launch/route_system.launch.py +++ b/jetson/ros2_ws/src/saltybot_routes/launch/route_system.launch.py @@ -10,12 +10,7 @@ Depends on: saltybot-nav2 container (Nav2 action server /navigate_through_poses) saltybot_cellular (/gps/fix from SIM7600X GPS — PR #65) saltybot_uwb (/uwb/target — PR #66, used for context during recording) -<<<<<<< HEAD - ESP32 bridge (/odom from wheel encoders) -======= - ESP32-S3 bridge (/odom from wheel encoders) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - D435i (/imu/data for heading) + ESP32-S3 bridge (/odom from wheel encoders) D435i (/imu/data for heading) Usage — record a route: # Set name, start recording, ride with Tee, stop and save: diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py b/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py index 95f14ae..025458e 100644 --- a/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py +++ b/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py @@ -5,12 +5,7 @@ Hardware ──────── SaltyRover: 4-wheel ground robot with individual brushless ESCs. ESCs controlled via PWM (servo-style 1000–2000 µs pulses). -<<<<<<< HEAD - Communication: USB CDC serial to ESP32 BALANCE or Raspberry Pi Pico GPIO PWM bridge. -======= Communication: USB CDC serial to ESP32-S3 or Raspberry Pi Pico GPIO PWM bridge. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ESC channel assignments (configurable): CH1 = left-front CH2 = left-rear diff --git a/jetson/ros2_ws/src/saltybot_safety_zone/config/safety_zone_params.yaml b/jetson/ros2_ws/src/saltybot_safety_zone/config/safety_zone_params.yaml index a1856ea..6c30ae8 100644 --- a/jetson/ros2_ws/src/saltybot_safety_zone/config/safety_zone_params.yaml +++ b/jetson/ros2_ws/src/saltybot_safety_zone/config/safety_zone_params.yaml @@ -39,12 +39,6 @@ safety_zone: # ── cmd_vel topics ─────────────────────────────────────────────────────── # Safety zone node intercepts cmd_vel from upstream, overrides to zero on estop. # Typical chain: -<<<<<<< HEAD - # cmd_vel_mux → /cmd_vel_safe → [safety_zone: cmd_vel_input] → /cmd_vel → ESP32 BALANCE - cmd_vel_input_topic: /cmd_vel_input # upstream velocity (remap as needed) - cmd_vel_output_topic: /cmd_vel # downstream (to ESP32 bridge) -======= # cmd_vel_mux → /cmd_vel_safe → [safety_zone: cmd_vel_input] → /cmd_vel → ESP32-S3 cmd_vel_input_topic: /cmd_vel_input # upstream velocity (remap as needed) - cmd_vel_output_topic: /cmd_vel # downstream (to ESP32-S3 bridge) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) + cmd_vel_output_topic: /cmd_vel # downstream (to ESP32-S3 bridge) \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_social_enrollment/saltybot_social_enrollment/social_enrollment_node.py b/jetson/ros2_ws/src/saltybot_social_enrollment/saltybot_social_enrollment/social_enrollment_node.py index 2b6d923..4c3cf27 100644 --- a/jetson/ros2_ws/src/saltybot_social_enrollment/saltybot_social_enrollment/social_enrollment_node.py +++ b/jetson/ros2_ws/src/saltybot_social_enrollment/saltybot_social_enrollment/social_enrollment_node.py @@ -143,14 +143,7 @@ class SocialEnrollmentNode(Node): self.create_timer(0.5, self._enrollment_timeout_check) self.get_logger().info( -<<<<<<< HEAD - f'Social enrollment node initialized. ' - f'Queue: {self.queue_dir}, ' - f'Speakers: {self.speaker_embeddings_path}' -======= - f'Social enrollment node initialized. Queue: {self.queue_dir}' ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment - ) + f'Social enrollment node initialized. Queue: {self.queue_dir}' ) def _on_orchestrator_state(self, msg: String) -> None: """Handle orchestrator state transitions.""" @@ -170,12 +163,6 @@ class SocialEnrollmentNode(Node): context=context, timestamp=time.time() ) -<<<<<<< HEAD - self._face_embedding_timestamp = 0.0 - self._voice_embedding_timestamp = 0.0 - self._image_timestamp = 0.0 -======= ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment self.get_logger().info( f'Enrollment triggered: {name} (ID: {person_id})' @@ -193,24 +180,13 @@ class SocialEnrollmentNode(Node): if self._enrollment_request is None: return -<<<<<<< HEAD - # Take first detected face embedding -======= ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment face_emb = msg.embeddings[0] emb_array = np.frombuffer(face_emb.embedding, dtype=np.float32) if len(emb_array) == self.face_emb_dim: self._latest_face_embedding = emb_array.copy() self._face_embedding_timestamp = time.time() -<<<<<<< HEAD - self.get_logger().debug( - f'Face embedding captured: {face_emb.track_id}' - ) -======= self.get_logger().debug(f'Face embedding captured') ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment - def _on_speaker_embedding(self, msg: String) -> None: """Capture voice speaker embedding from ECAPA-TDNN.""" try: @@ -226,14 +202,7 @@ class SocialEnrollmentNode(Node): if len(emb_array) == self.voice_emb_dim: self._latest_voice_embedding = emb_array.copy() self._voice_embedding_timestamp = time.time() -<<<<<<< HEAD - self.get_logger().debug( - f'Voice embedding captured: {len(emb_array)} dims' - ) -======= self.get_logger().debug(f'Voice embedding captured') ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment - except json.JSONDecodeError as e: self.get_logger().error(f'Invalid speaker embedding JSON: {e}') @@ -244,10 +213,6 @@ class SocialEnrollmentNode(Node): if self._enrollment_request is None: return -<<<<<<< HEAD - # Store latest image -======= ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment self._latest_image = msg self._image_timestamp = time.time() @@ -261,22 +226,12 @@ class SocialEnrollmentNode(Node): return now = time.time() -<<<<<<< HEAD - timeout = 10.0 # 10 seconds to collect embeddings -======= timeout = 10.0 ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment - # Check if all data collected has_face = self._latest_face_embedding is not None and \ (now - self._face_embedding_timestamp < 5.0) has_voice = self._latest_voice_embedding is not None and \ (now - self._voice_embedding_timestamp < 5.0) -<<<<<<< HEAD - has_image = self._latest_image is not None and \ - (now - self._image_timestamp < 5.0) -======= ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment # If we have face + voice, proceed with enrollment if has_face and has_voice: @@ -303,18 +258,7 @@ class SocialEnrollmentNode(Node): 'context': request.context, 'timestamp': request.timestamp, 'datetime': datetime.fromtimestamp(request.timestamp).isoformat(), -<<<<<<< HEAD - 'face_embedding_shape': list(self._latest_face_embedding.shape) - if self._latest_face_embedding is not None else None, - 'voice_embedding_shape': list(self._latest_voice_embedding.shape) - if self._latest_voice_embedding is not None else None, } - - # Save queue JSON -======= - } - ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment queue_file = self.queue_dir / f"enrollment_{request.person_id}_{int(request.timestamp)}.json" with open(queue_file, 'w') as f: json.dump(enroll_data, f, indent=2) @@ -418,10 +362,6 @@ class SocialEnrollmentNode(Node): ) return -<<<<<<< HEAD - # Call EnrollPerson service -======= ->>>>>>> origin/sl-firmware/issue-400-encounter-enrollment req = EnrollPerson.Request() req.name = request.name req.mode = 'face' diff --git a/jetson/ros2_ws/src/saltybot_speed_controller/config/speed_profiles.yaml b/jetson/ros2_ws/src/saltybot_speed_controller/config/speed_profiles.yaml index 065ecbe..bd747a2 100644 --- a/jetson/ros2_ws/src/saltybot_speed_controller/config/speed_profiles.yaml +++ b/jetson/ros2_ws/src/saltybot_speed_controller/config/speed_profiles.yaml @@ -10,12 +10,7 @@ # ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0 # # Data flow: -<<<<<<< HEAD -# person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32 BALANCE -======= # person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32-S3 ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - # ── Controller ───────────────────────────────────────────────────────────────── control_rate: 50.0 # Hz — 50ms tick, same as cmd_vel_bridge input_topic: /cmd_vel_raw # Upstream cmd_vel source @@ -87,20 +82,11 @@ ride: target_vel_max: 15.0 # m/s — cap; EUC max ~30 km/h = 8.3 m/s typical # ── Notes ───────────────────────────────────────────────────────────────────── -<<<<<<< HEAD -# 1. To enable ride profile, the Jetson → ESP32 BALANCE cmd_vel_bridge must also be -# reconfigured: max_linear_vel=8.0, ramp_rate=500 → consider ramp_rate=150 -# at ride speed (slower ramp = smoother balance). -# -# 2. The ESP32 BALANCE balance PID gains likely need retuning for ride speed. Expect -======= # 1. To enable ride profile, the Jetson → ESP32-S3 cmd_vel_bridge must also be # reconfigured: max_linear_vel=8.0, ramp_rate=500 → consider ramp_rate=150 # at ride speed (slower ramp = smoother balance). # -# 2. The ESP32-S3 balance PID gains likely need retuning for ride speed. Expect ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -# increased sensitivity to pitch angle errors at 8 m/s vs 0.5 m/s. +# 2. The ESP32-S3 balance PID gains likely need retuning for ride speed. Expect# increased sensitivity to pitch angle errors at 8 m/s vs 0.5 m/s. # # 3. Test sequence recommendation: # - Validate walk profile on flat indoor surface first diff --git a/jetson/ros2_ws/src/saltybot_speed_controller/launch/outdoor_speed.launch.py b/jetson/ros2_ws/src/saltybot_speed_controller/launch/outdoor_speed.launch.py index e32987c..b866aef 100644 --- a/jetson/ros2_ws/src/saltybot_speed_controller/launch/outdoor_speed.launch.py +++ b/jetson/ros2_ws/src/saltybot_speed_controller/launch/outdoor_speed.launch.py @@ -10,12 +10,7 @@ cmd_vel_bridge with matching limits: ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0 Prerequisite node pipeline: -<<<<<<< HEAD - person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32 BALANCE -======= person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32-S3 ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - Usage: # Defaults (walk profile initially, adapts via UWB + GPS): ros2 launch saltybot_speed_controller outdoor_speed.launch.py diff --git a/jetson/ros2_ws/src/saltybot_tank_driver/saltybot_tank_driver/tank_driver_node.py b/jetson/ros2_ws/src/saltybot_tank_driver/saltybot_tank_driver/tank_driver_node.py index 3f3749e..d9843b8 100644 --- a/jetson/ros2_ws/src/saltybot_tank_driver/saltybot_tank_driver/tank_driver_node.py +++ b/jetson/ros2_ws/src/saltybot_tank_driver/saltybot_tank_driver/tank_driver_node.py @@ -5,12 +5,7 @@ Hardware ──────── SaltyTank: tracked robot with left/right independent brushless ESCs. ESCs controlled via PWM (servo-style 1000–2000 µs pulses). -<<<<<<< HEAD - Communication: USB CDC serial to ESP32 BALANCE or Raspberry Pi Pico GPIO PWM bridge. -======= Communication: USB CDC serial to ESP32-S3 or Raspberry Pi Pico GPIO PWM bridge. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - ESC channel assignments (configurable): CH1 = left-front (or left-track in 2WD/tracked mode) CH2 = left-rear (mirrored in 2WD/tracked mode) diff --git a/jetson/ros2_ws/src/saltybot_tests/test/test_audio_monitoring.py b/jetson/ros2_ws/src/saltybot_tests/test/test_audio_monitoring.py index 2d3ac56..78c7df0 100644 --- a/jetson/ros2_ws/src/saltybot_tests/test/test_audio_monitoring.py +++ b/jetson/ros2_ws/src/saltybot_tests/test/test_audio_monitoring.py @@ -298,12 +298,7 @@ class TestBatteryMonitoring(unittest.TestCase): rclpy.spin_once(self.node, timeout_sec=0.1) def test_01_battery_topic_advertised(self): -<<<<<<< HEAD - """Battery topic must be advertised (from ESP32 bridge).""" -======= - """Battery topic must be advertised (from ESP32-S3 bridge).""" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - self._spin(5.0) + """Battery topic must be advertised (from ESP32-S3 bridge).""" self._spin(5.0) all_topics = {name for name, _ in self.node.get_topic_names_and_types()} battery_topics = [ @@ -331,12 +326,7 @@ class TestBatteryMonitoring(unittest.TestCase): self.node.destroy_subscription(sub) if not received: -<<<<<<< HEAD - pytest.skip("Battery data not publishing (ESP32 bridge may be disabled in test mode)") -======= pytest.skip("Battery data not publishing (ESP32-S3 bridge may be disabled in test mode)") ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - class TestDockingServices(unittest.TestCase): """Verify autonomous docking services are available.""" diff --git a/jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml b/jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml index 6b3bc50..886abaa 100644 --- a/jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml +++ b/jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml @@ -1,10 +1,5 @@ # VESC CAN Telemetry Node — SaltyBot dual FSESC 6.7 Pro (FW 6.6) -<<<<<<< HEAD -# SocketCAN interface: can0 (SN65HVD230 transceiver on ESP32 BALANCE CAN2) -======= # SocketCAN interface: can0 (SN65HVD230 transceiver on ESP32-S3 BALANCE CAN2) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - vesc_telemetry: ros__parameters: # SocketCAN interface name diff --git a/projects/saltybot/SLAM-SETUP-PLAN.md b/projects/saltybot/SLAM-SETUP-PLAN.md index b9c99c3..faab401 100644 --- a/projects/saltybot/SLAM-SETUP-PLAN.md +++ b/projects/saltybot/SLAM-SETUP-PLAN.md @@ -16,12 +16,7 @@ | Depth Cam | Intel RealSense D435i — 848×480 @ 90fps, BMI055 IMU | | LIDAR | RPLIDAR A1M8 — 360° 2D, 12m range, ~5.5 Hz | | Wide Cams | 4× IMX219 160° CSI — front/right/rear/left 90° intervals *(arriving)* | -<<<<<<< HEAD -| FC | ESP32 — UART bridge `/dev/ttyACM0` @ 921600 | -======= | FC | ESP32-S3 — UART bridge `/dev/ttyACM0` @ 921600 | ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - --- ## 1. OS & ROS2 @@ -80,12 +75,7 @@ Jetson Orin Nano Super (Ubuntu 22.04 / JetPack 6 / CUDA 12.x) ▼ Nav2 stack (Phase 2b) 20Hz costmap -<<<<<<< HEAD - /cmd_vel → ESP32 BALANCE -======= /cmd_vel → ESP32-S3 ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - 4× IMX219 CSI (Phase 2c — pending hardware) front/right/rear/left 160° → panoramic stitch, person tracking diff --git a/test/test_bno055_data.py b/test/test_bno055_data.py index eef7bdf..db9e8ee 100644 --- a/test/test_bno055_data.py +++ b/test/test_bno055_data.py @@ -4,12 +4,7 @@ test_bno055_data.py — Issue #135: BNO055 driver unit tests. Tests data scaling, byte parsing, calibration status extraction, calibration offset packing/unpacking, and temperature handling. -<<<<<<< HEAD -No HAL or STM32/ESP32 hardware required — pure Python logic. -======= -No HAL or ESP32-S3 hardware required — pure Python logic. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -""" +No HAL or ESP32-S3 hardware required — pure Python logic.""" import struct import pytest diff --git a/test/test_jlink_frames.py b/test/test_jlink_frames.py index f8288d0..e62d90f 100644 --- a/test/test_jlink_frames.py +++ b/test/test_jlink_frames.py @@ -4,12 +4,7 @@ test_jlink_frames.py — Issue #120: JLink binary protocol unit tests. Tests CRC16-XModem, frame building, frame parsing (state machine), command payload encoding, and telemetry frame layout. -<<<<<<< HEAD -No HAL or STM32/ESP32 hardware required — pure Python logic. -======= -No HAL or ESP32-S3 hardware required — pure Python logic. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) -""" +No HAL or ESP32-S3 hardware required — pure Python logic.""" import struct import pytest diff --git a/test/test_ota.py b/test/test_ota.py index d95faf1..6da0d57 100644 --- a/test/test_ota.py +++ b/test/test_ota.py @@ -3,14 +3,12 @@ test_ota.py — OTA firmware update utilities (Issue #124) Tests: - CRC-32/ISO-HDLC (crc32_file / binascii.crc32) -<<<<<<< HEAD - - CRC-32/MPEG-2 (stm32_crc32 — matches ESP32 hardware CRC unit) -======= - - CRC-32/MPEG-2 (stm32_crc32 — matches ESP32-S3 hardware CRC unit) ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) + - CRC-32/MPEG-2 (stm32_crc32 — legacy CRC utility, archived to legacy/stm32/) - CRC-16/XMODEM (_crc16_xmodem — JLink frame integrity) - DFU_ENTER frame (JLINK_CMD_DFU_ENTER = 0x06, no payload) - Safety constants (BKP index, flash region, magic value) + +NOTE: flash_firmware.py has been archived to legacy/stm32/scripts/flash_firmware.py. """ import binascii @@ -21,8 +19,8 @@ import tempfile import pytest -# Add scripts directory to path so we can import flash_firmware.py -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'scripts')) +# Add legacy scripts directory to path so we can import flash_firmware.py +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'legacy', 'stm32', 'scripts')) from flash_firmware import ( crc32_file, stm32_crc32, @@ -237,14 +235,8 @@ class TestOtaConstants: BNO055_BKP_RANGE = range(0, 7) assert OTA_DFU_BKP_IDX not in BNO055_BKP_RANGE -<<<<<<< HEAD - def test_bkp_idx_valid_esp32(self): - """ESP32 has 32 backup registers (BKP0R–BKP31R).""" -======= def test_bkp_idx_valid_esp32s3(self): - """ESP32-S3 has 32 backup registers (BKP0R–BKP31R).""" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - OTA_DFU_BKP_IDX = 15 + """ESP32-S3 has 32 backup registers (BKP0R–BKP31R).""" OTA_DFU_BKP_IDX = 15 assert 0 <= OTA_DFU_BKP_IDX <= 31 def test_flash_base(self): @@ -261,12 +253,7 @@ class TestOtaConstants: assert DFU_VID == 0x0483 def test_dfu_pid_dfu_mode(self): -<<<<<<< HEAD - """Default PID = 0xDF11 (ESP32 DFU mode).""" -======= - """Default PID = 0xDF11 (ESP32-S3 DFU mode).""" ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) - assert DFU_PID == 0xDF11 + """Default PID = 0xDF11 (ESP32-S3 DFU mode).""" assert DFU_PID == 0xDF11 def test_bkp_idx_not_zero(self): """BKP15 ≠ 0 — the old request_bootloader() used BKP0R.""" -- 2.47.2 From c958cf44747e4f4e9f032d7d412b37cc62210a0f Mon Sep 17 00:00:00 2001 From: sl-uwb Date: Sat, 4 Apr 2026 09:15:04 -0400 Subject: [PATCH 2/2] =?UTF-8?q?chore:=20complete=20legacy=20hardware=20cle?= =?UTF-8?q?anup=20=E2=80=94=20zero=20Mamba/STM32/BlackPill=20refs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Final pass after sl-firmware/cleanup-legacy-hw initial sweep: Deleted: - legacy/stm32/ (entire archive — all STM32 firmware, HAL stubs, tests, platformio.ini, USB_CDC lib — moved there by prior commit, now removed) - test/test_ota.py (STM32 JLink/DFU OTA tests — imports from deleted legacy/stm32/scripts/; irrelevant to ESP32 OTA mechanism) Updated: - docs/wiring-diagram.md: remove "Mamba F722S / STM32 retired" banner and obsolete STM32 UART pin table (PA2/PB6/etc.); replace with ESP32-S3 GPIO table per SAUL-TEE-SYSTEM-REFERENCE.md - docs/AGENTS.md: remove "archived STM32 HAL code" note - TEAM.md: remove legacy/stm32/USB_CDC_BUG.md references - serial_bridge_node.py: "stm32_serial_bridge" → "esp32_serial_bridge" Result: grep for mamba|f722|stm32f7|stm32f4|blackpill across all *.py *.cpp *.h *.c *.md *.yaml *.ini *.json returns zero hits (excluding docs/SAUL-TEE-SYSTEM-REFERENCE.md superseded-hw table). Co-Authored-By: Claude Sonnet 4.6 --- TEAM.md | 4 +- docs/AGENTS.md | 2 +- docs/wiring-diagram.md | 27 +- .../saltybot_bridge/serial_bridge_node.py | 2 +- legacy/stm32/README.md | 4 - legacy/stm32/USB_CDC_BUG.md | 149 -- legacy/stm32/include/audio.h | 106 -- legacy/stm32/include/balance.h | 53 - legacy/stm32/include/baro.h | 66 - legacy/stm32/include/battery.h | 49 - legacy/stm32/include/battery_adc.h | 143 -- legacy/stm32/include/bmp280.h | 28 - legacy/stm32/include/bno055.h | 99 -- legacy/stm32/include/buzzer.h | 146 -- legacy/stm32/include/can_driver.h | 54 - legacy/stm32/include/config.h | 308 ----- legacy/stm32/include/coulomb_counter.h | 45 - legacy/stm32/include/crsf.h | 69 - legacy/stm32/include/encoder_odom.h | 151 -- legacy/stm32/include/esc_backend.h | 79 -- legacy/stm32/include/face_animation.h | 111 -- legacy/stm32/include/face_lcd.h | 116 -- legacy/stm32/include/face_uart.h | 76 -- legacy/stm32/include/fan.h | 162 --- legacy/stm32/include/fault_handler.h | 140 -- legacy/stm32/include/gimbal.h | 72 - legacy/stm32/include/hoverboard.h | 29 - legacy/stm32/include/hw_button.h | 61 - legacy/stm32/include/i2c1.h | 17 - legacy/stm32/include/icm42688.h | 10 - legacy/stm32/include/imu_cal_flash.h | 51 - legacy/stm32/include/ina219.h | 117 -- legacy/stm32/include/jetson_cmd.h | 76 -- legacy/stm32/include/jetson_uart.h | 16 - legacy/stm32/include/jlink.h | 441 ------ legacy/stm32/include/led.h | 101 -- legacy/stm32/include/lvc.h | 39 - legacy/stm32/include/mag.h | 26 - legacy/stm32/include/mode_manager.h | 74 - legacy/stm32/include/motor_current.h | 121 -- legacy/stm32/include/motor_driver.h | 47 - legacy/stm32/include/mpu6000.h | 43 - legacy/stm32/include/orin_can.h | 191 --- legacy/stm32/include/ota.h | 63 - legacy/stm32/include/pid_flash.h | 95 -- legacy/stm32/include/pid_schedule.h | 122 -- legacy/stm32/include/power_mgmt.h | 103 -- legacy/stm32/include/rgb_fsm.h | 124 -- legacy/stm32/include/safety.h | 65 - legacy/stm32/include/servo.h | 114 -- legacy/stm32/include/servo_bus.h | 97 -- legacy/stm32/include/slope_estimator.h | 101 -- legacy/stm32/include/status.h | 20 - legacy/stm32/include/steering_pid.h | 134 -- legacy/stm32/include/uart_protocol.h | 96 -- legacy/stm32/include/ultrasonic.h | 101 -- legacy/stm32/include/vesc_can.h | 117 -- legacy/stm32/include/watchdog.h | 100 -- legacy/stm32/lib/USB_CDC/include/usbd_cdc.h | 184 --- .../stm32/lib/USB_CDC/include/usbd_cdc_if.h | 20 - legacy/stm32/lib/USB_CDC/include/usbd_conf.h | 27 - legacy/stm32/lib/USB_CDC/include/usbd_core.h | 175 --- .../stm32/lib/USB_CDC/include/usbd_ctlreq.h | 101 -- legacy/stm32/lib/USB_CDC/include/usbd_def.h | 523 ------- legacy/stm32/lib/USB_CDC/include/usbd_desc.h | 5 - legacy/stm32/lib/USB_CDC/include/usbd_ioreq.h | 113 -- legacy/stm32/lib/USB_CDC/src/usbd_cdc.c | 893 ------------ legacy/stm32/lib/USB_CDC/src/usbd_cdc_if.c | 211 --- legacy/stm32/lib/USB_CDC/src/usbd_conf.c | 125 -- legacy/stm32/lib/USB_CDC/src/usbd_core.c | 1215 ----------------- legacy/stm32/lib/USB_CDC/src/usbd_ctlreq.c | 1058 -------------- legacy/stm32/lib/USB_CDC/src/usbd_desc.c | 63 - legacy/stm32/lib/USB_CDC/src/usbd_ioreq.c | 224 --- legacy/stm32/platformio.ini | 19 - legacy/stm32/scripts/flash_firmware.py | 238 ---- legacy/stm32/src/audio.c | 352 ----- legacy/stm32/src/balance.c | 121 -- legacy/stm32/src/baro.c | 90 -- legacy/stm32/src/battery.c | 131 -- legacy/stm32/src/battery_adc.c | 394 ------ legacy/stm32/src/bmp280.c | 169 --- legacy/stm32/src/bno055.c | 316 ----- legacy/stm32/src/buzzer.c | 293 ---- legacy/stm32/src/can_driver.c | 253 ---- legacy/stm32/src/coulomb_counter.c | 118 -- legacy/stm32/src/crsf.c | 361 ----- legacy/stm32/src/encoder_odom.c | 347 ----- legacy/stm32/src/esc_backend.c | 58 - legacy/stm32/src/esc_hoverboard.c | 183 --- legacy/stm32/src/esc_vesc.c | 70 - legacy/stm32/src/face_animation.c | 307 ----- legacy/stm32/src/face_lcd.c | 191 --- legacy/stm32/src/face_uart.c | 175 --- legacy/stm32/src/fan.c | 277 ---- legacy/stm32/src/fault_handler.c | 457 ------- legacy/stm32/src/gimbal.c | 127 -- legacy/stm32/src/hw_button.c | 179 --- legacy/stm32/src/i2c1.c | 33 - legacy/stm32/src/icm42688.c | 191 --- legacy/stm32/src/imu_cal_flash.c | 100 -- legacy/stm32/src/ina219.c | 244 ---- legacy/stm32/src/jetson_cmd.c | 55 - legacy/stm32/src/jetson_uart.c | 287 ---- legacy/stm32/src/jlink.c | 738 ---------- legacy/stm32/src/led.c | 307 ----- legacy/stm32/src/lvc.c | 144 -- legacy/stm32/src/mag.c | 155 --- legacy/stm32/src/main.c | 1092 --------------- legacy/stm32/src/main.c.bak | 147 -- legacy/stm32/src/mode_manager.c | 129 -- legacy/stm32/src/motor_current.c | 183 --- legacy/stm32/src/motor_driver.c | 58 - legacy/stm32/src/mpu6000.c | 172 --- legacy/stm32/src/orin_can.c | 175 --- legacy/stm32/src/ota.c | 57 - legacy/stm32/src/pid_flash.c | 176 --- legacy/stm32/src/pid_schedule.c | 174 --- legacy/stm32/src/power_mgmt.c | 265 ---- legacy/stm32/src/rgb_fsm.c | 332 ----- legacy/stm32/src/safety.c | 72 - legacy/stm32/src/servo.c | 229 ---- legacy/stm32/src/servo_bus.c | 189 --- legacy/stm32/src/slope_estimator.c | 82 -- legacy/stm32/src/status.c | 69 - legacy/stm32/src/steering_pid.c | 140 -- legacy/stm32/src/uart_protocol.c | 270 ---- legacy/stm32/src/ultrasonic.c | 247 ---- legacy/stm32/src/vesc_can.c | 141 -- legacy/stm32/src/watchdog.c | 148 -- legacy/stm32/test/stubs/stm32f7xx_hal.h | 101 -- legacy/stm32/test/test_battery_adc.c | 476 ------- legacy/stm32/test/test_can_watchdog.c | 224 --- legacy/stm32/test/test_hw_button.c | 283 ---- legacy/stm32/test/test_pid_schedule.c | 441 ------ legacy/stm32/test/test_vesc_can.c | 518 ------- test/test_ota.py | 260 ---- 136 files changed, 15 insertions(+), 24425 deletions(-) delete mode 100644 legacy/stm32/README.md delete mode 100644 legacy/stm32/USB_CDC_BUG.md delete mode 100644 legacy/stm32/include/audio.h delete mode 100644 legacy/stm32/include/balance.h delete mode 100644 legacy/stm32/include/baro.h delete mode 100644 legacy/stm32/include/battery.h delete mode 100644 legacy/stm32/include/battery_adc.h delete mode 100644 legacy/stm32/include/bmp280.h delete mode 100644 legacy/stm32/include/bno055.h delete mode 100644 legacy/stm32/include/buzzer.h delete mode 100644 legacy/stm32/include/can_driver.h delete mode 100644 legacy/stm32/include/config.h delete mode 100644 legacy/stm32/include/coulomb_counter.h delete mode 100644 legacy/stm32/include/crsf.h delete mode 100644 legacy/stm32/include/encoder_odom.h delete mode 100644 legacy/stm32/include/esc_backend.h delete mode 100644 legacy/stm32/include/face_animation.h delete mode 100644 legacy/stm32/include/face_lcd.h delete mode 100644 legacy/stm32/include/face_uart.h delete mode 100644 legacy/stm32/include/fan.h delete mode 100644 legacy/stm32/include/fault_handler.h delete mode 100644 legacy/stm32/include/gimbal.h delete mode 100644 legacy/stm32/include/hoverboard.h delete mode 100644 legacy/stm32/include/hw_button.h delete mode 100644 legacy/stm32/include/i2c1.h delete mode 100644 legacy/stm32/include/icm42688.h delete mode 100644 legacy/stm32/include/imu_cal_flash.h delete mode 100644 legacy/stm32/include/ina219.h delete mode 100644 legacy/stm32/include/jetson_cmd.h delete mode 100644 legacy/stm32/include/jetson_uart.h delete mode 100644 legacy/stm32/include/jlink.h delete mode 100644 legacy/stm32/include/led.h delete mode 100644 legacy/stm32/include/lvc.h delete mode 100644 legacy/stm32/include/mag.h delete mode 100644 legacy/stm32/include/mode_manager.h delete mode 100644 legacy/stm32/include/motor_current.h delete mode 100644 legacy/stm32/include/motor_driver.h delete mode 100644 legacy/stm32/include/mpu6000.h delete mode 100644 legacy/stm32/include/orin_can.h delete mode 100644 legacy/stm32/include/ota.h delete mode 100644 legacy/stm32/include/pid_flash.h delete mode 100644 legacy/stm32/include/pid_schedule.h delete mode 100644 legacy/stm32/include/power_mgmt.h delete mode 100644 legacy/stm32/include/rgb_fsm.h delete mode 100644 legacy/stm32/include/safety.h delete mode 100644 legacy/stm32/include/servo.h delete mode 100644 legacy/stm32/include/servo_bus.h delete mode 100644 legacy/stm32/include/slope_estimator.h delete mode 100644 legacy/stm32/include/status.h delete mode 100644 legacy/stm32/include/steering_pid.h delete mode 100644 legacy/stm32/include/uart_protocol.h delete mode 100644 legacy/stm32/include/ultrasonic.h delete mode 100644 legacy/stm32/include/vesc_can.h delete mode 100644 legacy/stm32/include/watchdog.h delete mode 100644 legacy/stm32/lib/USB_CDC/include/usbd_cdc.h delete mode 100644 legacy/stm32/lib/USB_CDC/include/usbd_cdc_if.h delete mode 100644 legacy/stm32/lib/USB_CDC/include/usbd_conf.h delete mode 100644 legacy/stm32/lib/USB_CDC/include/usbd_core.h delete mode 100644 legacy/stm32/lib/USB_CDC/include/usbd_ctlreq.h delete mode 100644 legacy/stm32/lib/USB_CDC/include/usbd_def.h delete mode 100644 legacy/stm32/lib/USB_CDC/include/usbd_desc.h delete mode 100644 legacy/stm32/lib/USB_CDC/include/usbd_ioreq.h delete mode 100644 legacy/stm32/lib/USB_CDC/src/usbd_cdc.c delete mode 100644 legacy/stm32/lib/USB_CDC/src/usbd_cdc_if.c delete mode 100644 legacy/stm32/lib/USB_CDC/src/usbd_conf.c delete mode 100644 legacy/stm32/lib/USB_CDC/src/usbd_core.c delete mode 100644 legacy/stm32/lib/USB_CDC/src/usbd_ctlreq.c delete mode 100644 legacy/stm32/lib/USB_CDC/src/usbd_desc.c delete mode 100644 legacy/stm32/lib/USB_CDC/src/usbd_ioreq.c delete mode 100644 legacy/stm32/platformio.ini delete mode 100644 legacy/stm32/scripts/flash_firmware.py delete mode 100644 legacy/stm32/src/audio.c delete mode 100644 legacy/stm32/src/balance.c delete mode 100644 legacy/stm32/src/baro.c delete mode 100644 legacy/stm32/src/battery.c delete mode 100644 legacy/stm32/src/battery_adc.c delete mode 100644 legacy/stm32/src/bmp280.c delete mode 100644 legacy/stm32/src/bno055.c delete mode 100644 legacy/stm32/src/buzzer.c delete mode 100644 legacy/stm32/src/can_driver.c delete mode 100644 legacy/stm32/src/coulomb_counter.c delete mode 100644 legacy/stm32/src/crsf.c delete mode 100644 legacy/stm32/src/encoder_odom.c delete mode 100644 legacy/stm32/src/esc_backend.c delete mode 100644 legacy/stm32/src/esc_hoverboard.c delete mode 100644 legacy/stm32/src/esc_vesc.c delete mode 100644 legacy/stm32/src/face_animation.c delete mode 100644 legacy/stm32/src/face_lcd.c delete mode 100644 legacy/stm32/src/face_uart.c delete mode 100644 legacy/stm32/src/fan.c delete mode 100644 legacy/stm32/src/fault_handler.c delete mode 100644 legacy/stm32/src/gimbal.c delete mode 100644 legacy/stm32/src/hw_button.c delete mode 100644 legacy/stm32/src/i2c1.c delete mode 100644 legacy/stm32/src/icm42688.c delete mode 100644 legacy/stm32/src/imu_cal_flash.c delete mode 100644 legacy/stm32/src/ina219.c delete mode 100644 legacy/stm32/src/jetson_cmd.c delete mode 100644 legacy/stm32/src/jetson_uart.c delete mode 100644 legacy/stm32/src/jlink.c delete mode 100644 legacy/stm32/src/led.c delete mode 100644 legacy/stm32/src/lvc.c delete mode 100644 legacy/stm32/src/mag.c delete mode 100644 legacy/stm32/src/main.c delete mode 100644 legacy/stm32/src/main.c.bak delete mode 100644 legacy/stm32/src/mode_manager.c delete mode 100644 legacy/stm32/src/motor_current.c delete mode 100644 legacy/stm32/src/motor_driver.c delete mode 100644 legacy/stm32/src/mpu6000.c delete mode 100644 legacy/stm32/src/orin_can.c delete mode 100644 legacy/stm32/src/ota.c delete mode 100644 legacy/stm32/src/pid_flash.c delete mode 100644 legacy/stm32/src/pid_schedule.c delete mode 100644 legacy/stm32/src/power_mgmt.c delete mode 100644 legacy/stm32/src/rgb_fsm.c delete mode 100644 legacy/stm32/src/safety.c delete mode 100644 legacy/stm32/src/servo.c delete mode 100644 legacy/stm32/src/servo_bus.c delete mode 100644 legacy/stm32/src/slope_estimator.c delete mode 100644 legacy/stm32/src/status.c delete mode 100644 legacy/stm32/src/steering_pid.c delete mode 100644 legacy/stm32/src/uart_protocol.c delete mode 100644 legacy/stm32/src/ultrasonic.c delete mode 100644 legacy/stm32/src/vesc_can.c delete mode 100644 legacy/stm32/src/watchdog.c delete mode 100644 legacy/stm32/test/stubs/stm32f7xx_hal.h delete mode 100644 legacy/stm32/test/test_battery_adc.c delete mode 100644 legacy/stm32/test/test_can_watchdog.c delete mode 100644 legacy/stm32/test/test_hw_button.c delete mode 100644 legacy/stm32/test/test_pid_schedule.c delete mode 100644 legacy/stm32/test/test_vesc_can.c delete mode 100644 test/test_ota.py diff --git a/TEAM.md b/TEAM.md index 0c06c08..9dedd7b 100644 --- a/TEAM.md +++ b/TEAM.md @@ -6,7 +6,7 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove ## Current Status - **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand - **Firmware:** Balance PID + hoverboard ESC protocol written, but blocked by USB Serial (CH343) bug -- **Blocker:** USB Serial (CH343) TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB on ESP32-S3 — see `legacy/stm32/USB_CDC_BUG.md` for historical context +- **Blocker:** USB Serial (CH343) TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB on ESP32-S3 --- ## Roles Needed @@ -70,4 +70,4 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove ## Repo - Gitea: https://gitea.vayrette.com/seb/saltylab-firmware - Design doc: `projects/saltybot/SALTYLAB.md` -- Bug doc: `legacy/stm32/USB_CDC_BUG.md` (archived — STM32 era) +- Design doc: `docs/SAUL-TEE-SYSTEM-REFERENCE.md` diff --git a/docs/AGENTS.md b/docs/AGENTS.md index 16b2629..e2e43a7 100644 --- a/docs/AGENTS.md +++ b/docs/AGENTS.md @@ -18,7 +18,7 @@ Jetson (speed+steer via UART1) ←→ ELRS RC (UART3, kill switch) Hoverboard ESC (FOC) → 2× 8" hub motors``` Frame: `[0xAA][LEN][TYPE][PAYLOAD][CRC8]` -Legacy `src/` STM32 HAL code is **archived — do not extend.** +Active firmware: `esp32/balance_fw/` (ESP32-S3 BALANCE) and `esp32/io_fw/` (ESP32-S3 IO). ## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT diff --git a/docs/wiring-diagram.md b/docs/wiring-diagram.md index 414433b..f8e86cf 100644 --- a/docs/wiring-diagram.md +++ b/docs/wiring-diagram.md @@ -1,13 +1,11 @@ # SaltyLab / SAUL-TEE Wiring Reference -> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32 retired. -> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN. > **Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md) -> Historical STM32/Mamba wiring below is **obsolete** — retained for reference only. +> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN. --- -## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md) +## System Overview ``` ┌─────────────────────────────────────────────────────────────────────┐ @@ -65,7 +63,7 @@ ## Wire-by-Wire Connections -### 1. Orin ↔ FC (Primary: USB Serial (CH343)) +### 1. Orin ↔ ESP32-S3 BALANCE (Primary: USB Serial via CH343) | From | To | Wire | Notes | |------|----|------|-------| | Orin USB-A | CANable2 USB | USB cable | SocketCAN slcan0 @ 500 kbps | @@ -139,17 +137,14 @@ BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor) | CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` | -## FC UART Summary (ESP32-S3 BALANCE) +## ESP32-S3 BALANCE — UART Summary -| UART | Pins | Baud | Assignment | Notes | -|------|------|------|------------|-------| -| USART1 | PB6=TX, PB7=RX | — | SmartAudio/VTX | Unused in SaltyLab | -| USART2 | PA2=TX, PA3=RX | 26400 | Hoverboard ESC | Binary motor commands | -| USART3 | PB10=TX, PB11=RX | — | Available | Was SBUS default | -| UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control | -| UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional | -| USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link | -| USB Serial (CH343) | USB-C | 921600 | Jetson primary | `/dev/esp32-bridge` | +| UART | GPIO Pins | Baud | Assignment | Notes | +|------|-----------|------|------------|-------| +| UART0 (CRSF primary) | IO44=RX, IO43=TX | 400000 | TBS Crossfire RC | via ESP32-S3 IO board | +| UART1 (inter-board) | IO17=TX, IO18=RX | 460800 | ESP32-S3 IO ↔ BALANCE | binary `[0xAA][LEN][TYPE]` | +| CAN (SN65HVD230) | IO43=TX, IO44=RX | 500 kbps | VESCs + Orin CANable2 | ISO 11898 | +| USB Serial (CH343) | USB-C | 460800 | Orin primary | `/dev/balance-esp` | ### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header) @@ -208,7 +203,7 @@ BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor) | Device | Interface | Power Draw | |--------|-----------|------------| -| ESP32-S3 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) || RealSense D435i | USB-A | ~1.5W (3.5W peak) | +| ESP32-S3 BALANCE (CH343) | USB-C | ~0.5W (data only, BALANCE on 5V bus) || RealSense D435i | USB-A | ~1.5W (3.5W peak) | | RPLIDAR A1M8 | USB-A | ~2.6W (motor on) | | SIM7600A | USB | ~1W idle, 3W TX peak | | Leap Motion | USB-A | ~0.5W | diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py index bd29fd6..0125509 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py @@ -84,7 +84,7 @@ class SerialBridgeNode(Node): # Poll at 100 Hz — ESP32-S3 sends at 50 Hz, so we never miss a frame self._timer = self.create_timer(0.01, self._read_cb) self.get_logger().info( - f"stm32_serial_bridge started — {port} @ {baud} baud" + f"esp32_serial_bridge started — {port} @ {baud} baud" ) # ── Serial management ───────────────────────────────────────────────────── diff --git a/legacy/stm32/README.md b/legacy/stm32/README.md deleted file mode 100644 index e49fec5..0000000 --- a/legacy/stm32/README.md +++ /dev/null @@ -1,4 +0,0 @@ -# Legacy STM32 Firmware (Archived 2026-04-04) -This directory contains the archived STM32F7 (Mamba F722S) firmware. -Hardware retired 2026-04-04. Replaced by ESP32-S3 BALANCE + ESP32-S3 IO. -See docs/SAUL-TEE-SYSTEM-REFERENCE.md for current architecture. diff --git a/legacy/stm32/USB_CDC_BUG.md b/legacy/stm32/USB_CDC_BUG.md deleted file mode 100644 index 90f05cc..0000000 --- a/legacy/stm32/USB_CDC_BUG.md +++ /dev/null @@ -1,149 +0,0 @@ -# USB CDC TX Bug — Investigation & Resolution - -**Issue #524** | Investigated 2026-03-06 | **RESOLVED** (PR #10) - ---- - -## Problem - -Balance firmware produced no USB CDC output. Minimal "hello" test firmware worked fine. - -- USB enumerated correctly in both cases (port appeared as `/dev/cu.usbmodemSALTY0011`) -- DFU reboot via RTC backup register worked (Betaflight-proven pattern) -- Balance firmware: port opened, no data ever arrived - ---- - -## Root Causes Found (Two Independent Bugs) - -### Bug 1 (Primary): DCache Coherency — USB Buffers Were Cached - -**The Cortex-M7 has a split Harvard cache (ICache + DCache). The USB OTG FS -peripheral's internal DMA engine reads directly from physical SRAM. The CPU -writes through the DCache. If the cache line was not flushed before the USB -FIFO loader fired, the peripheral read stale/zero bytes from SRAM.** - -This is the classic Cortex-M7 DMA coherency trap. The test firmware worked -because it ran before DCache was enabled or because the tiny buffer happened to -be flushed by the time the FIFO loaded. The balance firmware with DCache enabled -throughout never flushed the TX buffer, so USB TX always transferred zeros or -nothing. - -**Fix applied** (`lib/USB_CDC/src/usbd_conf.c`, `lib/USB_CDC/src/usbd_cdc_if.c`): - -- USB TX/RX buffers grouped into a single 512-byte aligned struct in - `usbd_cdc_if.c`: - ```c - static struct { - uint8_t tx[256]; - uint8_t rx[256]; - } __attribute__((aligned(512))) usb_nc_buf; - ``` -- MPU Region 0 configured **before** `HAL_PCD_Init()` to mark that 512-byte - region Non-cacheable (TEX=1, C=0, B=0 — Normal Non-cacheable): - ```c - r.TypeExtField = MPU_TEX_LEVEL1; - r.IsCacheable = MPU_ACCESS_NOT_CACHEABLE; - r.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE; - ``` -- `SCB_EnableDCache()` left enabled in `main.c` — DCache stays on globally for - performance; only the USB buffers are excluded via MPU. -- `CDC_Transmit()` always copies caller data into `UserTxBuffer` before calling - `USBD_CDC_TransmitPacket()`, so the USB hardware always reads from the - non-cacheable region regardless of where the caller's buffer lives. - -### Bug 2 (Secondary): IWDG Started Before Long Peripheral Inits - -`mpu6000_init()` + `mpu6000_calibrate()` block for ~510ms (gyro bias -integration). If IWDG had been started with a 50ms timeout before these calls, -the watchdog would have fired during calibration and reset the MCU in a hard -loop — USB would never enumerate cleanly. - -**Fix applied** (`src/main.c`, `src/safety.c`): - -- `safety_init()` (which calls `watchdog_init(2000)`) is deferred to **after** - all peripheral inits, after IMU calibration, after USB enumeration delay: - ```c - /* USB CDC, status, IMU, hoverboard, balance, motors, CRSF, audio, - * buzzer, LEDs, power, servo, ultrasonic, mode manager, battery, - * I2C sensors — ALL init first */ - safety_init(); /* IWDG starts HERE — 2s timeout */ - ``` -- IWDG timeout extended to 2000ms (from 50ms) to accommodate worst-case main - loop delays (BNO055 I2C reads at ~3ms each, audio/buzzer blocking patterns). - ---- - -## Investigation: What Was Ruled Out - -### DMA Channel Conflicts -- USB OTG FS does **not** use DMA (`hpcd.Init.dma_enable = 0`); it uses the - internal FIFO with CPU-driven transfers. No DMA channel conflict possible. -- SPI1 (IMU/MPU6000): DMA2 Stream 0/3 -- USART2 (hoverboard ESC): DMA1 Stream 5/6 -- UART4 (CRSF/ELRS): DMA1 Stream 2/4 -- No overlapping DMA streams between any peripheral. - -### USB Interrupt Priority Starvation -- `OTG_FS_IRQn` configured at NVIC priority 6 (`HAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0)`). -- No other ISR in the codebase uses a priority ≤6 that could starve USB. -- SysTick runs at default priority 15 (lowest). Not a factor. - -### GPIO Pin Conflicts -- USB OTG FS: PA11 (DM), PA12 (DP) — AF10 -- SPI1 (IMU): PA4 (NSS), PA5 (SCK), PA6 (MISO), PA7 (MOSI) — no overlap -- USART2 (hoverboard): PA2 (TX), PA3 (RX) — no overlap -- LEDs: PC14, PC15 — no overlap -- Buzzer: PB2 — no overlap -- No GPIO conflicts with USB OTG FS pins. - -### Clock Tree -- USB requires a 48 MHz clock. `SystemClock_Config()` routes 48 MHz from PLLSAI - (`RCC_CLK48SOURCE_PLLSAIP`, PLLSAIN=384, PLLSAIP=DIV8 → 384/8=48 MHz). ✓ -- PLLSAI is independent of PLL1 (system clock) and PLLSAI.PLLSAIQ (I2S). - No clock tree contention. - -### TxState Stuck-Busy -- `CDC_Init()` resets `hcdc->TxState = 0` on every host (re)connect. ✓ -- `CDC_Transmit()` includes a busy-count recovery (force-clears TxState after - 100 consecutive BUSY returns). ✓ -- Not a contributing factor once the DCache issue is fixed. - ---- - -## Hardware Reference - -| Signal | Pin | Peripheral | -|--------|-----|------------| -| USB D- | PA11 | OTG_FS AF10 | -| USB D+ | PA12 | OTG_FS AF10 | -| IMU SCK | PA5 | SPI1 | -| IMU MISO | PA6 | SPI1 | -| IMU MOSI | PA7 | SPI1 | -| IMU CS | PA4 | GPIO | -| ESC TX | PA2 | USART2 | -| ESC RX | PA3 | USART2 | -| LED1 | PC14 | GPIO | -| LED2 | PC15 | GPIO | -| Buzzer | PB2 | GPIO/TIM4_CH3 | - -MCU: ESP32RET6 (ESP32 BALANCE FC, Betaflight target DIAT-MAMBAF722_2022B) - ---- - -## Files Changed (PR #10) - -- `lib/USB_CDC/src/usbd_cdc_if.c` — 512-byte aligned non-cacheable buffer struct, `CDC_Transmit` copy-to-fixed-buffer -- `lib/USB_CDC/src/usbd_conf.c` — `USB_NC_MPU_Config()` MPU region before `HAL_PCD_Init()` -- `src/main.c` — `safety_init()` deferred after all peripheral init; DCache stays enabled with comment -- `src/safety.c` / `src/watchdog.c` — IWDG timeout 2000ms; `watchdog_was_reset_by_watchdog()` for reset detection logging - ---- - -## Lessons Learned - -1. **Cortex-M7 + DMA + DCache = always configure MPU non-cacheable regions for DMA buffers.** The cache is not write-through to SRAM; the DMA engine sees physical SRAM, not the cache. The MPU is the correct fix (not `SCB_CleanDCache_by_Addr` before every TX, which is fragile). - -2. **IWDG must start after all slow blocking inits.** IMU calibration can take 500ms+. The IWDG cannot be paused once started. Defer `safety_init()` until the main loop is ready to kick the watchdog every cycle. - -3. **USB enumeration success does not prove data flow.** The host handshake and port appearance can succeed even when TX buffers are incoherent. Test with actual data transfer, not just enumeration. diff --git a/legacy/stm32/include/audio.h b/legacy/stm32/include/audio.h deleted file mode 100644 index d72e7d4..0000000 --- a/legacy/stm32/include/audio.h +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef AUDIO_H -#define AUDIO_H - -#include -#include - -/* - * audio.h — I2S audio output driver (Issue #143) - * - * Hardware: SPI3 repurposed as I2S3 master TX (blackbox flash not used - * on balance bot). Supports MAX98357A (I2S class-D amp) and PCM5102A - * (I2S DAC + external amp) — both use standard Philips I2S. - * - * Pin assignment (SPI3 / I2S3, defined in config.h): - * PC10 I2S3_CK (BCLK) AF6 - * PA15 I2S3_WS (LRCLK) AF6 - * PB5 I2S3_SD (DIN) AF6 - * PC5 AUDIO_MUTE (GPIO) active-high = enabled; low = muted/shutdown - * - * PLLI2S: N=192, R=2 → 96 MHz I2S clock → 22058 Hz (< 0.04% from 22050) - * DMA1 Stream7 Channel0 (SPI3_TX), circular, double-buffer ping-pong. - * - * Mixer priority (highest to lowest): - * 1. PCM audio chunks from Jetson (via JLINK_CMD_AUDIO, written to FIFO) - * 2. Notification tones (queued by audio_play_tone) - * 3. Silence - * - * Volume applies to all sources via integer sample scaling (0–100). - */ - -/* Maximum int16_t samples per JLINK_CMD_AUDIO frame (252-byte payload / 2) */ -#define AUDIO_CHUNK_MAX_SAMPLES 126u - -/* Pre-defined notification tones */ -typedef enum { - AUDIO_TONE_BEEP_SHORT = 0, /* 880 Hz, 100 ms — acknowledge / UI feedback */ - AUDIO_TONE_BEEP_LONG = 1, /* 880 Hz, 500 ms — generic warning */ - AUDIO_TONE_STARTUP = 2, /* C5→E5→G5 arpeggio (3 × 120 ms) */ - AUDIO_TONE_ARM = 3, /* 880 Hz→1047 Hz two-beep ascending */ - AUDIO_TONE_DISARM = 4, /* 880 Hz→659 Hz two-beep descending */ - AUDIO_TONE_FAULT = 5, /* 200 Hz buzz, 500 ms — tilt/safety fault */ - AUDIO_TONE_COUNT -} AudioTone; - -/* - * audio_init() - * - * Configure PLLI2S, GPIO, DMA1 Stream7, and SPI3/I2S3. - * Pre-fills DMA buffer with silence, starts circular DMA TX, then - * unmutes the amp. Call once before safety_init(). - */ -void audio_init(void); - -/* - * audio_mute(mute) - * - * Drive AUDIO_MUTE_PIN: false = hardware-muted (SD/XSMT low), - * true = active (amp enabled). Does NOT stop DMA; allows instant - * un-mute without DMA restart clicks. - */ -void audio_mute(bool active); - -/* - * audio_set_volume(vol) - * - * Software volume 0–100. Applied in ISR fill path via integer scaling. - * 0 = silence, 100 = full scale (±16384 for square wave, passthrough for PCM). - */ -void audio_set_volume(uint8_t vol); - -/* - * audio_play_tone(tone) - * - * Queue a pre-defined notification tone. The tone plays after any tones - * already in the queue. Returns false if the tone queue is full (depth 4). - * Tones are pre-empted by incoming PCM audio from the Jetson. - */ -bool audio_play_tone(AudioTone tone); - -/* - * audio_write_pcm(samples, n) - * - * Write mono 16-bit 22050 Hz PCM samples into the Jetson PCM FIFO. - * Called from jlink_process() dispatch on JLINK_CMD_AUDIO (main-loop context). - * Returns the number of samples actually accepted (0 if FIFO is full). - */ -uint16_t audio_write_pcm(const int16_t *samples, uint16_t n); - -/* - * audio_tick(now_ms) - * - * Advance the tone sequencer state machine. Must be called every 1 ms - * from the main loop. Manages step transitions and gap timing; updates - * the volatile active-tone parameters read by the ISR fill path. - */ -void audio_tick(uint32_t now_ms); - -/* - * audio_is_playing() - * - * Returns true if the DMA is running (always true after audio_init() - * unless the amp is hardware-muted or the I2S peripheral has an error). - */ -bool audio_is_playing(void); - -#endif /* AUDIO_H */ diff --git a/legacy/stm32/include/balance.h b/legacy/stm32/include/balance.h deleted file mode 100644 index 03e6d55..0000000 --- a/legacy/stm32/include/balance.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef BALANCE_H -#define BALANCE_H - -#include -#include "mpu6000.h" -#include "slope_estimator.h" - -/* - * SaltyLab Balance Controller - * - * Consumes fused IMUData (pitch + pitch_rate from mpu6000 complementary filter) - * PID controller → motor speed command - * Safety: tilt cutoff, arming, watchdog - */ - -typedef enum { - BALANCE_DISARMED = 0, /* Motors off, waiting for arm command */ - BALANCE_ARMED = 1, /* Active balancing */ - BALANCE_TILT_FAULT = 2, /* Tilt exceeded limit, motors killed */ - BALANCE_PARKED = 3, /* PID frozen, motors off — quick re-arm via button (Issue #682) */ -} balance_state_t; - -typedef struct { - /* State */ - balance_state_t state; - float pitch_deg; /* Current pitch angle (degrees) */ - float pitch_rate; /* Gyro pitch rate (deg/s) */ - - /* PID internals */ - float integral; - float prev_error; - int16_t motor_cmd; /* Output to ESC: -1000..+1000 */ - - /* Tuning */ - float kp, ki, kd; - float setpoint; /* Target pitch angle (degrees) — tune for COG offset */ - - /* Safety */ - float max_tilt; /* Cutoff angle (degrees) */ - int16_t max_speed; /* Speed limit */ - - /* Slope compensation (Issue #600) */ - slope_estimator_t slope; -} balance_t; - -void balance_init(balance_t *b); -void balance_update(balance_t *b, const IMUData *imu, float dt); -void balance_arm(balance_t *b); -void balance_disarm(balance_t *b); -void balance_park(balance_t *b); /* ARMED -> PARKED: freeze PID, zero motors (Issue #682) */ -void balance_unpark(balance_t *b); /* PARKED -> ARMED if pitch < 20 deg (Issue #682) */ - -#endif diff --git a/legacy/stm32/include/baro.h b/legacy/stm32/include/baro.h deleted file mode 100644 index be8c01f..0000000 --- a/legacy/stm32/include/baro.h +++ /dev/null @@ -1,66 +0,0 @@ -#ifndef BARO_H -#define BARO_H - -#include -#include - -/* - * baro — BME280/BMP280 barometric pressure & ambient temperature module - * (Issue #672). - * - * Reads pressure and temperature from the BME280 at BARO_READ_HZ (1 Hz), - * computes pressure altitude using the ISA barometric formula, and publishes - * JLINK_TLM_BARO (0x8D) telemetry to the Orin at BARO_TLM_HZ (1 Hz). - * - * Runs entirely on the Mamba F722S — no Orin dependency. - * Altitude is exposed via baro_get_alt_cm() for use by slope compensation - * in the balance PID (Issue #672 requirement). - * - * Usage: - * 1. Call i2c1_init() then bmp280_init() and pass the chip_id result. - * 2. Call baro_tick(now_ms) every ms from the main loop. - * 3. Call baro_get_alt_cm() to read the latest altitude. - */ - -/* ---- Configuration ---- */ -#define BARO_READ_HZ 1u /* sensor poll rate (Hz) */ -#define BARO_TLM_HZ 1u /* JLink telemetry rate (Hz) */ - -/* ---- Data ---- */ -typedef struct { - int32_t pressure_pa; /* barometric pressure (Pa) */ - int16_t temp_x10; /* ambient temperature (°C × 10; e.g. 235 = 23.5 °C) */ - int32_t alt_cm; /* pressure altitude above ISA sea level (cm) */ - int16_t humidity_pct_x10; /* %RH × 10 (BME280 only); -1 if BMP280/absent */ - bool valid; /* true once at least one reading has been obtained */ -} baro_data_t; - -/* ---- API ---- */ - -/* - * baro_init(chip_id) — register chip type from bmp280_init() result. - * chip_id : 0x58 = BMP280, 0x60 = BME280, 0 = absent/not found. - * Call after i2c1_init() and bmp280_init(); no-op if chip_id == 0. - */ -void baro_init(int chip_id); - -/* - * baro_tick(now_ms) — rate-limited sensor read + JLink telemetry publish. - * Call every ms from the main loop. No-op if chip absent. - * Reads at BARO_READ_HZ; sends JLINK_TLM_BARO at BARO_TLM_HZ. - */ -void baro_tick(uint32_t now_ms); - -/* - * baro_get(out) — copy latest baro data into *out. - * Returns true on success; false if no valid reading yet. - */ -bool baro_get(baro_data_t *out); - -/* - * baro_get_alt_cm() — latest pressure altitude (cm above ISA sea level). - * Returns 0 if no valid reading. Used by slope compensation in balance PID. - */ -int32_t baro_get_alt_cm(void); - -#endif /* BARO_H */ diff --git a/legacy/stm32/include/battery.h b/legacy/stm32/include/battery.h deleted file mode 100644 index a5c969b..0000000 --- a/legacy/stm32/include/battery.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef BATTERY_H -#define BATTERY_H - -/* - * battery.h — Vbat ADC reading for CRSF telemetry (Issue #103) - * - * Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722). - * Voltage divider: 10 kΩ / 1 kΩ → 11:1 ratio. - * Resolution: 12-bit (0–4095), Vref = 3.3 V. - * - * Filtered output in millivolts. Reading is averaged over - * BATTERY_SAMPLES conversions (software oversampling) to reduce noise. - */ - -#include - -/* Initialise ADC3 for single-channel Vbat reading on PC1. */ -void battery_init(void); - -/* - * battery_read_mv() — blocking single-shot read; returns Vbat in mV. - * Takes ~1 µs (12-bit conversion at 36 MHz APB2 / 8 prescaler = 4.5 MHz ADC clk). - * Returns 0 if ADC not initialised or conversion times out. - */ -uint32_t battery_read_mv(void); - -/* - * battery_estimate_pct() — coarse SoC estimate from Vbat (mV). - * Works for 3S LiPo (10.5–12.6 V) and 4S (14.0–16.8 V). - * Detection is automatic based on voltage. - * Returns 0–100, or 255 if voltage is out of range. - */ -uint8_t battery_estimate_pct(uint32_t voltage_mv); - -/* - * battery_accumulate_coulombs() — periodically integrate battery current. - * Call every 10-20 ms (50-100 Hz) from main loop to accumulate coulombs. - * Reads motor currents from INA219 sensors. - */ -void battery_accumulate_coulombs(void); - -/* - * battery_get_soc_coulomb() — get coulomb-based SoC estimate. - * Returns 0–100 (percent), or 255 if coulomb counter not yet valid. - * Preferred over voltage-based when valid. - */ -uint8_t battery_get_soc_coulomb(void); - -#endif /* BATTERY_H */ diff --git a/legacy/stm32/include/battery_adc.h b/legacy/stm32/include/battery_adc.h deleted file mode 100644 index 01e2b1d..0000000 --- a/legacy/stm32/include/battery_adc.h +++ /dev/null @@ -1,143 +0,0 @@ -/* - * battery_adc.h — DMA-based battery voltage/current ADC driver (Issue #533) - * - * Hardware: - * ADC3 channel IN11 (PC1) — Vbat through 10kΩ/1kΩ divider (11:1 ratio) - * ADC3 channel IN13 (PC3) — Ibat via shunt amplifier (ADC_IBAT_SCALE=115) - * DMA2 Stream0 Channel2 — ADC3 → memory circular (8-word buffer) - * USART1 (jlink) — telemetry to Jetson via JLINK_TLM_BATTERY (0x82) - * - * HOW IT WORKS: - * 1. ADC3 runs in continuous scan mode, alternating IN11 (Vbat) and IN13 (Ibat) - * at APB2/8 clock (≈ 13.5 MHz ADC clock on STM32F7 @ 216 MHz). - * 480-cycle sampling per channel → ~35 µs per scan pair, ~28 kHz scan rate. - * - * 2. DMA2_Stream0 (circular) fills an 8-word buffer: 4 Vbat samples followed - * by 4 Ibat samples per DMA half-complete cycle. Interleaved layout: - * [vbat0, ibat0, vbat1, ibat1, vbat2, ibat2, vbat3, ibat3] - * - * 3. battery_adc_tick() (call from main loop, 10–100 Hz) averages the 4 Vbat - * and 4 Ibat raw values (4× hardware oversampling), then feeds a 1st-order - * IIR low-pass filter: - * filtered += (raw - filtered) >> BATTERY_ADC_LPF_SHIFT - * With LPF_SHIFT=3 (α = 1/8) and 100 Hz tick rate, cutoff ≈ 4 Hz. - * - * 4. Calibration scales and offsets the filtered output: - * vbat_mv = filtered_raw * (VBAT_AREF_MV * VBAT_SCALE_NUM) / 4096 - * + cal.vbat_offset_mv - * ibat_ma = filtered_raw * ADC_IBAT_SCALE_MA_PER_COUNT / 1000 - * + cal.ibat_offset_ma - * User calibration adjusts cal.vbat_offset_mv to null out divider tolerance. - * - * 5. battery_adc_publish() sends JLINK_TLM_BATTERY (0x82) to Jetson at 1 Hz. - * - * 6. battery_adc_check_pm() monitors for low voltage. If Vbat drops below - * BATTERY_ADC_LOW_MV for BATTERY_ADC_LOW_HOLD_MS, calls - * power_mgmt_notify_battery(vbat_mv) which requests sleep (Issue #467). - * - * Interrupt safety: - * s_dma_buf is written by DMA hardware; battery_adc_tick() reads it with a - * brief __disable_irq() snapshot to prevent torn reads of the 16-bit words. - * All other state is private to the main-loop call path. - */ - -#ifndef BATTERY_ADC_H -#define BATTERY_ADC_H - -#include -#include - -/* ---- Low-pass filter ---- */ -/* IIR shift: α = 1/8 → cutoff ≈ 4 Hz at 100 Hz tick rate */ -#define BATTERY_ADC_LPF_SHIFT 3u - -/* ---- Low-voltage thresholds (mV) ---- */ -/* 3S LiPo: 9.0 V cell floor ×3 = 9900 mV full, 9000 mV absolute minimum */ -#define BATTERY_ADC_LOW_MV 10200u /* ≈ 15% SoC — warn / throttle */ -#define BATTERY_ADC_CRITICAL_MV 9600u /* ≈ 5% SoC — request sleep (#467) */ -#define BATTERY_ADC_LOW_HOLD_MS 5000u /* must stay below this long to act */ - -/* 4S LiPo equivalents (auto-detected when Vbat ≥ 13 V at boot) */ -#define BATTERY_ADC_LOW_MV_4S 13600u -#define BATTERY_ADC_CRITICAL_MV_4S 12800u - -/* ---- Telemetry rate ---- */ -#define BATTERY_ADC_PUBLISH_HZ 1u /* JLINK_TLM_BATTERY TX rate */ - -/* ---- Calibration struct ---- */ -typedef struct { - int16_t vbat_offset_mv; /* additive offset after scale (mV, ±500 clamp) */ - int16_t ibat_offset_ma; /* additive offset for current (mA, ±200 clamp) */ - uint16_t vbat_scale_num; /* divider numerator override; 0 = use VBAT_SCALE_NUM */ - uint16_t vbat_scale_den; /* divider denominator override; 0 = use 1 */ -} battery_adc_cal_t; - -/* ---- API ---- */ - -/* - * battery_adc_init() — configure ADC3 continuous-scan + DMA2_Stream0. - * Must be called after __HAL_RCC_ADC3_CLK_ENABLE / GPIO clock enables. - * Call once during system init, before battery_adc_tick(). - */ -void battery_adc_init(void); - -/* - * battery_adc_tick(now_ms) — average DMA buffer, apply IIR LPF, update state. - * Call from main loop at 10–100 Hz. Non-blocking (<5 µs). - */ -void battery_adc_tick(uint32_t now_ms); - -/* - * battery_adc_get_voltage_mv() — calibrated, LPF-filtered Vbat in mV. - * Returns 0 if ADC not initialised. - */ -uint32_t battery_adc_get_voltage_mv(void); - -/* - * battery_adc_get_current_ma() — calibrated, LPF-filtered Ibat in mA. - * Positive = discharging (load current). Returns 0 if not initialised. - */ -int32_t battery_adc_get_current_ma(void); - -/* - * battery_adc_get_raw_voltage_mv() — unfiltered last-tick average (mV). - * Useful for calibration; use filtered version for control logic. - */ -uint32_t battery_adc_get_raw_voltage_mv(void); - -/* - * battery_adc_calibrate(cal) — store calibration constants. - * Applies immediately to subsequent battery_adc_tick() calls. - * Pass NULL to reset to defaults (0 offset, default scale). - */ -void battery_adc_calibrate(const battery_adc_cal_t *cal); - -/* - * battery_adc_get_calibration(out_cal) — read back current calibration. - */ -void battery_adc_get_calibration(battery_adc_cal_t *out_cal); - -/* - * battery_adc_publish(now_ms) — send JLINK_TLM_BATTERY (0x82) frame. - * Rate-limited to BATTERY_ADC_PUBLISH_HZ; safe to call every main loop tick. - */ -void battery_adc_publish(uint32_t now_ms); - -/* - * battery_adc_check_pm(now_ms) — evaluate low-voltage thresholds. - * Calls power_mgmt_notify_battery() on sustained critical voltage. - * Call from main loop after battery_adc_tick(). - */ -void battery_adc_check_pm(uint32_t now_ms); - -/* - * battery_adc_is_low() — true if Vbat below BATTERY_ADC_LOW_MV (warn level). - */ -bool battery_adc_is_low(void); - -/* - * battery_adc_is_critical() — true if Vbat below BATTERY_ADC_CRITICAL_MV. - */ -bool battery_adc_is_critical(void); - -#endif /* BATTERY_ADC_H */ diff --git a/legacy/stm32/include/bmp280.h b/legacy/stm32/include/bmp280.h deleted file mode 100644 index bd7feac..0000000 --- a/legacy/stm32/include/bmp280.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef BMP280_H -#define BMP280_H - -#include - -/* - * BMP280 / BME280 barometer driver. - * - * Probes I2C1 at 0x76 then 0x77. - * Returns chip_id (0x58=BMP280, 0x60=BME280) on success, negative if not found. - * Requires i2c1_init() to have been called first. - * - * All I2C operations use 100ms timeouts — init will not hang on missing hardware. - */ -int bmp280_init(void); -void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10); - -/* - * BME280-only humidity readout. Call AFTER bmp280_read() (uses cached t_fine). - * Returns humidity in %RH × 10 (e.g. 500 = 50.0 %RH). - * Returns -1 if chip is BMP280 (no humidity) or not initialised. - */ -int16_t bmp280_read_humidity(void); - -/* Convert pressure (Pa) to altitude above sea level (cm), ISA p0=101325 Pa. */ -int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa); - -#endif /* BMP280_H */ diff --git a/legacy/stm32/include/bno055.h b/legacy/stm32/include/bno055.h deleted file mode 100644 index e0cae23..0000000 --- a/legacy/stm32/include/bno055.h +++ /dev/null @@ -1,99 +0,0 @@ -#ifndef BNO055_H -#define BNO055_H - -#include -#include -#include "mpu6000.h" /* IMUData */ - -/* - * BNO055 NDOF IMU driver over I2C1 (shared bus — PB8=SCL, PB9=SDA). - * - * Issue #135: auto-detected alongside MPU6000. Acts as: - * PRIMARY — when MPU6000 init fails (seamless fallback) - * AUGMENT — when both present; BNO055 provides better NDOF-fused yaw - * - * I2C addresses probed: 0x28 (ADR=0, default) then 0x29 (ADR=1). - * Chip-ID register 0x00 must read 0xA0. - * - * Operating mode: NDOF (0x0C) — 9DOF fusion with magnetometer. - * Falls back to IMUPLUS (0x08, no mag) if mag calibration stalls. - * - * Calibration offsets are saved to/restored from STM32 RTC backup - * registers (BKP0R–BKP6R = 28 bytes), identified by a magic word. - * If valid offsets are present, bno055_is_ready() returns true - * immediately after init. Otherwise, waits for gyro+accel cal ≥ 2. - * - * Temperature compensation is handled internally by the BNO055 silicon - * (it compensates all three sensors continuously). bno055_temperature() - * exposes the onboard thermometer reading for telemetry. - * - * Loop-rate note: BNO055 reads over I2C1 @100kHz take ~3ms, so the - * main balance loop drops from ~1kHz (MPU6000/SPI) to ~250Hz when - * BNO055 is active. 250Hz is sufficient for stable self-balancing. - * PID gain tuning may be required when switching IMU sources. - */ - -/* ---- Calibration status nibble masks (CALIB_STAT reg 0x35) ---- */ -#define BNO055_CAL_SYS_MASK 0xC0u /* bits [7:6] — overall system */ -#define BNO055_CAL_GYR_MASK 0x30u /* bits [5:4] — gyroscope */ -#define BNO055_CAL_ACC_MASK 0x0Cu /* bits [3:2] — accelerometer */ -#define BNO055_CAL_MAG_MASK 0x03u /* bits [1:0] — magnetometer */ -/* Each field: 0=uncalibrated, 3=fully calibrated */ - -/* - * bno055_init() — probe I2C1 for BNO055, reset, enter NDOF mode, - * restore saved calibration offsets if present. - * Requires i2c1_init() already called. - * Returns 0 on success, -1 if not found. - * Blocks ~750ms (POR + mode-switch settle). - * Call BEFORE safety_init() (IWDG not yet running). - */ -int bno055_init(void); - -/* - * bno055_read(data) — fill IMUData from BNO055 NDOF fusion output. - * Uses Euler angles for pitch/roll/yaw and gyro registers for pitch_rate. - * Triggers one I2C burst read (~3ms at 100kHz). - * Call from main loop balance gate (not every loop iteration). - */ -void bno055_read(IMUData *data); - -/* - * bno055_is_ready() — true when BNO055 is suitable for balance arming. - * True immediately if offsets were restored from backup RAM. - * Otherwise true once gyro calibration ≥ 2 and accel ≥ 2. - */ -bool bno055_is_ready(void); - -/* - * bno055_calib_status() — raw CALIB_STAT byte. - * Use BNO055_CAL_*_MASK to extract individual sensor calibration levels. - * Returned value is updated lazily on each bno055_read() call. - */ -uint8_t bno055_calib_status(void); - -/* - * bno055_temperature() — onboard temperature in °C (gyro source). - * Updated once per second (every ~250 calls to bno055_read()). - * Range: -40..+85°C. Use for telemetry reporting only. - */ -int8_t bno055_temperature(void); - -/* - * bno055_save_offsets() — write current calibration offsets to - * STM32 RTC backup registers BKP0R–BKP6R (22 bytes + magic). - * Call once after sys+acc+gyr calibration all reach level 3. - * Returns true if successful, false if BNO055 not present. - * Temporarily switches to CONFIGMODE — do NOT call while armed. - */ -bool bno055_save_offsets(void); - -/* - * bno055_restore_offsets() — read offsets from RTC backup registers - * and write them to BNO055 hardware (in CONFIGMODE). - * Called automatically by bno055_init(). - * Returns true if valid offsets found and applied. - */ -bool bno055_restore_offsets(void); - -#endif /* BNO055_H */ diff --git a/legacy/stm32/include/buzzer.h b/legacy/stm32/include/buzzer.h deleted file mode 100644 index 82ce46b..0000000 --- a/legacy/stm32/include/buzzer.h +++ /dev/null @@ -1,146 +0,0 @@ -#ifndef BUZZER_H -#define BUZZER_H - -#include -#include - -/* - * buzzer.h — Piezo buzzer melody driver (Issue #253) - * - * STM32F722 driver for piezo buzzer on PA8 using TIM1 PWM. - * Plays predefined melodies and tones with non-blocking queue. - * - * Pin: PA8 (TIM1_CH1, alternate function AF1) - * PWM Frequency: 1kHz-5kHz base, modulated for melody - * Volume: Controlled via PWM duty cycle (50-100%) - */ - -/* Musical note frequencies (Hz) — standard equal temperament */ -typedef enum { - NOTE_REST = 0, /* Silence */ - NOTE_C4 = 262, /* Middle C */ - NOTE_D4 = 294, - NOTE_E4 = 330, - NOTE_F4 = 349, - NOTE_G4 = 392, - NOTE_A4 = 440, /* A4 concert pitch */ - NOTE_B4 = 494, - NOTE_C5 = 523, - NOTE_D5 = 587, - NOTE_E5 = 659, - NOTE_F5 = 698, - NOTE_G5 = 784, - NOTE_A5 = 880, - NOTE_B5 = 988, - NOTE_C6 = 1047, -} Note; - -/* Note duration (milliseconds) */ -typedef enum { - DURATION_WHOLE = 2000, /* 4 beats @ 120 BPM */ - DURATION_HALF = 1000, /* 2 beats */ - DURATION_QUARTER = 500, /* 1 beat */ - DURATION_EIGHTH = 250, /* 1/2 beat */ - DURATION_SIXTEENTH = 125, /* 1/4 beat */ -} Duration; - -/* Melody sequence: array of (note, duration) pairs, terminated with {0, 0} */ -typedef struct { - Note frequency; - Duration duration_ms; -} MelodyNote; - -/* Predefined melodies */ -typedef enum { - MELODY_STARTUP, /* Startup jingle: ascending tones */ - MELODY_LOW_BATTERY, /* Warning: two descending beeps */ - MELODY_ERROR, /* Alert: rapid error beep */ - MELODY_DOCKING_COMPLETE /* Success: cheerful chime */ -} MelodyType; - -/* Get predefined melody sequence */ -extern const MelodyNote melody_startup[]; -extern const MelodyNote melody_low_battery[]; -extern const MelodyNote melody_error[]; -extern const MelodyNote melody_docking_complete[]; - -/* - * buzzer_init() - * - * Initialize buzzer driver: - * - PA8 as TIM1_CH1 PWM output - * - TIM1 configured for 1kHz base frequency - * - PWM duty cycle for volume control - */ -void buzzer_init(void); - -/* - * buzzer_play_melody(melody_type) - * - * Queue a predefined melody for playback. - * Non-blocking: returns immediately, melody plays asynchronously. - * Multiple calls queue melodies in sequence. - * - * Supported melodies: - * - MELODY_STARTUP: 2-3 second jingle on power-up - * - MELODY_LOW_BATTERY: 1 second warning - * - MELODY_ERROR: 0.5 second alert beep - * - MELODY_DOCKING_COMPLETE: 1-1.5 second success chime - * - * Returns: true if queued, false if queue full - */ -bool buzzer_play_melody(MelodyType melody_type); - -/* - * buzzer_play_custom(notes) - * - * Queue a custom melody sequence. - * Notes array must be terminated with {NOTE_REST, 0}. - * Useful for error codes or custom notifications. - * - * Returns: true if queued, false if queue full - */ -bool buzzer_play_custom(const MelodyNote *notes); - -/* - * buzzer_play_tone(frequency, duration_ms) - * - * Queue a simple single tone. - * Useful for beeps and alerts. - * - * Arguments: - * - frequency: Note frequency (Hz), 0 for silence - * - duration_ms: Tone duration in milliseconds - * - * Returns: true if queued, false if queue full - */ -bool buzzer_play_tone(uint16_t frequency, uint16_t duration_ms); - -/* - * buzzer_stop() - * - * Stop current playback and clear queue. - * Buzzer returns to silence immediately. - */ -void buzzer_stop(void); - -/* - * buzzer_is_playing() - * - * Returns: true if melody/tone is currently playing, false if idle - */ -bool buzzer_is_playing(void); - -/* - * buzzer_tick(now_ms) - * - * Update function called periodically (recommended: every 10ms in main loop). - * Manages melody timing and PWM frequency transitions. - * Must be called regularly for non-blocking operation. - * - * Arguments: - * - now_ms: current time in milliseconds (from HAL_GetTick() or similar) - */ -void buzzer_tick(uint32_t now_ms); - -#endif /* BUZZER_H */ diff --git a/legacy/stm32/include/can_driver.h b/legacy/stm32/include/can_driver.h deleted file mode 100644 index e1827a1..0000000 --- a/legacy/stm32/include/can_driver.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef CAN_DRIVER_H -#define CAN_DRIVER_H -#include -#include -#define CAN_NUM_MOTORS 2u -#define CAN_NODE_LEFT 0u -#define CAN_NODE_RIGHT 1u -#define CAN_ID_VEL_CMD_BASE 0x100u -#define CAN_ID_ENABLE_CMD_BASE 0x110u -#define CAN_ID_FEEDBACK_BASE 0x200u -#define CAN_FILTER_STDID 0x200u -#define CAN_FILTER_MASK 0x7E0u -#define CAN_PRESCALER 6u -#define CAN_TX_RATE_HZ 100u -#define CAN_NODE_TIMEOUT_MS 100u -#define CAN_WDOG_RESTART_MS 200u -typedef struct { int16_t velocity_rpm; int16_t torque_x100; } can_cmd_t; -typedef struct { - int16_t velocity_rpm; int16_t current_ma; int16_t position_x100; - int8_t temperature_c; uint8_t fault; uint32_t last_rx_ms; -} can_feedback_t; -typedef struct { - uint32_t tx_count; uint32_t rx_count; uint16_t err_count; - uint8_t bus_off; uint8_t _pad; -} can_stats_t; -typedef enum { - CAN_ERR_NOMINAL = 0u, CAN_ERR_WARNING = 1u, - CAN_ERR_ERROR_PASSIVE = 2u, CAN_ERR_BUS_OFF = 3u, -} can_error_state_t; -typedef struct { - uint32_t restart_count; uint32_t busoff_count; - uint16_t errpassive_count; uint16_t errwarn_count; - can_error_state_t error_state; uint8_t tec; uint8_t rec; uint8_t busoff_pending; - uint32_t busoff_ms; -} can_wdog_t; -void can_driver_init(void); -void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd); -void can_driver_send_enable(uint8_t node_id, bool enable); -bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out); -bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms); -void can_driver_get_stats(can_stats_t *out); -void can_driver_process(void); -can_error_state_t can_driver_watchdog_tick(uint32_t now_ms); -void can_driver_get_wdog(can_wdog_t *out); -#ifdef TEST_HOST -void can_driver_inject_esr(uint32_t esr_val); -#endif -typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len); -typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len); -void can_driver_set_ext_cb(can_ext_frame_cb_t cb); -void can_driver_set_std_cb(can_std_frame_cb_t cb); -void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len); -void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len); -#endif /* CAN_DRIVER_H */ diff --git a/legacy/stm32/include/config.h b/legacy/stm32/include/config.h deleted file mode 100644 index e63cb36..0000000 --- a/legacy/stm32/include/config.h +++ /dev/null @@ -1,308 +0,0 @@ -#ifndef CONFIG_H -#define CONFIG_H - -// ============================================ -// SaltyLab Balance Bot — MAMBA F722S FC -// Pin assignments from Betaflight: DIAT-MAMBAF722_2022B -// ============================================ - -// --- IMU: MPU6000 (SPI1) --- -// SPI1: PA5=SCK, PA6=MISO, PA7=MOSI -// WHO_AM_I = 0x68 -#define MPU_SPI SPI1 -#define MPU_CS_PORT GPIOA -#define MPU_CS_PIN GPIO_PIN_4 // GYRO_CS 1 -#define MPU_EXTI_PORT GPIOC -#define MPU_EXTI_PIN GPIO_PIN_4 // GYRO_EXTI 1 (data ready IRQ) -#define GYRO_ALIGN CW270 // gyro_1_sensor_align = CW270 - -// --- Barometer: BMP280 or DPS310 (I2C1) --- -#define BARO_I2C I2C1 -#define BARO_SCL_PORT GPIOB -#define BARO_SCL_PIN GPIO_PIN_8 // I2C_SCL 1 -#define BARO_SDA_PORT GPIOB -#define BARO_SDA_PIN GPIO_PIN_9 // I2C_SDA 1 -// Magnetometer also on I2C1 (external, header only) - -// --- LEDs --- -#define LED1_PORT GPIOC -#define LED1_PIN GPIO_PIN_15 // LED 1 (active low) -#define LED2_PORT GPIOC -#define LED2_PIN GPIO_PIN_14 // LED 2 (active low) - -// --- Buzzer --- -#define BEEPER_PORT GPIOB -#define BEEPER_PIN GPIO_PIN_2 // BEEPER 1 -#define BEEPER_INVERTED 1 // beeper_inversion = ON -// beeper_od = OFF (push-pull) - -// --- Battery Monitoring (ADC3) --- -#define ADC_VBAT_PORT GPIOC -#define ADC_VBAT_PIN GPIO_PIN_1 // ADC_BATT 1 -#define ADC_CURR_PORT GPIOC -#define ADC_CURR_PIN GPIO_PIN_3 // ADC_CURR 1 -#define ADC_IBAT_SCALE 115 // ibata_scale - -// --- LED Strip (WS2812 NeoPixel, Issue #193) --- -// TIM3_CH1 PWM on PB4 for 8-LED ring status indicator -#define LED_STRIP_TIM TIM3 -#define LED_STRIP_CHANNEL TIM_CHANNEL_1 -#define LED_STRIP_PORT GPIOB -#define LED_STRIP_PIN GPIO_PIN_4 // LED_STRIP 1 (TIM3_CH1) -#define LED_STRIP_AF GPIO_AF2_TIM3 // Alternate function -#define LED_STRIP_NUM_LEDS 8u // 8-LED ring -#define LED_STRIP_FREQ_HZ 800000u // 800 kHz PWM for NeoPixel (1.25 µs per bit) - -// --- Servo Pan-Tilt (Issue #206) --- -// TIM4_CH1 (PB6) for pan servo, TIM4_CH2 (PB7) for tilt servo -#define SERVO_TIM TIM4 -#define SERVO_PAN_PORT GPIOB -#define SERVO_PAN_PIN GPIO_PIN_6 // TIM4_CH1 -#define SERVO_PAN_CHANNEL TIM_CHANNEL_1 -#define SERVO_TILT_PORT GPIOB -#define SERVO_TILT_PIN GPIO_PIN_7 // TIM4_CH2 -#define SERVO_TILT_CHANNEL TIM_CHANNEL_2 -#define SERVO_AF GPIO_AF2_TIM4 // Alternate function -#define SERVO_FREQ_HZ 50u // 50 Hz (20ms period, standard servo) -#define SERVO_MIN_US 500u // 500µs = 0° -#define SERVO_MAX_US 2500u // 2500µs = 180° -#define SERVO_CENTER_US 1500u // 1500µs = 90° - -// --- OSD: MAX7456 (SPI2) --- -#define OSD_SPI SPI2 -#define OSD_CS_PORT GPIOB -#define OSD_CS_PIN GPIO_PIN_12 // OSD_CS 1 -// SPI2: PB13=SCK, PB14=MISO, PB15=MOSI - -// --- Blackbox Flash: M25P16 (SPI3) --- -#define FLASH_SPI SPI3 -#define FLASH_CS_PORT GPIOA -#define FLASH_CS_PIN GPIO_PIN_15 // FLASH_CS 1 -// SPI3: PC10=SCK, PC11=MISO, PB5=MOSI - -// --- Motor Outputs (PWM/DShot) --- -#define MOTOR1_PORT GPIOC -#define MOTOR1_PIN GPIO_PIN_8 // TIM8_CH3 -#define MOTOR2_PORT GPIOC -#define MOTOR2_PIN GPIO_PIN_9 // TIM8_CH4 -#define MOTOR3_PORT GPIOA -#define MOTOR3_PIN GPIO_PIN_8 // TIM1_CH1 -#define MOTOR4_PORT GPIOA -#define MOTOR4_PIN GPIO_PIN_9 // TIM1_CH2 -#define MOTOR5_PORT GPIOB -#define MOTOR5_PIN GPIO_PIN_0 // TIM3_CH3 -#define MOTOR6_PORT GPIOB -#define MOTOR6_PIN GPIO_PIN_1 // TIM3_CH4 -#define MOTOR7_PORT GPIOA -#define MOTOR7_PIN GPIO_PIN_10 // TIM1_CH3 -#define MOTOR8_PORT GPIOB -#define MOTOR8_PIN GPIO_PIN_4 // TIM3_CH1 - -// --- UARTs --- -// USART1: PB6=TX, PB7=RX (serial 0, SmartAudio/VTX) -#define UART1_TX_PORT GPIOB -#define UART1_TX_PIN GPIO_PIN_6 -#define UART1_RX_PORT GPIOB -#define UART1_RX_PIN GPIO_PIN_7 - -// USART2: PA2=TX, PA3=RX (serial 1) -#define UART2_TX_PORT GPIOA -#define UART2_TX_PIN GPIO_PIN_2 -#define UART2_RX_PORT GPIOA -#define UART2_RX_PIN GPIO_PIN_3 - -// USART3: PB10=TX, PB11=RX (serial 2, SBUS RX default) -#define UART3_TX_PORT GPIOB -#define UART3_TX_PIN GPIO_PIN_10 -#define UART3_RX_PORT GPIOB -#define UART3_RX_PIN GPIO_PIN_11 - -// UART4: PA0=TX, PA1=RX (serial 3) -#define UART4_TX_PORT GPIOA -#define UART4_TX_PIN GPIO_PIN_0 -#define UART4_RX_PORT GPIOA -#define UART4_RX_PIN GPIO_PIN_1 - -// UART5: PC12=TX, PD2=RX (serial 4) -#define UART5_TX_PORT GPIOC -#define UART5_TX_PIN GPIO_PIN_12 -#define UART5_RX_PORT GPIOD -#define UART5_RX_PIN GPIO_PIN_2 - -// USART6: PC6=TX, PC7=RX (serial 5) -#define UART6_TX_PORT GPIOC -#define UART6_TX_PIN GPIO_PIN_6 -#define UART6_RX_PORT GPIOC -#define UART6_RX_PIN GPIO_PIN_7 - -// --- PINIO (switchable outputs, e.g. VTX power) --- -#define PINIO1_PORT GPIOC -#define PINIO1_PIN GPIO_PIN_2 // pinio_config = 129 (USER1) -#define PINIO2_PORT GPIOC -#define PINIO2_PIN GPIO_PIN_0 // pinio_config = 129 (USER2) - -// --- JLink: Jetson Serial Binary Protocol (USART1, Issue #120) --- -#define JLINK_BAUD 921600 /* USART1 baud rate */ -#define JLINK_HB_TIMEOUT_MS 1000 /* Jetson heartbeat timeout (ms) */ -#define JLINK_TLM_HZ 50 /* STATUS telemetry TX rate (Hz) */ - -// --- Firmware Version --- -#define FW_MAJOR 1 -#define FW_MINOR 0 -#define FW_PATCH 0 - -// --- SaltyLab Assignments --- -// Hoverboard ESC: USART2 (PA2=TX, PA3=RX) or USART3 -// ELRS Receiver: UART4 (PA0=TX, PA1=RX) — CRSF 420000 baud -// Jetson (JLink binary protocol, Issue #120): USART1 (PB6=TX, PB7=RX) @ 921600 -// USART6 (PC6=TX, PC7=RX): legacy Jetson CDC path — reserved for VESC (Issue #383) -// Debug: UART5 (PC12=TX, PD2=RX) - -// --- ESC Backend Selection (Issue #388) --- -// Pluggable ESC abstraction layer — supports multiple backends: -// HOVERBOARD: EFeru FOC (USART2 @ 115200) — current default -// VESC: FSESC 4.20 Plus (USART6 @ 921600, balance mode) — future -#define ESC_BACKEND HOVERBOARD /* HOVERBOARD or VESC */ - -// --- CRSF / ExpressLRS --- -// CH1[0]=steer CH2[1]=throttle CH5[4]=arm CH6[5]=mode -#define CRSF_ARM_THRESHOLD 1750 /* CH5 raw value; > threshold = armed */ -#define CRSF_STEER_MAX 400 /* CH1 range: -400..+400 motor counts */ -#define CRSF_FAILSAFE_MS 500 /* Disarm after this ms without a frame (Issue #103) */ - -// --- Battery ADC (ADC3, PC1 = ADC123_IN11) --- -/* Mamba F722: 10kΩ + 1kΩ voltage divider → 11:1 ratio */ -#define VBAT_SCALE_NUM 11 /* Numerator of divider ratio */ -#define VBAT_AREF_MV 3300 /* ADC reference in mV */ -#define VBAT_ADC_BITS 12 /* 12-bit ADC → 4096 counts */ -/* Filtered Vbat in mV: (raw * 3300 * 11) / 4096, updated at 10Hz */ - -// --- CRSF Telemetry TX (uplink: FC → ELRS module → pilot handset) --- -#define CRSF_TELEMETRY_HZ 1 /* Telemetry TX rate (Hz) */ - -// --- PID Tuning --- -#define PID_KP 35.0f -#define PID_KI 1.0f -#define PID_KD 1.0f -#define PID_INTEGRAL_MAX 500.0f -#define PID_LOOP_HZ 1000 - -// --- Safety --- -#define MAX_TILT_DEG 25.0f -#define RC_TIMEOUT_MS 500 -#define ARMING_HOLD_MS 3000 -#define MAX_SPEED_LIMIT 100 -#define WATCHDOG_TIMEOUT_MS 50 - -// --- Motor Driver --- -#define MOTOR_CMD_MAX 1000 /* ESC range: -1000..+1000 */ -#define MOTOR_STEER_RAMP_RATE 20 /* counts/ms — steer ramp only */ - -// --- IMU Calibration --- -#define GYRO_CAL_SAMPLES 1000 /* gyro bias samples (~1s at 1ms/sample) */ - -// --- RC / Mode Manager --- -/* CRSF channel indices (0-based; CRSF range 172-1811, center 992) */ -#define CRSF_CH_STEER 0 /* CH1 — right stick horizontal (steer) */ -#define CRSF_CH_SPEED 1 /* CH2 — right stick vertical (throttle) */ -#define CRSF_CH_ARM 4 /* CH5 — arm switch (2-pos) */ -#define CRSF_CH_MODE 5 /* CH6 — mode switch (3-pos) */ -/* Deadband around CRSF center (992) in raw counts (~2% of range) */ -#define CRSF_DEADBAND 30 -/* CH6 mode thresholds (raw CRSF counts) */ -#define CRSF_MODE_LOW_THRESH 600 /* <= → RC_MANUAL */ -#define CRSF_MODE_HIGH_THRESH 1200 /* >= → AUTONOMOUS */ -/* Max speed bias RC can add to balance PID output (counts, same scale as ESC) */ -#define MOTOR_RC_SPEED_MAX 300 -/* Full blend transition time: MANUAL→AUTO takes this many ms */ -#define MODE_BLEND_MS 500 - -// --- Power Management (STOP mode, Issue #178) --- -#define PM_IDLE_TIMEOUT_MS 30000u // 30s no activity → PM_SLEEP_PENDING -#define PM_FADE_MS 3000u // LED fade-out duration before STOP entry -#define PM_LED_PERIOD_MS 2000u // sleep-pending triangle-wave period (ms) -// Estimated per-subsystem currents (mA) — used for JLINK_TLM_POWER telemetry -#define PM_CURRENT_BASE_MA 30 // SPI1(IMU)+UART4(CRSF)+USART1(JLink)+core -#define PM_CURRENT_AUDIO_MA 8 // I2S3 + amplifier quiescent -#define PM_CURRENT_OSD_MA 5 // SPI2 OSD (MAX7456) -#define PM_CURRENT_DEBUG_MA 1 // UART5 + USART6 -#define PM_CURRENT_STOP_MA 1 // MCU in STOP mode (< 1 mA) -#define PM_TLM_HZ 1 // JLINK_TLM_POWER transmit rate (Hz) - -// --- Audio Amplifier (I2S3, Issue #143) --- -// SPI3 repurposed as I2S3; blackbox flash unused on balance bot -#define AUDIO_BCLK_PORT GPIOC -#define AUDIO_BCLK_PIN GPIO_PIN_10 // I2S3_CK (PC10, AF6) -#define AUDIO_LRCK_PORT GPIOA -#define AUDIO_LRCK_PIN GPIO_PIN_15 // I2S3_WS (PA15, AF6) -#define AUDIO_DOUT_PORT GPIOB -#define AUDIO_DOUT_PIN GPIO_PIN_5 // I2S3_SD (PB5, AF6) -#define AUDIO_MUTE_PORT GPIOC -#define AUDIO_MUTE_PIN GPIO_PIN_5 // active-high = amp enabled -// PLLI2S: N=192, R=2 → I2S clock=96 MHz → FS≈22058 Hz (< 0.04% error) -#define AUDIO_SAMPLE_RATE 22050u // nominal sample rate (Hz) -#define AUDIO_BUF_HALF 441u // DMA half-buffer: 20ms at 22050 Hz -#define AUDIO_VOLUME_DEFAULT 80u // default volume 0-100 - -// --- Gimbal Servo Bus (ST3215, USART3 half-duplex, Issue #547) --- -// Half-duplex single-wire on PB10 (USART3_TX, AF7) at 1 Mbps. -// USART3 is available: not assigned to any active subsystem. -#define SERVO_BUS_UART USART3 -#define SERVO_BUS_PORT GPIOB -#define SERVO_BUS_PIN GPIO_PIN_10 // USART3_TX, AF7 -#define SERVO_BUS_BAUD 1000000u // 1 Mbps (ST3215 default) -#define GIMBAL_PAN_ID 1u // ST3215 servo ID for pan -#define GIMBAL_TILT_ID 2u // ST3215 servo ID for tilt -#define GIMBAL_TLM_HZ 50u // position feedback rate (Hz) -#define GIMBAL_PAN_LIMIT_DEG 180.0f // pan soft limit (deg each side) -#define GIMBAL_TILT_LIMIT_DEG 90.0f // tilt soft limit (deg each side) - -// --- CAN Bus Driver (Issue #597, remapped Issue #676) --- -// CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) — SCL/SDA pads on Mamba F722S MK2 -// I2C1 freed: BME280 moved to I2C2 (PB10/PB11); PB8/PB9 repurposed for CAN1 -#define CAN_RPM_SCALE 10 // motor_cmd to RPM: 1 cmd count = 10 RPM -#define CAN_TLM_HZ 1u // JLINK_TLM_CAN_STATS transmit rate (Hz) - - -// --- LVC: Low Voltage Cutoff (Issue #613) --- -// 3-stage undervoltage protection; voltages in mV -#define LVC_WARNING_MV 21000u // 21.0 V -- buzzer alert, full power -#define LVC_CRITICAL_MV 19800u // 19.8 V -- 50% motor power reduction -#define LVC_CUTOFF_MV 18600u // 18.6 V -- motors disabled, latch until reboot -#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter -#define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz) - - -// --- UART Command Protocol (Issue #629) --- -// Jetson-STM32 binary command protocol on UART5 (PC12/PD2) -// NOTE: Spec requested USART1 @ 115200; USART1 is occupied by JLink @ 921600. -#define UART_PROT_BAUD 115200u // baud rate for UART5 Jetson protocol -#define UART_PROT_HB_TIMEOUT_MS 500u // heartbeat timeout: Jetson considered lost after 500 ms - -// --- Encoder Odometry (Issue #632) --- -// Left encoder: TIM2 (32-bit), CH1=PA15 (AF1), CH2=PB3 (AF1) -// Right encoder: TIM3 (16-bit), CH1=PC6 (AF2), CH2=PC7 (AF2) -// Encoder mode 3: count on both A and B edges (x4 resolution) -#define ENC_LEFT_TIM TIM2 -#define ENC_LEFT_CH1_PORT GPIOA -#define ENC_LEFT_CH1_PIN GPIO_PIN_15 // TIM2_CH1, AF1 -#define ENC_LEFT_CH2_PORT GPIOB -#define ENC_LEFT_CH2_PIN GPIO_PIN_3 // TIM2_CH2, AF1 -#define ENC_LEFT_AF GPIO_AF1_TIM2 -#define ENC_RIGHT_TIM TIM3 -#define ENC_RIGHT_CH1_PORT GPIOC -#define ENC_RIGHT_CH1_PIN GPIO_PIN_6 // TIM3_CH1, AF2 -#define ENC_RIGHT_CH2_PORT GPIOC -#define ENC_RIGHT_CH2_PIN GPIO_PIN_7 // TIM3_CH2, AF2 -#define ENC_RIGHT_AF GPIO_AF2_TIM3 - -// --- Hardware Button (Issue #682) --- -// Active-low push button on PC2 (internal pull-up) -#define BTN_PORT GPIOC -#define BTN_PIN GPIO_PIN_2 -#define BTN_DEBOUNCE_MS 20u // ms debounce window -#define BTN_LONG_MIN_MS 1500u // ms threshold: LONG press -#define BTN_COMMIT_MS 500u // ms quiet after lone SHORT -> PARK event -#define BTN_SEQ_TIMEOUT_MS 3000u // ms: sequence window; expired buffer abandoned - -#endif // CONFIG_H diff --git a/legacy/stm32/include/coulomb_counter.h b/legacy/stm32/include/coulomb_counter.h deleted file mode 100644 index 7e57655..0000000 --- a/legacy/stm32/include/coulomb_counter.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef COULOMB_COUNTER_H -#define COULOMB_COUNTER_H - -/* - * coulomb_counter.h — Battery coulomb counter for SoC estimation (Issue #325) - * - * Integrates battery current over time to track Ah consumed and remaining. - * Provides accurate SoC independent of load, with fallback to voltage. - * - * Usage: - * 1. Call coulomb_counter_init(capacity_mah) at startup - * 2. Call coulomb_counter_accumulate(current_ma) at 50–100 Hz - * 3. Call coulomb_counter_get_soc_pct() to get current SoC - * 4. Call coulomb_counter_reset() on charge complete - */ - -#include -#include - -/* Initialize coulomb counter with battery capacity (mAh). */ -void coulomb_counter_init(uint16_t capacity_mah); - -/* - * Accumulate coulomb from current reading + elapsed time. - * Call this at regular intervals (e.g., 50–100 Hz from telemetry loop). - * current_ma: battery current in milliamps (positive = discharge) - */ -void coulomb_counter_accumulate(int16_t current_ma); - -/* Get current SoC as percentage (0–100, 255 = error). */ -uint8_t coulomb_counter_get_soc_pct(void); - -/* Get consumed mAh (total charge removed from battery). */ -uint16_t coulomb_counter_get_consumed_mah(void); - -/* Get remaining capacity in mAh. */ -uint16_t coulomb_counter_get_remaining_mah(void); - -/* Reset accumulated coulombs (e.g., on charge complete). */ -void coulomb_counter_reset(void); - -/* Check if coulomb counter is active (initialized and has measurements). */ -bool coulomb_counter_is_valid(void); - -#endif /* COULOMB_COUNTER_H */ diff --git a/legacy/stm32/include/crsf.h b/legacy/stm32/include/crsf.h deleted file mode 100644 index 585e68f..0000000 --- a/legacy/stm32/include/crsf.h +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef CRSF_H -#define CRSF_H - -#include -#include - -/* - * CRSF/ExpressLRS RC receiver state. - * - * Updated from ISR context on every valid frame. - * Read from main loop — values are naturally atomic (8/16-bit on Cortex-M). - * last_rx_ms == 0 means no frame received yet (USB-only mode). - */ -typedef struct { - uint16_t channels[16]; /* Raw CRSF values, 172 (988µs) – 1811 (2012µs) */ - uint32_t last_rx_ms; /* HAL_GetTick() at last valid RC frame */ - bool armed; /* CH5 arm switch: true when channels[4] > CRSF_ARM_THRESHOLD */ - - /* Link statistics (from 0x14 frames, optional) */ - int8_t rssi_dbm; /* Uplink RSSI in dBm (negative, e.g. -85) */ - uint8_t link_quality; /* Uplink link quality 0–100 % */ - int8_t snr; /* Uplink SNR in dB */ -} CRSFState; - -/* - * crsf_init() — configure UART4 (PA0=TX, PA1=RX) at 420000 baud with - * DMA1 circular RX and IDLE interrupt. Call once before safety_init(). - */ -void crsf_init(void); - -/* - * crsf_parse_byte() — feed one byte into the frame parser. - * Called automatically from DMA/IDLE ISR. Available for unit tests. - */ -void crsf_parse_byte(uint8_t byte); - -/* - * crsf_to_range() — map raw CRSF value (172–1811) linearly to [min, max]. - * Clamps at boundaries. Midpoint 992 → (min+max)/2. - */ -int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max); - -/* - * crsf_send_battery() — transmit CRSF battery-sensor telemetry frame (type 0x08) - * back to the ELRS TX module over UART4 TX. Call at CRSF_TELEMETRY_HZ (1 Hz). - * - * voltage_mv : battery voltage in millivolts (e.g. 12600 for 3S full) - * capacity_mah : remaining battery capacity in mAh (Issue #325, coulomb counter) - * remaining_pct: state-of-charge 0–100 % (255 = unknown) - * - * Frame: [0xC8][12][0x08][v16_hi][v16_lo][c16_hi][c16_lo][cap24×3][rem][CRC] - * voltage unit: 100 mV (12600 mV → 126) - * capacity unit: mAh (3-byte big-endian, max 16.7M mAh) - */ -void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah, - uint8_t remaining_pct); - -/* - * crsf_send_flight_mode() — transmit CRSF flight-mode frame (type 0x21) - * for display on the pilot's handset OSD. - * - * armed: true → "ARMED\0" - * false → "DISARM\0" - */ -void crsf_send_flight_mode(bool armed); - -extern volatile CRSFState crsf_state; - -#endif /* CRSF_H */ diff --git a/legacy/stm32/include/encoder_odom.h b/legacy/stm32/include/encoder_odom.h deleted file mode 100644 index bbe9624..0000000 --- a/legacy/stm32/include/encoder_odom.h +++ /dev/null @@ -1,151 +0,0 @@ -#ifndef ENCODER_ODOM_H -#define ENCODER_ODOM_H - -#include -#include - -/* - * encoder_odom — quadrature encoder reading and differential-drive odometry - * (Issue #632). - * - * HARDWARE: - * Left encoder : TIM2 (32-bit) in encoder mode 3 - * CH1 = PA15 (AF1), CH2 = PB3 (AF1) - * Right encoder : TIM3 (16-bit) in encoder mode 3 - * CH1 = PC6 (AF2), CH2 = PC7 (AF2) - * - * Both channels count on every edge (×4 resolution). - * TIM2 ARR = 0xFFFFFFFF (32-bit, never overflows in practice). - * TIM3 ARR = 0xFFFF (16-bit, delta decoded via int16_t subtraction). - * - * ODOMETRY MODEL (differential drive): - * - * meters_per_tick = (π × wheel_diam_mm × 1e-3) / ticks_per_rev - * - * d_left = Δticks_left × meters_per_tick - * d_right = Δticks_right × meters_per_tick - * - * d_center = (d_left + d_right) / 2 - * dθ = (d_right - d_left) / wheel_base_mm × 1e-3 (radians) - * - * x += d_center × cos(θ) - * y += d_center × sin(θ) - * θ += dθ - * - * For small dt this is the standard Euler-forward integration; suitable for - * the 50 Hz odometry tick rate. - * - * RPM: - * rpm = Δticks × 60.0 / (ticks_per_rev × dt_s) - * - * FLASH CONFIG (ENC_FLASH_ADDR in sector 7): - * Stores ticks_per_rev, wheel_diam_mm, wheel_base_mm validated by magic. - * Falls back to compile-time defaults on magic mismatch. - * Sector 7 is shared with PID flash; saving encoder config must be - * coordinated with pid_flash_save_all() to avoid mutual erasure. - * - * TELEMETRY: - * JLINK_TLM_ODOM (0x8C) published at ENC_TLM_HZ (50 Hz): - * jlink_tlm_odom_t { int16 rpm_left, int16 rpm_right, - * int32 x_mm, int32 y_mm, - * int16 theta_cdeg, int16 speed_mmps } - * 16 bytes, 22-byte frame. - */ - -/* ---- Default hardware parameters (override in flash config) ---- */ -/* Hoverboard 6.5" wheels with typical geared-motor encoder: */ -#define ENC_TICKS_PER_REV_DEFAULT 1320u /* 33 CPR × 40:1 gear = 1320 ticks/rev */ -#define ENC_WHEEL_DIAM_MM_DEFAULT 165u /* 6.5" ≈ 165 mm diameter */ -#define ENC_WHEEL_BASE_MM_DEFAULT 540u /* ~540 mm axle-to-axle separation */ - -/* ---- Flash config ---- */ -/* Stored in sector 7 immediately before the PID schedule area (0x0807FF40). - * 64-byte block: magic(4) + config(12) + pad(48). */ -#define ENC_FLASH_ADDR 0x0807FF00UL -#define ENC_FLASH_MAGIC 0x534C4503UL /* 'SLE\x03' — encoder config v3 */ - -typedef struct __attribute__((packed)) { - uint32_t magic; /* ENC_FLASH_MAGIC when valid */ - uint32_t ticks_per_rev; /* encoder ticks per full wheel revolution */ - uint16_t wheel_diam_mm; /* wheel outer diameter (mm) */ - uint16_t wheel_base_mm; /* lateral wheel separation centre-to-centre (mm) */ - uint8_t _pad[48]; /* reserved — total 64 bytes */ -} enc_flash_config_t; - -/* ---- Runtime configuration ---- */ -typedef struct { - uint32_t ticks_per_rev; - uint16_t wheel_diam_mm; - uint16_t wheel_base_mm; -} enc_config_t; - -/* ---- Runtime state ---- */ -typedef struct { - /* Encoder counters (last sampled) */ - uint32_t cnt_left; /* last TIM2->CNT */ - uint16_t cnt_right; /* last TIM3->CNT */ - - /* Wheel speeds */ - int16_t rpm_left; /* left wheel RPM (signed; + = forward) */ - int16_t rpm_right; /* right wheel RPM (signed) */ - int16_t speed_mmps; /* linear speed of centre point (mm/s) */ - - /* Pose (relative to last reset) */ - float x_mm; /* forward displacement (mm) */ - float y_mm; /* lateral displacement (mm, + = left) */ - float theta_rad; /* heading (radians, + = CCW from start) */ - - /* Internal */ - float meters_per_tick; /* pre-computed from config */ - float wheel_base_m; /* wheel_base_mm / 1000.0 */ - uint32_t last_tick_ms; /* HAL_GetTick() at last encoder_odom_tick() */ - uint32_t last_tlm_ms; /* HAL_GetTick() at last TLM transmission */ - - enc_config_t cfg; /* active hardware parameters */ -} encoder_odom_t; - -/* ---- Configuration ---- */ -#define ENC_TLM_HZ 50u /* JLINK_TLM_ODOM transmit rate (Hz) */ - -/* ---- API ---- */ - -/* - * encoder_odom_init(eo) — configure TIM2/TIM3 in encoder mode, load flash - * config (falling back to defaults), reset pose. - * Call once during system init. - */ -void encoder_odom_init(encoder_odom_t *eo); - -/* - * encoder_odom_tick(eo, now_ms) — sample encoder counters, update RPM and - * integrate odometry. Call from main loop at any rate ≥ 10 Hz (50 Hz ideal). - */ -void encoder_odom_tick(encoder_odom_t *eo, uint32_t now_ms); - -/* - * encoder_odom_reset_pose(eo) — zero x/y/theta without resetting counters or - * config. Call whenever odometry reference frame should be re-anchored. - */ -void encoder_odom_reset_pose(encoder_odom_t *eo); - -/* - * encoder_odom_save_config(cfg) — write enc_flash_config_t to ENC_FLASH_ADDR. - * WARNING: erases sector 7 — must NOT be called while armed and must be - * coordinated with PID flash saves (both records are in sector 7). - * Returns true on success. - */ -bool encoder_odom_save_config(const enc_config_t *cfg); - -/* - * encoder_odom_load_config(cfg) — load config from flash. - * Returns true if flash magic valid; false = defaults applied to *cfg. - */ -bool encoder_odom_load_config(enc_config_t *cfg); - -/* - * encoder_odom_send_tlm(eo, now_ms) — transmit JLINK_TLM_ODOM (0x8C) frame. - * Rate-limited to ENC_TLM_HZ; safe to call every tick. - */ -void encoder_odom_send_tlm(const encoder_odom_t *eo, uint32_t now_ms); - -#endif /* ENCODER_ODOM_H */ diff --git a/legacy/stm32/include/esc_backend.h b/legacy/stm32/include/esc_backend.h deleted file mode 100644 index d1ef848..0000000 --- a/legacy/stm32/include/esc_backend.h +++ /dev/null @@ -1,79 +0,0 @@ -#ifndef ESC_BACKEND_H -#define ESC_BACKEND_H - -#include -#include - -/* - * ESC Backend Abstraction Layer - * - * Provides a pluggable interface for different ESC implementations: - * - Hoverboard (EFeru FOC firmware, UART @ 115200) - * - VESC (via UART @ 921600, with balance mode) — future - * - * Allows motor_driver.c to remain ESC-agnostic. Backend selection - * via ESC_BACKEND compile-time define in config.h. - * - * Issue #388: ESC abstraction layer - * Blocks Issue #383: VESC integration - */ - -/* Telemetry snapshot from ESC (polled on-demand) */ -typedef struct { - int16_t speed; /* Motor speed (PWM duty or RPM, backend-dependent) */ - int16_t steer; /* Steering position (0 = centered) */ - uint16_t voltage_mv; /* Battery voltage in millivolts */ - int16_t current_ma; /* Motor current in milliamps (signed: discharge/charge) */ - int16_t temperature_c; /* ESC temperature in °C */ - uint16_t fault; /* Fault code (backend-specific) */ -} esc_telemetry_t; - -/* Virtual function table for ESC backends */ -typedef struct { - /* Initialize ESC hardware and UART (called once at startup) */ - void (*init)(void); - - /* Send motor command to ESC (called at ~50Hz from motor_driver_update) - * speed: -1000..+1000 (forward/reverse) - * steer: -1000..+1000 (left/right) - */ - void (*send)(int16_t speed, int16_t steer); - - /* Emergency stop: send zero and disable output - * (called from safety or mode manager) - */ - void (*estop)(void); - - /* Query current ESC state - * Returns latest telemetry snapshot (may be cached/stale on some backends). - * Safe to call from any context (non-blocking). - */ - void (*get_telemetry)(esc_telemetry_t *out); - - /* Optional: resume from estop (not all backends use this) */ - void (*resume)(void); -} esc_backend_t; - -/* - * Register a backend implementation at runtime. - * Typically called during init sequence before motor_driver_init(). - */ -void esc_backend_register(const esc_backend_t *backend); - -/* - * Get the currently active backend. - * Returns pointer to vtable; nullptr if no backend registered. - */ -const esc_backend_t *esc_backend_get(void); - -/* - * High-level convenience wrappers (match motor_driver.c interface). - * These call through the active backend if registered. - */ -void esc_init(void); -void esc_send(int16_t speed, int16_t steer); -void esc_estop(void); -void esc_resume(void); -void esc_get_telemetry(esc_telemetry_t *out); - -#endif /* ESC_BACKEND_H */ diff --git a/legacy/stm32/include/face_animation.h b/legacy/stm32/include/face_animation.h deleted file mode 100644 index 981a957..0000000 --- a/legacy/stm32/include/face_animation.h +++ /dev/null @@ -1,111 +0,0 @@ -/* - * face_animation.h — Face Emotion Renderer for LCD Display - * - * Renders expressive face animations for 5 core emotions: - * - HAPPY: upturned eyes, curved smile - * - SAD: downturned eyes, frown - * - CURIOUS: raised eyebrows, wide eyes, slight tilt - * - ANGRY: downturned brows, narrowed eyes, clenched mouth - * - SLEEPING: closed eyes, relaxed mouth, gentle sway (optional) - * - * HOW IT WORKS: - * - State machine with smooth transitions (easing over N frames) - * - Idle behavior: periodic blinking (duration configurable) - * - Each emotion has parameterized eye/mouth shapes (position, angle, curvature) - * - Transitions interpolate between emotion parameter sets - * - render() draws current state to LCD framebuffer via face_lcd_*() API - * - tick() advances frame counter, handles transitions, triggers blink - * - * ANIMATION SPECS: - * - Frame rate: 30 Hz (via systick) - * - Transition time: 0.5–1.0s (15–30 frames) - * - Blink duration: 100–150 ms (3–5 frames) - * - Blink interval: 4–6 seconds (120–180 frames at 30Hz) - * - * API: - * - face_animation_init() — Initialize state machine - * - face_animation_set_emotion(emotion) — Request state change (with smooth transition) - * - face_animation_tick() — Advance animation by 1 frame (call at 30Hz from systick) - * - face_animation_render() — Draw current face to LCD framebuffer - */ - -#ifndef FACE_ANIMATION_H -#define FACE_ANIMATION_H - -#include -#include - -/* === Emotion Types === */ -typedef enum { - FACE_HAPPY = 0, - FACE_SAD = 1, - FACE_CURIOUS = 2, - FACE_ANGRY = 3, - FACE_SLEEPING = 4, - FACE_NEUTRAL = 5, /* Default state */ -} face_emotion_t; - -/* === Animation Parameters (per emotion) === */ -typedef struct { - int16_t eye_x; /* Eye horizontal offset from center (pixels) */ - int16_t eye_y; /* Eye vertical offset from center (pixels) */ - int16_t eye_open_y; /* Eye open height (pixels) */ - int16_t eye_close_y; /* Eye close height (pixels, 0=fully closed) */ - int16_t brow_angle; /* Eyebrow angle (-30..+30 degrees, tilt) */ - int16_t brow_y_offset; /* Eyebrow vertical offset (pixels) */ - int16_t mouth_x; /* Mouth horizontal offset (pixels) */ - int16_t mouth_y; /* Mouth vertical offset (pixels) */ - int16_t mouth_width; /* Mouth width (pixels) */ - int16_t mouth_curve; /* Curvature: >0=smile, <0=frown, 0=neutral */ - uint8_t blink_interval_ms; /* Idle blink interval (seconds, in 30Hz ticks) */ -} face_params_t; - -/* === Public API === */ - -/** - * Initialize face animation system. - * Sets initial emotion to NEUTRAL, clears blink timer. - */ -void face_animation_init(void); - -/** - * Request a state change to a new emotion. - * Triggers smooth transition (easing) over TRANSITION_FRAMES. - */ -void face_animation_set_emotion(face_emotion_t emotion); - -/** - * Advance animation by one frame. - * Called by systick ISR at 30 Hz. - * Handles: - * - Transition interpolation - * - Blink timing and rendering - * - Idle animations (sway, subtle movements) - */ -void face_animation_tick(void); - -/** - * Render current face state to LCD framebuffer. - * Draws eyes, brows, mouth, and optional idle animations. - * Should be called after face_animation_tick(). - */ -void face_animation_render(void); - -/** - * Get current emotion (transition-aware). - * Returns the target emotion, or current if transition in progress. - */ -face_emotion_t face_animation_get_emotion(void); - -/** - * Trigger a blink immediately (for special events). - * Overrides idle blink timer. - */ -void face_animation_blink_now(void); - -/** - * Check if animation is idle (no active transition). - */ -bool face_animation_is_idle(void); - -#endif // FACE_ANIMATION_H diff --git a/legacy/stm32/include/face_lcd.h b/legacy/stm32/include/face_lcd.h deleted file mode 100644 index d25e276..0000000 --- a/legacy/stm32/include/face_lcd.h +++ /dev/null @@ -1,116 +0,0 @@ -/* - * face_lcd.h — STM32 LCD Display Driver for Face Animations - * - * Low-level abstraction for driving a small LCD/OLED display via SPI or I2C. - * Supports pixel/line drawing primitives and full framebuffer operations. - * - * HOW IT WORKS: - * - Initializes display (SPI/I2C, resolution, rotation) - * - Provides framebuffer (in RAM or on-device) - * - Exposes primitives: draw_pixel, draw_line, draw_circle, fill_rect - * - Implements vsync-driven 30Hz refresh from systick - * - Non-blocking DMA transfers for rapid display updates - * - * HARDWARE ASSUMPTIONS: - * - SPI2 or I2C (configurable via #define LCD_INTERFACE) - * - Typical sizes: 128×64, 240×135, 320×240 - * - Pixel depth: 1-bit (monochrome) or 16-bit (RGB565) - * - Controller: SSD1306, ILI9341, ST7789, etc. - * - * API: - * - face_lcd_init(width, height, bpp) — Initialize display - * - face_lcd_clear() — Clear framebuffer - * - face_lcd_pixel(x, y, color) — Set pixel - * - face_lcd_line(x0, y0, x1, y1, color) — Draw line (Bresenham) - * - face_lcd_circle(cx, cy, r, color) — Draw circle - * - face_lcd_fill_rect(x, y, w, h, color) — Filled rectangle - * - face_lcd_flush() — Push framebuffer to display (async via DMA) - * - face_lcd_is_busy() — Check if transfer in progress - * - face_lcd_tick() — Called by systick ISR for 30Hz vsync - */ - -#ifndef FACE_LCD_H -#define FACE_LCD_H - -#include -#include - -/* === Configuration === */ -#define LCD_INTERFACE SPI /* SPI or I2C */ -#define LCD_WIDTH 128 /* pixels */ -#define LCD_HEIGHT 64 /* pixels */ -#define LCD_BPP 1 /* bits per pixel (1=mono, 16=RGB565) */ -#define LCD_REFRESH_HZ 30 /* target refresh rate */ - -#if LCD_BPP == 1 - typedef uint8_t lcd_color_t; - #define LCD_BLACK 0x00 - #define LCD_WHITE 0x01 - #define LCD_FBSIZE (LCD_WIDTH * LCD_HEIGHT / 8) /* 1024 bytes */ -#else /* RGB565 */ - typedef uint16_t lcd_color_t; - #define LCD_BLACK 0x0000 - #define LCD_WHITE 0xFFFF - #define LCD_FBSIZE (LCD_WIDTH * LCD_HEIGHT * 2) /* 16384 bytes */ -#endif - -/* === Public API === */ - -/** - * Initialize LCD display and framebuffer. - * Called once at startup. - */ -void face_lcd_init(void); - -/** - * Clear entire framebuffer to black. - */ -void face_lcd_clear(void); - -/** - * Set a single pixel in the framebuffer. - * (Does NOT push to display immediately.) - */ -void face_lcd_pixel(uint16_t x, uint16_t y, lcd_color_t color); - -/** - * Draw a line from (x0,y0) to (x1,y1) using Bresenham algorithm. - */ -void face_lcd_line(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, - lcd_color_t color); - -/** - * Draw a circle with center (cx, cy) and radius r. - */ -void face_lcd_circle(uint16_t cx, uint16_t cy, uint16_t r, lcd_color_t color); - -/** - * Fill a rectangle at (x, y) with width w and height h. - */ -void face_lcd_fill_rect(uint16_t x, uint16_t y, uint16_t w, uint16_t h, - lcd_color_t color); - -/** - * Push framebuffer to display (async via DMA if available). - * Returns immediately; transfer happens in background. - */ -void face_lcd_flush(void); - -/** - * Check if a display transfer is currently in progress. - * Returns true if DMA/SPI is busy, false if idle. - */ -bool face_lcd_is_busy(void); - -/** - * Called by systick ISR (~30Hz) to drive vsync and maintain refresh. - * Updates frame counter and triggers flush if a new frame is needed. - */ -void face_lcd_tick(void); - -/** - * Get framebuffer address (for direct access if needed). - */ -uint8_t *face_lcd_get_fb(void); - -#endif // FACE_LCD_H diff --git a/legacy/stm32/include/face_uart.h b/legacy/stm32/include/face_uart.h deleted file mode 100644 index 7f40ea8..0000000 --- a/legacy/stm32/include/face_uart.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - * face_uart.h — UART Command Interface for Face Animations - * - * Receives emotion commands from Jetson Orin via UART (USART3 by default). - * Parses simple text commands and updates face animation state. - * - * PROTOCOL: - * Text-based commands (newline-terminated): - * HAPPY — Set emotion to happy - * SAD — Set emotion to sad - * CURIOUS — Set emotion to curious - * ANGRY — Set emotion to angry - * SLEEP — Set emotion to sleeping - * NEUTRAL — Set emotion to neutral - * BLINK — Trigger immediate blink - * STATUS — Echo current emotion + animation state - * - * Example: - * > HAPPY\n - * < OK: HAPPY\n - * - * INTERFACE: - * - UART3 (PB10=TX, PB11=RX) at 115200 baud - * - RX ISR pushes bytes into ring buffer - * - face_uart_process() checks for complete commands (polling) - * - Case-insensitive command parsing - * - Echoes command results to TX for debugging - * - * API: - * - face_uart_init() — Configure UART3 @ 115200 - * - face_uart_process() — Parse and execute commands (call from main loop) - * - face_uart_rx_isr() — Called by UART3 RX interrupt - * - face_uart_send() — Send response string (used internally) - */ - -#ifndef FACE_UART_H -#define FACE_UART_H - -#include -#include - -/* === Configuration === */ -#define FACE_UART_INSTANCE USART3 /* USART3 (PB10=TX, PB11=RX) */ -#define FACE_UART_BAUD 115200 /* 115200 baud */ -#define FACE_UART_RX_BUF_SZ 128 /* RX ring buffer size */ - -/* === Public API === */ - -/** - * Initialize UART for face commands. - * Configures USART3 @ 115200, enables RX interrupt. - */ -void face_uart_init(void); - -/** - * Process any pending RX data and execute commands. - * Should be called periodically from main loop (or low-priority task). - * Returns immediately if no complete command available. - */ -void face_uart_process(void); - -/** - * UART3 RX interrupt handler. - * Called by HAL when a byte is received. - * Pushes byte into ring buffer. - */ -void face_uart_rx_isr(uint8_t byte); - -/** - * Send a response string to UART3 TX. - * Used for echoing status/ack messages. - * Non-blocking (pushes to TX queue). - */ -void face_uart_send(const char *str); - -#endif // FACE_UART_H diff --git a/legacy/stm32/include/fan.h b/legacy/stm32/include/fan.h deleted file mode 100644 index ef3e2fb..0000000 --- a/legacy/stm32/include/fan.h +++ /dev/null @@ -1,162 +0,0 @@ -#ifndef FAN_H -#define FAN_H - -#include -#include - -/* - * fan.h — Cooling fan PWM speed controller (Issue #263) - * - * STM32F722 driver for brushless cooling fan on PA9 using TIM1_CH2 PWM. - * Temperature-based speed curve with smooth ramp transitions. - * - * Pin: PA9 (TIM1_CH2, alternate function AF1) - * PWM Frequency: 25 kHz (suitable for brushless DC fan) - * Speed Range: 0-100% duty cycle - * - * Temperature Curve: - * - Below 40°C: Fan off (0%) - * - 40-50°C: Linear ramp from 0% to 30% - * - 50-70°C: Linear ramp from 30% to 100% - * - Above 70°C: Fan at maximum (100%) - */ - -/* Fan speed state */ -typedef enum { - FAN_OFF, /* Motor disabled (0% duty) */ - FAN_LOW, /* Low speed (5-30%) */ - FAN_MEDIUM, /* Medium speed (31-60%) */ - FAN_HIGH, /* High speed (61-99%) */ - FAN_FULL /* Maximum speed (100%) */ -} FanState; - -/* - * fan_init() - * - * Initialize fan controller: - * - PA9 as TIM1_CH2 PWM output - * - TIM1 configured for 25 kHz frequency - * - PWM duty cycle control (0-100%) - * - Ramp rate limiter for smooth transitions - */ -void fan_init(void); - -/* - * fan_set_speed(percentage) - * - * Set fan speed directly (bypasses temperature control). - * Used for manual testing or emergency cooling. - * - * Arguments: - * - percentage: 0-100% duty cycle - * - * Returns: true if set successfully, false if invalid value - */ -bool fan_set_speed(uint8_t percentage); - -/* - * fan_get_speed() - * - * Get current fan speed setting. - * - * Returns: Current speed 0-100% - */ -uint8_t fan_get_speed(void); - -/* - * fan_set_target_speed(percentage) - * - * Set target speed with smooth ramping. - * Speed transitions over time according to ramp rate. - * - * Arguments: - * - percentage: Target speed 0-100% - * - * Returns: true if set successfully - */ -bool fan_set_target_speed(uint8_t percentage); - -/* - * fan_update_temperature(temp_celsius) - * - * Update temperature reading and apply speed curve. - * Calculates target speed based on temperature curve. - * Speed transition is smoothed via ramp limiter. - * - * Temperature Curve: - * - temp < 40°C: 0% (off) - * - 40°C ≤ temp < 50°C: 0% + (temp - 40) * 3% per °C = linear to 30% - * - 50°C ≤ temp < 70°C: 30% + (temp - 50) * 3.5% per °C = linear to 100% - * - temp ≥ 70°C: 100% (full) - * - * Arguments: - * - temp_celsius: Temperature in degrees Celsius (int16_t for negative values) - */ -void fan_update_temperature(int16_t temp_celsius); - -/* - * fan_get_temperature() - * - * Get last recorded temperature. - * - * Returns: Temperature in °C (or 0 if not yet set) - */ -int16_t fan_get_temperature(void); - -/* - * fan_get_state() - * - * Get current fan operational state. - * - * Returns: FAN_OFF, FAN_LOW, FAN_MEDIUM, FAN_HIGH, or FAN_FULL - */ -FanState fan_get_state(void); - -/* - * fan_set_ramp_rate(percentage_per_ms) - * - * Configure speed ramp rate for smooth transitions. - * Default: 5% per 100ms = 0.05% per ms. - * Higher values = faster transitions. - * - * Arguments: - * - percentage_per_ms: Speed change per millisecond (e.g., 1 = 1% per ms) - * - * Typical ranges: - * - 0.01 = very slow (100% change in 10 seconds) - * - 0.05 = slow (100% change in 2 seconds) - * - 0.1 = medium (100% change in 1 second) - * - 1.0 = fast (100% change in 100ms) - */ -void fan_set_ramp_rate(float percentage_per_ms); - -/* - * fan_is_ramping() - * - * Check if speed is currently transitioning. - * - * Returns: true if speed is ramping toward target, false if at target - */ -bool fan_is_ramping(void); - -/* - * fan_tick(now_ms) - * - * Update function called periodically (recommended: every 10-100ms). - * Processes speed ramp transitions. - * Must be called regularly for smooth ramping operation. - * - * Arguments: - * - now_ms: current time in milliseconds (from HAL_GetTick() or similar) - */ -void fan_tick(uint32_t now_ms); - -/* - * fan_disable() - * - * Disable fan immediately (set to 0% duty). - * Useful for shutdown or emergency stop. - */ -void fan_disable(void); - -#endif /* FAN_H */ diff --git a/legacy/stm32/include/fault_handler.h b/legacy/stm32/include/fault_handler.h deleted file mode 100644 index 1266524..0000000 --- a/legacy/stm32/include/fault_handler.h +++ /dev/null @@ -1,140 +0,0 @@ -#ifndef FAULT_HANDLER_H -#define FAULT_HANDLER_H - -#include -#include - -/* - * fault_handler.h — STM32F7 fault detection and recovery (Issue #565) - * - * Features: - * - HardFault / BusFault / UsageFault / MemManage vector hooks with full - * Cortex-M7 register dump (R0-R3, LR, PC, xPSR, CFSR, HFSR, MMFAR, BFAR) - * - .noinit SRAM ring: fault frame captured and magic-tagged, survives - * NVIC_SystemReset(); persisted to flash on the subsequent boot - * - MPU Region 0 stack-guard (32 bytes at __stack_end, no-access) → MemManage - * fault detected as FAULT_STACK_OVF - * - Brownout detect via RCC_CSR_BORRSTF on boot → FAULT_BROWNOUT - * - Persistent fault log: last 8 entries × 64 bytes in flash sector 7 - * at 0x08060000 (below the PID store at 0x0807FFC0) - * - JLINK_TLM_FAULT_LOG (0x85): 20-byte summary sent via JLink on boot - * and on JLINK_CMD_FAULT_LOG_GET (0x0C) request - * - LED blink codes on LED2 (PC14, active-low) for 10 s after recovery: - * HARDFAULT = 3 fast blinks (100 ms) - * WATCHDOG = 2 slow blinks (300 ms) - * BROWNOUT = 1 long blink (500 ms) - * STACK_OVF = 4 fast blinks (100 ms) - * BUS_FAULT = alternating 3+1 - * USAGE_FAULT = 2 fast blinks - * - Auto-recovery: fault → .noinit capture → NVIC_SystemReset() - * On next boot fault_handler_init() re-runs safely: persists, prints, blinks - * - * Flash layout within sector 7 (0x08060000, 128 KB): - * Slot 0-7: 0x08060000 – 0x080601FF (8 × 64 bytes = 512 bytes fault log) - * PID store: 0x0807FFC0 – 0x0807FFFF (64 bytes, managed by pid_flash.c) - */ - -/* ---- Fault types ---- */ -typedef enum { - FAULT_NONE = 0x00, - FAULT_HARDFAULT = 0x01, /* HardFault escalation */ - FAULT_WATCHDOG = 0x02, /* IWDG timeout reset */ - FAULT_BROWNOUT = 0x03, /* Brown-out reset (BOR) */ - FAULT_STACK_OVF = 0x04, /* MPU stack guard MemManage */ - FAULT_BUS_FAULT = 0x05, /* BusFault */ - FAULT_USAGE_FAULT = 0x06, /* UsageFault */ - FAULT_MEM_FAULT = 0x07, /* MemManageFault (non-stack-guard) */ - FAULT_ASSERT = 0x08, /* Software assertion */ -} FaultType; - -/* ---- Flash fault log constants ---- */ -#define FAULT_LOG_MAX_ENTRIES 8u -#define FAULT_LOG_MAGIC 0xFADE5A01u -#define FAULT_LOG_BASE_ADDR 0x08060000UL /* start of flash sector 7 */ -#define FAULT_LOG_ENTRY_SIZE 64u /* bytes per entry */ - -/* ---- Flash fault log entry (64 bytes, packed) ---- */ -typedef struct __attribute__((packed)) { - uint32_t magic; /* FAULT_LOG_MAGIC when valid */ - uint8_t fault_type; /* FaultType */ - uint8_t reset_count; /* lifetime reset counter */ - uint16_t _pad0; - uint32_t timestamp_ms; /* HAL_GetTick() at reset (0 if pre-tick) */ - uint32_t pc; /* faulting instruction address */ - uint32_t lr; /* link register at fault */ - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t cfsr; /* SCB->CFSR: combined fault status register */ - uint32_t hfsr; /* SCB->HFSR: hard fault status register */ - uint32_t mmfar; /* SCB->MMFAR: memory manage fault address */ - uint32_t bfar; /* SCB->BFAR: bus fault address */ - uint32_t sp; /* stack pointer value at fault */ - uint8_t _pad1[4]; /* pad to 64 bytes */ -} fault_log_entry_t; /* 64 bytes */ - -/* - * fault_handler_init() — call early in main(), before safety_init(). - * 1. Increments reset counter (.noinit SRAM). - * 2. Checks .noinit SRAM for a pending fault capture; if found: persists to - * flash, prints CDC register dump, starts LED blink code. - * 3. Detects brownout via RCC_CSR_BORRSTF; logs if detected. - * 4. Clears RCC reset flags. - * 5. Installs MPU Region 0 stack guard. - * 6. Enables MemManage, BusFault, UsageFault (SCB->SHCSR). - */ -void fault_handler_init(void); - -/* - * fault_mpu_guard_init() — configure MPU Region 0 as a 32-byte no-access - * guard at __stack_end (bottom of main stack). Generates MemManage on - * stack overflow. Called automatically by fault_handler_init(). - */ -void fault_mpu_guard_init(void); - -/* - * fault_get_last_type() — most recent fault type from flash log, or FAULT_NONE. - */ -FaultType fault_get_last_type(void); - -/* - * fault_log_read(idx, out) — read flash slot 0..7. - * Returns false if slot empty or idx out of range. - */ -bool fault_log_read(uint8_t idx, fault_log_entry_t *out); - -/* - * fault_log_get_count() — number of valid (occupied) log slots, 0-8. - */ -uint8_t fault_log_get_count(void); - -/* - * fault_log_clear() — erase fault log, restore PID if previously saved. - * Erases all of sector 7 (~1 s stall). Do not call while armed. - */ -void fault_log_clear(void); - -/* - * fault_assert(file, line) — software fault at runtime; captures return - * address, writes SRAM magic, triggers NVIC_SystemReset(). - * Use via FAULT_ASSERT(cond) macro below. - */ -void fault_assert_impl(const char *file, int line); - -#define FAULT_ASSERT(cond) \ - do { if (!(cond)) fault_assert_impl(__FILE__, __LINE__); } while (0) - -/* - * fault_led_tick(now_ms) — drive LED2 blink code from main loop (1 ms). - * Self-disables after 10 s so it doesn't interfere with normal LED state. - */ -void fault_led_tick(uint32_t now_ms); - -/* C-level fault dispatch (called from naked asm stubs; not for direct use) */ -void fault_hard_c(uint32_t *frame); -void fault_mem_c(uint32_t *frame); -void fault_bus_c(uint32_t *frame); -void fault_usage_c(uint32_t *frame); - -#endif /* FAULT_HANDLER_H */ diff --git a/legacy/stm32/include/gimbal.h b/legacy/stm32/include/gimbal.h deleted file mode 100644 index 65ed07e..0000000 --- a/legacy/stm32/include/gimbal.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef GIMBAL_H -#define GIMBAL_H - -#include -#include - -/* - * gimbal.h — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547) - * - * Manages dual ST3215 serial bus servos: - * Pan servo: ID GIMBAL_PAN_ID (config.h, default 1) - * Tilt servo: ID GIMBAL_TILT_ID (config.h, default 2) - * - * Position units: degrees x10 (int16), matching JLink protocol convention. - * e.g. 900 = 90.0°, -450 = -45.0° - * - * Limits: - * Pan: -1800..+1800 (x10 deg) = -180..+180 deg - * Tilt: -900..+900 (x10 deg) = -90..+90 deg - * - * The gimbal_tick() function polls servo feedback at GIMBAL_TLM_HZ (50 Hz). - * Alternates reading pan position on even ticks, tilt on odd ticks — each - * servo polled at 25 Hz to keep bus utilization low. - */ - -typedef struct { - /* Command state */ - int16_t cmd_pan_x10; /* Commanded pan (deg x10) */ - int16_t cmd_tilt_x10; /* Commanded tilt (deg x10) */ - uint16_t cmd_speed; /* Servo bus speed (0=max, 1-4095) */ - bool torque_enabled; /* True when torques are enabled */ - - /* Feedback state (updated at ~25 Hz per axis) */ - int16_t fb_pan_x10; /* Measured pan (deg x10) */ - int16_t fb_tilt_x10; /* Measured tilt (deg x10) */ - uint16_t fb_pan_speed; /* Raw speed register, pan servo */ - uint16_t fb_tilt_speed; /* Raw speed register, tilt servo */ - - /* Diagnostics */ - uint32_t rx_ok; /* Successful position reads */ - uint32_t rx_err; /* Failed position reads */ - - uint32_t _last_tick_ms; /* Internal: last tick timestamp */ - uint8_t _poll_phase; /* Internal: alternates 0=pan 1=tilt */ -} gimbal_t; - -/* - * gimbal_init(g) — enable torque on both servos, center them. - * servo_bus_init() must be called first. - */ -void gimbal_init(gimbal_t *g); - -/* - * gimbal_set_pos(g, pan_x10, tilt_x10, speed) — command a new pan/tilt - * position. pan_x10 and tilt_x10 are degrees×10, clamped to servo limits. - * speed: 0=max servo speed, 1-4095 = scaled. - */ -void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10, - uint16_t speed); - -/* - * gimbal_torque(g, enable) — enable or disable torque on both servos. - */ -void gimbal_torque(gimbal_t *g, bool enable); - -/* - * gimbal_tick(g, now_ms) — poll servo feedback at GIMBAL_TLM_HZ. - * Call every 1 ms from the main loop; function self-throttles. - */ -void gimbal_tick(gimbal_t *g, uint32_t now_ms); - -#endif /* GIMBAL_H */ diff --git a/legacy/stm32/include/hoverboard.h b/legacy/stm32/include/hoverboard.h deleted file mode 100644 index b5270dd..0000000 --- a/legacy/stm32/include/hoverboard.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef HOVERBOARD_H -#define HOVERBOARD_H - -#include - -/* - * Hoverboard ESC UART protocol (EFeru FOC firmware) - * USART2: PA2=TX, PA3=RX @ 115200 baud - * - * Packet: [0xABCD] [steer:i16] [speed:i16] [checksum:u16] - * Checksum = start ^ steer ^ speed - * Speed range: -1000 to +1000 - * Must send at >=50Hz or ESC times out (TIMEOUT=20 * DELAY_IN_MAIN_LOOP=5ms = 100ms) - */ - -#define HOVERBOARD_START_FRAME 0xABCD -#define HOVERBOARD_BAUD 38400 - -typedef struct __attribute__((packed)) { - uint16_t start; - int16_t steer; - int16_t speed; - uint16_t checksum; -} hoverboard_cmd_t; - -void hoverboard_init(void); -void hoverboard_send(int16_t speed, int16_t steer); - -#endif diff --git a/legacy/stm32/include/hw_button.h b/legacy/stm32/include/hw_button.h deleted file mode 100644 index 75fe8f2..0000000 --- a/legacy/stm32/include/hw_button.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef HW_BUTTON_H -#define HW_BUTTON_H - -#include -#include - -/* - * hw_button — hardware button debounce + gesture detection (Issue #682). - * - * Debounce FSM: - * IDLE → (raw press detected) → DEBOUNCING - * DEBOUNCING → (still pressed after BTN_DEBOUNCE_MS) → HELD - * HELD → (released) → classify press type, back to IDLE - * - * Press types: - * SHORT held < BTN_LONG_MIN_MS from confirmed start - * LONG held >= BTN_LONG_MIN_MS - * - * Sequence detection: - * [SHORT, SHORT, LONG] -> BTN_EVENT_REARM_COMBO (fires on LONG release) - * [SHORT] + BTN_COMMIT_MS quiet timeout -> BTN_EVENT_PARK - * - * Config constants (can be overridden in config.h): - * BTN_DEBOUNCE_MS 20 ms debounce window - * BTN_LONG_MIN_MS 1500 ms threshold for LONG press - * BTN_COMMIT_MS 500 ms quiet after lone SHORT -> PARK - * BTN_SEQ_TIMEOUT_MS 3000 ms sequence window; expired sequence is abandoned - * BTN_PORT GPIOC - * BTN_PIN GPIO_PIN_2 - */ - -typedef enum { - BTN_EVENT_NONE = 0, - BTN_EVENT_PARK = 1, /* single short press + quiet */ - BTN_EVENT_REARM_COMBO = 2, /* SHORT + SHORT + LONG */ -} hw_btn_event_t; - -/* - * hw_button_init() — configure GPIO (active-low pull-up), zero FSM state. - * Call once at startup. - */ -void hw_button_init(void); - -/* - * hw_button_tick(now_ms) — advance debounce FSM and sequence detector. - * Call every ms from the main loop. Returns BTN_EVENT_NONE unless a - * complete gesture was recognised this tick. - */ -hw_btn_event_t hw_button_tick(uint32_t now_ms); - -/* - * hw_button_is_pressed() — true while button is confirmed held (post-debounce). - */ -bool hw_button_is_pressed(void); - -#ifdef TEST_HOST -/* Inject a simulated raw pin state for host-side unit tests. */ -void hw_button_inject(bool pressed); -#endif - -#endif /* HW_BUTTON_H */ diff --git a/legacy/stm32/include/i2c1.h b/legacy/stm32/include/i2c1.h deleted file mode 100644 index c4c7859..0000000 --- a/legacy/stm32/include/i2c1.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef I2C1_H -#define I2C1_H - -#include "stm32f7xx_hal.h" - -/* - * Shared I2C1 bus handle — used by baro (BMP280/DPS310) and mag - * (QMC5883L/HMC5883L/IST8310) drivers. - * - * Call i2c1_init() once in main() before any I2C probes. - * PB8 = SCL, PB9 = SDA (AF4_I2C1, open-drain, 100 kHz). - */ -extern I2C_HandleTypeDef hi2c1; - -int i2c1_init(void); - -#endif /* I2C1_H */ diff --git a/legacy/stm32/include/icm42688.h b/legacy/stm32/include/icm42688.h deleted file mode 100644 index 47e7463..0000000 --- a/legacy/stm32/include/icm42688.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef ICM42688_H -#define ICM42688_H -#include -typedef struct { - int16_t ax, ay, az, gx, gy, gz, temp_x10; -} icm42688_data_t; -int icm42688_init(void); -void icm42688_read(icm42688_data_t *d); -void icm42688_get_trace(uint8_t *out, int *len); -#endif diff --git a/legacy/stm32/include/imu_cal_flash.h b/legacy/stm32/include/imu_cal_flash.h deleted file mode 100644 index d0e6658..0000000 --- a/legacy/stm32/include/imu_cal_flash.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef IMU_CAL_FLASH_H -#define IMU_CAL_FLASH_H - -#include -#include - -/* - * IMU mount angle calibration flash storage (Issue #680). - * - * Sector 7 (128 KB at 0x08060000) layout: - * 0x0807FF00 imu_cal_flash_t (64 bytes) ← this module - * 0x0807FF40 pid_sched_flash_t (128 bytes) ← pid_flash.c - * 0x0807FFC0 pid_flash_t (64 bytes) ← pid_flash.c - * - * Calibration flow: - * 1. Mount robot at its installed angle, power on, let IMU converge (~5s). - * 2. Send 'O' via USB CDC (dev-only path). - * 3. Firmware captures current pitch + roll as mount offsets, saves to flash. - * 4. mpu6000_read() subtracts offsets from output on every subsequent read. - * - * The sector erase preserves existing PID data by reading it first. - */ - -#define IMU_CAL_FLASH_ADDR 0x0807FF00UL -#define IMU_CAL_FLASH_MAGIC 0x534C5403UL /* 'SLT\x03' — version 3 */ - -typedef struct __attribute__((packed)) { - uint32_t magic; /* IMU_CAL_FLASH_MAGIC when valid */ - float pitch_offset; /* degrees subtracted from IMU pitch output */ - float roll_offset; /* degrees subtracted from IMU roll output */ - uint8_t _pad[52]; /* padding to 64 bytes */ -} imu_cal_flash_t; /* 64 bytes total */ - -/* - * imu_cal_flash_load() — read saved mount offsets from flash. - * Returns true and fills *pitch_offset / *roll_offset if magic is valid. - * Returns false if no valid calibration stored (caller keeps 0.0f defaults). - */ -bool imu_cal_flash_load(float *pitch_offset, float *roll_offset); - -/* - * imu_cal_flash_save() — erase sector 7 and write all three records atomically: - * imu_cal_flash_t at 0x0807FF00 - * pid_sched_flash_t at 0x0807FF40 (preserved from existing flash) - * pid_flash_t at 0x0807FFC0 (preserved from existing flash) - * Must be called while disarmed — sector erase stalls CPU ~1s. - * Returns true on success. - */ -bool imu_cal_flash_save(float pitch_offset, float roll_offset); - -#endif /* IMU_CAL_FLASH_H */ diff --git a/legacy/stm32/include/ina219.h b/legacy/stm32/include/ina219.h deleted file mode 100644 index 4817958..0000000 --- a/legacy/stm32/include/ina219.h +++ /dev/null @@ -1,117 +0,0 @@ -#ifndef INA219_H -#define INA219_H - -#include -#include - -/* - * ina219.h — INA219 power monitor driver (Issue #214) - * - * I2C1 driver for motor current/voltage/power monitoring. - * Supports 2 sensors (left/right motor) on I2C1 (PB8=SCL, PB9=SDA). - * - * INA219 specs: - * - I2C addresses: 0x40–0x4F (configurable via address pins) - * - Bus voltage: 0–26V, 4mV/LSB - * - Shunt voltage: ±327mV, 10µV/LSB - * - Current: derived from shunt voltage (calibration-dependent) - * - Power: (Bus V × Current) / internal gain - * - * Typical usage for motor monitoring: - * - 0.1Ω shunt resistor → ~3.27A max (at ±327mV) - * - Calibration: set max expected current, driver calculates LSB - * - Read functions return actual voltage/current/power values - */ - -/* INA219 sensors (2 motors) */ -typedef enum { - INA219_LEFT_MOTOR = 0, /* Address 0x40 */ - INA219_RIGHT_MOTOR = 1, /* Address 0x41 */ - INA219_COUNT -} INA219Sensor; - -/* INA219 measurement data */ -typedef struct { - uint16_t bus_voltage_mv; /* Bus voltage in mV (0–26000) */ - int16_t shunt_voltage_uv; /* Shunt voltage in µV (±327000) */ - int16_t current_ma; /* Current in mA (signed) */ - uint32_t power_mw; /* Power in mW */ -} INA219Data; - -/* - * ina219_init() - * - * Initialize I2C1 and both INA219 sensors (left + right motor). - * Performs auto-calibration for typical motor current monitoring. - * Call once at startup after i2c1_init(). - */ -void ina219_init(void); - -/* - * ina219_calibrate(sensor, max_current_ma, shunt_ohms_milli) - * - * Manually calibrate a sensor for expected max current and shunt resistance. - * Calculates internal calibration register value. - * - * Example: - * ina219_calibrate(INA219_LEFT_MOTOR, 5000, 100); // 5A max, 0.1Ω shunt - */ -void ina219_calibrate(INA219Sensor sensor, uint16_t max_current_ma, uint16_t shunt_ohms_milli); - -/* - * ina219_read(sensor, data) - * - * Read all measurements from a sensor (voltage, current, power). - * Blocks until measurements are ready (typically <1ms at default ADC resolution). - * - * Returns: true if read successful, false on I2C error. - */ -bool ina219_read(INA219Sensor sensor, INA219Data *data); - -/* - * ina219_read_bus_voltage_mv(sensor, voltage_mv) - * - * Read bus voltage only (faster than full read). - * Returns: true if successful. - */ -bool ina219_read_bus_voltage_mv(INA219Sensor sensor, uint16_t *voltage_mv); - -/* - * ina219_read_current_ma(sensor, current_ma) - * - * Read current only (requires prior calibration). - * Returns: true if successful. - */ -bool ina219_read_current_ma(INA219Sensor sensor, int16_t *current_ma); - -/* - * ina219_read_power_mw(sensor, power_mw) - * - * Read power consumption only. - * Returns: true if successful. - */ -bool ina219_read_power_mw(INA219Sensor sensor, uint32_t *power_mw); - -/* - * ina219_alert_enable(sensor, current_limit_ma) - * - * Enable alert pin when current exceeds limit (overcurrent protection). - * Alert pin: GPIO, active high, open-drain output. - */ -void ina219_alert_enable(INA219Sensor sensor, uint16_t current_limit_ma); - -/* - * ina219_alert_disable(sensor) - * - * Disable alert for a sensor. - */ -void ina219_alert_disable(INA219Sensor sensor); - -/* - * ina219_reset(sensor) - * - * Perform soft reset on a sensor (clears all registers to default). - */ -void ina219_reset(INA219Sensor sensor); - -#endif /* INA219_H */ diff --git a/legacy/stm32/include/jetson_cmd.h b/legacy/stm32/include/jetson_cmd.h deleted file mode 100644 index 16432f2..0000000 --- a/legacy/stm32/include/jetson_cmd.h +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef JETSON_CMD_H -#define JETSON_CMD_H - -#include -#include - -/* - * Jetson→STM32 command protocol over USB CDC (bidirectional, same /dev/ttyACM0) - * - * Commands (newline-terminated ASCII, sent by Jetson): - * H\n — heartbeat (every 200ms). Must arrive within 500ms or - * jetson_cmd_is_active() returns false → steer reverts to 0. - * C,\n — drive command: speed -1000..+1000, steer -1000..+1000. - * Also refreshes the heartbeat timer. - * - * Speed→setpoint: - * Speed is converted to a setpoint offset (degrees) before calling balance_update(). - * Positive speed → forward tilt → robot moves forward. - * Max offset is ±JETSON_SPEED_MAX_DEG (see below). - * - * Steer: - * Passed directly to motor_driver_update() as steer_cmd. - * Motor driver ramps and clamps with balance headroom (see motor_driver.h). - * - * Integration pattern in main.c (after the cdc_cmd_ready block): - * - * // Process buffered C command (parsed here, not in ISR) - * if (jetson_cmd_ready) { jetson_cmd_ready = 0; jetson_cmd_process(); } - * - * // Apply setpoint offset and steer when active - * float base_sp = bal.setpoint; - * if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset(); - * balance_update(&bal, &imu, dt); - * bal.setpoint = base_sp; - * - * // Steer injection in 50Hz ESC block - * int16_t jsteer = jetson_cmd_is_active(now) ? jetson_cmd_steer() : 0; - * motor_driver_update(&motors, bal.motor_cmd, jsteer, now); - */ - -/* Heartbeat timeout: if no H or C within this window, commands deactivate */ -#define JETSON_HB_TIMEOUT_MS 500 - -/* Max setpoint offset from Jetson speed command (speed=1000 → +N degrees tilt) */ -#define JETSON_SPEED_MAX_DEG 4.0f /* ±4° → enough for ~0.5 m/s */ - -/* - * jetson_cmd_process() - * Call from main loop (NOT ISR) when jetson_cmd_ready is set. - * Parses jetson_cmd_buf (the C, frame) with sscanf. - */ -void jetson_cmd_process(void); - -/* - * jetson_cmd_is_active(now) - * Returns true if a heartbeat (H or C command) arrived within JETSON_HB_TIMEOUT_MS. - * If false, main loop should fall back to RC or zero steer. - */ -bool jetson_cmd_is_active(uint32_t now_ms); - -/* Current steer command after latest C frame, clamped to ±1000 */ -int16_t jetson_cmd_steer(void); - -/* Setpoint offset (degrees) derived from latest speed command. */ -float jetson_cmd_sp_offset(void); - -/* - * Externals — declared here, defined in usbd_cdc_if.c alongside the other - * CDC volatile flags (cdc_streaming, cdc_arm_request, etc.). - * Main loop checks jetson_cmd_ready; ISR sets it. - */ -extern volatile uint8_t jetson_cmd_ready; /* set by ISR on C frame */ -extern volatile char jetson_cmd_buf[32]; /* C,\0 from ISR */ -extern volatile uint32_t jetson_hb_tick; /* HAL_GetTick() of last H or C */ - -#endif /* JETSON_CMD_H */ diff --git a/legacy/stm32/include/jetson_uart.h b/legacy/stm32/include/jetson_uart.h deleted file mode 100644 index ed63640..0000000 --- a/legacy/stm32/include/jetson_uart.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef JETSON_UART_H -#define JETSON_UART_H - -#include -#include "stm32f7xx_hal.h" - -/* Initialize USART6 for Jetson communication (921600 baud) */ -void jetson_uart_init(void); - -/* Send data back to Jetson (telemetry, status) */ -void jetson_uart_send(const uint8_t *data, uint16_t len); - -/* Called from HAL_UART_RxCpltCallback — handles byte accumulation */ -void jetson_uart_rx_callback(UART_HandleTypeDef *huart); - -#endif /* JETSON_UART_H */ diff --git a/legacy/stm32/include/jlink.h b/legacy/stm32/include/jlink.h deleted file mode 100644 index d99436c..0000000 --- a/legacy/stm32/include/jlink.h +++ /dev/null @@ -1,441 +0,0 @@ -#ifndef JLINK_H -#define JLINK_H - -#include -#include -#include "pid_flash.h" /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */ - -/* - * JLink -- Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX). - * - * Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated - * hardware UART at 921600 baud using DMA circular RX and IDLE interrupt. - * - * Frame format (both directions): - * [STX=0x02][LEN][CMD][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03] - * - * STX : frame start sentinel (0x02) - * LEN : count of CMD + PAYLOAD bytes (1 + payload_len) - * CMD : command/telemetry type byte - * PAYLOAD: 0..N bytes depending on CMD - * CRC16 : CRC16-XModem over CMD+PAYLOAD (poly 0x1021, init 0), big-endian - * ETX : frame end sentinel (0x03) - * - * Jetson to STM32 commands: - * 0x01 HEARTBEAT - no payload; refreshes heartbeat timer - * 0x02 DRIVE - int16 speed (-1000..+1000), int16 steer (-1000..+1000) - * 0x03 ARM - no payload; request arm (same interlock as CDC 'A') - * 0x04 DISARM - no payload; disarm immediately - * 0x05 PID_SET - float kp, float ki, float kd (12 bytes, IEEE-754 LE) - * 0x06 DFU_ENTER - no payload; request OTA DFU reboot (denied while armed) - * 0x07 ESTOP - no payload; engage emergency stop - * 0x08 AUDIO - int16 PCM samples (up to 126 samples) - * 0x09 SLEEP - no payload; request STOP-mode sleep - * 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531) - * 0x0B GIMBAL_POS - int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) - * 0x0C SCHED_GET - no payload; reply with TLM_SCHED (Issue #550) - * 0x0D SCHED_SET - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550) - * 0x0E SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550) - * 0x0F FAULT_LOG_GET - no payload; reply with TLM_FAULT_LOG (Issue #565) - * 0x10 CAN_STATS_GET - no payload; reply with TLM_CAN_STATS (Issue #597) - * - * STM32 to Jetson telemetry: - * 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ - * 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ - * 0x82 BATTERY - jlink_tlm_battery_t (10 bytes, Issue #533) - * 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531) - * 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547) - * 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550) - * 0x86 MOTOR_CURRENT - jlink_tlm_motor_current_t (8 bytes, Issue #584) - * 0x87 FAULT_LOG - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565) - * 0x88 SLOPE - jlink_tlm_slope_t (4 bytes), sent at SLOPE_TLM_HZ (Issue #600) - * 0x89 CAN_STATS - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597) - * 0x8A STEERING - jlink_tlm_steering_t (8 bytes), sent at STEER_TLM_HZ (Issue #616) - * 0x8B LVC - jlink_tlm_lvc_t (4 bytes), sent at LVC_TLM_HZ (Issue #613) - * 0x8C ODOM - jlink_tlm_odom_t (16 bytes), sent at ENC_TLM_HZ (Issue #632) - * - * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied - * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and - * RC_ASSISTED modes the Jetson speed offset and steer are injected via - * mode_manager_set_auto_cmd() and blended per the existing blend ramp. - * - * Heartbeat: if no valid frame arrives within JLINK_HB_TIMEOUT_MS (1000ms), - * jlink_is_active() returns false and the main loop clears the auto command. - */ - -/* ---- Frame constants ---- */ -#define JLINK_STX 0x02u -#define JLINK_ETX 0x03u - -/* ---- Command IDs (Jetson to STM32) ---- */ -#define JLINK_CMD_HEARTBEAT 0x01u -#define JLINK_CMD_DRIVE 0x02u -#define JLINK_CMD_ARM 0x03u -#define JLINK_CMD_DISARM 0x04u -#define JLINK_CMD_PID_SET 0x05u -#define JLINK_CMD_DFU_ENTER 0x06u -#define JLINK_CMD_ESTOP 0x07u -#define JLINK_CMD_AUDIO 0x08u /* PCM audio chunk: int16 samples, up to 126 */ -#define JLINK_CMD_SLEEP 0x09u /* no payload; request STOP-mode sleep */ -#define JLINK_CMD_PID_SAVE 0x0Au /* no payload; save Kp/Ki/Kd to flash (Issue #531) */ -#define JLINK_CMD_GIMBAL_POS 0x0Bu /* int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) */ -#define JLINK_CMD_SCHED_GET 0x0Cu /* no payload; reply TLM_SCHED (Issue #550) */ -#define JLINK_CMD_SCHED_SET 0x0Du /* uint8 num_bands + N*16-byte entries (Issue #550) */ -#define JLINK_CMD_SCHED_SAVE 0x0Eu /* float kp,ki,kd; save sched+single to flash (Issue #550) */ -#define JLINK_CMD_FAULT_LOG_GET 0x0Fu /* no payload; reply TLM_FAULT_LOG (Issue #565) */ -#define JLINK_CMD_CAN_STATS_GET 0x10u /* no payload; reply TLM_CAN_STATS (Issue #597) */ - -/* ---- Telemetry IDs (STM32 to Jetson) ---- */ -#define JLINK_TLM_STATUS 0x80u -#define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */ -#define JLINK_TLM_BATTERY 0x82u /* jlink_tlm_battery_t (10 bytes, Issue #533) */ -#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */ -#define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */ -#define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */ -#define JLINK_TLM_MOTOR_CURRENT 0x86u /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */ -#define JLINK_TLM_FAULT_LOG 0x87u /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */ -#define JLINK_TLM_SLOPE 0x88u /* jlink_tlm_slope_t (4 bytes, Issue #600) */ -#define JLINK_TLM_CAN_STATS 0x89u /* jlink_tlm_can_stats_t (16 bytes, Issue #597) */ -#define JLINK_TLM_STEERING 0x8Au /* jlink_tlm_steering_t (8 bytes, Issue #616) */ -#define JLINK_TLM_LVC 0x8Bu /* jlink_tlm_lvc_t (4 bytes, Issue #613) */ -#define JLINK_TLM_ODOM 0x8Cu /* jlink_tlm_odom_t (16 bytes, Issue #632) */ -#define JLINK_TLM_BARO 0x8Du /* jlink_tlm_baro_t (12 bytes, Issue #672) */ -#define JLINK_TLM_VESC_STATE 0x8Eu /* jlink_tlm_vesc_state_t (22 bytes, Issue #674) */ -#define JLINK_TLM_CAN_WDOG 0x8Fu /* jlink_tlm_can_wdog_t (16 bytes, Issue #694) */ - -/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ -typedef struct __attribute__((packed)) { - int16_t pitch_x10; /* pitch degrees x10 */ - int16_t roll_x10; /* roll degrees x10 */ - int16_t yaw_x10; /* yaw degrees x10 (gyro-integrated) */ - int16_t motor_cmd; /* ESC output -1000..+1000 */ - uint16_t vbat_mv; /* battery millivolts */ - int8_t rssi_dbm; /* CRSF RSSI (dBm, negative) */ - uint8_t link_quality; /* CRSF LQ 0-100 */ - uint8_t balance_state; /* 0=DISARMED, 1=ARMED, 2=TILT_FAULT */ - uint8_t rc_armed; /* crsf_state.armed (1=armed) */ - uint8_t mode; /* robot_mode_t: 0=RC_MANUAL,1=ASSISTED,2=AUTONOMOUS */ - uint8_t estop; /* EstopSource value */ - uint8_t soc_pct; /* state-of-charge 0-100, 255=unknown */ - uint8_t fw_major; - uint8_t fw_minor; - uint8_t fw_patch; -} jlink_tlm_status_t; /* 20 bytes */ - -/* ---- Telemetry POWER payload (11 bytes, packed) ---- */ -typedef struct __attribute__((packed)) { - uint8_t power_state; /* PowerState: 0=ACTIVE,1=SLEEP_PENDING,2=SLEEPING,3=WAKING */ - uint16_t est_total_ma; /* estimated total current draw (mA) */ - uint16_t est_audio_ma; /* estimated I2S3+amp current (mA); 0 if gated */ - uint16_t est_osd_ma; /* estimated OSD SPI2 current (mA); 0 if gated */ - uint32_t idle_ms; /* ms since last cmd_vel activity */ -} jlink_tlm_power_t; /* 11 bytes */ - -/* ---- Telemetry BATTERY payload (10 bytes, packed) Issue #533 ---- */ -typedef struct __attribute__((packed)) { - uint16_t vbat_mv; /* DMA-sampled LPF-filtered Vbat (mV) */ - int16_t ibat_ma; /* DMA-sampled LPF-filtered Ibat (mA, + = discharge) */ - uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */ - uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */ - int8_t cal_offset; /* vbat_offset_mv / 4 (+-127 -> +-508 mV) */ - uint8_t lpf_shift; /* IIR shift factor (alpha = 1/2^lpf_shift) */ - uint8_t soc_pct; /* voltage-based SoC 0-100, 255 = unknown */ -} jlink_tlm_battery_t; /* 10 bytes */ - -/* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */ -/* Sent after JLINK_CMD_PID_SAVE is processed; confirms gains written to flash. */ -typedef struct __attribute__((packed)) { - float kp; /* Kp saved */ - float ki; /* Ki saved */ - float kd; /* Kd saved */ - uint8_t saved_ok; /* 1 = flash write verified, 0 = write failed */ -} jlink_tlm_pid_result_t; /* 13 bytes */ - -/* ---- Telemetry GIMBAL_STATE payload (10 bytes, packed) Issue #547 ---- */ -/* Sent at GIMBAL_TLM_HZ (50 Hz); reports measured pan/tilt and speed. */ -typedef struct __attribute__((packed)) { - int16_t pan_x10; /* Measured pan angle, deg x10 */ - int16_t tilt_x10; /* Measured tilt angle, deg x10 */ - uint16_t pan_speed_raw; /* Present speed register, pan servo */ - uint16_t tilt_speed_raw; /* Present speed register, tilt servo */ - uint8_t torque_en; /* 1 = torque enabled */ - uint8_t rx_err_pct; /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */ -} jlink_tlm_gimbal_state_t; /* 10 bytes */ - -/* ---- Telemetry SCHED payload (1 + N*16 bytes, packed) Issue #550 ---- */ -/* Sent in response to JLINK_CMD_SCHED_GET; N = num_bands (1..PID_SCHED_MAX_BANDS). */ -typedef struct __attribute__((packed)) { - uint8_t num_bands; /* number of valid entries */ - pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* up to 6 x 16 = 96 bytes */ -} jlink_tlm_sched_t; /* 1 + 96 = 97 bytes max */ - -/* ---- Telemetry MOTOR_CURRENT payload (8 bytes, packed) Issue #584 ---- */ -/* Published at MOTOR_CURR_TLM_HZ; reports measured current and protection state. */ -typedef struct __attribute__((packed)) { - int32_t current_ma; /* filtered battery/motor current (mA, + = discharge) */ - uint8_t limit_pct; /* soft-limit reduction applied: 0=none, 100=full cutoff */ - uint8_t state; /* MotorCurrentState: 0=NORMAL,1=SOFT_LIMIT,2=COOLDOWN */ - uint8_t fault_count; /* lifetime hard-cutoff trips (saturates at 255) */ - uint8_t _pad; /* reserved */ -} jlink_tlm_motor_current_t; /* 8 bytes */ - -/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */ -/* Sent at SLOPE_TLM_HZ (1 Hz) by slope_estimator_send_tlm(). */ -typedef struct __attribute__((packed)) { - int16_t slope_x100; /* terrain slope estimate (degrees x100; + = nose-up) */ - uint8_t active; /* 1 = slope estimation enabled */ - uint8_t _pad; -} jlink_tlm_slope_t; /* 4 bytes */ - -/* ---- Telemetry FAULT_LOG payload (20 bytes, packed) Issue #565 ---- */ -/* Sent on boot (if last fault != NONE) and in response to FAULT_LOG_GET. */ -typedef struct __attribute__((packed)) { - uint8_t fault_type; /* FaultType of most recent entry */ - uint8_t entry_count; /* number of valid entries in flash log (0-8) */ - uint8_t reset_count; /* lifetime reset counter */ - uint8_t _pad; - uint32_t timestamp_ms; /* HAL_GetTick() at fault */ - uint32_t pc; /* faulting PC */ - uint32_t lr; /* link register at fault */ - uint32_t cfsr; /* SCB->CFSR */ - uint32_t hfsr; /* SCB->HFSR */ -} jlink_tlm_fault_log_t; /* 20 bytes */ - -/* ---- Telemetry CAN_STATS payload (16 bytes, packed) Issue #597 ---- */ -/* Sent at CAN_TLM_HZ (1 Hz) and in response to CAN_STATS_GET. */ -typedef struct __attribute__((packed)) { - uint32_t tx_count; /* total VELOCITY_CMD frames sent */ - uint32_t rx_count; /* total valid FEEDBACK frames received */ - uint16_t err_count; /* CAN error frame count */ - uint8_t bus_off; /* 1 = bus-off state */ - uint8_t node_faults; /* bit0 = node 0 fault active, bit1 = node 1 */ - int16_t vel0_rpm; /* node 0 current velocity (RPM) */ - int16_t vel1_rpm; /* node 1 current velocity (RPM) */ -} jlink_tlm_can_stats_t; /* 16 bytes */ - -/* ---- Telemetry STEERING payload (8 bytes, packed) Issue #616 ---- */ -/* Published at STEER_TLM_HZ (10 Hz); reports yaw-rate PID state. */ -typedef struct __attribute__((packed)) { - int16_t target_x10; /* target yaw rate, deg/s × 10 (0.1 deg/s resolution) */ - int16_t actual_x10; /* measured yaw rate, deg/s × 10 */ - int16_t output; /* differential motor output (-STEER_OUTPUT_MAX..+MAX) */ - uint8_t enabled; /* 1 = PID active */ - uint8_t _pad; /* reserved */ -} jlink_tlm_steering_t; /* 8 bytes */ - -/* ---- Telemetry LVC payload (4 bytes, packed) Issue #613 ---- */ -/* Sent at LVC_TLM_HZ (1 Hz); reports battery voltage and LVC protection state. */ -typedef struct __attribute__((packed)) { - uint16_t voltage_mv; /* battery voltage (mV) */ - uint8_t percent; /* 0-100: fuel gauge within CUTOFF..WARNING; 255=unknown */ - uint8_t protection_state; /* LvcState: 0=NORMAL,1=WARNING,2=CRITICAL,3=CUTOFF */ -} jlink_tlm_lvc_t; /* 4 bytes */ - -/* ---- Telemetry ODOM payload (16 bytes, packed) Issue #632 ---- */ -/* Sent at ENC_TLM_HZ (50 Hz); wheel RPM, pose, and linear speed. */ -typedef struct __attribute__((packed)) { - int16_t rpm_left; /* left wheel RPM (signed; + = forward) */ - int16_t rpm_right; /* right wheel RPM (signed) */ - int32_t x_mm; /* forward displacement (mm, int32) */ - int32_t y_mm; /* lateral displacement (mm, int32) */ - int16_t theta_cdeg; /* heading in centidegrees (0.01 deg steps) */ - int16_t speed_mmps; /* linear speed of centre point (mm/s) */ -} jlink_tlm_odom_t; /* 16 bytes */ - -/* ---- Telemetry BARO payload (12 bytes, packed) Issue #672 ---- */ -/* Sent at BARO_TLM_HZ (1 Hz); reports ambient pressure, temperature, altitude. */ -typedef struct __attribute__((packed)) { - int32_t pressure_pa; /* barometric pressure (Pa) */ - int16_t temp_x10; /* ambient temperature (°C × 10; e.g. 235 = 23.5 °C) */ - int32_t alt_cm; /* pressure altitude above ISA sea level (cm) */ - int16_t humidity_pct_x10; /* %RH × 10 (BME280 only); -1 = BMP280/absent */ -} jlink_tlm_baro_t; /* 12 bytes */ - -/* ---- Telemetry CAN_WDOG payload (16 bytes, packed) Issue #694 ---- */ -/* Sent at 1 Hz; reports CAN bus-error severity and restart history. */ -typedef struct __attribute__((packed)) { - uint32_t restart_count; /* SW bus-off restarts since boot */ - uint32_t busoff_count; /* lifetime bus-off entry events */ - uint16_t errpassive_count; /* error-passive transitions */ - uint16_t errwarn_count; /* error-warning transitions */ - uint8_t error_state; /* can_error_state_t: 0=OK,1=WARN,2=EP,3=BOFF */ - uint8_t tec; /* transmit error counter (ESR[23:16]) */ - uint8_t rec; /* receive error counter (ESR[31:24]) */ - uint8_t _pad; /* reserved */ -} jlink_tlm_can_wdog_t; /* 16 bytes */ - -/* ---- Telemetry VESC_STATE payload (22 bytes, packed) Issue #674 ---- */ -/* Sent at VESC_TLM_HZ (1 Hz) by vesc_can_send_tlm(). */ -typedef struct __attribute__((packed)) { - int32_t left_rpm; /* left VESC actual RPM */ - int32_t right_rpm; /* right VESC actual RPM */ - int16_t left_current_x10; /* left phase current (A × 10) */ - int16_t right_current_x10; /* right phase current (A × 10) */ - int16_t left_temp_x10; /* left FET temperature (°C × 10) */ - int16_t right_temp_x10; /* right FET temperature (°C × 10) */ - int16_t voltage_x10; /* input voltage (V × 10; from STATUS_5) */ - uint8_t left_fault; /* left VESC fault code (0 = none) */ - uint8_t right_fault; /* right VESC fault code (0 = none) */ - uint8_t left_alive; /* 1 = left VESC alive (STATUS within 1 s) */ - uint8_t right_alive; /* 1 = right VESC alive (STATUS within 1 s) */ -} jlink_tlm_vesc_state_t; /* 22 bytes */ - -/* ---- Volatile state (read from main loop) ---- */ -typedef struct { - /* Drive command - updated on JLINK_CMD_DRIVE */ - volatile int16_t speed; /* -1000..+1000 */ - volatile int16_t steer; /* -1000..+1000 */ - - /* Heartbeat timer - updated on any valid frame */ - volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame; 0=none */ - - /* One-shot request flags - set by parser, cleared by main loop */ - volatile uint8_t arm_req; - volatile uint8_t disarm_req; - volatile uint8_t estop_req; - - /* PID update - set by parser, cleared by main loop */ - volatile uint8_t pid_updated; - volatile float pid_kp; - volatile float pid_ki; - volatile float pid_kd; - - /* DFU reboot request - set by parser, cleared by main loop */ - volatile uint8_t dfu_req; - /* Sleep request - set by JLINK_CMD_SLEEP, cleared by main loop */ - volatile uint8_t sleep_req; - /* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */ - volatile uint8_t pid_save_req; - - /* Gimbal position command - set by JLINK_CMD_GIMBAL_POS (Issue #547) */ - volatile uint8_t gimbal_updated; /* set by parser, cleared by main loop */ - volatile int16_t gimbal_pan_x10; /* pan angle deg x10 */ - volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */ - volatile uint16_t gimbal_speed; /* servo speed 0-4095 (0=max) */ - - /* PID schedule commands (Issue #550) - set by parser, cleared by main loop */ - volatile uint8_t sched_get_req; /* SCHED_GET: main loop calls jlink_send_sched_telemetry() */ - volatile uint8_t sched_save_req; /* SCHED_SAVE: main loop calls pid_schedule_flash_save() */ - volatile float sched_save_kp; /* kp for single-PID record in SCHED_SAVE */ - volatile float sched_save_ki; - volatile float sched_save_kd; - - /* Fault log request (Issue #565) - set by JLINK_CMD_FAULT_LOG_GET, cleared by main loop */ - volatile uint8_t fault_log_req; - - /* CAN stats request (Issue #597) - set by JLINK_CMD_CAN_STATS_GET, cleared by main loop */ - volatile uint8_t can_stats_req; -} JLinkState; - -extern volatile JLinkState jlink_state; - -/* ---- SCHED_SET receive buffer -- Issue #550 ---- */ -/* - * Populated by the parser on JLINK_CMD_SCHED_SET. Main loop reads via - * jlink_get_sched_set() and calls pid_schedule_set_table() before clearing. - */ -typedef struct { - volatile uint8_t ready; /* set by parser, cleared by main loop */ - volatile uint8_t num_bands; - pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* copied from frame */ -} JLinkSchedSetBuf; - -/* ---- API ---- */ - -void jlink_init(void); -bool jlink_is_active(uint32_t now_ms); -void jlink_process(void); -void jlink_send_telemetry(const jlink_tlm_status_t *status); -void jlink_send_power_telemetry(const jlink_tlm_power_t *power); -void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt); -void jlink_send_pid_result(const jlink_tlm_pid_result_t *result); - -/* - * jlink_send_gimbal_state(state) - transmit JLINK_TLM_GIMBAL_STATE (0x84) - * frame (16 bytes) at GIMBAL_TLM_HZ (50 Hz). Issue #547. - */ -void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state); - -/* - * jlink_send_sched_telemetry(tlm) - transmit JLINK_TLM_SCHED (0x85) in - * response to SCHED_GET. tlm->num_bands determines actual frame size. - * Issue #550. - */ -void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm); - -/* - * jlink_get_sched_set() - return pointer to the most-recently received - * SCHED_SET payload buffer (static storage in jlink.c). Main loop calls - * pid_schedule_set_table() from this buffer, then clears ready. Issue #550. - */ -JLinkSchedSetBuf *jlink_get_sched_set(void); - -/* - * jlink_send_motor_current_tlm(tlm) - transmit JLINK_TLM_MOTOR_CURRENT (0x86) - * frame (14 bytes total) to Jetson. Issue #584. - * Rate-limiting is handled by motor_current_send_tlm(); call from there only. - */ -void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm); - -/* - * jlink_send_fault_log(fl) - transmit JLINK_TLM_FAULT_LOG (0x87) frame - * (26 bytes) on boot (if fault log non-empty) and in response to - * FAULT_LOG_GET. Issue #565. - */ -void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl); - -/* - * jlink_send_slope_tlm(tlm) - transmit JLINK_TLM_SLOPE (0x88) frame - * (10 bytes) at SLOPE_TLM_HZ (1 Hz). Called from slope_estimator_send_tlm(). - * Issue #600. - */ -void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm); - -/* - * jlink_send_can_stats(tlm) - transmit JLINK_TLM_CAN_STATS (0x89) frame - * (22 bytes) at CAN_TLM_HZ (1 Hz) and in response to CAN_STATS_GET. - * Issue #597. - */ -void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm); - -/* - * jlink_send_steering_tlm(tlm) - transmit JLINK_TLM_STEERING (0x8A) frame - * (14 bytes total) to Jetson. Issue #616. - * Rate-limiting is handled by steering_pid_send_tlm(); call from there only. - */ -void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm); - -/* - * jlink_send_lvc_tlm(tlm) - transmit JLINK_TLM_LVC (0x8B) frame - * (10 bytes total) at LVC_TLM_HZ (1 Hz). Issue #613. - */ -void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm); - -/* - * jlink_send_odom_tlm(tlm) - transmit JLINK_TLM_ODOM (0x8C) frame - * (22 bytes total) at ENC_TLM_HZ (50 Hz). Issue #632. - * Rate-limiting handled by encoder_odom_send_tlm(); call from there only. - */ -void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm); - -/* - * jlink_send_baro_tlm(tlm) - transmit JLINK_TLM_BARO (0x8D) frame - * (18 bytes total) at BARO_TLM_HZ (1 Hz). Issue #672. - * Rate-limiting handled by baro_tick(); call from there only. - */ -void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm); - -/* - * jlink_send_vesc_state_tlm(tlm) - transmit JLINK_TLM_VESC_STATE (0x8E) frame - * (28 bytes total) at VESC_TLM_HZ (1 Hz). Issue #674. - * Rate-limiting handled by vesc_can_send_tlm(); call from there only. - */ -void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm); - -/* - * jlink_send_can_wdog_tlm(tlm) - transmit JLINK_TLM_CAN_WDOG (0x8F) frame - * (22 bytes total) at 1 Hz. Issue #694. - */ -void jlink_send_can_wdog_tlm(const jlink_tlm_can_wdog_t *tlm); - -#endif /* JLINK_H */ diff --git a/legacy/stm32/include/led.h b/legacy/stm32/include/led.h deleted file mode 100644 index 90f8dc2..0000000 --- a/legacy/stm32/include/led.h +++ /dev/null @@ -1,101 +0,0 @@ -#ifndef LED_H -#define LED_H - -#include -#include - -/* - * led.h — WS2812B NeoPixel status indicator driver (Issue #193) - * - * Hardware: TIM3_CH1 PWM on PB4 at 800 kHz (1.25 µs per bit). - * Controls an 8-LED ring with state-based animations: - * - Boot: Blue chase (startup sequence) - * - Armed: Solid green - * - Error: Red blinking (visual alert) - * - Low Battery: Yellow pulsing (warning) - * - Charging: Green breathing (soft indication) - * - E-Stop: Red strobe (immediate action required) - * - * State transitions are non-blocking via a 1 ms timer callback (led_tick). - * Each state defines its own animation envelope: color, timing, and brightness. - * - * WS2812 protocol (NRZ): - * - Bit "0": High 350 ns, Low 800 ns (1.25 µs total) - * - Bit "1": High 700 ns, Low 600 ns (1.25 µs total) - * - Reset: Low > 50 µs - * - * PWM-based implementation via DMA: - * - 10 levels: [350 ns, 400, 450, 500, 550, 600, 650, 700, 750, 800] - * - Bit "0" → High 350-400 ns Bit "1" → High 650-800 ns - * - Each bit requires one PWM cycle; 24 bits/LED × 8 LEDs = 192 cycles - * - DMA rings through buffer, auto-reloads on update events - */ - -/* LED state enumeration */ -typedef enum { - LED_STATE_BOOT = 0, /* Blue chase (startup) */ - LED_STATE_ARMED = 1, /* Solid green */ - LED_STATE_ERROR = 2, /* Red blinking */ - LED_STATE_LOW_BATT = 3, /* Yellow pulsing */ - LED_STATE_CHARGING = 4, /* Green breathing */ - LED_STATE_ESTOP = 5, /* Red strobe */ - LED_STATE_COUNT -} LEDState; - -/* RGB color (8-bit per channel) */ -typedef struct { - uint8_t r; - uint8_t g; - uint8_t b; -} RGBColor; - -/* - * led_init() - * - * Configure TIM3_CH1 PWM on PB4 at 800 kHz, set up DMA for bit streaming, - * and initialize the LED buffer. Call once at startup, after buzzer_init() - * but before the main loop. - */ -void led_init(void); - -/* - * led_set_state(state) - * - * Change the LED display state. The animation runs non-blocking via led_tick(). - * Valid states: LED_STATE_BOOT, LED_STATE_ARMED, LED_STATE_ERROR, etc. - */ -void led_set_state(LEDState state); - -/* - * led_get_state() - * - * Return the current LED state. - */ -LEDState led_get_state(void); - -/* - * led_set_color(r, g, b) - * - * Manually set the LED ring to a solid color. Overrides the current state - * animation until led_set_state() is called again. - */ -void led_set_color(uint8_t r, uint8_t g, uint8_t b); - -/* - * led_tick(now_ms) - * - * Advance animation state machine. Must be called every 1 ms from the main loop. - * Handles state-specific animations: chase timing, pulse envelope, strobe phase, etc. - * Updates the DMA buffer with new LED values without blocking. - */ -void led_tick(uint32_t now_ms); - -/* - * led_is_animating() - * - * Returns true if the current state is actively animating (e.g., chase, pulse, strobe). - * Returns false for static states (armed, error solid). - */ -bool led_is_animating(void); - -#endif /* LED_H */ diff --git a/legacy/stm32/include/lvc.h b/legacy/stm32/include/lvc.h deleted file mode 100644 index e3cf0b5..0000000 --- a/legacy/stm32/include/lvc.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef LVC_H -#define LVC_H - -#include -#include - -/* - * lvc.h -- Low Voltage Cutoff (LVC) protection (Issue #613) - * - * 3-stage battery voltage protection using battery_read_mv(): - * - * LVC_WARNING (21.0 V) -- periodic buzzer alert; full power maintained - * LVC_CRITICAL (19.8 V) -- faster buzzer; motor commands scaled to 50% - * LVC_CUTOFF (18.6 V) -- error buzzer; motors disabled; latched until reboot - * - * Recovery uses LVC_HYSTERESIS_MV to prevent threshold chatter. - * CUTOFF is one-way: once latched, only a power-cycle clears it. - * - * Integration: - * lvc_init() -- call once during system init - * lvc_tick(now_ms, vbat_mv) -- call each main loop tick (1 kHz) - * lvc_get_power_scale() -- returns 0/50/100; apply to motor speed - * lvc_is_cutoff() -- true when motors must be disabled - */ - -typedef enum { - LVC_NORMAL = 0, /* Vbat >= WARNING threshold */ - LVC_WARNING = 1, /* Vbat < 21.0 V -- alert only */ - LVC_CRITICAL = 2, /* Vbat < 19.8 V -- 50% power */ - LVC_CUTOFF = 3, /* Vbat < 18.6 V -- motors off */ -} LvcState; - -void lvc_init(void); -void lvc_tick(uint32_t now_ms, uint32_t vbat_mv); -LvcState lvc_get_state(void); -uint8_t lvc_get_power_scale(void); /* 100 = full, 50 = critical, 0 = cutoff */ -bool lvc_is_cutoff(void); - -#endif /* LVC_H */ diff --git a/legacy/stm32/include/mag.h b/legacy/stm32/include/mag.h deleted file mode 100644 index 80ba0de..0000000 --- a/legacy/stm32/include/mag.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef MAG_H -#define MAG_H - -#include - -typedef enum { - MAG_NONE = 0, - MAG_QMC5883L, /* I2C 0x0D — most common on external compass modules */ - MAG_HMC5883L, /* I2C 0x1E — legacy Honeywell part */ - MAG_IST8310, /* I2C 0x0E — iSentek part, common on CUAV boards */ -} mag_type_t; - -/* - * Auto-detect magnetometer on I2C1. - * Returns detected type (MAG_NONE if nothing found). - * Requires i2c1_init() before calling. - */ -mag_type_t mag_init(void); - -/* - * Read compass heading (degrees × 10, 0–3599). - * Returns -1 if data not ready or no sensor. - */ -int16_t mag_read_heading(void); - -#endif /* MAG_H */ diff --git a/legacy/stm32/include/mode_manager.h b/legacy/stm32/include/mode_manager.h deleted file mode 100644 index 4b8194c..0000000 --- a/legacy/stm32/include/mode_manager.h +++ /dev/null @@ -1,74 +0,0 @@ -#ifndef MODE_MANAGER_H -#define MODE_MANAGER_H - -#include -#include - -/* - * SaltyLab Mode Manager - * - * Resolves three operating modes selected by RC CH6 (3-pos switch): - * - * RC_MANUAL — RC steer (CH4) and speed bias (CH3) applied directly. - * Balance PID remains active for stability. - * RC_ASSISTED — RC inputs blended 50/50 with Jetson autonomous commands. - * AUTONOMOUS — Jetson commands only; RC CH5 arm switch still kills motors. - * - * Transitions between modes are smoothed over MODE_BLEND_MS (~500ms) to - * prevent jerky handoffs. A single `blend` scalar (0=pure RC, 1=pure auto) - * drives all interpolation; adjacent-mode steps take ~250ms each. - * - * RC safety rule: if RC is alive and CH5 is disarmed, the main loop MUST - * disarm regardless of mode. mode_manager only blends commands — kill - * authority lives in the main loop. - * - * Autonomous commands are set by the Jetson serial bridge via - * mode_manager_set_auto_cmd(). They default to zero (no motion). - */ - -typedef enum { - MODE_RC_MANUAL = 0, - MODE_RC_ASSISTED = 1, - MODE_AUTONOMOUS = 2, -} robot_mode_t; - -typedef struct { - robot_mode_t target; /* Mode requested by CH6 (or fallback) */ - float blend; /* 0.0=pure RC .. 1.0=pure auto, smoothly ramped */ - bool rc_alive; /* Cached RC liveness (set in update) */ - int16_t auto_steer; /* Jetson steer cmd (-1000..+1000) */ - int16_t auto_speed_bias;/* Jetson speed bias (-MOTOR_RC_SPEED_MAX..+) */ -} mode_manager_t; - -/* Initialise — call once before the main loop */ -void mode_manager_init(mode_manager_t *m); - -/* - * Call every main-loop tick (1ms) to: - * - read CH6, update target mode - * - cache RC liveness - * - advance blend ramp toward target blend value - */ -void mode_manager_update(mode_manager_t *m, uint32_t now); - -/* Set autonomous commands from the Jetson serial bridge */ -void mode_manager_set_auto_cmd(mode_manager_t *m, - int16_t steer, - int16_t speed_bias); - -/* - * Blended steer command to pass to motor_driver_update(). - * Returns 0 when RC is not alive and no autonomous steer set. - */ -int16_t mode_manager_get_steer(const mode_manager_t *m); - -/* - * Blended speed bias to add to bal.motor_cmd before motor_driver_update(). - * Returns 0 when RC is not alive and no autonomous speed set. - */ -int16_t mode_manager_get_speed_bias(const mode_manager_t *m); - -/* Quantised current mode (based on blend position, not target) */ -robot_mode_t mode_manager_active(const mode_manager_t *m); - -#endif diff --git a/legacy/stm32/include/motor_current.h b/legacy/stm32/include/motor_current.h deleted file mode 100644 index 9c4abd1..0000000 --- a/legacy/stm32/include/motor_current.h +++ /dev/null @@ -1,121 +0,0 @@ -#ifndef MOTOR_CURRENT_H -#define MOTOR_CURRENT_H - -#include -#include - -/* - * motor_current — ADC-based motor current monitoring and overload protection - * for Issue #584. - * - * Hardware: - * ADC3 IN13 (PC3, ADC_CURR_PIN) is already sampled by battery_adc.c via - * DMA2_Stream0 circular. This module reads battery_adc_get_current_ma() - * each tick rather than running a second ADC, since total discharge current - * on this single-motor balance bot equals motor current plus ~30 mA overhead. - * - * Behaviour: - * MC_NORMAL : current_ma < MOTOR_CURR_SOFT_MA — full output - * MC_SOFT_LIMIT : current_ma in [SOFT_MA, HARD_MA) — linear PWM reduction - * MC_COOLDOWN : hard cutoff latched after HARD_MA sustained for - * MOTOR_CURR_OVERLOAD_MS (2 s) — zero output for - * MOTOR_CURR_COOLDOWN_MS (10 s), then MC_NORMAL - * - * Soft limit formula (MC_SOFT_LIMIT): - * scale = (HARD_MA - current_ma) / (HARD_MA - SOFT_MA) [0..1] - * limited_cmd = (int16_t)(cmd * scale) - * - * Fault event: - * On each hard-cutoff trip, s_fault_count is incremented (saturates at 255) - * and motor_current_fault_pending() returns true for one main-loop tick so - * the caller can append a fault log entry. - * - * Main-loop integration (pseudo-code): - * - * void main_loop_tick(uint32_t now_ms) { - * battery_adc_tick(now_ms); - * motor_current_tick(now_ms); - * - * if (motor_current_fault_pending()) - * fault_log_append(FAULT_MOTOR_OVERCURRENT); - * - * int16_t cmd = balance_pid_output(); - * cmd = motor_current_apply_limit(cmd); - * motor_driver_update(&g_motor, cmd, steer, now_ms); - * - * motor_current_send_tlm(now_ms); // rate-limited to MOTOR_CURR_TLM_HZ - * } - */ - -/* ---- Thresholds ---- */ -#define MOTOR_CURR_HARD_MA 5000u /* 5 A — hard cutoff level */ -#define MOTOR_CURR_SOFT_MA 4000u /* 4 A — soft-limit onset (80% of hard) */ -#define MOTOR_CURR_OVERLOAD_MS 2000u /* sustained over HARD_MA before fault */ -#define MOTOR_CURR_COOLDOWN_MS 10000u /* zero-output recovery period (ms) */ -#define MOTOR_CURR_TLM_HZ 5u /* JLINK_TLM_MOTOR_CURRENT publish rate */ - -/* ---- State enum ---- */ -typedef enum { - MC_NORMAL = 0, - MC_SOFT_LIMIT = 1, - MC_COOLDOWN = 2, -} MotorCurrentState; - -/* ---- API ---- */ - -/* - * motor_current_init() — reset all state. - * Call once during system init, after battery_adc_init(). - */ -void motor_current_init(void); - -/* - * motor_current_tick(now_ms) — evaluate ADC reading, update state machine. - * Call from main loop after battery_adc_tick(), at any rate ≥ 10 Hz. - * Non-blocking (<1 µs). - */ -void motor_current_tick(uint32_t now_ms); - -/* - * motor_current_apply_limit(cmd) — scale motor command by current-limit factor. - * MC_NORMAL: returns cmd unchanged. - * MC_SOFT_LIMIT: returns cmd scaled down linearly. - * MC_COOLDOWN: returns 0. - * Call after motor_current_tick() each loop iteration. - */ -int16_t motor_current_apply_limit(int16_t cmd); - -/* - * motor_current_is_faulted() — true while in MC_COOLDOWN (output zeroed). - */ -bool motor_current_is_faulted(void); - -/* - * motor_current_state() — current state machine state. - */ -MotorCurrentState motor_current_state(void); - -/* - * motor_current_ma() — most recent ADC reading used by the state machine (mA). - */ -int32_t motor_current_ma(void); - -/* - * motor_current_fault_count() — lifetime hard-cutoff trip counter (0..255). - */ -uint8_t motor_current_fault_count(void); - -/* - * motor_current_fault_pending() — true for exactly one tick after a hard - * cutoff trip fires. Main loop should append a fault log entry and then the - * flag clears automatically on the next call. - */ -bool motor_current_fault_pending(void); - -/* - * motor_current_send_tlm(now_ms) — transmit JLINK_TLM_MOTOR_CURRENT (0x86) - * frame to Jetson. Rate-limited to MOTOR_CURR_TLM_HZ; safe to call every tick. - */ -void motor_current_send_tlm(uint32_t now_ms); - -#endif /* MOTOR_CURRENT_H */ diff --git a/legacy/stm32/include/motor_driver.h b/legacy/stm32/include/motor_driver.h deleted file mode 100644 index 30c4ae2..0000000 --- a/legacy/stm32/include/motor_driver.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef MOTOR_DRIVER_H -#define MOTOR_DRIVER_H - -#include -#include - -/* - * SaltyLab Motor Driver - * - * Sits between the balance PID and the raw hoverboard UART driver. - * Responsibilities: - * - Differential drive mixing: speed = balance_cmd, steer mixed in - * - Steer ramping to avoid sudden yaw torque disturbing balance - * - Headroom clamping: |speed| + |steer| <= MOTOR_CMD_MAX - * - Emergency stop: immediate zero + latch until explicitly cleared - * - * Balance PID output is NOT ramped — it needs full immediate authority. - * Only the steer channel is ramped. - * - * Call motor_driver_update() at the ESC send rate (50Hz / every 20ms). - */ - -typedef struct { - int16_t steer_actual; /* Ramped steer command currently sent */ - bool estop; /* Emergency stop latched */ -} motor_driver_t; - -void motor_driver_init(motor_driver_t *m); - -/* - * Update and send to ESC. - * balance_cmd : PID output, -1000..+1000 - * steer_cmd : desired yaw/steer, -1000..+1000 (future RC/autonomous input) - * now : HAL_GetTick() timestamp (ms) for ramp delta-time - */ -void motor_driver_update(motor_driver_t *m, - int16_t balance_cmd, - int16_t steer_cmd, - uint32_t now); - -/* Latch emergency stop — sends zero immediately */ -void motor_driver_estop(motor_driver_t *m); - -/* Clear emergency stop latch (only call from armed/ready context) */ -void motor_driver_estop_clear(motor_driver_t *m); - -#endif diff --git a/legacy/stm32/include/mpu6000.h b/legacy/stm32/include/mpu6000.h deleted file mode 100644 index 43e7f78..0000000 --- a/legacy/stm32/include/mpu6000.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef MPU6000_H -#define MPU6000_H - -#include -#include - -typedef struct { - float pitch; // degrees, filtered (complementary filter) - float pitch_rate; // degrees/sec (raw gyro pitch axis) - float roll; // degrees, filtered (complementary filter) - float yaw; // degrees, gyro-integrated (drifts — no magnetometer) - float yaw_rate; // degrees/sec (raw gyro Z / board_gz, Issue #616) - float accel_x; // g - float accel_z; // g -} IMUData; - -bool mpu6000_init(void); - -/* - * Sample gyro for ~1s to compute per-axis bias offsets. - * Must be called after mpu6000_init() with the board held still. - * Blocks ~1s. LED1+LED2 solid during calibration. - * IWDG must NOT be running when this is called (call before safety_init()). - */ -void mpu6000_calibrate(void); - -/* Returns true once mpu6000_calibrate() has completed. */ -bool mpu6000_is_calibrated(void); - -void mpu6000_read(IMUData *data); - -/* - * mpu6000_set_mount_offset(pitch_deg, roll_deg) — set mount angle offsets. - * These are subtracted from the pitch and roll outputs in mpu6000_read(). - * Load via imu_cal_flash_load() on boot; update after 'O' CDC command. - * Issue #680. - */ -void mpu6000_set_mount_offset(float pitch_deg, float roll_deg); - -/* Returns true if non-zero mount offsets have been applied (Issue #680). */ -bool mpu6000_has_mount_offset(void); - -#endif diff --git a/legacy/stm32/include/orin_can.h b/legacy/stm32/include/orin_can.h deleted file mode 100644 index aea84b0..0000000 --- a/legacy/stm32/include/orin_can.h +++ /dev/null @@ -1,191 +0,0 @@ -#ifndef ORIN_CAN_H -#define ORIN_CAN_H - -#include -#include - -/* - * orin_can — Orin↔FC CAN protocol driver (Issue #674). - * - * Standard 11-bit CAN IDs on CAN2, FIFO0. - * - * Orin → FC commands: - * 0x300 HEARTBEAT : uint32 sequence counter (4 bytes) - * 0x301 DRIVE : int16 speed (−1000..+1000), int16 steer (−1000..+1000) - * 0x302 MODE : uint8 mode (0=RC_MANUAL, 1=ASSISTED, 2=AUTONOMOUS) - * 0x303 ESTOP : uint8 action (1=ESTOP, 0=CLEAR) - * - * FC → Orin telemetry (broadcast at ORIN_TLM_HZ): - * 0x400 FC_STATUS : int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, - * uint8 balance_state, uint8 flags [bit0=estop, bit1=armed] - * 0x401 FC_VESC : int16 left_rpm_x10 (RPM/10), int16 right_rpm_x10, - * int16 left_current_x10, int16 right_current_x10 - * - * Balance independence: if no Orin heartbeat for ORIN_HB_TIMEOUT_MS, the FC - * continues balancing in-place — Orin commands are simply not injected. - * The balance PID loop runs entirely on Mamba and never depends on Orin. - */ - -/* ---- Orin → FC command IDs ---- */ -#define ORIN_CAN_ID_HEARTBEAT 0x300u -#define ORIN_CAN_ID_DRIVE 0x301u -#define ORIN_CAN_ID_MODE 0x302u -#define ORIN_CAN_ID_ESTOP 0x303u -#define ORIN_CAN_ID_LED_CMD 0x304u /* LED pattern override (Issue #685) */ -#define ORIN_CAN_ID_PID_SET 0x305u /* PID gain update: kp/ki/kd (Issue #693) */ - -/* ---- FC → Orin telemetry IDs ---- */ -#define ORIN_CAN_ID_FC_STATUS 0x400u /* balance state + pitch + vbat at 10 Hz */ -#define ORIN_CAN_ID_FC_VESC 0x401u /* VESC RPM + current at 10 Hz */ -#define ORIN_CAN_ID_FC_IMU 0x402u /* full IMU angles + cal status at 50 Hz (Issue #680) */ -#define ORIN_CAN_ID_FC_BARO 0x403u /* barometer pressure/temp/altitude at 1 Hz (Issue #672) */ -#define ORIN_CAN_ID_FC_BTN 0x404u /* button event on-demand (Issue #682) */ -#define ORIN_CAN_ID_FC_PID_ACK 0x405u /* PID gain ACK: echoes applied kp/ki/kd (Issue #693) */ - -/* ---- Timing ---- */ -#define ORIN_HB_TIMEOUT_MS 500u /* Orin offline after 500 ms without any frame */ -#define ORIN_TLM_HZ 10u /* FC_STATUS + FC_VESC broadcast rate (Hz) */ -#define ORIN_IMU_TLM_HZ 50u /* FC_IMU broadcast rate (Hz) */ -#define ORIN_BARO_TLM_HZ 1u /* FC_BARO broadcast rate (Hz) */ - -/* ---- Volatile state updated by orin_can_on_frame(), read by main loop ---- */ -typedef struct { - volatile int16_t speed; /* DRIVE: −1000..+1000 */ - volatile int16_t steer; /* DRIVE: −1000..+1000 */ - volatile uint8_t mode; /* MODE: robot_mode_t value */ - volatile uint8_t drive_updated; /* set on DRIVE, cleared by main */ - volatile uint8_t mode_updated; /* set on MODE, cleared by main */ - volatile uint8_t estop_req; /* set on ESTOP(1), cleared by main */ - volatile uint8_t estop_clear_req; /* set on ESTOP(0), cleared by main */ - volatile uint32_t last_rx_ms; /* HAL_GetTick() of last received frame */ - /* PID_SET (Issue #693) -- set by orin_can_on_frame(), consumed by main */ - volatile uint8_t pid_updated; /* set on PID_SET, cleared by main */ - volatile uint16_t pid_kp_x100; /* Kp * 100 (0..50000) */ - volatile uint16_t pid_ki_x100; /* Ki * 100 (0..5000) */ - volatile uint16_t pid_kd_x100; /* Kd * 100 (0..5000) */ -} OrinCanState; - -extern volatile OrinCanState orin_can_state; - -/* ---- FC → Orin broadcast payloads (packed, 8 bytes each) ---- */ - -typedef struct __attribute__((packed)) { - int16_t pitch_x10; /* pitch degrees × 10 */ - int16_t motor_cmd; /* balance PID output −1000..+1000 */ - uint16_t vbat_mv; /* battery voltage (mV) */ - uint8_t balance_state; /* BalanceState: 0=DISARMED,1=ARMED,2=TILT_FAULT */ - uint8_t flags; /* bit0=estop_active, bit1=armed */ -} orin_can_fc_status_t; /* 8 bytes */ - -typedef struct __attribute__((packed)) { - int16_t left_rpm_x10; /* left wheel RPM / 10 (±32767 × 10 = ±327k RPM) */ - int16_t right_rpm_x10; /* right wheel RPM / 10 */ - int16_t left_current_x10; /* left phase current × 10 (A) */ - int16_t right_current_x10; /* right phase current × 10 (A) */ -} orin_can_fc_vesc_t; /* 8 bytes */ - -/* FC_IMU (0x402) — full IMU angles at 50 Hz (Issue #680) */ -typedef struct __attribute__((packed)) { - int16_t pitch_x10; /* pitch degrees × 10 (mount-offset corrected) */ - int16_t roll_x10; /* roll degrees × 10 (mount-offset corrected) */ - int16_t yaw_x10; /* yaw degrees × 10 (gyro-integrated, drifts) */ - uint8_t cal_status; /* 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */ - uint8_t balance_state; /* BalanceState: 0=DISARMED, 1=ARMED, 2=TILT_FAULT */ -} orin_can_fc_imu_t; /* 8 bytes */ - -/* FC_BARO (0x403) — barometer at 1 Hz (Issue #672) */ -typedef struct __attribute__((packed)) { - int32_t pressure_pa; /* barometric pressure in Pa */ - int16_t temp_x10; /* temperature × 10 (°C) */ - int16_t alt_cm; /* altitude in cm (reference = pressure at boot) */ -} orin_can_fc_baro_t; /* 8 bytes */ - -/* FC_BTN (0x404) — button event, sent on demand (Issue #682) - * event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED (pitch too large) */ -typedef struct __attribute__((packed)) { - uint8_t event_id; /* 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED */ - uint8_t balance_state; /* balance_state_t value after the event */ -} orin_can_fc_btn_t; /* 2 bytes */ - -/* LED_CMD (0x304) — Orin → FC LED pattern override (Issue #685) - * duration_ms = 0: hold until next state change; >0: revert after duration */ -typedef struct __attribute__((packed)) { - uint8_t pattern; /* 0=state_auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse */ - uint8_t brightness; /* 0-255 (0 = both LEDs off) */ - uint16_t duration_ms; /* override duration; 0 = permanent until state change */ -} orin_can_led_cmd_t; /* 4 bytes */ - -/* LED override state (updated by orin_can_on_frame, read by main loop) */ -extern volatile orin_can_led_cmd_t orin_can_led_override; -extern volatile uint8_t orin_can_led_updated; - -/* ---- API ---- */ - -/* - * orin_can_init() — zero state, register orin_can_on_frame as std_cb with - * can_driver. Call after can_driver_init(). - */ -void orin_can_init(void); - -/* - * orin_can_on_frame(std_id, data, len) — dispatched by can_driver for each - * standard-ID frame in FIFO0. Updates orin_can_state. - */ -void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len); - -/* - * orin_can_is_alive(now_ms) — true if a frame from Orin arrived within - * ORIN_HB_TIMEOUT_MS of now_ms. - */ -bool orin_can_is_alive(uint32_t now_ms); - -/* - * orin_can_broadcast(now_ms, status, vesc) — rate-limited broadcast of - * FC_STATUS (0x400) and FC_VESC (0x401) at ORIN_TLM_HZ (10 Hz). - * Safe to call every ms; internally rate-limited. - */ -void orin_can_broadcast(uint32_t now_ms, - const orin_can_fc_status_t *status, - const orin_can_fc_vesc_t *vesc); - -/* - * orin_can_broadcast_imu(now_ms, imu_tlm) — rate-limited broadcast of - * FC_IMU (0x402) at ORIN_IMU_TLM_HZ (50 Hz). - * Safe to call every ms; internally rate-limited. Issue #680. - */ -void orin_can_broadcast_imu(uint32_t now_ms, - const orin_can_fc_imu_t *imu_tlm); - -/* - * orin_can_broadcast_baro(now_ms, baro_tlm) — rate-limited broadcast of - * FC_BARO (0x403) at ORIN_BARO_TLM_HZ (1 Hz). - * Pass NULL to skip transmission. Issue #672. - */ -void orin_can_broadcast_baro(uint32_t now_ms, - const orin_can_fc_baro_t *baro_tlm); - -/* - * orin_can_send_btn_event(event_id, balance_state) — send FC_BTN (0x404) - * immediately. Call on button park/unpark events. Issue #682. - * event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED. - */ -void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state); - -/* orin_can_send_pid_ack() -- send FC_PID_ACK (0x405). Issue #693. */ -void orin_can_send_pid_ack(float kp, float ki, float kd); - -/* PID_SET (0x305) -- 6-byte payload: kp*100, ki*100, kd*100 (uint16 BE each) */ -typedef struct __attribute__((packed)) { - uint16_t kp_x100; - uint16_t ki_x100; - uint16_t kd_x100; -} orin_can_pid_set_t; - -/* FC_PID_ACK (0x405) -- FC -> Orin echo of applied gains */ -typedef struct __attribute__((packed)) { - uint16_t kp_x100; - uint16_t ki_x100; - uint16_t kd_x100; -} orin_can_fc_pid_ack_t; - -#endif /* ORIN_CAN_H */ diff --git a/legacy/stm32/include/ota.h b/legacy/stm32/include/ota.h deleted file mode 100644 index 2b3d9a5..0000000 --- a/legacy/stm32/include/ota.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef OTA_H -#define OTA_H - -#include -#include - -/* - * OTA firmware update — Issue #124 - * - * DFU entry triggered by JLINK_CMD_DFU_ENTER (0x06) or USB CDC 'R' command. - * Uses RTC backup register OTA_DFU_BKP_IDX to pass magic across the soft reset. - * - * RTC BKP register map: - * BKP0R–BKP5R : BNO055 calibration offsets (PR #150) - * BKP6R : BNO055 magic (0xB055CA10, PR #150) - * BKP7R–BKP14R : Reserved - * BKP15R : OTA DFU magic (this module) - * - * Using BKP15R avoids collision with BNO055 (BKP0–6) and the old BKP0R - * that the original request_bootloader() used before this module. - * - * Dual-bank note: STM32F722 has single-bank flash (512 KB). Hardware A/B - * rollback is not supported without a custom bootloader. DFU via the ST - * system bootloader at 0x1FF00000 is the supported update path. Rollback - * is handled by the host-side flash_firmware.py script, which keeps a - * backup of the previous binary. - */ - -/* RTC backup register index used for DFU magic — avoids BNO055 BKP0–6 */ -#define OTA_DFU_BKP_IDX 15u - -/* Magic value written before reset to trigger DFU entry on next boot */ -#define OTA_DFU_MAGIC 0xDEADBEEFu - -/* STM32F722 internal flash: 512 KB starting at 0x08000000 */ -#define OTA_FLASH_BASE 0x08000000u -#define OTA_FLASH_SIZE 0x00080000u /* 512 KB */ - -/* - * ota_enter_dfu(is_armed) - * - * Request entry to USB DFU mode (ST system bootloader at 0x1FF00000). - * Returns false without side effects if is_armed is true. - * Otherwise: enables backup domain, writes OTA_DFU_MAGIC to BKP15R, - * disables IRQs, calls NVIC_SystemReset(). Never returns on success. - * - * Call from the main loop only (not from ISR context). - */ -bool ota_enter_dfu(bool is_armed); - -/* - * ota_fw_crc32() - * - * Compute a CRC-32/MPEG-2 checksum of the full flash region using the - * STM32 hardware CRC peripheral (poly 0x04C11DB7, init 0xFFFFFFFF, - * 32-bit words, no reflection). Covers OTA_FLASH_SIZE bytes from - * OTA_FLASH_BASE including erased padding. - * - * Takes ~0.5 ms at 216 MHz. Call only while disarmed. - */ -uint32_t ota_fw_crc32(void); - -#endif /* OTA_H */ diff --git a/legacy/stm32/include/pid_flash.h b/legacy/stm32/include/pid_flash.h deleted file mode 100644 index db29718..0000000 --- a/legacy/stm32/include/pid_flash.h +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef PID_FLASH_H -#define PID_FLASH_H - -#include -#include - -/* - * pid_flash — persistent PID storage for Issue #531 (auto-tune). - * - * Stores Kp, Ki, Kd in the last 64 bytes of STM32F722 flash sector 7 - * (0x0807FFC0). Magic word validates presence of saved params. - * Sector 7 is 128KB starting at 0x08060000; firmware never exceeds sector 6. - * - * Flash writes require an erase of the full sector (128KB) before re-writing. - * The store address is the very last 64-byte block so future expansion can - * grow toward lower addresses within sector 7 without conflict. - */ - -#define PID_FLASH_SECTOR FLASH_SECTOR_7 -#define PID_FLASH_SECTOR_VOLTAGE VOLTAGE_RANGE_3 /* 2.7V-3.6V, 32-bit parallelism */ - -/* Sector 7: 128KB at 0x08060000; store in last 64 bytes */ -#define PID_FLASH_STORE_ADDR 0x0807FFC0UL -#define PID_FLASH_MAGIC 0x534C5401UL /* 'SLT\x01' — version 1 */ - -typedef struct __attribute__((packed)) { - uint32_t magic; /* PID_FLASH_MAGIC when valid */ - float kp; - float ki; - float kd; - uint8_t _pad[48]; /* padding to 64 bytes */ -} pid_flash_t; - -/* ---- Gain schedule flash storage (Issue #550) ---- */ - -/* Maximum number of speed-band entries in the gain schedule table */ -#define PID_SCHED_MAX_BANDS 6u - -/* - * Sector 7 layout (128KB at 0x08060000): - * 0x0807FF40 pid_sched_flash_t (128 bytes) — gain schedule record - * 0x0807FFC0 pid_flash_t ( 64 bytes) — single PID record (existing) - * Both records are written in a single sector erase via pid_flash_save_all(). - */ -#define PID_SCHED_FLASH_ADDR 0x0807FF40UL -#define PID_SCHED_MAGIC 0x534C5402UL /* 'SLT\x02' — version 2 */ - -typedef struct __attribute__((packed)) { - float speed_mps; /* velocity breakpoint (m/s) */ - float kp; - float ki; - float kd; -} pid_sched_entry_t; /* 16 bytes */ - -typedef struct __attribute__((packed)) { - uint32_t magic; /* PID_SCHED_MAGIC when valid */ - uint8_t num_bands; /* valid entries (1..PID_SCHED_MAX_BANDS) */ - uint8_t flags; /* reserved, must be 0 */ - uint8_t _pad0[2]; - pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* 6 × 16 = 96 bytes */ - uint8_t _pad1[24]; /* total = 4+1+1+2+96+24 = 128 bytes */ -} pid_sched_flash_t; /* 128 bytes */ - -/* - * pid_flash_load() — read saved PID from flash. - * Returns true and fills *kp/*ki/*kd if magic is valid. - * Returns false if no valid params stored (caller keeps defaults). - */ -bool pid_flash_load(float *kp, float *ki, float *kd); - -/* - * pid_flash_save() — erase sector 7 and write Kp/Ki/Kd (single-PID only). - * Use pid_flash_save_all() to save both single-PID and schedule atomically. - * Must not be called while armed (flash erase takes ~1s and stalls the CPU). - * Returns true on success. - */ -bool pid_flash_save(float kp, float ki, float kd); - -/* - * pid_flash_load_schedule() — read gain schedule from flash. - * Returns true and fills out_entries[0..n-1] and *out_n if magic is valid. - * Returns false if no valid schedule stored. - */ -bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n); - -/* - * pid_flash_save_all() — erase sector 7 once and atomically write both: - * - pid_sched_flash_t at PID_SCHED_FLASH_ADDR (0x0807FF40) - * - pid_flash_t at PID_FLASH_STORE_ADDR (0x0807FFC0) - * Must not be called while armed. Returns true on success. - */ -bool pid_flash_save_all(float kp_single, float ki_single, float kd_single, - const pid_sched_entry_t *entries, uint8_t num_bands); - -#endif /* PID_FLASH_H */ diff --git a/legacy/stm32/include/pid_schedule.h b/legacy/stm32/include/pid_schedule.h deleted file mode 100644 index 1a37dd2..0000000 --- a/legacy/stm32/include/pid_schedule.h +++ /dev/null @@ -1,122 +0,0 @@ -/* - * pid_schedule.h — Speed-dependent PID gain scheduling (Issue #550) - * - * Maps robot velocity to PID gain triplets (Kp, Ki, Kd) using a lookup - * table with linear interpolation between adjacent entries. The table - * supports 1–PID_SCHED_MAX_BANDS entries, each associating a velocity - * breakpoint (m/s) with gains that apply AT that velocity. - * - * HOW IT WORKS: - * 1. Each entry in the table defines: {speed_mps, kp, ki, kd}. - * The table is sorted by speed_mps ascending (pid_schedule_set_table - * sorts automatically). - * - * 2. pid_schedule_get_gains(speed_mps, ...) finds the two adjacent entries - * that bracket the query speed and linearly interpolates: - * t = (speed - bands[i-1].speed_mps) / - * (bands[i].speed_mps - bands[i-1].speed_mps) - * kp = bands[i-1].kp + t * (bands[i].kp - bands[i-1].kp) - * Speeds below the first entry or above the last entry clamp to the - * nearest endpoint (no extrapolation). - * The query speed is ABS(motor_speed) — scheduling is symmetric. - * - * 3. Default 3-entry table (loaded when flash has no valid schedule): - * Band 0: speed=0.00 m/s kp=40.0 ki=1.5 kd=1.2 (stopped — tight) - * Band 1: speed=0.30 m/s kp=35.0 ki=1.0 kd=1.0 (slow — balanced) - * Band 2: speed=0.80 m/s kp=28.0 ki=0.5 kd=0.8 (fast — relaxed) - * - * 4. pid_schedule_apply(balance, speed_mps) interpolates and writes the - * result directly into balance->kp/ki/kd. Call from the main loop at - * the same rate as the balance PID update (1 kHz) or slower (100 Hz - * for scheduling, 1 kHz for PID execution — gains change slowly enough). - * - * 5. Flash persistence: pid_schedule_flash_save() calls pid_flash_save_all() - * which erases sector 7 once and writes both the single-PID record at - * PID_FLASH_STORE_ADDR and the schedule at PID_SCHED_FLASH_ADDR. - * - * 6. JLINK interface (Issue #550): - * 0x0C SCHED_GET — no payload; triggers TLM_SCHED response - * 0x0D SCHED_SET — upload new table (num_bands + N×16-byte entries) - * 0x0E SCHED_SAVE — save current table + single PID to flash - * 0x85 TLM_SCHED — table dump response to SCHED_GET - */ - -#ifndef PID_SCHEDULE_H -#define PID_SCHEDULE_H - -#include -#include -#include "pid_flash.h" /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */ -#include "balance.h" /* balance_t */ - -/* ---- Default gain table ---- */ -/* Motor ESC range is ±1000 counts; 1000 counts ≈ full drive. - * Speed scale: MOTOR_CMD_MAX=1000 → ~0.8 m/s max tangential velocity. - * Adjust PID_SCHED_SPEED_SCALE if odometry calibration changes this. */ -#define PID_SCHED_SPEED_SCALE 0.0008f /* motor_cmd counts → m/s: 1000 × 0.0008 = 0.8 m/s */ - -/* ---- API ---- */ - -/* - * pid_schedule_init() — load table from flash (via pid_flash_load_schedule). - * Falls back to the built-in 3-band default if flash is empty or invalid. - * Call once after flash init during system startup. - */ -void pid_schedule_init(void); - -/* - * pid_schedule_get_gains(speed_mps, *kp, *ki, *kd) — interpolate gains. - * |speed_mps| is used (scheduling is symmetric for forward/reverse). - * Clamps to table endpoints; does not extrapolate outside the table range. - */ -void pid_schedule_get_gains(float speed_mps, float *kp, float *ki, float *kd); - -/* - * pid_schedule_apply(b, speed_mps) — compute interpolated gains and write - * them into b->kp, b->ki, b->kd. b->integral is reset to 0 when the - * active band changes to avoid integrator windup on transitions. - */ -void pid_schedule_apply(balance_t *b, float speed_mps); - -/* - * pid_schedule_set_table(entries, n) — replace the active gain table. - * Entries are copied and sorted by speed_mps ascending. - * n is clamped to [1, PID_SCHED_MAX_BANDS]. - * Does NOT automatically save to flash — call pid_schedule_flash_save(). - */ -void pid_schedule_set_table(const pid_sched_entry_t *entries, uint8_t n); - -/* - * pid_schedule_get_table(out_entries, out_n) — copy current table out. - * out_entries must have room for PID_SCHED_MAX_BANDS entries. - */ -void pid_schedule_get_table(pid_sched_entry_t *out_entries, uint8_t *out_n); - -/* - * pid_schedule_get_num_bands() — return current number of table entries. - */ -uint8_t pid_schedule_get_num_bands(void); - -/* - * pid_schedule_flash_save(kp_single, ki_single, kd_single) — save the - * current schedule table PLUS the caller-supplied single-PID values to - * flash in one atomic sector erase (pid_flash_save_all). - * Must NOT be called while armed (sector erase takes ~1s). - * Returns true on success. - */ -bool pid_schedule_flash_save(float kp_single, float ki_single, float kd_single); - -/* - * pid_schedule_active_band_idx() — index (0-based) of the lower bracket - * entry used in the most recent interpolation. Useful for telemetry. - * Returns 0 if speed is below the first entry. - */ -uint8_t pid_schedule_active_band_idx(void); - -/* - * pid_schedule_get_default_table(out_entries, out_n) — fill the 3-band - * default table into caller's buffer. Used for factory-reset. - */ -void pid_schedule_get_default_table(pid_sched_entry_t *out_entries, uint8_t *out_n); - -#endif /* PID_SCHEDULE_H */ diff --git a/legacy/stm32/include/power_mgmt.h b/legacy/stm32/include/power_mgmt.h deleted file mode 100644 index df705f1..0000000 --- a/legacy/stm32/include/power_mgmt.h +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef POWER_MGMT_H -#define POWER_MGMT_H - -#include -#include - -/* - * power_mgmt — STM32F7 STOP-mode sleep/wake manager (Issue #178). - * - * State machine: - * PM_ACTIVE ──(idle ≥ PM_IDLE_TIMEOUT_MS or sleep cmd)──► PM_SLEEP_PENDING - * PM_SLEEP_PENDING ──(fade complete, ≥ PM_FADE_MS)──► PM_SLEEPING (WFI) - * PM_SLEEPING ──(EXTI wake)──► PM_WAKING ──(clocks restored)──► PM_ACTIVE - * - * Any call to power_mgmt_activity() during SLEEP_PENDING or SLEEPING - * immediately transitions back toward PM_ACTIVE. - * - * Wake sources (EXTI, falling edge on UART idle-high RX pin or IMU INT): - * EXTI1 PA1 UART4_RX — CRSF/ELRS start bit - * EXTI7 PB7 USART1_RX — JLink start bit - * EXTI4 PC4 MPU6000 INT — IMU motion (handler owned by mpu6000.c) - * - * Peripheral gating on sleep entry (clock disable, state preserved): - * Disabled: SPI3/I2S3 (audio amp), SPI2 (OSD), USART6, UART5 (debug) - * Active: SPI1 (IMU), UART4 (CRSF), USART1 (JLink), I2C1 (baro/mag) - * - * Sleep LED (LED1, active-low PC15): - * PM_SLEEP_PENDING: triangle-wave pulse, period PM_LED_PERIOD_MS - * All other states: 0 (caller uses normal LED logic) - * - * IWDG: - * Fed immediately before WFI. STOP wakeup <10 ms typical — well within - * WATCHDOG_TIMEOUT_MS (50 ms). - * - * Safety interlock: - * Caller MUST NOT call power_mgmt_tick() while armed; call - * power_mgmt_activity() instead to keep the idle timer reset. - * - * JLink integration: - * JLINK_CMD_SLEEP (0x09) → power_mgmt_request_sleep() - * Any valid JLink frame → power_mgmt_activity() (handled in main loop) - */ - -typedef enum { - PM_ACTIVE = 0, /* Normal, all peripherals running */ - PM_SLEEP_PENDING = 1, /* Idle timeout reached; LED fade-out in progress */ - PM_SLEEPING = 2, /* In STOP mode (WFI); execution blocked in tick() */ - PM_WAKING = 3, /* Transitional; clocks/peripherals being restored */ -} PowerState; - -/* ---- API ---- */ - -/* - * power_mgmt_init() — configure wake EXTI lines (EXTI1, EXTI7). - * Call after crsf_init() and jlink_init(). - */ -void power_mgmt_init(void); - -/* - * power_mgmt_activity() — record cmd_vel event (CRSF frame, JLink frame). - * Resets idle timer; aborts any pending/active sleep. - */ -void power_mgmt_activity(void); - -/* - * power_mgmt_request_sleep() — force sleep regardless of idle timer - * (called on JLINK_CMD_SLEEP). Next tick() enters PM_SLEEP_PENDING. - */ -void power_mgmt_request_sleep(void); - -/* - * power_mgmt_tick(now_ms) — drive state machine. May block in WFI during - * STOP mode. Returns state after this tick. - * MUST NOT be called while balance_state == BALANCE_ARMED. - */ -PowerState power_mgmt_tick(uint32_t now_ms); - -/* power_mgmt_state() — non-blocking read of current state. */ -PowerState power_mgmt_state(void); - -/* - * power_mgmt_led_brightness() — 0-255 brightness for sleep-pending pulse. - * Returns 0 when not in PM_SLEEP_PENDING; caller uses normal LED logic. - */ -uint8_t power_mgmt_led_brightness(void); - -/* - * power_mgmt_current_ma() — estimated total current draw (mA) based on - * gating state; populated in JLINK_TLM_POWER telemetry. - */ -uint16_t power_mgmt_current_ma(void); - -/* power_mgmt_idle_ms() — ms elapsed since last power_mgmt_activity() call. */ -uint32_t power_mgmt_idle_ms(void); - -/* - * power_mgmt_notify_battery(vbat_mv) — Issue #467 integration. - * Called by battery_adc_check_pm() after BATTERY_ADC_LOW_HOLD_MS of sustained - * critical voltage. Forces PM_SLEEP_PENDING to protect the LiPo. - */ -void power_mgmt_notify_battery(uint32_t vbat_mv); - -#endif /* POWER_MGMT_H */ diff --git a/legacy/stm32/include/rgb_fsm.h b/legacy/stm32/include/rgb_fsm.h deleted file mode 100644 index 4e02984..0000000 --- a/legacy/stm32/include/rgb_fsm.h +++ /dev/null @@ -1,124 +0,0 @@ -#ifndef RGB_FSM_H -#define RGB_FSM_H - -#include -#include - -/* - * rgb_fsm.h — RGB Status LED State Machine (Issue #290) - * - * Manages an 8-LED WS2812 NeoPixel ring with 8 operational states. - * Each state has a specific color pattern and animation. - * - * States: - * BOOT — Blue pulse (startup sequence, 0.5 Hz) - * IDLE — Green breathe (standby, smooth 0.5 Hz pulse) - * ARMED — Solid green (ready to move) - * NAV — Cyan spin (autonomous navigation active, rotating pattern) - * ERROR — Red flash (fault detected, 2 Hz blink) - * LOW_BATT — Orange blink (battery low, 1 Hz blink) - * CHARGING — Green fill (charging, progressive LEDs filling) - * ESTOP — Red solid (emergency stop, full red, no animation) - * - * Transitions via UART command from Jetson. - * Non-blocking operation with tick-based timing. - */ - -/* LED State Machine States */ -typedef enum { - LED_STATE_BOOT = 0, - LED_STATE_IDLE, - LED_STATE_ARMED, - LED_STATE_NAV, - LED_STATE_ERROR, - LED_STATE_LOW_BATT, - LED_STATE_CHARGING, - LED_STATE_ESTOP, - LED_STATE_COUNT -} LedState; - -/* RGB Color (8-bit per channel) */ -typedef struct { - uint8_t r; /* Red (0-255) */ - uint8_t g; /* Green (0-255) */ - uint8_t b; /* Blue (0-255) */ -} RgbColor; - -/* - * rgb_fsm_init() - * - * Initialize LED state machine: - * - PB4 as TIM3_CH1 PWM output for WS2812 driver - * - Configure TIM3 for 800 kHz PWM frequency - * - Set initial state to BOOT - * - Initialize all LEDs to off - */ -void rgb_fsm_init(void); - -/* - * rgb_fsm_set_state(state) - * - * Transition to a new LED state immediately. - * Resets animation timing for the new state. - * - * Arguments: - * - state: Target LED state (LedState enum) - * - * Returns: true if state changed, false if already in that state - */ -bool rgb_fsm_set_state(LedState state); - -/* - * rgb_fsm_get_state() - * - * Get current LED state. - * - * Returns: Current LED state (LedState enum) - */ -LedState rgb_fsm_get_state(void); - -/* - * rgb_fsm_tick(now_ms) - * - * Update function called periodically (recommended: every 10-50ms). - * Processes animations and timing for current state. - * Updates LED strip via PWM. - * - * Arguments: - * - now_ms: Current time in milliseconds (from HAL_GetTick() or similar) - */ -void rgb_fsm_tick(uint32_t now_ms); - -/* - * rgb_fsm_set_color(led_index, color) - * - * Set color of a specific LED (for testing and manual control). - * Bypasses current animation. - * - * Arguments: - * - led_index: 0-7 (LED ring has 8 LEDs) - * - color: RgbColor with R, G, B values (0-255) - * - * Returns: true if set, false if index out of range - */ -bool rgb_fsm_set_color(uint8_t led_index, RgbColor color); - -/* - * rgb_fsm_all_off() - * - * Turn off all LEDs immediately. - * Useful for shutdown or error conditions. - */ -void rgb_fsm_all_off(void); - -/* - * rgb_fsm_get_animation_frame() - * - * Get current animation progress (0-255). - * Useful for testing and debugging animation timing. - * - * Returns: Current frame value for animation (0-255 represents full cycle) - */ -uint8_t rgb_fsm_get_animation_frame(void); - -#endif /* RGB_FSM_H */ diff --git a/legacy/stm32/include/safety.h b/legacy/stm32/include/safety.h deleted file mode 100644 index 66d6eef..0000000 --- a/legacy/stm32/include/safety.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef SAFETY_H -#define SAFETY_H - -#include -#include - -/* - * SaltyLab Safety Systems - * - * Covers: - * - IWDG hardware watchdog (MCU reset if main loop hangs) - * - RC signal timeout monitoring - * - Tilt fault alert via buzzer - * - Arm hold interlock (must hold arm for ARMING_HOLD_MS) - * - Remote e-stop over 4G MQTT (CDC 'E'/'F'/'Z' commands) - */ - -typedef enum { - ESTOP_CLEAR = 0, - ESTOP_TILT = 1, - ESTOP_RC_KILL = 2, - ESTOP_REMOTE = 3, - ESTOP_CELLULAR_TIMEOUT = 4, -} EstopSource; - -/* - * safety_init() — call once in main() after HAL_Init(). - * Starts IWDG with WATCHDOG_TIMEOUT_MS timeout from config.h. - * Starts ARMING_HOLD_MS countdown from config.h. - */ -void safety_init(void); - -/* - * safety_refresh() — call every main loop iteration. - * Resets IWDG counter. If not called within WATCHDOG_TIMEOUT_MS, - * the MCU will reset (independent of software — cannot be disabled). - */ -void safety_refresh(void); - -/* - * safety_rc_alive() — returns true if RC receiver has sent a frame - * within RC_TIMEOUT_MS. Call from the balance loop. - */ -bool safety_rc_alive(uint32_t now); - -/* - * safety_alert_tilt_fault() — one-shot buzzer beep for tilt fault. - * Safe to call repeatedly; only fires once per fault. - */ -void safety_alert_tilt_fault(bool faulted); - -/* - * safety_arm_interlock() — returns true once arm button has been - * held for ARMING_HOLD_MS from the moment safety_arm_start() was called. - */ -void safety_arm_start(uint32_t now); /* Call when arm requested */ -bool safety_arm_ready(uint32_t now); /* Poll until true, then arm */ -void safety_arm_cancel(void); /* Cancel pending arm */ - -void safety_remote_estop(EstopSource src); -void safety_remote_estop_clear(void); -EstopSource safety_get_estop(void); -bool safety_remote_estop_active(void); - -#endif /* SAFETY_H */ diff --git a/legacy/stm32/include/servo.h b/legacy/stm32/include/servo.h deleted file mode 100644 index 19e367b..0000000 --- a/legacy/stm32/include/servo.h +++ /dev/null @@ -1,114 +0,0 @@ -#ifndef SERVO_H -#define SERVO_H - -#include -#include - -/* - * servo.h — Pan-tilt servo driver for camera head (Issue #206) - * - * Hardware: TIM4 PWM at 50 Hz (20 ms period) - * - CH1 (PB6): Pan servo (0-180°) - * - CH2 (PB7): Tilt servo (0-180°) - * - * Servo pulse mapping: - * - 500 µs → 0° (full left/down) - * - 1500 µs → 90° (center) - * - 2500 µs → 180° (full right/up) - * - * Smooth sweeping via servo_sweep() for camera motion. - */ - -/* Servo channels */ -typedef enum { - SERVO_PAN = 0, /* CH1 (PB6) */ - SERVO_TILT = 1, /* CH2 (PB7) */ - SERVO_COUNT -} ServoChannel; - -/* Servo state */ -typedef struct { - uint16_t current_angle_deg[SERVO_COUNT]; /* Current angle in degrees (0-180) */ - uint16_t target_angle_deg[SERVO_COUNT]; /* Target angle in degrees */ - uint16_t pulse_us[SERVO_COUNT]; /* Pulse width in microseconds (500-2500) */ - - /* Sweep state (per-servo) */ - uint32_t sweep_start_ms[SERVO_COUNT]; - uint32_t sweep_duration_ms[SERVO_COUNT]; - uint16_t sweep_start_deg[SERVO_COUNT]; - uint16_t sweep_end_deg[SERVO_COUNT]; - bool is_sweeping[SERVO_COUNT]; -} ServoState; - -/* - * servo_init() - * - * Initialize TIM4 PWM on PB6 (CH1, pan) and PB7 (CH2, tilt) at 50 Hz. - * Centers both servos at 90° (1500 µs). Call once at startup. - */ -void servo_init(void); - -/* - * servo_set_angle(channel, degrees) - * - * Set target angle for a servo (0-180°). - * Immediately updates PWM without motion ramping. - * Valid channels: SERVO_PAN, SERVO_TILT - * - * Examples: - * servo_set_angle(SERVO_PAN, 0); // Pan left - * servo_set_angle(SERVO_PAN, 90); // Pan center - * servo_set_angle(SERVO_TILT, 180); // Tilt up - */ -void servo_set_angle(ServoChannel channel, uint16_t degrees); - -/* - * servo_get_angle(channel) - * - * Return current servo angle in degrees (0-180). - */ -uint16_t servo_get_angle(ServoChannel channel); - -/* - * servo_set_pulse_us(channel, pulse_us) - * - * Set servo pulse width directly in microseconds (500-2500). - * Used for fine-tuning or direct control. - */ -void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us); - -/* - * servo_sweep(channel, start_deg, end_deg, duration_ms) - * - * Smooth linear sweep from start to end angle over duration_ms. - * Non-blocking: must call servo_tick() every ~10 ms to update PWM. - * - * Examples: - * servo_sweep(SERVO_PAN, 0, 180, 2000); // Pan left-to-right in 2 seconds - * servo_sweep(SERVO_TILT, 45, 135, 1000); // Tilt up-down in 1 second - */ -void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms); - -/* - * servo_tick(now_ms) - * - * Update servo sweep animation (if active). Call every ~10 ms from main loop. - * No-op if not currently sweeping. - */ -void servo_tick(uint32_t now_ms); - -/* - * servo_is_sweeping() - * - * Returns true if any servo is currently sweeping. - */ -bool servo_is_sweeping(void); - -/* - * servo_stop_sweep(channel) - * - * Stop sweep immediately, hold current position. - */ -void servo_stop_sweep(ServoChannel channel); - -#endif /* SERVO_H */ diff --git a/legacy/stm32/include/servo_bus.h b/legacy/stm32/include/servo_bus.h deleted file mode 100644 index 5200c56..0000000 --- a/legacy/stm32/include/servo_bus.h +++ /dev/null @@ -1,97 +0,0 @@ -#ifndef SERVO_BUS_H -#define SERVO_BUS_H - -#include -#include - -/* - * servo_bus.h — Feetech/ST3215 serial bus servo driver (Issue #547) - * - * Half-duplex single-wire UART protocol at 1 Mbps. - * Hardware: USART3 half-duplex on PB10 (USART3_TX, AF7). - * No separate RX pin — the TX line is bidirectional with HDSEL. - * - * Packet format (host to servo): - * [0xFF][0xFF][ID][LEN][INSTR][PARAMS...][CKSUM] - * CKSUM = ~(ID + LEN + INSTR + PARAMS) & 0xFF - * - * Response (servo to host): - * [0xFF][0xFF][ID][LEN][ERROR][DATA...][CKSUM] - * - * Key ST3215 registers: - * 0x28 Torque Enable (1=on, 0=off) - * 0x2A Goal Position L (0-4095) - * 0x2B Goal Position H - * 0x2E Moving Speed L (0=max, 1-4095) - * 0x2F Moving Speed H - * 0x38 Present Position L (0-4095) - * 0x39 Present Position H - * 0x3A Present Speed L (sign+magnitude: bit15=dir) - * 0x3B Present Speed H - * - * Position encoding: 0-4095 maps to 0-360 degrees. - * Center (180 deg) = 2048 raw. - */ - -/* ST3215 register addresses */ -#define SB_REG_TORQUE_EN 0x28u -#define SB_REG_GOAL_POS_L 0x2Au -#define SB_REG_MOVING_SPD_L 0x2Eu -#define SB_REG_PRES_POS_L 0x38u -#define SB_REG_PRES_SPD_L 0x3Au - -/* ST3215 instructions */ -#define SB_INSTR_PING 0x01u -#define SB_INSTR_READ 0x02u -#define SB_INSTR_WRITE 0x03u - -/* Position encoding */ -#define SB_POS_CENTER 2048u /* 180 deg */ -#define SB_POS_MAX 4095u /* 360 deg */ -#define SB_SPEED_MAX 4095u /* counts/sec (0 = max speed) */ - -/* Timeout for servo response (ms) */ -#define SB_RX_TIMEOUT_MS 5u - -/* - * servo_bus_init() - configure USART3 in half-duplex mode (PB10, AF7) at - * SB_BAUD (1 Mbps, 8N1). Call once at startup before gimbal_init(). - */ -void servo_bus_init(void); - -/* - * servo_bus_write_pos(id, raw_pos, speed) - write goal position and moving - * speed in a single WRITE DATA packet. raw_pos: 0-4095. speed: 0=max, 1-4095. - * Returns true on successful TX (no response expected on write). - */ -bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed); - -/* - * servo_bus_write_torque(id, enable) - enable or disable servo torque. - */ -bool servo_bus_write_torque(uint8_t id, bool enable); - -/* - * servo_bus_read_pos(id, raw_pos) - read present position. - * Returns true on success; raw_pos is 0-4095. - */ -bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos); - -/* - * servo_bus_read_speed(id, speed) - read present speed. - * speed bit 15 is direction. Returns magnitude in lower 15 bits. - */ -bool servo_bus_read_speed(uint8_t id, uint16_t *speed); - -/* - * servo_bus_deg_to_raw(deg) - convert degree (-180..+180) to raw position. - * Center (0 deg) = 2048. Clamps to 0-4095. - */ -uint16_t servo_bus_deg_to_raw(float deg); - -/* - * servo_bus_raw_to_deg(raw) - convert raw position (0-4095) to degree (-180..+180). - */ -float servo_bus_raw_to_deg(uint16_t raw); - -#endif /* SERVO_BUS_H */ diff --git a/legacy/stm32/include/slope_estimator.h b/legacy/stm32/include/slope_estimator.h deleted file mode 100644 index 3141c86..0000000 --- a/legacy/stm32/include/slope_estimator.h +++ /dev/null @@ -1,101 +0,0 @@ -#ifndef SLOPE_ESTIMATOR_H -#define SLOPE_ESTIMATOR_H - -#include -#include - -/* - * slope_estimator — slow-adapting terrain slope estimator for Issue #600. - * - * On a slope the robot must lean slightly into the hill to stay balanced. - * The IMU pitch reading therefore includes both the robot's balance offset - * and the ground incline. This module decouples the two by tracking the - * slowly-changing DC component of the pitch signal with a first-order IIR - * low-pass filter (time constant SLOPE_TAU_S = 5 s). - * - * HOW IT WORKS: - * Every call to slope_estimator_update(pitch_deg, dt): - * - * alpha = dt / (SLOPE_TAU_S + dt) // ≈ 0.0002 at 1 kHz - * raw = slope * (1 - alpha) + pitch * alpha - * slope = clamp(raw, -SLOPE_MAX_DEG, +SLOPE_MAX_DEG) - * - * The IIR converges to the steady-state pitch in ~5 s. Fast tilt - * transients (balance corrections, steps, bumps) are attenuated by - * the long time constant and do not corrupt the estimate. - * - * INTEGRATION IN BALANCE PID: - * Subtract slope_estimate from the measured pitch before computing - * the PID error so the controller balances around the slope surface - * rather than absolute vertical: - * - * tilt_corrected = pitch_deg - slope_estimate_deg - * error = setpoint - tilt_corrected - * - * This is equivalent to continuously adjusting the balance setpoint - * to track the incline. - * - * SAFETY: - * - Estimate is clamped to ±SLOPE_MAX_DEG (15°) to prevent drift from - * extreme falls being mistaken for genuine slopes. - * - slope_estimator_reset() zeroes the state; call on disarm or after - * a tilt fault so re-arming starts fresh. - * - * TELEMETRY: - * JLINK_TLM_SLOPE (0x88) published at SLOPE_TLM_HZ (1 Hz): - * jlink_tlm_slope_t { int16 slope_x100, uint8 active, uint8 _pad } - */ - -/* ---- Configuration ---- */ -#define SLOPE_TAU_S 5.0f /* IIR time constant (seconds) */ -#define SLOPE_MAX_DEG 15.0f /* Maximum estimate magnitude (degrees) */ -#define SLOPE_TLM_HZ 1u /* JLINK_TLM_SLOPE publish rate (Hz) */ - -/* ---- State ---- */ -typedef struct { - float estimate_deg; /* current slope estimate (degrees, + = nose-up) */ - bool enabled; /* compensation on/off; off = estimate frozen */ - uint32_t last_tlm_ms; /* timestamp of last TLM transmission */ -} slope_estimator_t; - -/* ---- API ---- */ - -/* - * slope_estimator_init(se) — zero state, enable estimation. - * Call once during system init. - */ -void slope_estimator_init(slope_estimator_t *se); - -/* - * slope_estimator_reset(se) — zero estimate without changing enabled flag. - * Call on disarm or after BALANCE_TILT_FAULT to avoid stale state on rearm. - */ -void slope_estimator_reset(slope_estimator_t *se); - -/* - * slope_estimator_update(se, pitch_deg, dt) — advance the IIR filter. - * pitch_deg : current fused pitch angle from IMU (degrees) - * dt : loop interval (seconds) - * No-op if se->enabled == false or dt <= 0. - */ -void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt); - -/* - * slope_estimator_get_deg(se) — return current estimate (degrees). - * Returns 0 if disabled. - */ -float slope_estimator_get_deg(const slope_estimator_t *se); - -/* - * slope_estimator_set_enabled(se, en) — enable or disable compensation. - * Disabling freezes the estimate at its current value. - */ -void slope_estimator_set_enabled(slope_estimator_t *se, bool en); - -/* - * slope_estimator_send_tlm(se, now_ms) — transmit JLINK_TLM_SLOPE (0x88) - * frame to Jetson. Rate-limited to SLOPE_TLM_HZ; safe to call every tick. - */ -void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms); - -#endif /* SLOPE_ESTIMATOR_H */ diff --git a/legacy/stm32/include/status.h b/legacy/stm32/include/status.h deleted file mode 100644 index 454c4e0..0000000 --- a/legacy/stm32/include/status.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef STATUS_H -#define STATUS_H -#include -void status_init(void); -void status_boot_beep(void); -/* - * status_update() — call every main loop iteration. - * Controls LED1 (PC15) and LED2 (PC14), both active-low. - * - * Solid ON = good (normal operation) - * Slow blink (~1 Hz) = needs attention (error or fault) - * - * LED1 solid + LED2 off → disarmed, IMU OK - * LED1 solid + LED2 solid → armed - * Both slow blink → tilt fault - * Both fast blink (200ms) -- remote e-stop active (highest priority) - * LED1 slow blink + LED2 solid → IMU error (solid LED2 = always-on indicator) - */ -void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault, int remote_estop); -#endif diff --git a/legacy/stm32/include/steering_pid.h b/legacy/stm32/include/steering_pid.h deleted file mode 100644 index 66ef413..0000000 --- a/legacy/stm32/include/steering_pid.h +++ /dev/null @@ -1,134 +0,0 @@ -#ifndef STEERING_PID_H -#define STEERING_PID_H - -#include -#include - -/* - * steering_pid — closed-loop yaw-rate controller for differential drive - * (Issue #616). - * - * OVERVIEW: - * Converts a desired yaw rate (from Jetson Twist.angular.z) into a - * differential wheel speed offset. The balance PID remains the primary - * controller; the steering PID generates a small differential term that - * is added to the balance command inside motor_driver: - * - * left_speed = balance_cmd - steer_out - * right_speed = balance_cmd + steer_out - * - * This is the standard differential-drive mixing already performed by - * the ESC backend (hoverboard/VESC). - * - * INPUT SIGNALS: - * target_omega_dps : desired yaw rate in deg/s (+ = clockwise from above) - * Derived from JLINK_CMD_DRIVE steer field: - * target_omega_dps = steer * STEER_OMEGA_SCALE - * (steer is int16 -1000..+1000 from Jetson) - * actual_omega_dps : measured yaw rate from IMU gyro Z (deg/s) - * = IMUData.yaw_rate - * - * PID ALGORITHM: - * error = target_omega - actual_omega - * integral = clamp(integral + error*dt, ±STEER_INTEGRAL_MAX) - * raw_out = Kp*error + Ki*integral + Kd*(error - prev_error)/dt - * raw_out = clamp(raw_out, ±STEER_OUTPUT_MAX) - * output = rate_limit(raw_out, STEER_RAMP_RATE_PER_MS * dt_ms) - * - * ANTI-WINDUP: - * Integral is clamped to ±STEER_INTEGRAL_MAX counts before the Ki - * multiply, bounding the integrator contribution independently of Kp/Kd. - * - * RATE LIMITER: - * Output changes at most STEER_RAMP_RATE_PER_MS counts per millisecond. - * Prevents a sudden step in steering demand from disturbing the balance - * PID (which has no knowledge of the steering channel). - * - * OMEGA SCALING: - * STEER_OMEGA_SCALE = 0.1 deg/s per steer unit. - * Range: steer -1000..+1000 → omega -100..+100 deg/s. - * 100 deg/s ≈ 1.75 rad/s — covers aggressive turns without exceeding - * the hoverboard ESC differential authority (STEER_OUTPUT_MAX = 400). - * - * DISABLING: - * steering_pid_set_enabled(s, false) zeroes target_omega and integral, - * then freezes output at 0. Use when Jetson is not active or in - * RC_MANUAL mode to avoid fighting the RC steer channel. - * - * TELEMETRY: - * JLINK_TLM_STEERING (0x8A) published at STEER_TLM_HZ (10 Hz): - * jlink_tlm_steering_t { int16 target_x10, int16 actual_x10, - * int16 output, uint8 enabled, uint8 _pad } - * 8 bytes total. - */ - -/* ---- Configuration ---- */ -#define STEER_KP 2.0f /* proportional gain (counts / (deg/s)) */ -#define STEER_KI 0.5f /* integral gain (counts / (deg)) */ -#define STEER_KD 0.05f /* derivative gain (counts / (deg/s²)) */ -#define STEER_INTEGRAL_MAX 200.0f /* integrator clamp (motor counts) */ -#define STEER_OUTPUT_MAX 400 /* peak differential output (counts) */ -#define STEER_RAMP_RATE_PER_MS 10 /* max output change per ms (counts/ms) */ -#define STEER_OMEGA_SCALE 0.1f /* steer units → deg/s (0.1 deg/s/unit) */ -#define STEER_TLM_HZ 10u /* JLINK_TLM_STEERING publish rate (Hz) */ - -/* ---- State ---- */ -typedef struct { - float target_omega_dps; /* setpoint: desired yaw rate (deg/s) */ - float actual_omega_dps; /* feedback: measured yaw rate (deg/s) */ - float integral; /* PID integrator (motor counts·s) */ - float prev_error; /* error at last update (deg/s) */ - int16_t output; /* rate-limited differential output */ - bool enabled; /* false = output held at 0 */ - uint32_t last_tlm_ms; /* rate-limit for TLM */ -} steering_pid_t; - -/* ---- API ---- */ - -/* - * steering_pid_init(s) — zero state, enable controller. - * Call once during system init. - */ -void steering_pid_init(steering_pid_t *s); - -/* - * steering_pid_reset(s) — zero integrator, setpoint and output. - * Preserves enabled flag. Call on disarm. - */ -void steering_pid_reset(steering_pid_t *s); - -/* - * steering_pid_set_target(s, omega_dps) — update setpoint. - * omega_dps : desired yaw rate in deg/s. - * Converts from JLINK_CMD_DRIVE steer field: omega = steer * STEER_OMEGA_SCALE. - * No-op if disabled (output remains 0). - */ -void steering_pid_set_target(steering_pid_t *s, float omega_dps); - -/* - * steering_pid_update(s, actual_omega_dps, dt) — advance PID one step. - * actual_omega_dps : IMU gyro Z rate (deg/s) — use IMUData.yaw_rate. - * dt : loop interval (seconds). - * Returns differential output (-STEER_OUTPUT_MAX..+STEER_OUTPUT_MAX). - * Returns 0 if disabled or dt <= 0. - */ -int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt); - -/* - * steering_pid_get_output(s) — last computed differential output. - */ -int16_t steering_pid_get_output(const steering_pid_t *s); - -/* - * steering_pid_set_enabled(s, en) — enable or disable the controller. - * Disabling resets integrator and zeroes output. - */ -void steering_pid_set_enabled(steering_pid_t *s, bool en); - -/* - * steering_pid_send_tlm(s, now_ms) — transmit JLINK_TLM_STEERING (0x8A) - * frame to Jetson. Rate-limited to STEER_TLM_HZ; safe to call every tick. - */ -void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms); - -#endif /* STEERING_PID_H */ diff --git a/legacy/stm32/include/uart_protocol.h b/legacy/stm32/include/uart_protocol.h deleted file mode 100644 index 5f414d0..0000000 --- a/legacy/stm32/include/uart_protocol.h +++ /dev/null @@ -1,96 +0,0 @@ -#ifndef UART_PROTOCOL_H -#define UART_PROTOCOL_H - -/* - * uart_protocol.h — UART command protocol for Jetson-STM32 communication (Issue #629) - * - * Frame format: - * [STX][LEN][CMD][PAYLOAD...][CRC8][ETX] - * 0x02 1B 1B 0-12 B 1B 0x03 - * - * CRC8-SMBUS: poly=0x07, init=0x00, computed over CMD+PAYLOAD bytes. - * - * Physical layer: UART5 (PC12=TX / PD2=RX), GPIO_AF8_UART5, 115200 baud, no hw flow. - * NOTE: Spec requested USART1 @ 115200, but USART1 is occupied by JLink @ 921600. - * Implemented on UART5 instead; Jetson must connect to PC12/PD2. - * - * DMA: DMA1_Stream0_Channel4, circular 256-byte ring buffer. - * Heartbeat: if no frame received in UART_PROT_HB_TIMEOUT_MS (500 ms), Jetson is - * considered lost; caller must handle estop if needed. - */ - -#include -#include - -/* ── Frame delimiters ─────────────────────────────────────────────────────── */ -#define UPROT_STX 0x02u -#define UPROT_ETX 0x03u - -/* ── Command IDs (host → STM32) ───────────────────────────────────────────── */ -#define UCMD_SET_VELOCITY 0x01u /* payload: int16 left_rpm, int16 right_rpm (4 B) */ -#define UCMD_GET_STATUS 0x02u /* payload: none */ -#define UCMD_SET_PID 0x03u /* payload: float kp, float ki, float kd (12 B) */ -#define UCMD_ESTOP 0x04u /* payload: none */ -#define UCMD_CLEAR_ESTOP 0x05u /* payload: none */ - -/* ── Response IDs (STM32 → host) ──────────────────────────────────────────── */ -#define URESP_ACK 0x80u /* payload: 1 B — echoed CMD */ -#define URESP_NACK 0x81u /* payload: 2 B — CMD, error_code */ -#define URESP_STATUS 0x82u /* payload: sizeof(uart_prot_status_t) = 8 B */ - -/* ── NACK error codes ─────────────────────────────────────────────────────── */ -#define UERR_BAD_CRC 0x01u -#define UERR_BAD_LEN 0x02u -#define UERR_BAD_ETX 0x03u -#define UERR_ESTOP 0x04u /* command rejected — estop active */ -#define UERR_DISARMED 0x05u /* velocity rejected — not armed */ - -/* ── STATUS payload (URESP_STATUS, 8 bytes packed) ───────────────────────── */ -typedef struct __attribute__((packed)) { - int16_t pitch_x10; /* pitch angle ×10 deg (balance controller) */ - int16_t motor_cmd; /* ESC motor command -1000..+1000 */ - uint16_t vbat_mv; /* battery voltage in mV */ - uint8_t balance_state; /* BalanceState enum (0=DISARMED, 1=ARMED, …) */ - uint8_t estop_active; /* non-zero if remote estop is latched */ -} uart_prot_status_t; - -/* ── Shared state (read by main.c) ────────────────────────────────────────── */ -typedef struct { - volatile uint8_t vel_updated; /* 1 when SET_VELOCITY received */ - volatile int16_t left_rpm; - volatile int16_t right_rpm; - - volatile uint8_t pid_updated; /* 1 when SET_PID received */ - volatile float pid_kp; - volatile float pid_ki; - volatile float pid_kd; - - volatile uint8_t estop_req; /* 1 on UCMD_ESTOP */ - volatile uint8_t estop_clear_req; /* 1 on UCMD_CLEAR_ESTOP */ - - volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame */ -} UartProtState; - -extern UartProtState uart_prot_state; - -/* ── API ───────────────────────────────────────────────────────────────────── */ - -/** - * uart_protocol_init() — configure UART5 + DMA, start circular receive. - * Must be called once during system init, before main loop. - */ -void uart_protocol_init(void); - -/** - * uart_protocol_process() — drain DMA ring buffer, parse frames, dispatch commands. - * Call once per main loop iteration (every ~1 ms). - */ -void uart_protocol_process(void); - -/** - * uart_protocol_send_status() — build and TX a URESP_STATUS frame. - * @param s Pointer to status payload to send. - */ -void uart_protocol_send_status(const uart_prot_status_t *s); - -#endif /* UART_PROTOCOL_H */ diff --git a/legacy/stm32/include/ultrasonic.h b/legacy/stm32/include/ultrasonic.h deleted file mode 100644 index 2e1c349..0000000 --- a/legacy/stm32/include/ultrasonic.h +++ /dev/null @@ -1,101 +0,0 @@ -#ifndef ULTRASONIC_H -#define ULTRASONIC_H - -#include -#include - -/* - * ultrasonic.h — HC-SR04 ultrasonic distance sensor driver (Issue #243) - * - * STM32F722 driver for HC-SR04 ultrasonic ranger with TIM1 input capture. - * Trigger: PA0 (GPIO output, active high pulse 10µs) - * Echo: PA1 (TIM1_CH2, input capture on rising/falling edges) - * - * Non-blocking operation: trigger measurement, get result via callback. - * Distance calculated from echo pulse width: distance_mm = (pulse_us / 2) / 29.1 - * Typical range: 20-4000mm (accuracy ±3mm above 30mm) - */ - -/* Ultrasonic sensor states */ -typedef enum { - ULTRASONIC_IDLE, /* Ready for new measurement */ - ULTRASONIC_TRIGGERED, /* Trigger pulse sent, waiting for echo */ - ULTRASONIC_MEASURING, /* Echo rising edge detected, measuring */ - ULTRASONIC_COMPLETE, /* Measurement complete */ - ULTRASONIC_ERROR /* Timeout or out-of-range */ -} UltrasonicState; - -/* Measurement result callback: called when measurement completes - * Arguments: - * - distance_mm: measured distance in mm (0 if error/timeout) - * - is_valid: true if measurement valid, false if timeout/error - */ -typedef void (*ultrasonic_callback_t)(uint16_t distance_mm, bool is_valid); - -/* - * ultrasonic_init() - * - * Initialize HC-SR04 driver: - * - PA0 as GPIO output (trigger pin) - * - PA1 as TIM1_CH2 input capture (echo pin) - * - Configure TIM1 for input capture on both edges (rising/falling) - * - Enable timer interrupt for echo measurement - */ -void ultrasonic_init(void); - -/* - * ultrasonic_trigger() - * - * Start a non-blocking distance measurement. - * Sends 10µs trigger pulse on PA0, sets up echo measurement. - * Measurement completes asynchronously (typically 25-300ms depending on distance). - * Call ultrasonic_get_state() to check status or wait for callback. - * - * Returns: true if triggered successfully, false if still measuring previous result - */ -bool ultrasonic_trigger(void); - -/* - * ultrasonic_set_callback(callback) - * - * Register callback to be called when measurement completes. - * Callback receives: distance_mm (0 if error), is_valid (true if successful) - * Callback is optional; can poll with ultrasonic_get_result() instead. - */ -void ultrasonic_set_callback(ultrasonic_callback_t callback); - -/* - * ultrasonic_get_state() - * - * Returns current measurement state (IDLE, TRIGGERED, MEASURING, COMPLETE, ERROR). - * Useful for non-blocking polling. - */ -UltrasonicState ultrasonic_get_state(void); - -/* - * ultrasonic_get_result(distance_mm, is_valid) - * - * Retrieve result of last measurement (only valid when state == ULTRASONIC_COMPLETE). - * Resets state to IDLE after reading. - * - * Arguments: - * - distance_mm: pointer to store distance in mm - * - is_valid: pointer to store validity flag - * - * Returns: true if result retrieved, false if no measurement available - */ -bool ultrasonic_get_result(uint16_t *distance_mm, bool *is_valid); - -/* - * ultrasonic_tick(now_ms) - * - * Update function called periodically (recommended: every 1-10ms in main loop). - * Handles timeout detection for echo measurement. - * Must be called regularly for non-blocking operation. - * - * Arguments: - * - now_ms: current time in milliseconds (from HAL_GetTick() or similar) - */ -void ultrasonic_tick(uint32_t now_ms); - -#endif /* ULTRASONIC_H */ diff --git a/legacy/stm32/include/vesc_can.h b/legacy/stm32/include/vesc_can.h deleted file mode 100644 index 21f67d1..0000000 --- a/legacy/stm32/include/vesc_can.h +++ /dev/null @@ -1,117 +0,0 @@ -#ifndef VESC_CAN_H -#define VESC_CAN_H - -#include -#include - -/* - * vesc_can — VESC CAN protocol driver for FSESC 6.7 Pro Mini Dual (Issue #674). - * - * VESC uses 29-bit extended CAN IDs: - * arbitration_id = (packet_type << 8) | vesc_node_id - * - * Wire format is big-endian throughout (matches VESC FW 6.x). - * - * Physical layer: CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps. - * - * NOTE ON PA11/PA12 vs PB12/PB13: - * PA11/PA12 carry CAN1_RX/TX (AF9) BUT are also USB_OTG_FS DM/DP (AF10). - * USB CDC is active on this board, so PA11/PA12 are occupied. - * PB8/PB9 (CAN1 alternate) are occupied by I2C1 (barometer). - * CAN2 on PB12/PB13 is the only conflict-free choice. - * If the SN65HVD230 is wired to the pads labelled RX6/TX6 on the Mamba - * silkscreen, those pads connect to PB12/PB13 (SPI2/OSD, repurposed). - * - * VESC frames arrive in FIFO1 (extended-ID filter, bank 15). - * Orin standard frames arrive in FIFO0 (standard-ID filter, bank 14). - */ - -/* ---- VESC packet type IDs (upper byte of 29-bit arb ID) ---- */ -#define VESC_PKT_SET_DUTY 0u /* int32 duty × 100000 */ -#define VESC_PKT_SET_CURRENT 1u /* int32 current (mA) */ -#define VESC_PKT_SET_CURRENT_BRAKE 2u /* int32 brake current (mA) */ -#define VESC_PKT_SET_RPM 3u /* int32 target RPM */ -#define VESC_PKT_STATUS 9u /* int32 RPM, int16 I×10, int16 duty×1000 */ -#define VESC_PKT_STATUS_4 16u /* int16 T_fet×10, T_mot×10, I_in×10 */ -#define VESC_PKT_STATUS_5 27u /* int32 tacho, int16 V_in×10 */ - -/* ---- Default VESC node IDs (configurable via vesc_can_init) ---- */ -#define VESC_CAN_ID_LEFT 56u -#define VESC_CAN_ID_RIGHT 68u - -/* ---- Alive timeout ---- */ -#define VESC_ALIVE_TIMEOUT_MS 1000u /* node offline if no STATUS for 1 s */ - -/* ---- JLink telemetry rate ---- */ -#define VESC_TLM_HZ 1u - -/* ---- Fault codes (VESC FW 6.6) ---- */ -#define VESC_FAULT_NONE 0u -#define VESC_FAULT_OVER_VOLTAGE 1u -#define VESC_FAULT_UNDER_VOLTAGE 2u -#define VESC_FAULT_DRV 3u -#define VESC_FAULT_ABS_OVER_CURRENT 4u -#define VESC_FAULT_OVER_TEMP_FET 5u -#define VESC_FAULT_OVER_TEMP_MOTOR 6u -#define VESC_FAULT_GATE_DRIVER_OVER_VOLTAGE 7u -#define VESC_FAULT_GATE_DRIVER_UNDER_VOLTAGE 8u -#define VESC_FAULT_MCU_UNDER_VOLTAGE 9u -#define VESC_FAULT_WATCHDOG_RESET 10u - -/* ---- Telemetry state per VESC node ---- */ -typedef struct { - int32_t rpm; /* actual RPM (STATUS pkt, int32 BE) */ - int16_t current_x10; /* phase current (A × 10; STATUS pkt) */ - int16_t duty_x1000; /* duty cycle (× 1000; –1000..+1000) */ - int16_t temp_fet_x10; /* FET temperature (°C × 10; STATUS_4) */ - int16_t temp_motor_x10; /* motor temperature (°C × 10; STATUS_4) */ - int16_t current_in_x10; /* input (battery) current (A × 10; STATUS_4) */ - int16_t voltage_x10; /* input voltage (V × 10; STATUS_5) */ - uint8_t fault_code; /* VESC fault code (0 = none) */ - uint8_t _pad; - uint32_t last_rx_ms; /* HAL_GetTick() of last received STATUS frame */ -} vesc_state_t; - -/* ---- API ---- */ - -/* - * vesc_can_init(id_left, id_right) — store VESC node IDs and register the - * extended-frame callback with can_driver. - * Call after can_driver_init(). - */ -void vesc_can_init(uint8_t id_left, uint8_t id_right); - -/* - * vesc_can_send_rpm(vesc_id, rpm) — transmit VESC_PKT_SET_RPM (3) to the - * target VESC. arb_id = (3 << 8) | vesc_id. Payload: int32 big-endian. - */ -void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm); - -/* - * vesc_can_on_frame(ext_id, data, len) — called by can_driver when an - * extended-ID frame arrives (registered via can_driver_set_ext_cb). - * Parses STATUS / STATUS_4 / STATUS_5 into the matching vesc_state_t. - */ -void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len); - -/* - * vesc_can_get_state(vesc_id, out) — copy latest telemetry snapshot. - * vesc_id must match id_left or id_right passed to vesc_can_init. - * Returns false if vesc_id unknown or no frame has arrived yet. - */ -bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out); - -/* - * vesc_can_is_alive(vesc_id, now_ms) — true if a STATUS frame arrived - * within VESC_ALIVE_TIMEOUT_MS of now_ms. - */ -bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms); - -/* - * vesc_can_send_tlm(now_ms) — rate-limited JLINK_TLM_VESC_STATE (0x8E) - * telemetry to Orin over JLink. Safe to call every ms; internally - * rate-limited to VESC_TLM_HZ (1 Hz). - */ -void vesc_can_send_tlm(uint32_t now_ms); - -#endif /* VESC_CAN_H */ diff --git a/legacy/stm32/include/watchdog.h b/legacy/stm32/include/watchdog.h deleted file mode 100644 index 7a62b36..0000000 --- a/legacy/stm32/include/watchdog.h +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef WATCHDOG_H -#define WATCHDOG_H - -#include -#include - -/* - * watchdog.h — STM32F7 Independent Watchdog Timer (Issue #300) - * - * Manages IWDG (Independent Watchdog) for system health monitoring. - * Detects communication stalls from Jetson and resets the MCU. - * - * Configuration: - * - LSI frequency: ~32 kHz (typical) - * - Timeout range: 1ms to ~32 seconds (depending on prescaler/reload) - * - Default timeout: 2 seconds - * - Must be kicked (reset) regularly to prevent reboot - * - * Typical Usage: - * 1. Call watchdog_init(2000) in system startup - * 2. Call watchdog_kick() regularly from main loop (e.g., every 100ms) - * 3. If watchdog_kick() is not called for >= timeout, MCU resets - * 4. Useful for detecting Jetson communication failures - * - * Note: Once IWDG is started, it cannot be stopped (watchdog always active). - * It can only be reset via watchdog_kick() or by MCU reset/power cycle. - */ - -/* Watchdog timeout presets (in milliseconds) */ -typedef enum { - WATCHDOG_TIMEOUT_1S = 1000, /* 1 second timeout */ - WATCHDOG_TIMEOUT_2S = 2000, /* 2 seconds (default) */ - WATCHDOG_TIMEOUT_4S = 4000, /* 4 seconds */ - WATCHDOG_TIMEOUT_8S = 8000, /* 8 seconds */ - WATCHDOG_TIMEOUT_16S = 16000 /* 16 seconds */ -} WatchdogTimeout; - -/* - * watchdog_init(timeout_ms) - * - * Initialize the Independent Watchdog Timer. - * - * - Configures IWDG with specified timeout - * - Starts the watchdog timer (cannot be stopped) - * - Must call watchdog_kick() regularly to prevent reset - * - * Arguments: - * - timeout_ms: Timeout in milliseconds (e.g., 2000 for 2 seconds) - * Typical range: 1-16000 ms - * Will clamp to valid range - * - * Returns: true if initialized, false if invalid timeout - */ -bool watchdog_init(uint32_t timeout_ms); - -/* - * watchdog_kick() - * - * Reset the watchdog timer counter. - * Call this regularly from the main loop (e.g., every 100ms or faster). - * If not called within the configured timeout period, MCU resets. - * - * Note: This is typically called from a high-priority timer interrupt - * or the main application loop to ensure timing is deterministic. - */ -void watchdog_kick(void); - -/* - * watchdog_get_timeout() - * - * Get the configured watchdog timeout in milliseconds. - * - * Returns: Timeout value in ms - */ -uint32_t watchdog_get_timeout(void); - -/* - * watchdog_is_running() - * - * Check if watchdog timer is running. - * Once started, watchdog cannot be stopped (only reset via kick). - * - * Returns: true if watchdog is active, false if not initialized - */ -bool watchdog_is_running(void); - -/* - * watchdog_was_reset_by_watchdog() - * - * Detect if the last MCU reset was caused by watchdog timeout. - * Useful for diagnosing system failures (e.g., Jetson communication loss). - * - * Call this in early startup (before watchdog_init) to check reset reason. - * Typically used to log or report watchdog resets to debugging systems. - * - * Returns: true if last reset was by watchdog, false otherwise - */ -bool watchdog_was_reset_by_watchdog(void); - -#endif /* WATCHDOG_H */ diff --git a/legacy/stm32/lib/USB_CDC/include/usbd_cdc.h b/legacy/stm32/lib/USB_CDC/include/usbd_cdc.h deleted file mode 100644 index 1316976..0000000 --- a/legacy/stm32/lib/USB_CDC/include/usbd_cdc.h +++ /dev/null @@ -1,184 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_cdc.h - * @author MCD Application Team - * @brief header file for the usbd_cdc.c file. - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __USB_CDC_H -#define __USB_CDC_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_ioreq.h" - -/** @addtogroup STM32_USB_DEVICE_LIBRARY - * @{ - */ - -/** @defgroup usbd_cdc - * @brief This file is the Header file for usbd_cdc.c - * @{ - */ - - -/** @defgroup usbd_cdc_Exported_Defines - * @{ - */ -#ifndef CDC_IN_EP -#define CDC_IN_EP 0x81U /* EP1 for data IN */ -#endif /* CDC_IN_EP */ -#ifndef CDC_OUT_EP -#define CDC_OUT_EP 0x01U /* EP1 for data OUT */ -#endif /* CDC_OUT_EP */ -#ifndef CDC_CMD_EP -#define CDC_CMD_EP 0x82U /* EP2 for CDC commands */ -#endif /* CDC_CMD_EP */ - -#ifndef CDC_HS_BINTERVAL -#define CDC_HS_BINTERVAL 0x10U -#endif /* CDC_HS_BINTERVAL */ - -#ifndef CDC_FS_BINTERVAL -#define CDC_FS_BINTERVAL 0x10U -#endif /* CDC_FS_BINTERVAL */ - -/* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */ -#define CDC_DATA_HS_MAX_PACKET_SIZE 512U /* Endpoint IN & OUT Packet size */ -#define CDC_DATA_FS_MAX_PACKET_SIZE 64U /* Endpoint IN & OUT Packet size */ -#define CDC_CMD_PACKET_SIZE 8U /* Control Endpoint Packet size */ - -#define USB_CDC_CONFIG_DESC_SIZ 67U -#define CDC_DATA_HS_IN_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE -#define CDC_DATA_HS_OUT_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE - -#define CDC_DATA_FS_IN_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE -#define CDC_DATA_FS_OUT_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE - -#define CDC_REQ_MAX_DATA_SIZE 0x7U -/*---------------------------------------------------------------------*/ -/* CDC definitions */ -/*---------------------------------------------------------------------*/ -#define CDC_SEND_ENCAPSULATED_COMMAND 0x00U -#define CDC_GET_ENCAPSULATED_RESPONSE 0x01U -#define CDC_SET_COMM_FEATURE 0x02U -#define CDC_GET_COMM_FEATURE 0x03U -#define CDC_CLEAR_COMM_FEATURE 0x04U -#define CDC_SET_LINE_CODING 0x20U -#define CDC_GET_LINE_CODING 0x21U -#define CDC_SET_CONTROL_LINE_STATE 0x22U -#define CDC_SEND_BREAK 0x23U - -/** - * @} - */ - - -/** @defgroup USBD_CORE_Exported_TypesDefinitions - * @{ - */ - -/** - * @} - */ -typedef struct -{ - uint32_t bitrate; - uint8_t format; - uint8_t paritytype; - uint8_t datatype; -} USBD_CDC_LineCodingTypeDef; - -typedef struct _USBD_CDC_Itf -{ - int8_t (* Init)(void); - int8_t (* DeInit)(void); - int8_t (* Control)(uint8_t cmd, uint8_t *pbuf, uint16_t length); - int8_t (* Receive)(uint8_t *Buf, uint32_t *Len); - int8_t (* TransmitCplt)(uint8_t *Buf, uint32_t *Len, uint8_t epnum); -} USBD_CDC_ItfTypeDef; - - -typedef struct -{ - uint32_t data[CDC_DATA_HS_MAX_PACKET_SIZE / 4U]; /* Force 32-bit alignment */ - uint8_t CmdOpCode; - uint8_t CmdLength; - uint8_t *RxBuffer; - uint8_t *TxBuffer; - uint32_t RxLength; - uint32_t TxLength; - - __IO uint32_t TxState; - __IO uint32_t RxState; -} USBD_CDC_HandleTypeDef; - - - -/** @defgroup USBD_CORE_Exported_Macros - * @{ - */ - -/** - * @} - */ - -/** @defgroup USBD_CORE_Exported_Variables - * @{ - */ - -extern USBD_ClassTypeDef USBD_CDC; -#define USBD_CDC_CLASS &USBD_CDC -/** - * @} - */ - -/** @defgroup USB_CORE_Exported_Functions - * @{ - */ -uint8_t USBD_CDC_RegisterInterface(USBD_HandleTypeDef *pdev, - USBD_CDC_ItfTypeDef *fops); - -#ifdef USE_USBD_COMPOSITE -uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff, - uint32_t length, uint8_t ClassId); -uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t ClassId); -#else -uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff, - uint32_t length); -uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev); -#endif /* USE_USBD_COMPOSITE */ -uint8_t USBD_CDC_SetRxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff); -uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev); -/** - * @} - */ - -#ifdef __cplusplus -} -#endif - -#endif /* __USB_CDC_H */ -/** - * @} - */ - -/** - * @} - */ - diff --git a/legacy/stm32/lib/USB_CDC/include/usbd_cdc_if.h b/legacy/stm32/lib/USB_CDC/include/usbd_cdc_if.h deleted file mode 100644 index 8d512f0..0000000 --- a/legacy/stm32/lib/USB_CDC/include/usbd_cdc_if.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef USBD_CDC_IF_H -#define USBD_CDC_IF_H - -#include "usbd_cdc.h" - -extern USBD_CDC_ItfTypeDef USBD_CDC_fops; - -/* Send data over USB CDC */ -uint8_t CDC_Transmit(uint8_t *buf, uint16_t len); - -/* Betaflight-style DFU reboot check — call early in main() */ -void checkForBootloader(void); - -/* PID tuning command interface (written by USB IRQ, read by main loop) */ -extern volatile uint8_t cdc_cmd_ready; -extern volatile char cdc_cmd_buf[32]; - -extern volatile uint8_t cdc_estop_request; -extern volatile uint8_t cdc_estop_clear_request; -#endif diff --git a/legacy/stm32/lib/USB_CDC/include/usbd_conf.h b/legacy/stm32/lib/USB_CDC/include/usbd_conf.h deleted file mode 100644 index b9f6d3a..0000000 --- a/legacy/stm32/lib/USB_CDC/include/usbd_conf.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef USBD_CONF_H -#define USBD_CONF_H - -/* Match Betaflight's usbd_conf.h exactly */ -#include "stm32f7xx_hal.h" -#include -#include -#include - -#define USBD_MAX_NUM_INTERFACES 3 -#define USBD_MAX_NUM_CONFIGURATION 1 -#define USBD_MAX_STR_DESC_SIZ 0x100 -#define USBD_SUPPORT_USER_STRING 0 -#define USBD_SELF_POWERED 1 -#define USBD_DEBUG_LEVEL 0 -#define USE_USB_FS - -#define USBD_malloc malloc -#define USBD_free free -#define USBD_memset memset -#define USBD_memcpy memcpy - -#define USBD_UsrLog(...) -#define USBD_ErrLog(...) -#define USBD_DbgLog(...) - -#endif diff --git a/legacy/stm32/lib/USB_CDC/include/usbd_core.h b/legacy/stm32/lib/USB_CDC/include/usbd_core.h deleted file mode 100644 index 5d8a284..0000000 --- a/legacy/stm32/lib/USB_CDC/include/usbd_core.h +++ /dev/null @@ -1,175 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_core.h - * @author MCD Application Team - * @brief Header file for usbd_core.c file - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __USBD_CORE_H -#define __USBD_CORE_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_conf.h" -#include "usbd_def.h" -#include "usbd_ioreq.h" -#include "usbd_ctlreq.h" - -/** @addtogroup STM32_USB_DEVICE_LIBRARY - * @{ - */ - -/** @defgroup USBD_CORE - * @brief This file is the Header file for usbd_core.c file - * @{ - */ - - -/** @defgroup USBD_CORE_Exported_Defines - * @{ - */ -#ifndef USBD_DEBUG_LEVEL -#define USBD_DEBUG_LEVEL 0U -#endif /* USBD_DEBUG_LEVEL */ -/** - * @} - */ - - -/** @defgroup USBD_CORE_Exported_TypesDefinitions - * @{ - */ - - -/** - * @} - */ - - - -/** @defgroup USBD_CORE_Exported_Macros - * @{ - */ - -/** - * @} - */ - -/** @defgroup USBD_CORE_Exported_Variables - * @{ - */ -#define USBD_SOF USBD_LL_SOF -/** - * @} - */ - -/** @defgroup USBD_CORE_Exported_FunctionsPrototype - * @{ - */ -USBD_StatusTypeDef USBD_Init(USBD_HandleTypeDef *pdev, USBD_DescriptorsTypeDef *pdesc, uint8_t id); -USBD_StatusTypeDef USBD_DeInit(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_Start(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_Stop(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_RegisterClass(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass); -#if (USBD_USER_REGISTER_CALLBACK == 1U) -USBD_StatusTypeDef USBD_RegisterDevStateCallback(USBD_HandleTypeDef *pdev, USBD_DevStateCallbackTypeDef pUserCallback); -#endif /* USBD_USER_REGISTER_CALLBACK */ - -#ifdef USE_USBD_COMPOSITE -USBD_StatusTypeDef USBD_RegisterClassComposite(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass, - USBD_CompositeClassTypeDef classtype, uint8_t *EpAddr); - -USBD_StatusTypeDef USBD_UnRegisterClassComposite(USBD_HandleTypeDef *pdev); -uint8_t USBD_CoreGetEPAdd(USBD_HandleTypeDef *pdev, uint8_t ep_dir, uint8_t ep_type, uint8_t ClassId); -#endif /* USE_USBD_COMPOSITE */ - -uint8_t USBD_CoreFindIF(USBD_HandleTypeDef *pdev, uint8_t index); -uint8_t USBD_CoreFindEP(USBD_HandleTypeDef *pdev, uint8_t index); - -USBD_StatusTypeDef USBD_RunTestMode(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_SetClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx); -USBD_StatusTypeDef USBD_ClrClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx); - -USBD_StatusTypeDef USBD_LL_SetupStage(USBD_HandleTypeDef *pdev, uint8_t *psetup); -USBD_StatusTypeDef USBD_LL_DataOutStage(USBD_HandleTypeDef *pdev, uint8_t epnum, uint8_t *pdata); -USBD_StatusTypeDef USBD_LL_DataInStage(USBD_HandleTypeDef *pdev, uint8_t epnum, uint8_t *pdata); - -USBD_StatusTypeDef USBD_LL_Reset(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_LL_SetSpeed(USBD_HandleTypeDef *pdev, USBD_SpeedTypeDef speed); -USBD_StatusTypeDef USBD_LL_Suspend(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_LL_Resume(USBD_HandleTypeDef *pdev); - -USBD_StatusTypeDef USBD_LL_SOF(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum); -USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum); - -USBD_StatusTypeDef USBD_LL_DevConnected(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_LL_DevDisconnected(USBD_HandleTypeDef *pdev); - -/* USBD Low Level Driver */ -USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_LL_DeInit(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_LL_Start(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_LL_Stop(USBD_HandleTypeDef *pdev); - -USBD_StatusTypeDef USBD_LL_OpenEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr, - uint8_t ep_type, uint16_t ep_mps); - -USBD_StatusTypeDef USBD_LL_CloseEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); -USBD_StatusTypeDef USBD_LL_FlushEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); -USBD_StatusTypeDef USBD_LL_StallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); -USBD_StatusTypeDef USBD_LL_ClearStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); -USBD_StatusTypeDef USBD_LL_SetUSBAddress(USBD_HandleTypeDef *pdev, uint8_t dev_addr); - -USBD_StatusTypeDef USBD_LL_Transmit(USBD_HandleTypeDef *pdev, uint8_t ep_addr, - uint8_t *pbuf, uint32_t size); - -USBD_StatusTypeDef USBD_LL_PrepareReceive(USBD_HandleTypeDef *pdev, uint8_t ep_addr, - uint8_t *pbuf, uint32_t size); - -#ifdef USBD_HS_TESTMODE_ENABLE -USBD_StatusTypeDef USBD_LL_SetTestMode(USBD_HandleTypeDef *pdev, uint8_t testmode); -#endif /* USBD_HS_TESTMODE_ENABLE */ - -uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr); -uint32_t USBD_LL_GetRxDataSize(USBD_HandleTypeDef *pdev, uint8_t ep_addr); - -void USBD_LL_Delay(uint32_t Delay); - -void *USBD_GetEpDesc(uint8_t *pConfDesc, uint8_t EpAddr); -USBD_DescHeaderTypeDef *USBD_GetNextDesc(uint8_t *pbuf, uint16_t *ptr); - -/** - * @} - */ - -#ifdef __cplusplus -} -#endif - -#endif /* __USBD_CORE_H */ - -/** - * @} - */ - -/** - * @} - */ - - diff --git a/legacy/stm32/lib/USB_CDC/include/usbd_ctlreq.h b/legacy/stm32/lib/USB_CDC/include/usbd_ctlreq.h deleted file mode 100644 index 3c4eb16..0000000 --- a/legacy/stm32/lib/USB_CDC/include/usbd_ctlreq.h +++ /dev/null @@ -1,101 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_req.h - * @author MCD Application Team - * @brief Header file for the usbd_req.c file - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __USB_REQUEST_H -#define __USB_REQUEST_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_def.h" - - -/** @addtogroup STM32_USB_DEVICE_LIBRARY - * @{ - */ - -/** @defgroup USBD_REQ - * @brief header file for the usbd_req.c file - * @{ - */ - -/** @defgroup USBD_REQ_Exported_Defines - * @{ - */ -/** - * @} - */ - - -/** @defgroup USBD_REQ_Exported_Types - * @{ - */ -/** - * @} - */ - - - -/** @defgroup USBD_REQ_Exported_Macros - * @{ - */ -/** - * @} - */ - -/** @defgroup USBD_REQ_Exported_Variables - * @{ - */ -/** - * @} - */ - -/** @defgroup USBD_REQ_Exported_FunctionsPrototype - * @{ - */ - -USBD_StatusTypeDef USBD_StdDevReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -USBD_StatusTypeDef USBD_StdItfReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -USBD_StatusTypeDef USBD_StdEPReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); - -void USBD_CtlError(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -void USBD_ParseSetupRequest(USBD_SetupReqTypedef *req, uint8_t *pdata); -void USBD_GetString(uint8_t *desc, uint8_t *unicode, uint16_t *len); - -/** - * @} - */ - -#ifdef __cplusplus -} -#endif - -#endif /* __USB_REQUEST_H */ - -/** - * @} - */ - -/** - * @} - */ - - diff --git a/legacy/stm32/lib/USB_CDC/include/usbd_def.h b/legacy/stm32/lib/USB_CDC/include/usbd_def.h deleted file mode 100644 index 52778b3..0000000 --- a/legacy/stm32/lib/USB_CDC/include/usbd_def.h +++ /dev/null @@ -1,523 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_def.h - * @author MCD Application Team - * @brief General defines for the usb device library - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __USBD_DEF_H -#define __USBD_DEF_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_conf.h" - -/** @addtogroup STM32_USBD_DEVICE_LIBRARY - * @{ - */ - -/** @defgroup USB_DEF - * @brief general defines for the usb device library file - * @{ - */ - -/** @defgroup USB_DEF_Exported_Defines - * @{ - */ - -#ifndef NULL -#define NULL 0U -#endif /* NULL */ - -#ifndef USBD_MAX_NUM_INTERFACES -#define USBD_MAX_NUM_INTERFACES 1U -#endif /* USBD_MAX_NUM_CONFIGURATION */ - -#ifndef USBD_MAX_NUM_CONFIGURATION -#define USBD_MAX_NUM_CONFIGURATION 1U -#endif /* USBD_MAX_NUM_CONFIGURATION */ - -#ifdef USE_USBD_COMPOSITE -#ifndef USBD_MAX_SUPPORTED_CLASS -#define USBD_MAX_SUPPORTED_CLASS 4U -#endif /* USBD_MAX_SUPPORTED_CLASS */ -#else -#ifndef USBD_MAX_SUPPORTED_CLASS -#define USBD_MAX_SUPPORTED_CLASS 1U -#endif /* USBD_MAX_SUPPORTED_CLASS */ -#endif /* USE_USBD_COMPOSITE */ - -#ifndef USBD_MAX_CLASS_ENDPOINTS -#define USBD_MAX_CLASS_ENDPOINTS 5U -#endif /* USBD_MAX_CLASS_ENDPOINTS */ - -#ifndef USBD_MAX_CLASS_INTERFACES -#define USBD_MAX_CLASS_INTERFACES 5U -#endif /* USBD_MAX_CLASS_INTERFACES */ - -#ifndef USBD_LPM_ENABLED -#define USBD_LPM_ENABLED 0U -#endif /* USBD_LPM_ENABLED */ - -#ifndef USBD_SELF_POWERED -#define USBD_SELF_POWERED 1U -#endif /*USBD_SELF_POWERED */ - -#ifndef USBD_MAX_POWER -#define USBD_MAX_POWER 0x32U /* 100 mA */ -#endif /* USBD_MAX_POWER */ - -#ifndef USBD_SUPPORT_USER_STRING_DESC -#define USBD_SUPPORT_USER_STRING_DESC 0U -#endif /* USBD_SUPPORT_USER_STRING_DESC */ - -#ifndef USBD_CLASS_USER_STRING_DESC -#define USBD_CLASS_USER_STRING_DESC 0U -#endif /* USBD_CLASS_USER_STRING_DESC */ - -#define USB_LEN_DEV_QUALIFIER_DESC 0x0AU -#define USB_LEN_DEV_DESC 0x12U -#define USB_LEN_CFG_DESC 0x09U -#define USB_LEN_IF_DESC 0x09U -#define USB_LEN_EP_DESC 0x07U -#define USB_LEN_OTG_DESC 0x03U -#define USB_LEN_LANGID_STR_DESC 0x04U -#define USB_LEN_OTHER_SPEED_DESC_SIZ 0x09U - -#define USBD_IDX_LANGID_STR 0x00U -#define USBD_IDX_MFC_STR 0x01U -#define USBD_IDX_PRODUCT_STR 0x02U -#define USBD_IDX_SERIAL_STR 0x03U -#define USBD_IDX_CONFIG_STR 0x04U -#define USBD_IDX_INTERFACE_STR 0x05U - -#define USB_REQ_TYPE_STANDARD 0x00U -#define USB_REQ_TYPE_CLASS 0x20U -#define USB_REQ_TYPE_VENDOR 0x40U -#define USB_REQ_TYPE_MASK 0x60U - -#define USB_REQ_RECIPIENT_DEVICE 0x00U -#define USB_REQ_RECIPIENT_INTERFACE 0x01U -#define USB_REQ_RECIPIENT_ENDPOINT 0x02U -#define USB_REQ_RECIPIENT_MASK 0x03U - -#define USB_REQ_GET_STATUS 0x00U -#define USB_REQ_CLEAR_FEATURE 0x01U -#define USB_REQ_SET_FEATURE 0x03U -#define USB_REQ_SET_ADDRESS 0x05U -#define USB_REQ_GET_DESCRIPTOR 0x06U -#define USB_REQ_SET_DESCRIPTOR 0x07U -#define USB_REQ_GET_CONFIGURATION 0x08U -#define USB_REQ_SET_CONFIGURATION 0x09U -#define USB_REQ_GET_INTERFACE 0x0AU -#define USB_REQ_SET_INTERFACE 0x0BU -#define USB_REQ_SYNCH_FRAME 0x0CU - -#define USB_DESC_TYPE_DEVICE 0x01U -#define USB_DESC_TYPE_CONFIGURATION 0x02U -#define USB_DESC_TYPE_STRING 0x03U -#define USB_DESC_TYPE_INTERFACE 0x04U -#define USB_DESC_TYPE_ENDPOINT 0x05U -#define USB_DESC_TYPE_DEVICE_QUALIFIER 0x06U -#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION 0x07U -#define USB_DESC_TYPE_IAD 0x0BU -#define USB_DESC_TYPE_BOS 0x0FU - -#define USB_CONFIG_REMOTE_WAKEUP 0x02U -#define USB_CONFIG_SELF_POWERED 0x01U - -#define USB_FEATURE_EP_HALT 0x00U -#define USB_FEATURE_REMOTE_WAKEUP 0x01U -#define USB_FEATURE_TEST_MODE 0x02U - -#define USB_DEVICE_CAPABITY_TYPE 0x10U - -#define USB_CONF_DESC_SIZE 0x09U -#define USB_IF_DESC_SIZE 0x09U -#define USB_EP_DESC_SIZE 0x07U -#define USB_IAD_DESC_SIZE 0x08U - -#define USB_HS_MAX_PACKET_SIZE 512U -#define USB_FS_MAX_PACKET_SIZE 64U -#define USB_MAX_EP0_SIZE 64U - -/* Device Status */ -#define USBD_STATE_DEFAULT 0x01U -#define USBD_STATE_ADDRESSED 0x02U -#define USBD_STATE_CONFIGURED 0x03U -#define USBD_STATE_SUSPENDED 0x04U - - -/* EP0 State */ -#define USBD_EP0_IDLE 0x00U -#define USBD_EP0_SETUP 0x01U -#define USBD_EP0_DATA_IN 0x02U -#define USBD_EP0_DATA_OUT 0x03U -#define USBD_EP0_STATUS_IN 0x04U -#define USBD_EP0_STATUS_OUT 0x05U -#define USBD_EP0_STALL 0x06U - -#define USBD_EP_TYPE_CTRL 0x00U -#define USBD_EP_TYPE_ISOC 0x01U -#define USBD_EP_TYPE_BULK 0x02U -#define USBD_EP_TYPE_INTR 0x03U - -#ifdef USE_USBD_COMPOSITE -#define USBD_EP_IN 0x80U -#define USBD_EP_OUT 0x00U -#define USBD_FUNC_DESCRIPTOR_TYPE 0x24U -#define USBD_DESC_SUBTYPE_ACM 0x0FU -#define USBD_DESC_ECM_BCD_LOW 0x00U -#define USBD_DESC_ECM_BCD_HIGH 0x10U -#endif /* USE_USBD_COMPOSITE */ -/** - * @} - */ - - -/** @defgroup USBD_DEF_Exported_TypesDefinitions - * @{ - */ - -typedef struct usb_setup_req -{ - uint8_t bmRequest; - uint8_t bRequest; - uint16_t wValue; - uint16_t wIndex; - uint16_t wLength; -} USBD_SetupReqTypedef; - -typedef struct -{ - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t wTotalLength; - uint8_t bNumInterfaces; - uint8_t bConfigurationValue; - uint8_t iConfiguration; - uint8_t bmAttributes; - uint8_t bMaxPower; -} __PACKED USBD_ConfigDescTypeDef; - -typedef struct -{ - uint8_t bLength; - uint8_t bDescriptorType; - uint16_t wTotalLength; - uint8_t bNumDeviceCaps; -} USBD_BosDescTypeDef; - -typedef struct -{ - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bEndpointAddress; - uint8_t bmAttributes; - uint16_t wMaxPacketSize; - uint8_t bInterval; -} __PACKED USBD_EpDescTypeDef; - -typedef struct -{ - uint8_t bLength; - uint8_t bDescriptorType; - uint8_t bDescriptorSubType; -} USBD_DescHeaderTypeDef; - -struct _USBD_HandleTypeDef; - -typedef struct _Device_cb -{ - uint8_t (*Init)(struct _USBD_HandleTypeDef *pdev, uint8_t cfgidx); - uint8_t (*DeInit)(struct _USBD_HandleTypeDef *pdev, uint8_t cfgidx); - /* Control Endpoints*/ - uint8_t (*Setup)(struct _USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); - uint8_t (*EP0_TxSent)(struct _USBD_HandleTypeDef *pdev); - uint8_t (*EP0_RxReady)(struct _USBD_HandleTypeDef *pdev); - /* Class Specific Endpoints*/ - uint8_t (*DataIn)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum); - uint8_t (*DataOut)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum); - uint8_t (*SOF)(struct _USBD_HandleTypeDef *pdev); - uint8_t (*IsoINIncomplete)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum); - uint8_t (*IsoOUTIncomplete)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum); - - uint8_t *(*GetHSConfigDescriptor)(uint16_t *length); - uint8_t *(*GetFSConfigDescriptor)(uint16_t *length); - uint8_t *(*GetOtherSpeedConfigDescriptor)(uint16_t *length); - uint8_t *(*GetDeviceQualifierDescriptor)(uint16_t *length); -#if (USBD_SUPPORT_USER_STRING_DESC == 1U) - uint8_t *(*GetUsrStrDescriptor)(struct _USBD_HandleTypeDef *pdev, uint8_t index, uint16_t *length); -#endif /* USBD_SUPPORT_USER_STRING_DESC */ - -} USBD_ClassTypeDef; - -/* Following USB Device Speed */ -typedef enum -{ - USBD_SPEED_HIGH = 0U, - USBD_SPEED_FULL = 1U, - USBD_SPEED_LOW = 2U, -} USBD_SpeedTypeDef; - -/* Following USB Device status */ -typedef enum -{ - USBD_OK = 0U, - USBD_BUSY, - USBD_EMEM, - USBD_FAIL, -} USBD_StatusTypeDef; - -/* USB Device descriptors structure */ -typedef struct -{ - uint8_t *(*GetDeviceDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); - uint8_t *(*GetLangIDStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); - uint8_t *(*GetManufacturerStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); - uint8_t *(*GetProductStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); - uint8_t *(*GetSerialStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); - uint8_t *(*GetConfigurationStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); - uint8_t *(*GetInterfaceStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); -#if (USBD_CLASS_USER_STRING_DESC == 1) - uint8_t *(*GetUserStrDescriptor)(USBD_SpeedTypeDef speed, uint8_t idx, uint16_t *length); -#endif /* USBD_CLASS_USER_STRING_DESC */ -#if ((USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1)) - uint8_t *(*GetBOSDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length); -#endif /* (USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1) */ -} USBD_DescriptorsTypeDef; - -/* USB Device handle structure */ -typedef struct -{ - uint32_t status; - uint32_t total_length; - uint32_t rem_length; - uint32_t maxpacket; - uint16_t is_used; - uint16_t bInterval; -} USBD_EndpointTypeDef; - -#ifdef USE_USBD_COMPOSITE -typedef enum -{ - CLASS_TYPE_NONE = 0, - CLASS_TYPE_HID = 1, - CLASS_TYPE_CDC = 2, - CLASS_TYPE_MSC = 3, - CLASS_TYPE_DFU = 4, - CLASS_TYPE_CHID = 5, - CLASS_TYPE_AUDIO = 6, - CLASS_TYPE_ECM = 7, - CLASS_TYPE_RNDIS = 8, - CLASS_TYPE_MTP = 9, - CLASS_TYPE_VIDEO = 10, - CLASS_TYPE_PRINTER = 11, - CLASS_TYPE_CCID = 12, -} USBD_CompositeClassTypeDef; - - -/* USB Device handle structure */ -typedef struct -{ - uint8_t add; - uint8_t type; - uint8_t size; - uint8_t is_used; -} USBD_EPTypeDef; - -/* USB Device handle structure */ -typedef struct -{ - USBD_CompositeClassTypeDef ClassType; - uint32_t ClassId; - uint32_t Active; - uint32_t NumEps; - USBD_EPTypeDef Eps[USBD_MAX_CLASS_ENDPOINTS]; - uint8_t *EpAdd; - uint32_t NumIf; - uint8_t Ifs[USBD_MAX_CLASS_INTERFACES]; - uint32_t CurrPcktSze; -} USBD_CompositeElementTypeDef; -#endif /* USE_USBD_COMPOSITE */ - -/* USB Device handle structure */ -typedef struct _USBD_HandleTypeDef -{ - uint8_t id; - uint32_t dev_config; - uint32_t dev_default_config; - uint32_t dev_config_status; - USBD_SpeedTypeDef dev_speed; - USBD_EndpointTypeDef ep_in[16]; - USBD_EndpointTypeDef ep_out[16]; - __IO uint32_t ep0_state; - uint32_t ep0_data_len; - __IO uint8_t dev_state; - __IO uint8_t dev_old_state; - uint8_t dev_address; - uint8_t dev_connection_status; - uint8_t dev_test_mode; - uint32_t dev_remote_wakeup; - uint8_t ConfIdx; - - USBD_SetupReqTypedef request; - USBD_DescriptorsTypeDef *pDesc; - USBD_ClassTypeDef *pClass[USBD_MAX_SUPPORTED_CLASS]; - void *pClassData; - void *pClassDataCmsit[USBD_MAX_SUPPORTED_CLASS]; - void *pUserData[USBD_MAX_SUPPORTED_CLASS]; - void *pData; - void *pBosDesc; - void *pConfDesc; - uint32_t classId; - uint32_t NumClasses; -#ifdef USE_USBD_COMPOSITE - USBD_CompositeElementTypeDef tclasslist[USBD_MAX_SUPPORTED_CLASS]; -#endif /* USE_USBD_COMPOSITE */ -#if (USBD_USER_REGISTER_CALLBACK == 1U) - void (* DevStateCallback)(uint8_t dev_state, uint8_t cfgidx); /*!< User Notification callback */ -#endif /* USBD_USER_REGISTER_CALLBACK */ -} USBD_HandleTypeDef; - -#if (USBD_USER_REGISTER_CALLBACK == 1U) -typedef void (*USBD_DevStateCallbackTypeDef)(uint8_t dev_state, uint8_t cfgidx); /*!< pointer to User callback function */ -#endif /* USBD_USER_REGISTER_CALLBACK */ - -/* USB Device endpoint direction */ -typedef enum -{ - OUT = 0x00, - IN = 0x80, -} USBD_EPDirectionTypeDef; - -typedef enum -{ - NETWORK_CONNECTION = 0x00, - RESPONSE_AVAILABLE = 0x01, - CONNECTION_SPEED_CHANGE = 0x2A -} USBD_CDC_NotifCodeTypeDef; -/** - * @} - */ - - - -/** @defgroup USBD_DEF_Exported_Macros - * @{ - */ -__STATIC_INLINE uint16_t SWAPBYTE(uint8_t *addr) -{ - uint16_t _SwapVal; - uint16_t _Byte1; - uint16_t _Byte2; - uint8_t *_pbuff = addr; - - _Byte1 = *(uint8_t *)_pbuff; - _pbuff++; - _Byte2 = *(uint8_t *)_pbuff; - - _SwapVal = (_Byte2 << 8) | _Byte1; - - return _SwapVal; -} - -#ifndef LOBYTE -#define LOBYTE(x) ((uint8_t)((x) & 0x00FFU)) -#endif /* LOBYTE */ - -#ifndef HIBYTE -#define HIBYTE(x) ((uint8_t)(((x) & 0xFF00U) >> 8U)) -#endif /* HIBYTE */ - -#ifndef MIN -#define MIN(a, b) (((a) < (b)) ? (a) : (b)) -#endif /* MIN */ - -#ifndef MAX -#define MAX(a, b) (((a) > (b)) ? (a) : (b)) -#endif /* MAX */ - -#if defined ( __GNUC__ ) -#ifndef __weak -#define __weak __attribute__((weak)) -#endif /* __weak */ -#ifndef __packed -#define __packed __attribute__((__packed__)) -#endif /* __packed */ -#endif /* __GNUC__ */ - - -/* In HS mode and when the DMA is used, all variables and data structures dealing - with the DMA during the transaction process should be 4-bytes aligned */ - -#if defined ( __GNUC__ ) && !defined (__CC_ARM) /* GNU Compiler */ -#ifndef __ALIGN_END -#define __ALIGN_END __attribute__ ((aligned (4U))) -#endif /* __ALIGN_END */ -#ifndef __ALIGN_BEGIN -#define __ALIGN_BEGIN -#endif /* __ALIGN_BEGIN */ -#else -#ifndef __ALIGN_END -#define __ALIGN_END -#endif /* __ALIGN_END */ -#ifndef __ALIGN_BEGIN -#if defined (__CC_ARM) /* ARM Compiler */ -#define __ALIGN_BEGIN __align(4U) -#elif defined (__ICCARM__) /* IAR Compiler */ -#define __ALIGN_BEGIN -#endif /* __CC_ARM */ -#endif /* __ALIGN_BEGIN */ -#endif /* __GNUC__ */ - - -/** - * @} - */ - -/** @defgroup USBD_DEF_Exported_Variables - * @{ - */ - -/** - * @} - */ - -/** @defgroup USBD_DEF_Exported_FunctionsPrototype - * @{ - */ - -/** - * @} - */ - -#ifdef __cplusplus -} -#endif - -#endif /* __USBD_DEF_H */ - -/** - * @} - */ - -/** - * @} - */ - diff --git a/legacy/stm32/lib/USB_CDC/include/usbd_desc.h b/legacy/stm32/lib/USB_CDC/include/usbd_desc.h deleted file mode 100644 index 71bd41d..0000000 --- a/legacy/stm32/lib/USB_CDC/include/usbd_desc.h +++ /dev/null @@ -1,5 +0,0 @@ -#ifndef USBD_DESC_H -#define USBD_DESC_H -#include "usbd_def.h" -extern USBD_DescriptorsTypeDef SaltyLab_Desc; -#endif diff --git a/legacy/stm32/lib/USB_CDC/include/usbd_ioreq.h b/legacy/stm32/lib/USB_CDC/include/usbd_ioreq.h deleted file mode 100644 index bad1e36..0000000 --- a/legacy/stm32/lib/USB_CDC/include/usbd_ioreq.h +++ /dev/null @@ -1,113 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_ioreq.h - * @author MCD Application Team - * @brief Header file for the usbd_ioreq.c file - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __USBD_IOREQ_H -#define __USBD_IOREQ_H - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_def.h" -#include "usbd_core.h" - -/** @addtogroup STM32_USB_DEVICE_LIBRARY - * @{ - */ - -/** @defgroup USBD_IOREQ - * @brief header file for the usbd_ioreq.c file - * @{ - */ - -/** @defgroup USBD_IOREQ_Exported_Defines - * @{ - */ -/** - * @} - */ - - -/** @defgroup USBD_IOREQ_Exported_Types - * @{ - */ - - -/** - * @} - */ - - - -/** @defgroup USBD_IOREQ_Exported_Macros - * @{ - */ - -/** - * @} - */ - -/** @defgroup USBD_IOREQ_Exported_Variables - * @{ - */ - -/** - * @} - */ - -/** @defgroup USBD_IOREQ_Exported_FunctionsPrototype - * @{ - */ - -USBD_StatusTypeDef USBD_CtlSendData(USBD_HandleTypeDef *pdev, - uint8_t *pbuf, uint32_t len); - -USBD_StatusTypeDef USBD_CtlContinueSendData(USBD_HandleTypeDef *pdev, - uint8_t *pbuf, uint32_t len); - -USBD_StatusTypeDef USBD_CtlPrepareRx(USBD_HandleTypeDef *pdev, - uint8_t *pbuf, uint32_t len); - -USBD_StatusTypeDef USBD_CtlContinueRx(USBD_HandleTypeDef *pdev, - uint8_t *pbuf, uint32_t len); - -USBD_StatusTypeDef USBD_CtlSendStatus(USBD_HandleTypeDef *pdev); -USBD_StatusTypeDef USBD_CtlReceiveStatus(USBD_HandleTypeDef *pdev); - -uint32_t USBD_GetRxCount(USBD_HandleTypeDef *pdev, uint8_t ep_addr); - -/** - * @} - */ - -#ifdef __cplusplus -} -#endif - -#endif /* __USBD_IOREQ_H */ - -/** - * @} - */ - -/** - * @} - */ - diff --git a/legacy/stm32/lib/USB_CDC/src/usbd_cdc.c b/legacy/stm32/lib/USB_CDC/src/usbd_cdc.c deleted file mode 100644 index eb7f105..0000000 --- a/legacy/stm32/lib/USB_CDC/src/usbd_cdc.c +++ /dev/null @@ -1,893 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_cdc.c - * @author MCD Application Team - * @brief This file provides the high layer firmware functions to manage the - * following functionalities of the USB CDC Class: - * - Initialization and Configuration of high and low layer - * - Enumeration as CDC Device (and enumeration for each implemented memory interface) - * - OUT/IN data transfer - * - Command IN transfer (class requests management) - * - Error management - * - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - * @verbatim - * - * =================================================================== - * CDC Class Driver Description - * =================================================================== - * This driver manages the "Universal Serial Bus Class Definitions for Communications Devices - * Revision 1.2 November 16, 2007" and the sub-protocol specification of "Universal Serial Bus - * Communications Class Subclass Specification for PSTN Devices Revision 1.2 February 9, 2007" - * This driver implements the following aspects of the specification: - * - Device descriptor management - * - Configuration descriptor management - * - Enumeration as CDC device with 2 data endpoints (IN and OUT) and 1 command endpoint (IN) - * - Requests management (as described in section 6.2 in specification) - * - Abstract Control Model compliant - * - Union Functional collection (using 1 IN endpoint for control) - * - Data interface class - * - * These aspects may be enriched or modified for a specific user application. - * - * This driver doesn't implement the following aspects of the specification - * (but it is possible to manage these features with some modifications on this driver): - * - Any class-specific aspect relative to communication classes should be managed by user application. - * - All communication classes other than PSTN are not managed - * - * @endverbatim - * - ****************************************************************************** - */ - -/* BSPDependencies -- "stm32xxxxx_{eval}{discovery}{nucleo_144}.c" -- "stm32xxxxx_{eval}{discovery}_io.c" -EndBSPDependencies */ - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_cdc.h" -#include "usbd_ctlreq.h" - - -/** @addtogroup STM32_USB_DEVICE_LIBRARY - * @{ - */ - - -/** @defgroup USBD_CDC - * @brief usbd core module - * @{ - */ - -/** @defgroup USBD_CDC_Private_TypesDefinitions - * @{ - */ -/** - * @} - */ - - -/** @defgroup USBD_CDC_Private_Defines - * @{ - */ -/** - * @} - */ - - -/** @defgroup USBD_CDC_Private_Macros - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_CDC_Private_FunctionPrototypes - * @{ - */ - -static uint8_t USBD_CDC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx); -static uint8_t USBD_CDC_DeInit(USBD_HandleTypeDef *pdev, uint8_t cfgidx); -static uint8_t USBD_CDC_Setup(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static uint8_t USBD_CDC_DataIn(USBD_HandleTypeDef *pdev, uint8_t epnum); -static uint8_t USBD_CDC_DataOut(USBD_HandleTypeDef *pdev, uint8_t epnum); -static uint8_t USBD_CDC_EP0_RxReady(USBD_HandleTypeDef *pdev); -#ifndef USE_USBD_COMPOSITE -static uint8_t *USBD_CDC_GetFSCfgDesc(uint16_t *length); -static uint8_t *USBD_CDC_GetHSCfgDesc(uint16_t *length); -static uint8_t *USBD_CDC_GetOtherSpeedCfgDesc(uint16_t *length); -uint8_t *USBD_CDC_GetDeviceQualifierDescriptor(uint16_t *length); -#endif /* USE_USBD_COMPOSITE */ - -#ifndef USE_USBD_COMPOSITE -/* USB Standard Device Descriptor */ -__ALIGN_BEGIN static uint8_t USBD_CDC_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALIGN_END = -{ - USB_LEN_DEV_QUALIFIER_DESC, - USB_DESC_TYPE_DEVICE_QUALIFIER, - 0x00, - 0x02, - 0x00, - 0x00, - 0x00, - 0x40, - 0x01, - 0x00, -}; -#endif /* USE_USBD_COMPOSITE */ -/** - * @} - */ - -/** @defgroup USBD_CDC_Private_Variables - * @{ - */ - - -/* CDC interface class callbacks structure */ -USBD_ClassTypeDef USBD_CDC = -{ - USBD_CDC_Init, - USBD_CDC_DeInit, - USBD_CDC_Setup, - NULL, /* EP0_TxSent */ - USBD_CDC_EP0_RxReady, - USBD_CDC_DataIn, - USBD_CDC_DataOut, - NULL, - NULL, - NULL, -#ifdef USE_USBD_COMPOSITE - NULL, - NULL, - NULL, - NULL, -#else - USBD_CDC_GetHSCfgDesc, - USBD_CDC_GetFSCfgDesc, - USBD_CDC_GetOtherSpeedCfgDesc, - USBD_CDC_GetDeviceQualifierDescriptor, -#endif /* USE_USBD_COMPOSITE */ -}; - -#ifndef USE_USBD_COMPOSITE -/* USB CDC device Configuration Descriptor */ -__ALIGN_BEGIN static uint8_t USBD_CDC_CfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIGN_END = -{ - /* Configuration Descriptor */ - 0x09, /* bLength: Configuration Descriptor size */ - USB_DESC_TYPE_CONFIGURATION, /* bDescriptorType: Configuration */ - USB_CDC_CONFIG_DESC_SIZ, /* wTotalLength */ - 0x00, - 0x02, /* bNumInterfaces: 2 interfaces */ - 0x01, /* bConfigurationValue: Configuration value */ - 0x00, /* iConfiguration: Index of string descriptor - describing the configuration */ -#if (USBD_SELF_POWERED == 1U) - 0xC0, /* bmAttributes: Bus Powered according to user configuration */ -#else - 0x80, /* bmAttributes: Bus Powered according to user configuration */ -#endif /* USBD_SELF_POWERED */ - USBD_MAX_POWER, /* MaxPower (mA) */ - - /*---------------------------------------------------------------------------*/ - - /* Interface Descriptor */ - 0x09, /* bLength: Interface Descriptor size */ - USB_DESC_TYPE_INTERFACE, /* bDescriptorType: Interface */ - /* Interface descriptor type */ - 0x00, /* bInterfaceNumber: Number of Interface */ - 0x00, /* bAlternateSetting: Alternate setting */ - 0x01, /* bNumEndpoints: One endpoint used */ - 0x02, /* bInterfaceClass: Communication Interface Class */ - 0x02, /* bInterfaceSubClass: Abstract Control Model */ - 0x01, /* bInterfaceProtocol: Common AT commands */ - 0x00, /* iInterface */ - - /* Header Functional Descriptor */ - 0x05, /* bLength: Endpoint Descriptor size */ - 0x24, /* bDescriptorType: CS_INTERFACE */ - 0x00, /* bDescriptorSubtype: Header Func Desc */ - 0x10, /* bcdCDC: spec release number */ - 0x01, - - /* Call Management Functional Descriptor */ - 0x05, /* bFunctionLength */ - 0x24, /* bDescriptorType: CS_INTERFACE */ - 0x01, /* bDescriptorSubtype: Call Management Func Desc */ - 0x00, /* bmCapabilities: D0+D1 */ - 0x01, /* bDataInterface */ - - /* ACM Functional Descriptor */ - 0x04, /* bFunctionLength */ - 0x24, /* bDescriptorType: CS_INTERFACE */ - 0x02, /* bDescriptorSubtype: Abstract Control Management desc */ - 0x02, /* bmCapabilities */ - - /* Union Functional Descriptor */ - 0x05, /* bFunctionLength */ - 0x24, /* bDescriptorType: CS_INTERFACE */ - 0x06, /* bDescriptorSubtype: Union func desc */ - 0x00, /* bMasterInterface: Communication class interface */ - 0x01, /* bSlaveInterface0: Data Class Interface */ - - /* Endpoint 2 Descriptor */ - 0x07, /* bLength: Endpoint Descriptor size */ - USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ - CDC_CMD_EP, /* bEndpointAddress */ - 0x03, /* bmAttributes: Interrupt */ - LOBYTE(CDC_CMD_PACKET_SIZE), /* wMaxPacketSize */ - HIBYTE(CDC_CMD_PACKET_SIZE), - CDC_FS_BINTERVAL, /* bInterval */ - /*---------------------------------------------------------------------------*/ - - /* Data class interface descriptor */ - 0x09, /* bLength: Endpoint Descriptor size */ - USB_DESC_TYPE_INTERFACE, /* bDescriptorType: */ - 0x01, /* bInterfaceNumber: Number of Interface */ - 0x00, /* bAlternateSetting: Alternate setting */ - 0x02, /* bNumEndpoints: Two endpoints used */ - 0x0A, /* bInterfaceClass: CDC */ - 0x00, /* bInterfaceSubClass */ - 0x00, /* bInterfaceProtocol */ - 0x00, /* iInterface */ - - /* Endpoint OUT Descriptor */ - 0x07, /* bLength: Endpoint Descriptor size */ - USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ - CDC_OUT_EP, /* bEndpointAddress */ - 0x02, /* bmAttributes: Bulk */ - LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize */ - HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), - 0x00, /* bInterval */ - - /* Endpoint IN Descriptor */ - 0x07, /* bLength: Endpoint Descriptor size */ - USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */ - CDC_IN_EP, /* bEndpointAddress */ - 0x02, /* bmAttributes: Bulk */ - LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize */ - HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), - 0x00 /* bInterval */ -}; -#endif /* USE_USBD_COMPOSITE */ - -static uint8_t CDCInEpAdd = CDC_IN_EP; -static uint8_t CDCOutEpAdd = CDC_OUT_EP; -static uint8_t CDCCmdEpAdd = CDC_CMD_EP; - -/** - * @} - */ - -/** @defgroup USBD_CDC_Private_Functions - * @{ - */ - -/** - * @brief USBD_CDC_Init - * Initialize the CDC interface - * @param pdev: device instance - * @param cfgidx: Configuration index - * @retval status - */ -static uint8_t USBD_CDC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx) -{ - UNUSED(cfgidx); - USBD_CDC_HandleTypeDef *hcdc; - - hcdc = (USBD_CDC_HandleTypeDef *)USBD_malloc(sizeof(USBD_CDC_HandleTypeDef)); - - if (hcdc == NULL) - { - pdev->pClassDataCmsit[pdev->classId] = NULL; - return (uint8_t)USBD_EMEM; - } - - (void)USBD_memset(hcdc, 0, sizeof(USBD_CDC_HandleTypeDef)); - - pdev->pClassDataCmsit[pdev->classId] = (void *)hcdc; - pdev->pClassData = pdev->pClassDataCmsit[pdev->classId]; - -#ifdef USE_USBD_COMPOSITE - /* Get the Endpoints addresses allocated for this class instance */ - CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); - CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); - CDCCmdEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_INTR, (uint8_t)pdev->classId); -#endif /* USE_USBD_COMPOSITE */ - - if (pdev->dev_speed == USBD_SPEED_HIGH) - { - /* Open EP IN */ - (void)USBD_LL_OpenEP(pdev, CDCInEpAdd, USBD_EP_TYPE_BULK, - CDC_DATA_HS_IN_PACKET_SIZE); - - pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 1U; - - /* Open EP OUT */ - (void)USBD_LL_OpenEP(pdev, CDCOutEpAdd, USBD_EP_TYPE_BULK, - CDC_DATA_HS_OUT_PACKET_SIZE); - - pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 1U; - - /* Set bInterval for CDC CMD Endpoint */ - pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = CDC_HS_BINTERVAL; - } - else - { - /* Open EP IN */ - (void)USBD_LL_OpenEP(pdev, CDCInEpAdd, USBD_EP_TYPE_BULK, - CDC_DATA_FS_IN_PACKET_SIZE); - - pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 1U; - - /* Open EP OUT */ - (void)USBD_LL_OpenEP(pdev, CDCOutEpAdd, USBD_EP_TYPE_BULK, - CDC_DATA_FS_OUT_PACKET_SIZE); - - pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 1U; - - /* Set bInterval for CMD Endpoint */ - pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = CDC_FS_BINTERVAL; - } - - /* Open Command IN EP */ - (void)USBD_LL_OpenEP(pdev, CDCCmdEpAdd, USBD_EP_TYPE_INTR, CDC_CMD_PACKET_SIZE); - pdev->ep_in[CDCCmdEpAdd & 0xFU].is_used = 1U; - - hcdc->RxBuffer = NULL; - - /* Init physical Interface components */ - ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Init(); - - /* Init Xfer states */ - hcdc->TxState = 0U; - hcdc->RxState = 0U; - - if (hcdc->RxBuffer == NULL) - { - return (uint8_t)USBD_EMEM; - } - - if (pdev->dev_speed == USBD_SPEED_HIGH) - { - /* Prepare Out endpoint to receive next packet */ - (void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer, - CDC_DATA_HS_OUT_PACKET_SIZE); - } - else - { - /* Prepare Out endpoint to receive next packet */ - (void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer, - CDC_DATA_FS_OUT_PACKET_SIZE); - } - - return (uint8_t)USBD_OK; -} - -/** - * @brief USBD_CDC_Init - * DeInitialize the CDC layer - * @param pdev: device instance - * @param cfgidx: Configuration index - * @retval status - */ -static uint8_t USBD_CDC_DeInit(USBD_HandleTypeDef *pdev, uint8_t cfgidx) -{ - UNUSED(cfgidx); - - -#ifdef USE_USBD_COMPOSITE - /* Get the Endpoints addresses allocated for this CDC class instance */ - CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); - CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); - CDCCmdEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_INTR, (uint8_t)pdev->classId); -#endif /* USE_USBD_COMPOSITE */ - - /* Close EP IN */ - (void)USBD_LL_CloseEP(pdev, CDCInEpAdd); - pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 0U; - - /* Close EP OUT */ - (void)USBD_LL_CloseEP(pdev, CDCOutEpAdd); - pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 0U; - - /* Close Command IN EP */ - (void)USBD_LL_CloseEP(pdev, CDCCmdEpAdd); - pdev->ep_in[CDCCmdEpAdd & 0xFU].is_used = 0U; - pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = 0U; - - /* DeInit physical Interface components */ - if (pdev->pClassDataCmsit[pdev->classId] != NULL) - { - ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->DeInit(); - (void)USBD_free(pdev->pClassDataCmsit[pdev->classId]); - pdev->pClassDataCmsit[pdev->classId] = NULL; - pdev->pClassData = NULL; - } - - return (uint8_t)USBD_OK; -} - -/** - * @brief USBD_CDC_Setup - * Handle the CDC specific requests - * @param pdev: instance - * @param req: usb requests - * @retval status - */ -static uint8_t USBD_CDC_Setup(USBD_HandleTypeDef *pdev, - USBD_SetupReqTypedef *req) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; - uint16_t len; - uint8_t ifalt = 0U; - uint16_t status_info = 0U; - USBD_StatusTypeDef ret = USBD_OK; - - if (hcdc == NULL) - { - return (uint8_t)USBD_FAIL; - } - - switch (req->bmRequest & USB_REQ_TYPE_MASK) - { - case USB_REQ_TYPE_CLASS: - if (req->wLength != 0U) - { - if ((req->bmRequest & 0x80U) != 0U) - { - ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(req->bRequest, - (uint8_t *)hcdc->data, - req->wLength); - - len = MIN(CDC_REQ_MAX_DATA_SIZE, req->wLength); - (void)USBD_CtlSendData(pdev, (uint8_t *)hcdc->data, len); - } - else - { - hcdc->CmdOpCode = req->bRequest; - hcdc->CmdLength = (uint8_t)MIN(req->wLength, USB_MAX_EP0_SIZE); - - (void)USBD_CtlPrepareRx(pdev, (uint8_t *)hcdc->data, hcdc->CmdLength); - } - } - else - { - ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(req->bRequest, - (uint8_t *)req, 0U); - } - break; - - case USB_REQ_TYPE_STANDARD: - switch (req->bRequest) - { - case USB_REQ_GET_STATUS: - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - (void)USBD_CtlSendData(pdev, (uint8_t *)&status_info, 2U); - } - else - { - USBD_CtlError(pdev, req); - ret = USBD_FAIL; - } - break; - - case USB_REQ_GET_INTERFACE: - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - (void)USBD_CtlSendData(pdev, &ifalt, 1U); - } - else - { - USBD_CtlError(pdev, req); - ret = USBD_FAIL; - } - break; - - case USB_REQ_SET_INTERFACE: - if (pdev->dev_state != USBD_STATE_CONFIGURED) - { - USBD_CtlError(pdev, req); - ret = USBD_FAIL; - } - break; - - case USB_REQ_CLEAR_FEATURE: - break; - - default: - USBD_CtlError(pdev, req); - ret = USBD_FAIL; - break; - } - break; - - default: - USBD_CtlError(pdev, req); - ret = USBD_FAIL; - break; - } - - return (uint8_t)ret; -} - -/** - * @brief USBD_CDC_DataIn - * Data sent on non-control IN endpoint - * @param pdev: device instance - * @param epnum: endpoint number - * @retval status - */ -static uint8_t USBD_CDC_DataIn(USBD_HandleTypeDef *pdev, uint8_t epnum) -{ - USBD_CDC_HandleTypeDef *hcdc; - PCD_HandleTypeDef *hpcd = (PCD_HandleTypeDef *)pdev->pData; - - if (pdev->pClassDataCmsit[pdev->classId] == NULL) - { - return (uint8_t)USBD_FAIL; - } - - hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; - - if ((pdev->ep_in[epnum & 0xFU].total_length > 0U) && - ((pdev->ep_in[epnum & 0xFU].total_length % hpcd->IN_ep[epnum & 0xFU].maxpacket) == 0U)) - { - /* Update the packet total length */ - pdev->ep_in[epnum & 0xFU].total_length = 0U; - - /* Send ZLP */ - (void)USBD_LL_Transmit(pdev, epnum, NULL, 0U); - } - else - { - hcdc->TxState = 0U; - - if (((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->TransmitCplt != NULL) - { - ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->TransmitCplt(hcdc->TxBuffer, &hcdc->TxLength, epnum); - } - } - - return (uint8_t)USBD_OK; -} - -/** - * @brief USBD_CDC_DataOut - * Data received on non-control Out endpoint - * @param pdev: device instance - * @param epnum: endpoint number - * @retval status - */ -static uint8_t USBD_CDC_DataOut(USBD_HandleTypeDef *pdev, uint8_t epnum) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; - - if (pdev->pClassDataCmsit[pdev->classId] == NULL) - { - return (uint8_t)USBD_FAIL; - } - - /* Get the received data length */ - hcdc->RxLength = USBD_LL_GetRxDataSize(pdev, epnum); - - /* USB data will be immediately processed, this allow next USB traffic being - NAKed till the end of the application Xfer */ - - ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Receive(hcdc->RxBuffer, &hcdc->RxLength); - - return (uint8_t)USBD_OK; -} - -/** - * @brief USBD_CDC_EP0_RxReady - * Handle EP0 Rx Ready event - * @param pdev: device instance - * @retval status - */ -static uint8_t USBD_CDC_EP0_RxReady(USBD_HandleTypeDef *pdev) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; - - if (hcdc == NULL) - { - return (uint8_t)USBD_FAIL; - } - - if ((pdev->pUserData[pdev->classId] != NULL) && (hcdc->CmdOpCode != 0xFFU)) - { - ((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(hcdc->CmdOpCode, - (uint8_t *)hcdc->data, - (uint16_t)hcdc->CmdLength); - hcdc->CmdOpCode = 0xFFU; - } - - return (uint8_t)USBD_OK; -} -#ifndef USE_USBD_COMPOSITE -/** - * @brief USBD_CDC_GetFSCfgDesc - * Return configuration descriptor - * @param length : pointer data length - * @retval pointer to descriptor buffer - */ -static uint8_t *USBD_CDC_GetFSCfgDesc(uint16_t *length) -{ - USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP); - USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP); - USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP); - - if (pEpCmdDesc != NULL) - { - pEpCmdDesc->bInterval = CDC_FS_BINTERVAL; - } - - if (pEpOutDesc != NULL) - { - pEpOutDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE; - } - - if (pEpInDesc != NULL) - { - pEpInDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE; - } - - *length = (uint16_t)sizeof(USBD_CDC_CfgDesc); - return USBD_CDC_CfgDesc; -} - -/** - * @brief USBD_CDC_GetHSCfgDesc - * Return configuration descriptor - * @param length : pointer data length - * @retval pointer to descriptor buffer - */ -static uint8_t *USBD_CDC_GetHSCfgDesc(uint16_t *length) -{ - USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP); - USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP); - USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP); - - if (pEpCmdDesc != NULL) - { - pEpCmdDesc->bInterval = CDC_HS_BINTERVAL; - } - - if (pEpOutDesc != NULL) - { - pEpOutDesc->wMaxPacketSize = CDC_DATA_HS_MAX_PACKET_SIZE; - } - - if (pEpInDesc != NULL) - { - pEpInDesc->wMaxPacketSize = CDC_DATA_HS_MAX_PACKET_SIZE; - } - - *length = (uint16_t)sizeof(USBD_CDC_CfgDesc); - return USBD_CDC_CfgDesc; -} - -/** - * @brief USBD_CDC_GetOtherSpeedCfgDesc - * Return configuration descriptor - * @param length : pointer data length - * @retval pointer to descriptor buffer - */ -static uint8_t *USBD_CDC_GetOtherSpeedCfgDesc(uint16_t *length) -{ - USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP); - USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP); - USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP); - - if (pEpCmdDesc != NULL) - { - pEpCmdDesc->bInterval = CDC_FS_BINTERVAL; - } - - if (pEpOutDesc != NULL) - { - pEpOutDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE; - } - - if (pEpInDesc != NULL) - { - pEpInDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE; - } - - *length = (uint16_t)sizeof(USBD_CDC_CfgDesc); - return USBD_CDC_CfgDesc; -} - -/** - * @brief USBD_CDC_GetDeviceQualifierDescriptor - * return Device Qualifier descriptor - * @param length : pointer data length - * @retval pointer to descriptor buffer - */ -uint8_t *USBD_CDC_GetDeviceQualifierDescriptor(uint16_t *length) -{ - *length = (uint16_t)sizeof(USBD_CDC_DeviceQualifierDesc); - - return USBD_CDC_DeviceQualifierDesc; -} -#endif /* USE_USBD_COMPOSITE */ -/** - * @brief USBD_CDC_RegisterInterface - * @param pdev: device instance - * @param fops: CD Interface callback - * @retval status - */ -uint8_t USBD_CDC_RegisterInterface(USBD_HandleTypeDef *pdev, - USBD_CDC_ItfTypeDef *fops) -{ - if (fops == NULL) - { - return (uint8_t)USBD_FAIL; - } - - pdev->pUserData[pdev->classId] = fops; - - return (uint8_t)USBD_OK; -} - - -/** - * @brief USBD_CDC_SetTxBuffer - * @param pdev: device instance - * @param pbuff: Tx Buffer - * @param length: length of data to be sent - * @param ClassId: The Class ID - * @retval status - */ -#ifdef USE_USBD_COMPOSITE -uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, - uint8_t *pbuff, uint32_t length, uint8_t ClassId) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[ClassId]; -#else -uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, - uint8_t *pbuff, uint32_t length) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; -#endif /* USE_USBD_COMPOSITE */ - - if (hcdc == NULL) - { - return (uint8_t)USBD_FAIL; - } - - hcdc->TxBuffer = pbuff; - hcdc->TxLength = length; - - return (uint8_t)USBD_OK; -} - -/** - * @brief USBD_CDC_SetRxBuffer - * @param pdev: device instance - * @param pbuff: Rx Buffer - * @retval status - */ -uint8_t USBD_CDC_SetRxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; - - if (hcdc == NULL) - { - return (uint8_t)USBD_FAIL; - } - - hcdc->RxBuffer = pbuff; - - return (uint8_t)USBD_OK; -} - - -/** - * @brief USBD_CDC_TransmitPacket - * Transmit packet on IN endpoint - * @param pdev: device instance - * @param ClassId: The Class ID - * @retval status - */ -#ifdef USE_USBD_COMPOSITE -uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t ClassId) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[ClassId]; -#else -uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; -#endif /* USE_USBD_COMPOSITE */ - - USBD_StatusTypeDef ret = USBD_BUSY; - -#ifdef USE_USBD_COMPOSITE - /* Get the Endpoints addresses allocated for this class instance */ - CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, ClassId); -#endif /* USE_USBD_COMPOSITE */ - - if (hcdc == NULL) - { - return (uint8_t)USBD_FAIL; - } - - if (hcdc->TxState == 0U) - { - /* Tx Transfer in progress */ - hcdc->TxState = 1U; - - /* Update the packet total length */ - pdev->ep_in[CDCInEpAdd & 0xFU].total_length = hcdc->TxLength; - - /* Transmit next packet */ - (void)USBD_LL_Transmit(pdev, CDCInEpAdd, hcdc->TxBuffer, hcdc->TxLength); - - ret = USBD_OK; - } - - return (uint8_t)ret; -} - -/** - * @brief USBD_CDC_ReceivePacket - * prepare OUT Endpoint for reception - * @param pdev: device instance - * @retval status - */ -uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev) -{ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId]; - -#ifdef USE_USBD_COMPOSITE - /* Get the Endpoints addresses allocated for this class instance */ - CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId); -#endif /* USE_USBD_COMPOSITE */ - - if (pdev->pClassDataCmsit[pdev->classId] == NULL) - { - return (uint8_t)USBD_FAIL; - } - - if (pdev->dev_speed == USBD_SPEED_HIGH) - { - /* Prepare Out endpoint to receive next packet */ - (void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer, - CDC_DATA_HS_OUT_PACKET_SIZE); - } - else - { - /* Prepare Out endpoint to receive next packet */ - (void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer, - CDC_DATA_FS_OUT_PACKET_SIZE); - } - - return (uint8_t)USBD_OK; -} -/** - * @} - */ - -/** - * @} - */ - -/** - * @} - */ - diff --git a/legacy/stm32/lib/USB_CDC/src/usbd_cdc_if.c b/legacy/stm32/lib/USB_CDC/src/usbd_cdc_if.c deleted file mode 100644 index 7307ae9..0000000 --- a/legacy/stm32/lib/USB_CDC/src/usbd_cdc_if.c +++ /dev/null @@ -1,211 +0,0 @@ -#include "usbd_cdc_if.h" -#include "stm32f7xx_hal.h" -#include "ota.h" - -extern USBD_HandleTypeDef hUsbDevice; -volatile uint8_t cdc_streaming = 1; /* auto-stream */ -static volatile uint8_t cdc_port_open = 0; /* set when host asserts DTR */ -volatile uint8_t cdc_arm_request = 0; /* set by A command */ -volatile uint8_t cdc_disarm_request = 0; /* set by D command */ -volatile uint8_t cdc_recal_request = 0; /* set by G command — gyro recalibration */ -volatile uint8_t cdc_imu_cal_request = 0; /* set by O command — mount offset calibration (Issue #680) */ -volatile uint32_t cdc_rx_count = 0; /* total CDC packets received from host */ - -volatile uint8_t cdc_estop_request = 0; -volatile uint8_t cdc_estop_clear_request = 0; - -/* - * PID tuning command buffer. - * CDC_Receive (USB IRQ) copies multi-char commands here. - * Main loop polls cdc_cmd_ready, parses, and clears. - * Commands: P I D T M ? - */ -volatile uint8_t cdc_cmd_ready = 0; -volatile char cdc_cmd_buf[32]; - -/* - * Jetson command buffer (bidirectional protocol). - * 'H'\n — heartbeat, ISR updates jetson_hb_tick only (no buf copy needed). - * 'C',\n — drive command: ISR copies to buf, main loop parses with sscanf. - * jetson_hb_tick is also refreshed on every C command. - */ -volatile uint8_t jetson_cmd_ready = 0; -volatile char jetson_cmd_buf[32]; -volatile uint32_t jetson_hb_tick = 0; /* HAL_GetTick() of last H or C */ - -/* - * USB TX/RX buffers grouped into a single 512-byte aligned struct so that - * one MPU region (configured in usbd_conf.c) can mark them non-cacheable. - * Size must be a power-of-2 >= total size for MPU RASR SIZE encoding. - */ -static struct { - uint8_t tx[256]; - uint8_t rx[256]; -} __attribute__((aligned(512))) usb_nc_buf; - -#define UserTxBuffer usb_nc_buf.tx -#define UserRxBuffer usb_nc_buf.rx - -/* Exported so usbd_conf.c USB_NC_MPU_Config() can set the region base */ -void * const usb_nc_buf_base = &usb_nc_buf; - -/* - * Betaflight-proven DFU reboot: - * 1. Write magic to RTC backup register (persists across soft reset) - * 2. NVIC_SystemReset() — clean hardware reset - * 3. Early startup checks magic, clears it, jumps to system bootloader - * - * Magic is written to BKP15R (OTA_DFU_BKP_IDX) — not BKP0R — so that - * BKP0R–BKP6R are available for BNO055 calibration offsets (PR #150). - * The magic check in checkForBootloader() reads the same BKP15R register. - */ -static void request_bootloader(void) { - /* Betaflight-proven: write magic, disable IRQs, reset. - * checkForBootloader() runs on next boot before anything else. */ - __HAL_RCC_PWR_CLK_ENABLE(); - HAL_PWR_EnableBkUpAccess(); - __HAL_RCC_RTC_ENABLE(); - - /* Write magic to BKP15R via OTA module constants (avoids BNO055 BKP0–6) */ - (&RTC->BKP0R)[OTA_DFU_BKP_IDX] = OTA_DFU_MAGIC; - - __disable_irq(); - NVIC_SystemReset(); -} - -/* - * Call this VERY early in main(), before HAL_Init(). - * Checks RTC backup register for magic value left by request_bootloader(). - * If found: clear magic, jump to STM32F7 system bootloader at 0x1FF00000. - */ -void checkForBootloader(void) { - /* - * Betaflight-proven bootloader jump for STM32F7. - * Called VERY early, before HAL_Init/caches/clocks. - * At this point only RCC PWR is needed to read RTC backup regs. - */ - - /* Enable backup domain access to read RTC backup register */ - __HAL_RCC_PWR_CLK_ENABLE(); - HAL_PWR_EnableBkUpAccess(); - __HAL_RCC_RTC_ENABLE(); - - uint32_t magic = (&RTC->BKP0R)[OTA_DFU_BKP_IDX]; /* read BKP15R */ - - if (magic != OTA_DFU_MAGIC) { - return; /* Normal boot */ - } - - /* Clear magic so next boot is normal */ - (&RTC->BKP0R)[OTA_DFU_BKP_IDX] = 0; - - /* Jump to STM32F7 system bootloader at 0x1FF00000. - * Exactly as Betaflight does it — no cache/VTOR/MEMRMP games needed - * because we run before any of that is configured. */ - - __HAL_RCC_SYSCFG_CLK_ENABLE(); - - __set_MSP(*(uint32_t *)0x1FF00000); - ((void (*)(void))(*(uint32_t *)0x1FF00004))(); - - while (1); -} - -static int8_t CDC_Init(void) { - USBD_CDC_SetTxBuffer(&hUsbDevice, UserTxBuffer, 0); - USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer); - USBD_CDC_ReceivePacket(&hUsbDevice); - - /* Reset TxState so CDC_Transmit works after host (re)connects. - * Without this, if transmits happen before host opens port, - * TxState stays BUSY forever since host never ACKs. */ - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)hUsbDevice.pClassData; - if (hcdc) hcdc->TxState = 0; - - return USBD_OK; -} - -static int8_t CDC_DeInit(void) { return USBD_OK; } - -static int8_t CDC_Control(uint8_t cmd, uint8_t *pbuf, uint16_t length) { - (void)pbuf; (void)length; - if (cmd == 0x22) { /* CDC_SET_CONTROL_LINE_STATE — host opened port */ - cdc_port_open = 1; - cdc_streaming = 1; - } - return USBD_OK; -} - -static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) { - if (*len < 1) goto done; - - switch (buf[0]) { - case 'S': cdc_streaming = !cdc_streaming; break; - case 'A': cdc_arm_request = 1; break; - case 'D': cdc_disarm_request = 1; break; - case 'G': cdc_recal_request = 1; break; /* gyro recalibration */ - case 'O': cdc_imu_cal_request = 1; break; /* mount offset cal (Issue #680) */ - case 'R': request_bootloader(); break; /* never returns */ - - case 'E': cdc_estop_request = 1; break; - case 'F': cdc_estop_request = 2; break; - case 'Z': cdc_estop_clear_request = 1; break; - - /* - * PID tuning: P I D T M ? - * Copy to cmd buffer; main loop parses float (avoids sscanf in IRQ). - */ - case 'P': case 'I': case 'K': case 'T': case 'M': case '?': { - uint32_t copy_len = *len < 31 ? *len : 31; - for (uint32_t i = 0; i < copy_len; i++) cdc_cmd_buf[i] = (char)buf[i]; - cdc_cmd_buf[copy_len] = '\0'; - cdc_cmd_ready = 1; - break; - } - - /* Jetson heartbeat — just refresh the tick, no buffer copy needed */ - case 'H': - jetson_hb_tick = HAL_GetTick(); - break; - - /* Jetson drive command: C,\n - * Copy to buffer; main loop parses ints (keeps sscanf out of ISR). */ - case 'C': { - uint32_t copy_len = *len < 31 ? *len : 31; - for (uint32_t i = 0; i < copy_len; i++) jetson_cmd_buf[i] = (char)buf[i]; - jetson_cmd_buf[copy_len] = '\0'; - jetson_hb_tick = HAL_GetTick(); /* C command also refreshes heartbeat */ - jetson_cmd_ready = 1; - break; - } - - default: break; - } - -done: - cdc_rx_count++; - USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer); - USBD_CDC_ReceivePacket(&hUsbDevice); - return USBD_OK; -} - -USBD_CDC_ItfTypeDef USBD_CDC_fops = { CDC_Init, CDC_DeInit, CDC_Control, CDC_Receive }; - -uint8_t CDC_Transmit(uint8_t *buf, uint16_t len) { - USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)hUsbDevice.pClassData; - if (hcdc == NULL) return USBD_FAIL; - if (hcdc->TxState != 0) { - /* If stuck busy (no host ACK), force reset after a while */ - static uint32_t busy_count = 0; - if (++busy_count > 100) { hcdc->TxState = 0; busy_count = 0; } - return USBD_BUSY; - } - /* Always copy into the static UserTxBuffer so the USB hardware reads - * from a known fixed SRAM address — never from the caller's stack. - * The USB TXFE IRQ fires asynchronously; a stack buffer could be - * overwritten by the time the FIFO is loaded. */ - if (len > sizeof(UserTxBuffer)) len = sizeof(UserTxBuffer); - memcpy(UserTxBuffer, buf, len); - USBD_CDC_SetTxBuffer(&hUsbDevice, UserTxBuffer, len); - return USBD_CDC_TransmitPacket(&hUsbDevice); -} diff --git a/legacy/stm32/lib/USB_CDC/src/usbd_conf.c b/legacy/stm32/lib/USB_CDC/src/usbd_conf.c deleted file mode 100644 index 78ace0a..0000000 --- a/legacy/stm32/lib/USB_CDC/src/usbd_conf.c +++ /dev/null @@ -1,125 +0,0 @@ -/* Taken directly from Betaflight: usbd_conf_stm32f7xx.c */ -#include "stm32f7xx_hal.h" -#include "usbd_core.h" -#include "usbd_desc.h" -#include "usbd_cdc.h" -#include "usbd_conf.h" - -/* - * Mark USB TX/RX buffers non-cacheable via MPU Region 0. - * Cortex-M7 TEX=1, C=0, B=0 → Normal Non-cacheable. - * Called before HAL_PCD_Init() so the region is active before the USB - * hardware ever touches the buffers. - */ -extern void * const usb_nc_buf_base; /* defined in usbd_cdc_if.c */ - -static void USB_NC_MPU_Config(void) -{ - MPU_Region_InitTypeDef r = {0}; - HAL_MPU_Disable(); - r.Enable = MPU_REGION_ENABLE; - r.Number = MPU_REGION_NUMBER0; - r.BaseAddress = (uint32_t)usb_nc_buf_base; - r.Size = MPU_REGION_SIZE_512B; - r.SubRegionDisable = 0x00; - r.TypeExtField = MPU_TEX_LEVEL1; /* TEX=1 */ - r.AccessPermission = MPU_REGION_FULL_ACCESS; - r.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE; - r.IsShareable = MPU_ACCESS_NOT_SHAREABLE; - r.IsCacheable = MPU_ACCESS_NOT_CACHEABLE; /* C=0 */ - r.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE; /* B=0 */ - HAL_MPU_ConfigRegion(&r); - HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT); -} - -PCD_HandleTypeDef hpcd; - -void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd) -{ - GPIO_InitTypeDef GPIO_InitStruct; - - if (hpcd->Instance == USB_OTG_FS) { - __HAL_RCC_GPIOA_CLK_ENABLE(); - - GPIO_InitStruct.Pin = (GPIO_PIN_11 | GPIO_PIN_12); - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS; - HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - __HAL_RCC_USB_OTG_FS_CLK_ENABLE(); - - HAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0); - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); - } -} - -void HAL_PCD_MspDeInit(PCD_HandleTypeDef *hpcd) -{ - if (hpcd->Instance == USB_OTG_FS) { - __HAL_RCC_USB_OTG_FS_CLK_DISABLE(); - __HAL_RCC_SYSCFG_CLK_DISABLE(); - } -} - -void HAL_PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_SetupStage(hpcd->pData, (uint8_t *)hpcd->Setup); } -void HAL_PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_DataOutStage(hpcd->pData, epnum, hpcd->OUT_ep[epnum].xfer_buff); } -void HAL_PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_DataInStage(hpcd->pData, epnum, hpcd->IN_ep[epnum].xfer_buff); } -void HAL_PCD_SOFCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_SOF(hpcd->pData); } -void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd) { - USBD_LL_Reset(hpcd->pData); - USBD_LL_SetSpeed(hpcd->pData, USBD_SPEED_FULL); -} -void HAL_PCD_SuspendCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_Suspend(hpcd->pData); } -void HAL_PCD_ResumeCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_Resume(hpcd->pData); } -void HAL_PCD_ISOOUTIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_IsoOUTIncomplete(hpcd->pData, epnum); } -void HAL_PCD_ISOINIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_IsoINIncomplete(hpcd->pData, epnum); } -void HAL_PCD_ConnectCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_DevConnected(hpcd->pData); } -void HAL_PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_DevDisconnected(hpcd->pData); } - -USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev) -{ - hpcd.Instance = USB_OTG_FS; - hpcd.Init.dev_endpoints = 4; - hpcd.Init.use_dedicated_ep1 = 0; - hpcd.Init.ep0_mps = 0x40; - hpcd.Init.dma_enable = 0; - hpcd.Init.low_power_enable = 0; - hpcd.Init.phy_itface = PCD_PHY_EMBEDDED; - hpcd.Init.Sof_enable = 0; - hpcd.Init.speed = PCD_SPEED_FULL; - hpcd.Init.vbus_sensing_enable = 0; - hpcd.Init.lpm_enable = 0; - - hpcd.pData = pdev; - pdev->pData = &hpcd; - - USB_NC_MPU_Config(); /* Mark USB buffers non-cacheable before USB hardware init */ - HAL_PCD_Init(&hpcd); - - HAL_PCDEx_SetRxFiFo(&hpcd, 0x80); - HAL_PCDEx_SetTxFiFo(&hpcd, 0, 0x40); - HAL_PCDEx_SetTxFiFo(&hpcd, 1, 0x80); - - return USBD_OK; -} - -USBD_StatusTypeDef USBD_LL_DeInit(USBD_HandleTypeDef *pdev) { HAL_PCD_DeInit(pdev->pData); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_Start(USBD_HandleTypeDef *pdev) { HAL_PCD_Start(pdev->pData); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_Stop(USBD_HandleTypeDef *pdev) { HAL_PCD_Stop(pdev->pData); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_OpenEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t ep_type, uint16_t ep_mps) { HAL_PCD_EP_Open(pdev->pData, ep_addr, ep_mps, ep_type); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_CloseEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_Close(pdev->pData, ep_addr); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_FlushEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_Flush(pdev->pData, ep_addr); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_StallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_SetStall(pdev->pData, ep_addr); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_ClearStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_ClrStall(pdev->pData, ep_addr); return USBD_OK; } -uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { - PCD_HandleTypeDef *hpcd = pdev->pData; - if ((ep_addr & 0x80) == 0x80) return hpcd->IN_ep[ep_addr & 0x7F].is_stall; - else return hpcd->OUT_ep[ep_addr & 0x7F].is_stall; -} -USBD_StatusTypeDef USBD_LL_SetUSBAddress(USBD_HandleTypeDef *pdev, uint8_t dev_addr) { HAL_PCD_SetAddress(pdev->pData, dev_addr); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_Transmit(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t *pbuf, uint32_t size) { HAL_PCD_EP_Transmit(pdev->pData, ep_addr, pbuf, size); return USBD_OK; } -USBD_StatusTypeDef USBD_LL_PrepareReceive(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t *pbuf, uint32_t size) { HAL_PCD_EP_Receive(pdev->pData, ep_addr, pbuf, size); return USBD_OK; } -uint32_t USBD_LL_GetRxDataSize(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { return HAL_PCD_EP_GetRxCount(pdev->pData, ep_addr); } -void USBD_LL_Delay(uint32_t Delay) { HAL_Delay(Delay); } diff --git a/legacy/stm32/lib/USB_CDC/src/usbd_core.c b/legacy/stm32/lib/USB_CDC/src/usbd_core.c deleted file mode 100644 index ad4dd12..0000000 --- a/legacy/stm32/lib/USB_CDC/src/usbd_core.c +++ /dev/null @@ -1,1215 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_core.c - * @author MCD Application Team - * @brief This file provides all the USBD core functions. - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_core.h" - -#ifdef USE_USBD_COMPOSITE -#include "usbd_composite_builder.h" -#endif /* USE_USBD_COMPOSITE */ - -/** @addtogroup STM32_USBD_DEVICE_LIBRARY - * @{ - */ - - -/** @defgroup USBD_CORE - * @brief usbd core module - * @{ - */ - -/** @defgroup USBD_CORE_Private_TypesDefinitions - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_CORE_Private_Defines - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_CORE_Private_Macros - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_CORE_Private_FunctionPrototypes - * @{ - */ - -/** - * @} - */ - -/** @defgroup USBD_CORE_Private_Variables - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_CORE_Private_Functions - * @{ - */ - -/** - * @brief USBD_Init - * Initialize the device stack and load the class driver - * @param pdev: device instance - * @param pdesc: Descriptor structure address - * @param id: Low level core index - * @retval status: USBD Status - */ -USBD_StatusTypeDef USBD_Init(USBD_HandleTypeDef *pdev, - USBD_DescriptorsTypeDef *pdesc, uint8_t id) -{ - USBD_StatusTypeDef ret; - - /* Check whether the USB Host handle is valid */ - if (pdev == NULL) - { -#if (USBD_DEBUG_LEVEL > 1U) - USBD_ErrLog("Invalid Device handle"); -#endif /* (USBD_DEBUG_LEVEL > 1U) */ - return USBD_FAIL; - } - -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Unlink previous class*/ - pdev->pClass[i] = NULL; - pdev->pUserData[i] = NULL; - - /* Set class as inactive */ - pdev->tclasslist[i].Active = 0; - pdev->NumClasses = 0; - pdev->classId = 0; - } -#else - /* Unlink previous class*/ - pdev->pClass[0] = NULL; - pdev->pUserData[0] = NULL; -#endif /* USE_USBD_COMPOSITE */ - - pdev->pConfDesc = NULL; - - /* Assign USBD Descriptors */ - if (pdesc != NULL) - { - pdev->pDesc = pdesc; - } - - /* Set Device initial State */ - pdev->dev_state = USBD_STATE_DEFAULT; - pdev->id = id; - - /* Initialize low level driver */ - ret = USBD_LL_Init(pdev); - - return ret; -} - -/** - * @brief USBD_DeInit - * De-Initialize the device library - * @param pdev: device instance - * @retval status: USBD Status - */ -USBD_StatusTypeDef USBD_DeInit(USBD_HandleTypeDef *pdev) -{ - USBD_StatusTypeDef ret; - - /* Disconnect the USB Device */ - (void)USBD_LL_Stop(pdev); - - /* Set Default State */ - pdev->dev_state = USBD_STATE_DEFAULT; - -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - if (pdev->pClass[i] != NULL) - { - pdev->classId = i; - /* Free Class Resources */ - pdev->pClass[i]->DeInit(pdev, (uint8_t)pdev->dev_config); - } - } - } -#else - /* Free Class Resources */ - if (pdev->pClass[0] != NULL) - { - pdev->pClass[0]->DeInit(pdev, (uint8_t)pdev->dev_config); - } - - pdev->pUserData[0] = NULL; - -#endif /* USE_USBD_COMPOSITE */ - - /* Free Device descriptors resources */ - pdev->pDesc = NULL; - pdev->pConfDesc = NULL; - - /* DeInitialize low level driver */ - ret = USBD_LL_DeInit(pdev); - - return ret; -} - -/** - * @brief USBD_RegisterClass - * Link class driver to Device Core. - * @param pdev: Device Handle - * @param pclass: Class handle - * @retval USBD Status - */ -USBD_StatusTypeDef USBD_RegisterClass(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass) -{ - uint16_t len = 0U; - - if (pclass == NULL) - { -#if (USBD_DEBUG_LEVEL > 1U) - USBD_ErrLog("Invalid Class handle"); -#endif /* (USBD_DEBUG_LEVEL > 1U) */ - return USBD_FAIL; - } - - /* link the class to the USB Device handle */ - pdev->pClass[0] = pclass; - - /* Get Device Configuration Descriptor */ -#ifdef USE_USB_HS - if (pdev->pClass[pdev->classId]->GetHSConfigDescriptor != NULL) - { - pdev->pConfDesc = (void *)pdev->pClass[pdev->classId]->GetHSConfigDescriptor(&len); - } -#else /* Default USE_USB_FS */ - if (pdev->pClass[pdev->classId]->GetFSConfigDescriptor != NULL) - { - pdev->pConfDesc = (void *)pdev->pClass[pdev->classId]->GetFSConfigDescriptor(&len); - } -#endif /* USE_USB_FS */ - - /* Increment the NumClasses */ - pdev->NumClasses++; - - return USBD_OK; -} - -#ifdef USE_USBD_COMPOSITE -/** - * @brief USBD_RegisterClassComposite - * Link class driver to Device Core. - * @param pdev : Device Handle - * @param pclass: Class handle - * @param classtype: Class type - * @param EpAddr: Endpoint Address handle - * @retval USBD Status - */ -USBD_StatusTypeDef USBD_RegisterClassComposite(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass, - USBD_CompositeClassTypeDef classtype, uint8_t *EpAddr) -{ - USBD_StatusTypeDef ret = USBD_OK; - uint16_t len = 0U; - - if ((pdev->classId < USBD_MAX_SUPPORTED_CLASS) && (pdev->NumClasses < USBD_MAX_SUPPORTED_CLASS)) - { - if ((uint32_t)pclass != 0U) - { - /* Link the class to the USB Device handle */ - pdev->pClass[pdev->classId] = pclass; - ret = USBD_OK; - - pdev->tclasslist[pdev->classId].EpAdd = EpAddr; - - /* Call the composite class builder */ - (void)USBD_CMPSIT_AddClass(pdev, pclass, classtype, 0); - - /* Increment the ClassId for the next occurrence */ - pdev->classId ++; - pdev->NumClasses ++; - } - else - { -#if (USBD_DEBUG_LEVEL > 1U) - USBD_ErrLog("Invalid Class handle"); -#endif /* (USBD_DEBUG_LEVEL > 1U) */ - ret = USBD_FAIL; - } - } - - if (ret == USBD_OK) - { - /* Get Device Configuration Descriptor */ -#ifdef USE_USB_HS - pdev->pConfDesc = USBD_CMPSIT.GetHSConfigDescriptor(&len); -#else /* Default USE_USB_FS */ - pdev->pConfDesc = USBD_CMPSIT.GetFSConfigDescriptor(&len); -#endif /* USE_USB_FS */ - } - - return ret; -} - -/** - * @brief USBD_UnRegisterClassComposite - * UnLink all composite class drivers from Device Core. - * @param pdev: Device Handle - * @retval USBD Status - */ -USBD_StatusTypeDef USBD_UnRegisterClassComposite(USBD_HandleTypeDef *pdev) -{ - USBD_StatusTypeDef ret = USBD_FAIL; - uint8_t idx1; - uint8_t idx2; - - /* Unroll all activated classes */ - for (idx1 = 0; idx1 < pdev->NumClasses; idx1++) - { - /* Check if the class correspond to the requested type and if it is active */ - if (pdev->tclasslist[idx1].Active == 1U) - { - /* Set the new class ID */ - pdev->classId = idx1; - - /* Free resources used by the selected class */ - if (pdev->pClass[pdev->classId] != NULL) - { - /* Free Class Resources */ - if (pdev->pClass[pdev->classId]->DeInit(pdev, (uint8_t)pdev->dev_config) != 0U) - { -#if (USBD_DEBUG_LEVEL > 1U) - USBD_ErrLog("Class DeInit didn't succeed!, can't unregister selected class"); -#endif /* (USBD_DEBUG_LEVEL > 1U) */ - - ret = USBD_FAIL; - } - } - - /* Free the class pointer */ - pdev->pClass[pdev->classId] = NULL; - - /* Free the class location in classes table and reset its parameters to zero */ - pdev->tclasslist[pdev->classId].ClassType = CLASS_TYPE_NONE; - pdev->tclasslist[pdev->classId].ClassId = 0U; - pdev->tclasslist[pdev->classId].Active = 0U; - pdev->tclasslist[pdev->classId].NumEps = 0U; - pdev->tclasslist[pdev->classId].NumIf = 0U; - pdev->tclasslist[pdev->classId].CurrPcktSze = 0U; - - for (idx2 = 0U; idx2 < USBD_MAX_CLASS_ENDPOINTS; idx2++) - { - pdev->tclasslist[pdev->classId].Eps[idx2].add = 0U; - pdev->tclasslist[pdev->classId].Eps[idx2].type = 0U; - pdev->tclasslist[pdev->classId].Eps[idx2].size = 0U; - pdev->tclasslist[pdev->classId].Eps[idx2].is_used = 0U; - } - - for (idx2 = 0U; idx2 < USBD_MAX_CLASS_INTERFACES; idx2++) - { - pdev->tclasslist[pdev->classId].Ifs[idx2] = 0U; - } - } - } - - /* Reset the configuration descriptor */ - (void)USBD_CMPST_ClearConfDesc(pdev); - - /* Reset the class ID and number of classes */ - pdev->classId = 0U; - pdev->NumClasses = 0U; - - return ret; -} -#endif /* USE_USBD_COMPOSITE */ - -#if (USBD_USER_REGISTER_CALLBACK == 1U) -/** - * @brief USBD_RegisterDevStateCallback - * @param pdev : Device Handle - * @param pUserCallback: User Callback - * @retval USBD Status - */ -USBD_StatusTypeDef USBD_RegisterDevStateCallback(USBD_HandleTypeDef *pdev, USBD_DevStateCallbackTypeDef pUserCallback) -{ - pdev->DevStateCallback = pUserCallback; - - return USBD_OK; -} -#endif /* USBD_USER_REGISTER_CALLBACK */ - -/** - * @brief USBD_Start - * Start the USB Device Core. - * @param pdev: Device Handle - * @retval USBD Status - */ -USBD_StatusTypeDef USBD_Start(USBD_HandleTypeDef *pdev) -{ -#ifdef USE_USBD_COMPOSITE - pdev->classId = 0U; -#endif /* USE_USBD_COMPOSITE */ - - /* Start the low level driver */ - return USBD_LL_Start(pdev); -} - -/** - * @brief USBD_Stop - * Stop the USB Device Core. - * @param pdev: Device Handle - * @retval USBD Status - */ -USBD_StatusTypeDef USBD_Stop(USBD_HandleTypeDef *pdev) -{ - /* Disconnect USB Device */ - (void)USBD_LL_Stop(pdev); - - /* Free Class Resources */ -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - if (pdev->pClass[i] != NULL) - { - pdev->classId = i; - /* Free Class Resources */ - (void)pdev->pClass[i]->DeInit(pdev, (uint8_t)pdev->dev_config); - } - } - } - - /* Reset the class ID */ - pdev->classId = 0U; -#else - if (pdev->pClass[0] != NULL) - { - (void)pdev->pClass[0]->DeInit(pdev, (uint8_t)pdev->dev_config); - } -#endif /* USE_USBD_COMPOSITE */ - - return USBD_OK; -} - -/** - * @brief USBD_RunTestMode - * Launch test mode process - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_RunTestMode(USBD_HandleTypeDef *pdev) -{ -#ifdef USBD_HS_TESTMODE_ENABLE - USBD_StatusTypeDef ret; - - /* Run USB HS test mode */ - ret = USBD_LL_SetTestMode(pdev, pdev->dev_test_mode); - - return ret; -#else - /* Prevent unused argument compilation warning */ - UNUSED(pdev); - - return USBD_OK; -#endif /* USBD_HS_TESTMODE_ENABLE */ -} - -/** - * @brief USBD_SetClassConfig - * Configure device and start the interface - * @param pdev: device instance - * @param cfgidx: configuration index - * @retval status - */ - -USBD_StatusTypeDef USBD_SetClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx) -{ - USBD_StatusTypeDef ret = USBD_OK; - -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - if (pdev->pClass[i] != NULL) - { - pdev->classId = i; - /* Set configuration and Start the Class*/ - if (pdev->pClass[i]->Init(pdev, cfgidx) != 0U) - { - ret = USBD_FAIL; - } - } - } - } -#else - if (pdev->pClass[0] != NULL) - { - /* Set configuration and Start the Class */ - ret = (USBD_StatusTypeDef)pdev->pClass[0]->Init(pdev, cfgidx); - } -#endif /* USE_USBD_COMPOSITE */ - - return ret; -} - -/** - * @brief USBD_ClrClassConfig - * Clear current configuration - * @param pdev: device instance - * @param cfgidx: configuration index - * @retval status - */ -USBD_StatusTypeDef USBD_ClrClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx) -{ - USBD_StatusTypeDef ret = USBD_OK; - -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - if (pdev->pClass[i] != NULL) - { - pdev->classId = i; - /* Clear configuration and De-initialize the Class process */ - if (pdev->pClass[i]->DeInit(pdev, cfgidx) != 0U) - { - ret = USBD_FAIL; - } - } - } - } -#else - /* Clear configuration and De-initialize the Class process */ - if (pdev->pClass[0]->DeInit(pdev, cfgidx) != 0U) - { - ret = USBD_FAIL; - } -#endif /* USE_USBD_COMPOSITE */ - - return ret; -} - - -/** - * @brief USBD_LL_SetupStage - * Handle the setup stage - * @param pdev: device instance - * @param psetup: setup packet buffer pointer - * @retval status - */ -USBD_StatusTypeDef USBD_LL_SetupStage(USBD_HandleTypeDef *pdev, uint8_t *psetup) -{ - USBD_StatusTypeDef ret; - - USBD_ParseSetupRequest(&pdev->request, psetup); - - pdev->ep0_state = USBD_EP0_SETUP; - - pdev->ep0_data_len = pdev->request.wLength; - - switch (pdev->request.bmRequest & 0x1FU) - { - case USB_REQ_RECIPIENT_DEVICE: - ret = USBD_StdDevReq(pdev, &pdev->request); - break; - - case USB_REQ_RECIPIENT_INTERFACE: - ret = USBD_StdItfReq(pdev, &pdev->request); - break; - - case USB_REQ_RECIPIENT_ENDPOINT: - ret = USBD_StdEPReq(pdev, &pdev->request); - break; - - default: - ret = USBD_LL_StallEP(pdev, (pdev->request.bmRequest & 0x80U)); - break; - } - - return ret; -} - -/** - * @brief USBD_LL_DataOutStage - * Handle data OUT stage - * @param pdev: device instance - * @param epnum: endpoint index - * @param pdata: data pointer - * @retval status - */ -USBD_StatusTypeDef USBD_LL_DataOutStage(USBD_HandleTypeDef *pdev, - uint8_t epnum, uint8_t *pdata) -{ - USBD_EndpointTypeDef *pep; - USBD_StatusTypeDef ret = USBD_OK; - uint8_t idx; - - if (epnum == 0U) - { - pep = &pdev->ep_out[0]; - - if (pdev->ep0_state == USBD_EP0_DATA_OUT) - { - if (pep->rem_length > pep->maxpacket) - { - pep->rem_length -= pep->maxpacket; - - (void)USBD_CtlContinueRx(pdev, pdata, MIN(pep->rem_length, pep->maxpacket)); - } - else - { - /* Find the class ID relative to the current request */ - switch (pdev->request.bmRequest & 0x1FU) - { - case USB_REQ_RECIPIENT_DEVICE: - /* Device requests must be managed by the first instantiated class - (or duplicated by all classes for simplicity) */ - idx = 0U; - break; - - case USB_REQ_RECIPIENT_INTERFACE: - idx = USBD_CoreFindIF(pdev, LOBYTE(pdev->request.wIndex)); - break; - - case USB_REQ_RECIPIENT_ENDPOINT: - idx = USBD_CoreFindEP(pdev, LOBYTE(pdev->request.wIndex)); - break; - - default: - /* Back to the first class in case of doubt */ - idx = 0U; - break; - } - - if (idx < USBD_MAX_SUPPORTED_CLASS) - { - /* Setup the class ID and route the request to the relative class function */ - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - if (pdev->pClass[idx]->EP0_RxReady != NULL) - { - pdev->classId = idx; - pdev->pClass[idx]->EP0_RxReady(pdev); - } - } - } - - (void)USBD_CtlSendStatus(pdev); - } - } - } - else - { - /* Get the class index relative to this interface */ - idx = USBD_CoreFindEP(pdev, (epnum & 0x7FU)); - - if (((uint16_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) - { - /* Call the class data out function to manage the request */ - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - if (pdev->pClass[idx]->DataOut != NULL) - { - pdev->classId = idx; - ret = (USBD_StatusTypeDef)pdev->pClass[idx]->DataOut(pdev, epnum); - } - } - if (ret != USBD_OK) - { - return ret; - } - } - } - - return USBD_OK; -} - -/** - * @brief USBD_LL_DataInStage - * Handle data in stage - * @param pdev: device instance - * @param epnum: endpoint index - * @param pdata: data pointer - * @retval status - */ -USBD_StatusTypeDef USBD_LL_DataInStage(USBD_HandleTypeDef *pdev, - uint8_t epnum, uint8_t *pdata) -{ - USBD_EndpointTypeDef *pep; - USBD_StatusTypeDef ret; - uint8_t idx; - - if (epnum == 0U) - { - pep = &pdev->ep_in[0]; - - if (pdev->ep0_state == USBD_EP0_DATA_IN) - { - if (pep->rem_length > pep->maxpacket) - { - pep->rem_length -= pep->maxpacket; - - (void)USBD_CtlContinueSendData(pdev, pdata, pep->rem_length); - - /* Prepare endpoint for premature end of transfer */ - (void)USBD_LL_PrepareReceive(pdev, 0U, NULL, 0U); - } - else - { - /* last packet is MPS multiple, so send ZLP packet */ - if ((pep->maxpacket == pep->rem_length) && - (pep->total_length >= pep->maxpacket) && - (pep->total_length < pdev->ep0_data_len)) - { - (void)USBD_CtlContinueSendData(pdev, NULL, 0U); - pdev->ep0_data_len = 0U; - - /* Prepare endpoint for premature end of transfer */ - (void)USBD_LL_PrepareReceive(pdev, 0U, NULL, 0U); - } - else - { - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - if (pdev->pClass[0]->EP0_TxSent != NULL) - { - pdev->classId = 0U; - pdev->pClass[0]->EP0_TxSent(pdev); - } - } - (void)USBD_LL_StallEP(pdev, 0x80U); - (void)USBD_CtlReceiveStatus(pdev); - } - } - } - - if (pdev->dev_test_mode != 0U) - { - (void)USBD_RunTestMode(pdev); - pdev->dev_test_mode = 0U; - } - } - else - { - /* Get the class index relative to this interface */ - idx = USBD_CoreFindEP(pdev, ((uint8_t)epnum | 0x80U)); - - if (((uint16_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) - { - /* Call the class data out function to manage the request */ - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - if (pdev->pClass[idx]->DataIn != NULL) - { - pdev->classId = idx; - ret = (USBD_StatusTypeDef)pdev->pClass[idx]->DataIn(pdev, epnum); - - if (ret != USBD_OK) - { - return ret; - } - } - } - } - } - - return USBD_OK; -} - -/** - * @brief USBD_LL_Reset - * Handle Reset event - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_LL_Reset(USBD_HandleTypeDef *pdev) -{ - USBD_StatusTypeDef ret = USBD_OK; - - /* Upon Reset call user call back */ - pdev->dev_state = USBD_STATE_DEFAULT; - pdev->ep0_state = USBD_EP0_IDLE; - pdev->dev_config = 0U; - pdev->dev_remote_wakeup = 0U; - pdev->dev_test_mode = 0U; - -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - if (pdev->pClass[i] != NULL) - { - pdev->classId = i; - /* Clear configuration and De-initialize the Class process*/ - - if (pdev->pClass[i]->DeInit != NULL) - { - if (pdev->pClass[i]->DeInit(pdev, (uint8_t)pdev->dev_config) != USBD_OK) - { - ret = USBD_FAIL; - } - } - } - } - } -#else - - if (pdev->pClass[0] != NULL) - { - if (pdev->pClass[0]->DeInit != NULL) - { - if (pdev->pClass[0]->DeInit(pdev, (uint8_t)pdev->dev_config) != USBD_OK) - { - ret = USBD_FAIL; - } - } - } -#endif /* USE_USBD_COMPOSITE */ - - /* Open EP0 OUT */ - (void)USBD_LL_OpenEP(pdev, 0x00U, USBD_EP_TYPE_CTRL, USB_MAX_EP0_SIZE); - pdev->ep_out[0x00U & 0xFU].is_used = 1U; - - pdev->ep_out[0].maxpacket = USB_MAX_EP0_SIZE; - - /* Open EP0 IN */ - (void)USBD_LL_OpenEP(pdev, 0x80U, USBD_EP_TYPE_CTRL, USB_MAX_EP0_SIZE); - pdev->ep_in[0x80U & 0xFU].is_used = 1U; - - pdev->ep_in[0].maxpacket = USB_MAX_EP0_SIZE; - - return ret; -} - -/** - * @brief USBD_LL_SetSpeed - * Handle Reset event - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_LL_SetSpeed(USBD_HandleTypeDef *pdev, - USBD_SpeedTypeDef speed) -{ - pdev->dev_speed = speed; - - return USBD_OK; -} - -/** - * @brief USBD_LL_Suspend - * Handle Suspend event - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_LL_Suspend(USBD_HandleTypeDef *pdev) -{ - if (pdev->dev_state != USBD_STATE_SUSPENDED) - { - pdev->dev_old_state = pdev->dev_state; - } - - pdev->dev_state = USBD_STATE_SUSPENDED; - - return USBD_OK; -} - -/** - * @brief USBD_LL_Resume - * Handle Resume event - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_LL_Resume(USBD_HandleTypeDef *pdev) -{ - if (pdev->dev_state == USBD_STATE_SUSPENDED) - { - pdev->dev_state = pdev->dev_old_state; - } - - return USBD_OK; -} - -/** - * @brief USBD_LL_SOF - * Handle SOF event - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_LL_SOF(USBD_HandleTypeDef *pdev) -{ - /* The SOF event can be distributed for all classes that support it */ - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - if (pdev->pClass[i] != NULL) - { - if (pdev->pClass[i]->SOF != NULL) - { - pdev->classId = i; - (void)pdev->pClass[i]->SOF(pdev); - } - } - } - } -#else - if (pdev->pClass[0] != NULL) - { - if (pdev->pClass[0]->SOF != NULL) - { - (void)pdev->pClass[0]->SOF(pdev); - } - } -#endif /* USE_USBD_COMPOSITE */ - } - - return USBD_OK; -} - -/** - * @brief USBD_LL_IsoINIncomplete - * Handle iso in incomplete event - * @param pdev: device instance - * @param epnum: Endpoint number - * @retval status - */ -USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, - uint8_t epnum) -{ - if (pdev->pClass[pdev->classId] == NULL) - { - return USBD_FAIL; - } - - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - if (pdev->pClass[pdev->classId]->IsoINIncomplete != NULL) - { - (void)pdev->pClass[pdev->classId]->IsoINIncomplete(pdev, epnum); - } - } - - return USBD_OK; -} - -/** - * @brief USBD_LL_IsoOUTIncomplete - * Handle iso out incomplete event - * @param pdev: device instance - * @param epnum: Endpoint number - * @retval status - */ -USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, - uint8_t epnum) -{ - if (pdev->pClass[pdev->classId] == NULL) - { - return USBD_FAIL; - } - - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - if (pdev->pClass[pdev->classId]->IsoOUTIncomplete != NULL) - { - (void)pdev->pClass[pdev->classId]->IsoOUTIncomplete(pdev, epnum); - } - } - - return USBD_OK; -} - -/** - * @brief USBD_LL_DevConnected - * Handle device connection event - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_LL_DevConnected(USBD_HandleTypeDef *pdev) -{ - /* Prevent unused argument compilation warning */ - UNUSED(pdev); - - return USBD_OK; -} - -/** - * @brief USBD_LL_DevDisconnected - * Handle device disconnection event - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_LL_DevDisconnected(USBD_HandleTypeDef *pdev) -{ - USBD_StatusTypeDef ret = USBD_OK; - - /* Free Class Resources */ - pdev->dev_state = USBD_STATE_DEFAULT; - -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - if (pdev->pClass[i] != NULL) - { - pdev->classId = i; - /* Clear configuration and De-initialize the Class process*/ - if (pdev->pClass[i]->DeInit(pdev, (uint8_t)pdev->dev_config) != 0U) - { - ret = USBD_FAIL; - } - } - } - } -#else - if (pdev->pClass[0] != NULL) - { - if (pdev->pClass[0]->DeInit(pdev, (uint8_t)pdev->dev_config) != 0U) - { - ret = USBD_FAIL; - } - } -#endif /* USE_USBD_COMPOSITE */ - - return ret; -} - -/** - * @brief USBD_CoreFindIF - * return the class index relative to the selected interface - * @param pdev: device instance - * @param index : selected interface number - * @retval index of the class using the selected interface number. OxFF if no class found. - */ -uint8_t USBD_CoreFindIF(USBD_HandleTypeDef *pdev, uint8_t index) -{ -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - /* Parse all interfaces listed in the current class */ - for (uint32_t j = 0U; j < pdev->tclasslist[i].NumIf; j++) - { - /* Check if requested Interface matches the current class interface */ - if (pdev->tclasslist[i].Ifs[j] == index) - { - if (pdev->pClass[i]->Setup != NULL) - { - return (uint8_t)i; - } - } - } - } - } - - return 0xFFU; -#else - UNUSED(pdev); - UNUSED(index); - - return 0x00U; -#endif /* USE_USBD_COMPOSITE */ -} - -/** - * @brief USBD_CoreFindEP - * return the class index relative to the selected endpoint - * @param pdev: device instance - * @param index : selected endpoint number - * @retval index of the class using the selected endpoint number. 0xFF if no class found. - */ -uint8_t USBD_CoreFindEP(USBD_HandleTypeDef *pdev, uint8_t index) -{ -#ifdef USE_USBD_COMPOSITE - /* Parse the table of classes in use */ - for (uint32_t i = 0U; i < USBD_MAX_SUPPORTED_CLASS; i++) - { - /* Check if current class is in use */ - if ((pdev->tclasslist[i].Active) == 1U) - { - /* Parse all endpoints listed in the current class */ - for (uint32_t j = 0U; j < pdev->tclasslist[i].NumEps; j++) - { - /* Check if requested endpoint matches the current class endpoint */ - if (pdev->tclasslist[i].Eps[j].add == index) - { - if (pdev->pClass[i]->Setup != NULL) - { - return (uint8_t)i; - } - } - } - } - } - - return 0xFFU; -#else - UNUSED(pdev); - UNUSED(index); - - return 0x00U; -#endif /* USE_USBD_COMPOSITE */ -} - -#ifdef USE_USBD_COMPOSITE -/** - * @brief USBD_CoreGetEPAdd - * Get the endpoint address relative to a selected class - * @param pdev: device instance - * @param ep_dir: USBD_EP_IN or USBD_EP_OUT - * @param ep_type: USBD_EP_TYPE_CTRL, USBD_EP_TYPE_ISOC, USBD_EP_TYPE_BULK or USBD_EP_TYPE_INTR - * @param ClassId: The Class ID - * @retval Address of the selected endpoint or 0xFFU if no endpoint found. - */ -uint8_t USBD_CoreGetEPAdd(USBD_HandleTypeDef *pdev, uint8_t ep_dir, uint8_t ep_type, uint8_t ClassId) -{ - uint8_t idx; - - /* Find the EP address in the selected class table */ - for (idx = 0; idx < pdev->tclasslist[ClassId].NumEps; idx++) - { - if (((pdev->tclasslist[ClassId].Eps[idx].add & USBD_EP_IN) == ep_dir) && \ - (pdev->tclasslist[ClassId].Eps[idx].type == ep_type) && \ - (pdev->tclasslist[ClassId].Eps[idx].is_used != 0U)) - { - return (pdev->tclasslist[ClassId].Eps[idx].add); - } - } - - /* If reaching this point, then no endpoint was found */ - return 0xFFU; -} -#endif /* USE_USBD_COMPOSITE */ - -/** - * @brief USBD_GetEpDesc - * This function return the Endpoint descriptor - * @param pdev: device instance - * @param pConfDesc: pointer to Bos descriptor - * @param EpAddr: endpoint address - * @retval pointer to video endpoint descriptor - */ -void *USBD_GetEpDesc(uint8_t *pConfDesc, uint8_t EpAddr) -{ - USBD_DescHeaderTypeDef *pdesc = (USBD_DescHeaderTypeDef *)(void *)pConfDesc; - USBD_ConfigDescTypeDef *desc = (USBD_ConfigDescTypeDef *)(void *)pConfDesc; - USBD_EpDescTypeDef *pEpDesc = NULL; - uint16_t ptr; - - if (desc->wTotalLength > desc->bLength) - { - ptr = desc->bLength; - - while (ptr < desc->wTotalLength) - { - pdesc = USBD_GetNextDesc((uint8_t *)pdesc, &ptr); - - if (pdesc->bDescriptorType == USB_DESC_TYPE_ENDPOINT) - { - pEpDesc = (USBD_EpDescTypeDef *)(void *)pdesc; - - if (pEpDesc->bEndpointAddress == EpAddr) - { - break; - } - else - { - pEpDesc = NULL; - } - } - } - } - - return (void *)pEpDesc; -} - -/** - * @brief USBD_GetNextDesc - * This function return the next descriptor header - * @param buf: Buffer where the descriptor is available - * @param ptr: data pointer inside the descriptor - * @retval next header - */ -USBD_DescHeaderTypeDef *USBD_GetNextDesc(uint8_t *pbuf, uint16_t *ptr) -{ - USBD_DescHeaderTypeDef *pnext = (USBD_DescHeaderTypeDef *)(void *)pbuf; - - *ptr += pnext->bLength; - pnext = (USBD_DescHeaderTypeDef *)(void *)(pbuf + pnext->bLength); - - return (pnext); -} - -/** - * @} - */ - - -/** - * @} - */ - - -/** - * @} - */ - diff --git a/legacy/stm32/lib/USB_CDC/src/usbd_ctlreq.c b/legacy/stm32/lib/USB_CDC/src/usbd_ctlreq.c deleted file mode 100644 index fa6ffcf..0000000 --- a/legacy/stm32/lib/USB_CDC/src/usbd_ctlreq.c +++ /dev/null @@ -1,1058 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_req.c - * @author MCD Application Team - * @brief This file provides the standard USB requests following chapter 9. - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_ctlreq.h" -#include "usbd_ioreq.h" - -#ifdef USE_USBD_COMPOSITE -#include "usbd_composite_builder.h" -#endif /* USE_USBD_COMPOSITE */ - -/** @addtogroup STM32_USBD_STATE_DEVICE_LIBRARY - * @{ - */ - - -/** @defgroup USBD_REQ - * @brief USB standard requests module - * @{ - */ - -/** @defgroup USBD_REQ_Private_TypesDefinitions - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_REQ_Private_Defines - * @{ - */ -#ifndef USBD_MAX_STR_DESC_SIZ -#define USBD_MAX_STR_DESC_SIZ 64U -#endif /* USBD_MAX_STR_DESC_SIZ */ -/** - * @} - */ - - -/** @defgroup USBD_REQ_Private_Macros - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_REQ_Private_Variables - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_REQ_Private_FunctionPrototypes - * @{ - */ -static void USBD_GetDescriptor(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static void USBD_SetAddress(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static USBD_StatusTypeDef USBD_SetConfig(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static void USBD_GetConfig(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static void USBD_GetStatus(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static void USBD_SetFeature(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static void USBD_ClrFeature(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); -static uint8_t USBD_GetLen(uint8_t *buf); - -/** - * @} - */ - - -/** @defgroup USBD_REQ_Private_Functions - * @{ - */ - - -/** - * @brief USBD_StdDevReq - * Handle standard usb device requests - * @param pdev: device instance - * @param req: usb request - * @retval status - */ -USBD_StatusTypeDef USBD_StdDevReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - USBD_StatusTypeDef ret = USBD_OK; - - switch (req->bmRequest & USB_REQ_TYPE_MASK) - { - case USB_REQ_TYPE_CLASS: - case USB_REQ_TYPE_VENDOR: - ret = (USBD_StatusTypeDef)pdev->pClass[pdev->classId]->Setup(pdev, req); - break; - - case USB_REQ_TYPE_STANDARD: - switch (req->bRequest) - { - case USB_REQ_GET_DESCRIPTOR: - USBD_GetDescriptor(pdev, req); - break; - - case USB_REQ_SET_ADDRESS: - USBD_SetAddress(pdev, req); - break; - - case USB_REQ_SET_CONFIGURATION: - ret = USBD_SetConfig(pdev, req); - break; - - case USB_REQ_GET_CONFIGURATION: - USBD_GetConfig(pdev, req); - break; - - case USB_REQ_GET_STATUS: - USBD_GetStatus(pdev, req); - break; - - case USB_REQ_SET_FEATURE: - USBD_SetFeature(pdev, req); - break; - - case USB_REQ_CLEAR_FEATURE: - USBD_ClrFeature(pdev, req); - break; - - default: - USBD_CtlError(pdev, req); - break; - } - break; - - default: - USBD_CtlError(pdev, req); - break; - } - - return ret; -} - -/** - * @brief USBD_StdItfReq - * Handle standard usb interface requests - * @param pdev: device instance - * @param req: usb request - * @retval status - */ -USBD_StatusTypeDef USBD_StdItfReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - USBD_StatusTypeDef ret = USBD_OK; - uint8_t idx; - - switch (req->bmRequest & USB_REQ_TYPE_MASK) - { - case USB_REQ_TYPE_CLASS: - case USB_REQ_TYPE_VENDOR: - case USB_REQ_TYPE_STANDARD: - switch (pdev->dev_state) - { - case USBD_STATE_DEFAULT: - case USBD_STATE_ADDRESSED: - case USBD_STATE_CONFIGURED: - - if (LOBYTE(req->wIndex) <= USBD_MAX_NUM_INTERFACES) - { - /* Get the class index relative to this interface */ - idx = USBD_CoreFindIF(pdev, LOBYTE(req->wIndex)); - if (((uint8_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) - { - /* Call the class data out function to manage the request */ - if (pdev->pClass[idx]->Setup != NULL) - { - pdev->classId = idx; - ret = (USBD_StatusTypeDef)(pdev->pClass[idx]->Setup(pdev, req)); - } - else - { - /* should never reach this condition */ - ret = USBD_FAIL; - } - } - else - { - /* No relative interface found */ - ret = USBD_FAIL; - } - - if ((req->wLength == 0U) && (ret == USBD_OK)) - { - (void)USBD_CtlSendStatus(pdev); - } - } - else - { - USBD_CtlError(pdev, req); - } - break; - - default: - USBD_CtlError(pdev, req); - break; - } - break; - - default: - USBD_CtlError(pdev, req); - break; - } - - return ret; -} - -/** - * @brief USBD_StdEPReq - * Handle standard usb endpoint requests - * @param pdev: device instance - * @param req: usb request - * @retval status - */ -USBD_StatusTypeDef USBD_StdEPReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - USBD_EndpointTypeDef *pep; - uint8_t ep_addr; - uint8_t idx; - USBD_StatusTypeDef ret = USBD_OK; - - ep_addr = LOBYTE(req->wIndex); - - switch (req->bmRequest & USB_REQ_TYPE_MASK) - { - case USB_REQ_TYPE_CLASS: - case USB_REQ_TYPE_VENDOR: - /* Get the class index relative to this endpoint */ - idx = USBD_CoreFindEP(pdev, ep_addr); - if (((uint8_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) - { - pdev->classId = idx; - /* Call the class data out function to manage the request */ - if (pdev->pClass[idx]->Setup != NULL) - { - ret = (USBD_StatusTypeDef)pdev->pClass[idx]->Setup(pdev, req); - } - } - break; - - case USB_REQ_TYPE_STANDARD: - switch (req->bRequest) - { - case USB_REQ_SET_FEATURE: - switch (pdev->dev_state) - { - case USBD_STATE_ADDRESSED: - if ((ep_addr != 0x00U) && (ep_addr != 0x80U)) - { - (void)USBD_LL_StallEP(pdev, ep_addr); - (void)USBD_LL_StallEP(pdev, 0x80U); - } - else - { - USBD_CtlError(pdev, req); - } - break; - - case USBD_STATE_CONFIGURED: - if (req->wValue == USB_FEATURE_EP_HALT) - { - if ((ep_addr != 0x00U) && (ep_addr != 0x80U) && (req->wLength == 0x00U)) - { - (void)USBD_LL_StallEP(pdev, ep_addr); - } - } - (void)USBD_CtlSendStatus(pdev); - - break; - - default: - USBD_CtlError(pdev, req); - break; - } - break; - - case USB_REQ_CLEAR_FEATURE: - - switch (pdev->dev_state) - { - case USBD_STATE_ADDRESSED: - if ((ep_addr != 0x00U) && (ep_addr != 0x80U)) - { - (void)USBD_LL_StallEP(pdev, ep_addr); - (void)USBD_LL_StallEP(pdev, 0x80U); - } - else - { - USBD_CtlError(pdev, req); - } - break; - - case USBD_STATE_CONFIGURED: - if (req->wValue == USB_FEATURE_EP_HALT) - { - if ((ep_addr & 0x7FU) != 0x00U) - { - (void)USBD_LL_ClearStallEP(pdev, ep_addr); - } - (void)USBD_CtlSendStatus(pdev); - - /* Get the class index relative to this interface */ - idx = USBD_CoreFindEP(pdev, ep_addr); - if (((uint8_t)idx != 0xFFU) && (idx < USBD_MAX_SUPPORTED_CLASS)) - { - pdev->classId = idx; - /* Call the class data out function to manage the request */ - if (pdev->pClass[idx]->Setup != NULL) - { - ret = (USBD_StatusTypeDef)(pdev->pClass[idx]->Setup(pdev, req)); - } - } - } - break; - - default: - USBD_CtlError(pdev, req); - break; - } - break; - - case USB_REQ_GET_STATUS: - switch (pdev->dev_state) - { - case USBD_STATE_ADDRESSED: - if ((ep_addr != 0x00U) && (ep_addr != 0x80U)) - { - USBD_CtlError(pdev, req); - break; - } - pep = ((ep_addr & 0x80U) == 0x80U) ? &pdev->ep_in[ep_addr & 0x7FU] : \ - &pdev->ep_out[ep_addr & 0x7FU]; - - pep->status = 0x0000U; - - (void)USBD_CtlSendData(pdev, (uint8_t *)&pep->status, 2U); - break; - - case USBD_STATE_CONFIGURED: - if ((ep_addr & 0x80U) == 0x80U) - { - if (pdev->ep_in[ep_addr & 0xFU].is_used == 0U) - { - USBD_CtlError(pdev, req); - break; - } - } - else - { - if (pdev->ep_out[ep_addr & 0xFU].is_used == 0U) - { - USBD_CtlError(pdev, req); - break; - } - } - - pep = ((ep_addr & 0x80U) == 0x80U) ? &pdev->ep_in[ep_addr & 0x7FU] : \ - &pdev->ep_out[ep_addr & 0x7FU]; - - if ((ep_addr == 0x00U) || (ep_addr == 0x80U)) - { - pep->status = 0x0000U; - } - else if (USBD_LL_IsStallEP(pdev, ep_addr) != 0U) - { - pep->status = 0x0001U; - } - else - { - pep->status = 0x0000U; - } - - (void)USBD_CtlSendData(pdev, (uint8_t *)&pep->status, 2U); - break; - - default: - USBD_CtlError(pdev, req); - break; - } - break; - - default: - USBD_CtlError(pdev, req); - break; - } - break; - - default: - USBD_CtlError(pdev, req); - break; - } - - return ret; -} - - -/** - * @brief USBD_GetDescriptor - * Handle Get Descriptor requests - * @param pdev: device instance - * @param req: usb request - * @retval None - */ -static void USBD_GetDescriptor(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - uint16_t len = 0U; - uint8_t *pbuf = NULL; - uint8_t err = 0U; - - switch (req->wValue >> 8) - { -#if ((USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1U)) - case USB_DESC_TYPE_BOS: - if (pdev->pDesc->GetBOSDescriptor != NULL) - { - pbuf = pdev->pDesc->GetBOSDescriptor(pdev->dev_speed, &len); - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; -#endif /* (USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1U) */ - case USB_DESC_TYPE_DEVICE: - pbuf = pdev->pDesc->GetDeviceDescriptor(pdev->dev_speed, &len); - break; - - case USB_DESC_TYPE_CONFIGURATION: - if (pdev->dev_speed == USBD_SPEED_HIGH) - { -#ifdef USE_USBD_COMPOSITE - if ((uint8_t)(pdev->NumClasses) > 0U) - { - pbuf = (uint8_t *)USBD_CMPSIT.GetHSConfigDescriptor(&len); - } - else -#endif /* USE_USBD_COMPOSITE */ - { - pbuf = (uint8_t *)pdev->pClass[0]->GetHSConfigDescriptor(&len); - } - pbuf[1] = USB_DESC_TYPE_CONFIGURATION; - } - else - { -#ifdef USE_USBD_COMPOSITE - if ((uint8_t)(pdev->NumClasses) > 0U) - { - pbuf = (uint8_t *)USBD_CMPSIT.GetFSConfigDescriptor(&len); - } - else -#endif /* USE_USBD_COMPOSITE */ - { - pbuf = (uint8_t *)pdev->pClass[0]->GetFSConfigDescriptor(&len); - } - pbuf[1] = USB_DESC_TYPE_CONFIGURATION; - } - break; - - case USB_DESC_TYPE_STRING: - switch ((uint8_t)(req->wValue)) - { - case USBD_IDX_LANGID_STR: - if (pdev->pDesc->GetLangIDStrDescriptor != NULL) - { - pbuf = pdev->pDesc->GetLangIDStrDescriptor(pdev->dev_speed, &len); - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; - - case USBD_IDX_MFC_STR: - if (pdev->pDesc->GetManufacturerStrDescriptor != NULL) - { - pbuf = pdev->pDesc->GetManufacturerStrDescriptor(pdev->dev_speed, &len); - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; - - case USBD_IDX_PRODUCT_STR: - if (pdev->pDesc->GetProductStrDescriptor != NULL) - { - pbuf = pdev->pDesc->GetProductStrDescriptor(pdev->dev_speed, &len); - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; - - case USBD_IDX_SERIAL_STR: - if (pdev->pDesc->GetSerialStrDescriptor != NULL) - { - pbuf = pdev->pDesc->GetSerialStrDescriptor(pdev->dev_speed, &len); - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; - - case USBD_IDX_CONFIG_STR: - if (pdev->pDesc->GetConfigurationStrDescriptor != NULL) - { - pbuf = pdev->pDesc->GetConfigurationStrDescriptor(pdev->dev_speed, &len); - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; - - case USBD_IDX_INTERFACE_STR: - if (pdev->pDesc->GetInterfaceStrDescriptor != NULL) - { - pbuf = pdev->pDesc->GetInterfaceStrDescriptor(pdev->dev_speed, &len); - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; - - default: -#if (USBD_SUPPORT_USER_STRING_DESC == 1U) - pbuf = NULL; - - for (uint32_t idx = 0U; (idx < pdev->NumClasses); idx++) - { - if (pdev->pClass[idx]->GetUsrStrDescriptor != NULL) - { - pdev->classId = idx; - pbuf = pdev->pClass[idx]->GetUsrStrDescriptor(pdev, LOBYTE(req->wValue), &len); - - if (pbuf == NULL) /* This means that no class recognized the string index */ - { - continue; - } - else - { - break; - } - } - } -#endif /* USBD_SUPPORT_USER_STRING_DESC */ - -#if (USBD_CLASS_USER_STRING_DESC == 1U) - if (pdev->pDesc->GetUserStrDescriptor != NULL) - { - pbuf = pdev->pDesc->GetUserStrDescriptor(pdev->dev_speed, LOBYTE(req->wValue), &len); - } - else - { - USBD_CtlError(pdev, req); - err++; - } -#endif /* USBD_SUPPORT_USER_STRING_DESC */ - -#if ((USBD_CLASS_USER_STRING_DESC == 0U) && (USBD_SUPPORT_USER_STRING_DESC == 0U)) - USBD_CtlError(pdev, req); - err++; -#endif /* (USBD_CLASS_USER_STRING_DESC == 0U) && (USBD_SUPPORT_USER_STRING_DESC == 0U) */ - break; - } - break; - - case USB_DESC_TYPE_DEVICE_QUALIFIER: - if (pdev->dev_speed == USBD_SPEED_HIGH) - { -#ifdef USE_USBD_COMPOSITE - if ((uint8_t)(pdev->NumClasses) > 0U) - { - pbuf = (uint8_t *)USBD_CMPSIT.GetDeviceQualifierDescriptor(&len); - } - else -#endif /* USE_USBD_COMPOSITE */ - { - pbuf = (uint8_t *)pdev->pClass[0]->GetDeviceQualifierDescriptor(&len); - } - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; - - case USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION: - if (pdev->dev_speed == USBD_SPEED_HIGH) - { -#ifdef USE_USBD_COMPOSITE - if ((uint8_t)(pdev->NumClasses) > 0U) - { - pbuf = (uint8_t *)USBD_CMPSIT.GetOtherSpeedConfigDescriptor(&len); - } - else -#endif /* USE_USBD_COMPOSITE */ - { - pbuf = (uint8_t *)pdev->pClass[0]->GetOtherSpeedConfigDescriptor(&len); - } - pbuf[1] = USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION; - } - else - { - USBD_CtlError(pdev, req); - err++; - } - break; - - default: - USBD_CtlError(pdev, req); - err++; - break; - } - - if (err != 0U) - { - return; - } - - if (req->wLength != 0U) - { - if (len != 0U) - { - len = MIN(len, req->wLength); - (void)USBD_CtlSendData(pdev, pbuf, len); - } - else - { - USBD_CtlError(pdev, req); - } - } - else - { - (void)USBD_CtlSendStatus(pdev); - } -} - - -/** - * @brief USBD_SetAddress - * Set device address - * @param pdev: device instance - * @param req: usb request - * @retval None - */ -static void USBD_SetAddress(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - uint8_t dev_addr; - - if ((req->wIndex == 0U) && (req->wLength == 0U) && (req->wValue < 128U)) - { - dev_addr = (uint8_t)(req->wValue) & 0x7FU; - - if (pdev->dev_state == USBD_STATE_CONFIGURED) - { - USBD_CtlError(pdev, req); - } - else - { - pdev->dev_address = dev_addr; - (void)USBD_LL_SetUSBAddress(pdev, dev_addr); - (void)USBD_CtlSendStatus(pdev); - - if (dev_addr != 0U) - { - pdev->dev_state = USBD_STATE_ADDRESSED; - } - else - { - pdev->dev_state = USBD_STATE_DEFAULT; - } - } - } - else - { - USBD_CtlError(pdev, req); - } -} - -/** - * @brief USBD_SetConfig - * Handle Set device configuration request - * @param pdev: device instance - * @param req: usb request - * @retval status - */ -static USBD_StatusTypeDef USBD_SetConfig(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - USBD_StatusTypeDef ret = USBD_OK; - static uint8_t cfgidx; - - cfgidx = (uint8_t)(req->wValue); - - if (cfgidx > USBD_MAX_NUM_CONFIGURATION) - { - USBD_CtlError(pdev, req); - return USBD_FAIL; - } - - switch (pdev->dev_state) - { - case USBD_STATE_ADDRESSED: - if (cfgidx != 0U) - { - pdev->dev_config = cfgidx; - - ret = USBD_SetClassConfig(pdev, cfgidx); - - if (ret != USBD_OK) - { - USBD_CtlError(pdev, req); - pdev->dev_state = USBD_STATE_ADDRESSED; - } - else - { - (void)USBD_CtlSendStatus(pdev); - pdev->dev_state = USBD_STATE_CONFIGURED; - -#if (USBD_USER_REGISTER_CALLBACK == 1U) - if (pdev->DevStateCallback != NULL) - { - pdev->DevStateCallback(USBD_STATE_CONFIGURED, cfgidx); - } -#endif /* USBD_USER_REGISTER_CALLBACK */ - } - } - else - { - (void)USBD_CtlSendStatus(pdev); - } - break; - - case USBD_STATE_CONFIGURED: - if (cfgidx == 0U) - { - pdev->dev_state = USBD_STATE_ADDRESSED; - pdev->dev_config = cfgidx; - (void)USBD_ClrClassConfig(pdev, cfgidx); - (void)USBD_CtlSendStatus(pdev); - } - else if (cfgidx != pdev->dev_config) - { - /* Clear old configuration */ - (void)USBD_ClrClassConfig(pdev, (uint8_t)pdev->dev_config); - - /* set new configuration */ - pdev->dev_config = cfgidx; - - ret = USBD_SetClassConfig(pdev, cfgidx); - - if (ret != USBD_OK) - { - USBD_CtlError(pdev, req); - (void)USBD_ClrClassConfig(pdev, (uint8_t)pdev->dev_config); - pdev->dev_state = USBD_STATE_ADDRESSED; - } - else - { - (void)USBD_CtlSendStatus(pdev); - } - } - else - { - (void)USBD_CtlSendStatus(pdev); - } - break; - - default: - USBD_CtlError(pdev, req); - (void)USBD_ClrClassConfig(pdev, cfgidx); - ret = USBD_FAIL; - break; - } - - return ret; -} - -/** - * @brief USBD_GetConfig - * Handle Get device configuration request - * @param pdev: device instance - * @param req: usb request - * @retval None - */ -static void USBD_GetConfig(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - if (req->wLength != 1U) - { - USBD_CtlError(pdev, req); - } - else - { - switch (pdev->dev_state) - { - case USBD_STATE_DEFAULT: - case USBD_STATE_ADDRESSED: - pdev->dev_default_config = 0U; - (void)USBD_CtlSendData(pdev, (uint8_t *)&pdev->dev_default_config, 1U); - break; - - case USBD_STATE_CONFIGURED: - (void)USBD_CtlSendData(pdev, (uint8_t *)&pdev->dev_config, 1U); - break; - - default: - USBD_CtlError(pdev, req); - break; - } - } -} - -/** - * @brief USBD_GetStatus - * Handle Get Status request - * @param pdev: device instance - * @param req: usb request - * @retval None - */ -static void USBD_GetStatus(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - switch (pdev->dev_state) - { - case USBD_STATE_DEFAULT: - case USBD_STATE_ADDRESSED: - case USBD_STATE_CONFIGURED: - if (req->wLength != 0x2U) - { - USBD_CtlError(pdev, req); - break; - } - -#if (USBD_SELF_POWERED == 1U) - pdev->dev_config_status = USB_CONFIG_SELF_POWERED; -#else - pdev->dev_config_status = 0U; -#endif /* USBD_SELF_POWERED */ - - if (pdev->dev_remote_wakeup != 0U) - { - pdev->dev_config_status |= USB_CONFIG_REMOTE_WAKEUP; - } - - (void)USBD_CtlSendData(pdev, (uint8_t *)&pdev->dev_config_status, 2U); - break; - - default: - USBD_CtlError(pdev, req); - break; - } -} - - -/** - * @brief USBD_SetFeature - * Handle Set device feature request - * @param pdev: device instance - * @param req: usb request - * @retval None - */ -static void USBD_SetFeature(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - if (req->wValue == USB_FEATURE_REMOTE_WAKEUP) - { - pdev->dev_remote_wakeup = 1U; - (void)USBD_CtlSendStatus(pdev); - } - else if (req->wValue == USB_FEATURE_TEST_MODE) - { - pdev->dev_test_mode = (uint8_t)(req->wIndex >> 8); - (void)USBD_CtlSendStatus(pdev); - } - else - { - USBD_CtlError(pdev, req); - } -} - - -/** - * @brief USBD_ClrFeature - * Handle clear device feature request - * @param pdev: device instance - * @param req: usb request - * @retval None - */ -static void USBD_ClrFeature(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - switch (pdev->dev_state) - { - case USBD_STATE_DEFAULT: - case USBD_STATE_ADDRESSED: - case USBD_STATE_CONFIGURED: - if (req->wValue == USB_FEATURE_REMOTE_WAKEUP) - { - pdev->dev_remote_wakeup = 0U; - (void)USBD_CtlSendStatus(pdev); - } - break; - - default: - USBD_CtlError(pdev, req); - break; - } -} - - -/** - * @brief USBD_ParseSetupRequest - * Copy buffer into setup structure - * @param req: usb request - * @param pdata: setup data pointer - * @retval None - */ -void USBD_ParseSetupRequest(USBD_SetupReqTypedef *req, uint8_t *pdata) -{ - uint8_t *pbuff = pdata; - - req->bmRequest = *(uint8_t *)(pbuff); - - pbuff++; - req->bRequest = *(uint8_t *)(pbuff); - - pbuff++; - req->wValue = SWAPBYTE(pbuff); - - pbuff++; - pbuff++; - req->wIndex = SWAPBYTE(pbuff); - - pbuff++; - pbuff++; - req->wLength = SWAPBYTE(pbuff); -} - - -/** - * @brief USBD_CtlError - * Handle USB low level Error - * @param pdev: device instance - * @param req: usb request - * @retval None - */ -void USBD_CtlError(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) -{ - UNUSED(req); - - (void)USBD_LL_StallEP(pdev, 0x80U); - (void)USBD_LL_StallEP(pdev, 0U); -} - - -/** - * @brief USBD_GetString - * Convert Ascii string into unicode one - * @param desc : descriptor buffer - * @param unicode : Formatted string buffer (unicode) - * @param len : descriptor length - * @retval None - */ -void USBD_GetString(uint8_t *desc, uint8_t *unicode, uint16_t *len) -{ - uint8_t idx = 0U; - uint8_t *pdesc; - - if (desc == NULL) - { - return; - } - - pdesc = desc; - *len = MIN(USBD_MAX_STR_DESC_SIZ, ((uint16_t)USBD_GetLen(pdesc) * 2U) + 2U); - - unicode[idx] = *(uint8_t *)len; - idx++; - unicode[idx] = USB_DESC_TYPE_STRING; - idx++; - - while (*pdesc != (uint8_t)'\0') - { - unicode[idx] = *pdesc; - pdesc++; - idx++; - - unicode[idx] = 0U; - idx++; - } -} - - -/** - * @brief USBD_GetLen - * return the string length - * @param buf : pointer to the ascii string buffer - * @retval string length - */ -static uint8_t USBD_GetLen(uint8_t *buf) -{ - uint8_t len = 0U; - uint8_t *pbuff = buf; - - while (*pbuff != (uint8_t)'\0') - { - len++; - pbuff++; - } - - return len; -} -/** - * @} - */ - - -/** - * @} - */ - - -/** - * @} - */ - diff --git a/legacy/stm32/lib/USB_CDC/src/usbd_desc.c b/legacy/stm32/lib/USB_CDC/src/usbd_desc.c deleted file mode 100644 index 3c467cd..0000000 --- a/legacy/stm32/lib/USB_CDC/src/usbd_desc.c +++ /dev/null @@ -1,63 +0,0 @@ -#include "usbd_core.h" -#include "usbd_desc.h" -#include "usbd_conf.h" - -#define USBD_VID 0x0483 /* STMicroelectronics */ -#define USBD_PID_FS 0x5740 /* CDC Virtual COM Port */ -#define USBD_LANGID_STRING 0x0409 /* English US */ -#define USBD_MFR_STRING "SaltyLab" -#define USBD_PRODUCT_STRING "SaltyLab IMU" -#define USBD_SERIAL_STRING "SALTY001" - -static uint8_t *USBD_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); -static uint8_t *USBD_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); -static uint8_t *USBD_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); -static uint8_t *USBD_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); -static uint8_t *USBD_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); -static uint8_t *USBD_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); -static uint8_t *USBD_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); - -USBD_DescriptorsTypeDef SaltyLab_Desc = { - USBD_DeviceDescriptor, - USBD_LangIDStrDescriptor, - USBD_ManufacturerStrDescriptor, - USBD_ProductStrDescriptor, - USBD_SerialStrDescriptor, - USBD_ConfigStrDescriptor, - USBD_InterfaceStrDescriptor, -}; - -static uint8_t USBD_DeviceDesc[USB_LEN_DEV_DESC] = { - 0x12, USB_DESC_TYPE_DEVICE, 0x00, 0x02, 0x02, 0x02, 0x00, - 64, LOBYTE(USBD_VID), HIBYTE(USBD_VID), LOBYTE(USBD_PID_FS), HIBYTE(USBD_PID_FS), - 0x00, 0x02, 1, 2, 3, 1 -}; - -static uint8_t USBD_LangIDDesc[USB_LEN_LANGID_STR_DESC] = { - USB_LEN_LANGID_STR_DESC, USB_DESC_TYPE_STRING, - LOBYTE(USBD_LANGID_STRING), HIBYTE(USBD_LANGID_STRING) -}; - -static uint8_t USBD_StrDesc[USBD_MAX_STR_DESC_SIZ]; - -static uint8_t *USBD_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - (void)speed; *length = sizeof(USBD_DeviceDesc); return USBD_DeviceDesc; -} -static uint8_t *USBD_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - (void)speed; *length = sizeof(USBD_LangIDDesc); return USBD_LangIDDesc; -} -static uint8_t *USBD_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - USBD_GetString((uint8_t *)USBD_MFR_STRING, USBD_StrDesc, length); return USBD_StrDesc; -} -static uint8_t *USBD_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - USBD_GetString((uint8_t *)USBD_PRODUCT_STRING, USBD_StrDesc, length); return USBD_StrDesc; -} -static uint8_t *USBD_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - USBD_GetString((uint8_t *)USBD_SERIAL_STRING, USBD_StrDesc, length); return USBD_StrDesc; -} -static uint8_t *USBD_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - USBD_GetString((uint8_t *)"CDC Config", USBD_StrDesc, length); return USBD_StrDesc; -} -static uint8_t *USBD_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - USBD_GetString((uint8_t *)"CDC Interface", USBD_StrDesc, length); return USBD_StrDesc; -} diff --git a/legacy/stm32/lib/USB_CDC/src/usbd_ioreq.c b/legacy/stm32/lib/USB_CDC/src/usbd_ioreq.c deleted file mode 100644 index e8a44db..0000000 --- a/legacy/stm32/lib/USB_CDC/src/usbd_ioreq.c +++ /dev/null @@ -1,224 +0,0 @@ -/** - ****************************************************************************** - * @file usbd_ioreq.c - * @author MCD Application Team - * @brief This file provides the IO requests APIs for control endpoints. - ****************************************************************************** - * @attention - * - * Copyright (c) 2015 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ - -/* Includes ------------------------------------------------------------------*/ -#include "usbd_ioreq.h" - -/** @addtogroup STM32_USB_DEVICE_LIBRARY - * @{ - */ - - -/** @defgroup USBD_IOREQ - * @brief control I/O requests module - * @{ - */ - -/** @defgroup USBD_IOREQ_Private_TypesDefinitions - * @{ - */ -/** - * @} - */ - - -/** @defgroup USBD_IOREQ_Private_Defines - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_IOREQ_Private_Macros - * @{ - */ -/** - * @} - */ - - -/** @defgroup USBD_IOREQ_Private_Variables - * @{ - */ - -/** - * @} - */ - - -/** @defgroup USBD_IOREQ_Private_FunctionPrototypes - * @{ - */ -/** - * @} - */ - - -/** @defgroup USBD_IOREQ_Private_Functions - * @{ - */ - -/** - * @brief USBD_CtlSendData - * send data on the ctl pipe - * @param pdev: device instance - * @param buff: pointer to data buffer - * @param len: length of data to be sent - * @retval status - */ -USBD_StatusTypeDef USBD_CtlSendData(USBD_HandleTypeDef *pdev, - uint8_t *pbuf, uint32_t len) -{ - /* Set EP0 State */ - pdev->ep0_state = USBD_EP0_DATA_IN; - pdev->ep_in[0].total_length = len; - -#ifdef USBD_AVOID_PACKET_SPLIT_MPS - pdev->ep_in[0].rem_length = 0U; -#else - pdev->ep_in[0].rem_length = len; -#endif /* USBD_AVOID_PACKET_SPLIT_MPS */ - - /* Start the transfer */ - (void)USBD_LL_Transmit(pdev, 0x00U, pbuf, len); - - return USBD_OK; -} - -/** - * @brief USBD_CtlContinueSendData - * continue sending data on the ctl pipe - * @param pdev: device instance - * @param buff: pointer to data buffer - * @param len: length of data to be sent - * @retval status - */ -USBD_StatusTypeDef USBD_CtlContinueSendData(USBD_HandleTypeDef *pdev, - uint8_t *pbuf, uint32_t len) -{ - /* Start the next transfer */ - (void)USBD_LL_Transmit(pdev, 0x00U, pbuf, len); - - return USBD_OK; -} - -/** - * @brief USBD_CtlPrepareRx - * receive data on the ctl pipe - * @param pdev: device instance - * @param buff: pointer to data buffer - * @param len: length of data to be received - * @retval status - */ -USBD_StatusTypeDef USBD_CtlPrepareRx(USBD_HandleTypeDef *pdev, - uint8_t *pbuf, uint32_t len) -{ - /* Set EP0 State */ - pdev->ep0_state = USBD_EP0_DATA_OUT; - pdev->ep_out[0].total_length = len; - -#ifdef USBD_AVOID_PACKET_SPLIT_MPS - pdev->ep_out[0].rem_length = 0U; -#else - pdev->ep_out[0].rem_length = len; -#endif /* USBD_AVOID_PACKET_SPLIT_MPS */ - - /* Start the transfer */ - (void)USBD_LL_PrepareReceive(pdev, 0U, pbuf, len); - - return USBD_OK; -} - -/** - * @brief USBD_CtlContinueRx - * continue receive data on the ctl pipe - * @param pdev: device instance - * @param buff: pointer to data buffer - * @param len: length of data to be received - * @retval status - */ -USBD_StatusTypeDef USBD_CtlContinueRx(USBD_HandleTypeDef *pdev, - uint8_t *pbuf, uint32_t len) -{ - (void)USBD_LL_PrepareReceive(pdev, 0U, pbuf, len); - - return USBD_OK; -} - -/** - * @brief USBD_CtlSendStatus - * send zero lzngth packet on the ctl pipe - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_CtlSendStatus(USBD_HandleTypeDef *pdev) -{ - /* Set EP0 State */ - pdev->ep0_state = USBD_EP0_STATUS_IN; - - /* Start the transfer */ - (void)USBD_LL_Transmit(pdev, 0x00U, NULL, 0U); - - return USBD_OK; -} - -/** - * @brief USBD_CtlReceiveStatus - * receive zero lzngth packet on the ctl pipe - * @param pdev: device instance - * @retval status - */ -USBD_StatusTypeDef USBD_CtlReceiveStatus(USBD_HandleTypeDef *pdev) -{ - /* Set EP0 State */ - pdev->ep0_state = USBD_EP0_STATUS_OUT; - - /* Start the transfer */ - (void)USBD_LL_PrepareReceive(pdev, 0U, NULL, 0U); - - return USBD_OK; -} - -/** - * @brief USBD_GetRxCount - * returns the received data length - * @param pdev: device instance - * @param ep_addr: endpoint address - * @retval Rx Data blength - */ -uint32_t USBD_GetRxCount(USBD_HandleTypeDef *pdev, uint8_t ep_addr) -{ - return USBD_LL_GetRxDataSize(pdev, ep_addr); -} - -/** - * @} - */ - - -/** - * @} - */ - - -/** - * @} - */ - diff --git a/legacy/stm32/platformio.ini b/legacy/stm32/platformio.ini deleted file mode 100644 index 3d7cedc..0000000 --- a/legacy/stm32/platformio.ini +++ /dev/null @@ -1,19 +0,0 @@ -[env:f722] -platform = ststm32 -board = nucleo_f722ze -framework = stm32cube -upload_protocol = dfu -upload_command = /opt/homebrew/bin/dfu-util -a 0 -s 0x08000000:leave -D $SOURCE -monitor_speed = 115200 -board_build.mcu = stm32f722ret6 -board_build.f_cpu = 216000000L -build_flags = - -DESP32xx - -DUSE_HAL_DRIVER - -DHSE_VALUE=8000000U - -DUSE_USB_FS - -I include - -Os - -Wl,--defsym,_Min_Heap_Size=0x2000 - -Wl,--defsym,_Min_Stack_Size=0x1000 - -Wl,--defsym,__stack_end=_estack-0x1000 diff --git a/legacy/stm32/scripts/flash_firmware.py b/legacy/stm32/scripts/flash_firmware.py deleted file mode 100644 index 26f0938..0000000 --- a/legacy/stm32/scripts/flash_firmware.py +++ /dev/null @@ -1,238 +0,0 @@ -#!/usr/bin/env python3 -"""SaltyLab Firmware OTA Flash Script — Issue #124 - -Flashes firmware via USB DFU using dfu-util. -Supports CRC32 integrity verification and host-side backup/rollback. - -Usage: - python flash_firmware.py firmware.bin [options] - python flash_firmware.py --rollback - python flash_firmware.py firmware.bin --trigger-dfu /dev/ttyUSB0 - -Options: - --vid HEX USB vendor ID (default: 0x0483 — STMicroelectronics) - --pid HEX USB product ID (default: 0xDF11 — DFU mode) - --alt N DFU alt setting (default: 0 — internal flash) - --rollback Flash the previous firmware backup - --trigger-dfu PORT Send DFU_ENTER over JLink UART before flashing - --dry-run Print dfu-util command but do not execute - -Requirements: - pip install pyserial (only if using --trigger-dfu) - dfu-util >= 0.9 installed and in PATH - -Dual-bank note: - ESP32 has single-bank 512 KB flash; hardware A/B rollback is not - supported. Rollback is implemented here by saving a backup of the - previous binary (.firmware_backup.bin) before each flash. -""" - -import argparse -import binascii -import os -import shutil -import struct -import subprocess -import sys -import time - -# ---- ESP32 flash constants ---- -FLASH_BASE = 0x08000000 -FLASH_SIZE = 0x80000 # 512 KB - -# ---- DFU device defaults (ESP32/STM32 system bootloader) ---- -DFU_VID = 0x0483 # STMicroelectronics -DFU_PID = 0xDF11 # DFU mode - -BACKUP_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), - '.firmware_backup.bin') - - -# ---- CRC utilities ---- - -def crc32_file(path: str) -> int: - """ - Compute CRC-32/ISO-HDLC (standard Python binascii.crc32) of a file. - Used for pre-flash integrity verification; consistent across runs. - """ - with open(path, 'rb') as fh: - data = fh.read() - return binascii.crc32(data) & 0xFFFFFFFF - - -def stm32_crc32(data: bytes) -> int: - """ - Compute CRC-32/MPEG-2 matching ESP32 hardware CRC unit. - - ESP32/STM32 algorithm: - Polynomial : 0x04C11DB7 - Initial : 0xFFFFFFFF - Width : 32 bits - Reflection : none (MSB-first) - Feed size : 32-bit words from flash (little-endian CPU read) - - When the ESP32 BALANCE reads a flash word it gets a little-endian uint32; - the hardware CRC unit processes bits[31:24] first, then [23:16], - [15:8], [7:0]. This Python implementation replicates that behaviour. - - data should be padded to a 4-byte boundary with 0xFF before calling. - """ - if len(data) % 4: - data += b'\xff' * (4 - len(data) % 4) - - crc = 0xFFFFFFFF - for i in range(0, len(data), 4): - # Little-endian: byte0 is LSB, byte3 is MSB of the 32-bit word - word = struct.unpack_from(' int: - """CRC-16/XMODEM (poly 0x1021, init 0x0000) — JLink frame CRC.""" - crc = 0x0000 - for b in data: - crc ^= b << 8 - for _ in range(8): - if crc & 0x8000: - crc = ((crc << 1) ^ 0x1021) & 0xFFFF - else: - crc = (crc << 1) & 0xFFFF - return crc - - -def _build_jlink_frame(cmd: int, payload: bytes = b'') -> bytes: - """Build a JLink binary frame: [STX][LEN][CMD][PAYLOAD][CRC_hi][CRC_lo][ETX].""" - STX, ETX = 0x02, 0x03 - body = bytes([cmd]) + payload - length = len(body) - crc = _crc16_xmodem(body) - return bytes([STX, length, cmd]) + payload + bytes([crc >> 8, crc & 0xFF, ETX]) - - -def trigger_dfu_via_jlink(port: str, baud: int = 921600) -> None: - """Send JLINK_CMD_DFU_ENTER (0x06) over USART1 to put device in DFU mode.""" - try: - import serial - except ImportError: - print("ERROR: pyserial not installed. Run: pip install pyserial", - file=sys.stderr) - sys.exit(1) - - frame = _build_jlink_frame(0x06) # JLINK_CMD_DFU_ENTER, no payload - with serial.Serial(port, baud, timeout=2) as ser: - ser.write(frame) - time.sleep(0.1) - print(f"DFU_ENTER sent to {port} ({len(frame)} bytes)") - - -# ---- Flash ---- - -def flash(bin_path: str, vid: int, pid: int, alt: int = 0, - dry_run: bool = False) -> int: - """ - Flash firmware using dfu-util. Returns the process exit code. - - Uses --dfuse-address with :leave to reset into application after flash. - """ - addr = f'0x{FLASH_BASE:08X}' - cmd = [ - 'dfu-util', - '--device', f'{vid:04x}:{pid:04x}', - '--alt', str(alt), - '--dfuse-address', f'{addr}:leave', - '--download', bin_path, - ] - print('Running:', ' '.join(cmd)) - if dry_run: - print('[dry-run] skipping dfu-util execution') - return 0 - return subprocess.call(cmd) - - -# ---- Main ---- - -def main() -> int: - parser = argparse.ArgumentParser( - description='SaltyLab firmware OTA flash via USB DFU (Issue #124)' - ) - parser.add_argument('firmware', nargs='?', - help='Firmware .bin file to flash') - parser.add_argument('--vid', type=lambda x: int(x, 0), default=DFU_VID, - help=f'USB vendor ID (default: 0x{DFU_VID:04X})') - parser.add_argument('--pid', type=lambda x: int(x, 0), default=DFU_PID, - help=f'USB product ID (default: 0x{DFU_PID:04X})') - parser.add_argument('--alt', type=int, default=0, - help='DFU alt setting (default: 0 — internal flash)') - parser.add_argument('--rollback', action='store_true', - help='Flash the previous firmware backup') - parser.add_argument('--trigger-dfu', metavar='PORT', - help='Trigger DFU via JLink UART before flashing ' - '(e.g. /dev/ttyUSB0 or COM3)') - parser.add_argument('--dry-run', action='store_true', - help='Print dfu-util command without executing it') - args = parser.parse_args() - - # Optionally trigger DFU mode over JLink serial - if args.trigger_dfu: - trigger_dfu_via_jlink(args.trigger_dfu) - print('Waiting 3 s for USB DFU enumeration…') - time.sleep(3) - - # Determine target binary - if args.rollback: - if not os.path.exists(BACKUP_PATH): - print(f'ERROR: No backup found at {BACKUP_PATH}', file=sys.stderr) - return 1 - target = BACKUP_PATH - print(f'Rolling back to {BACKUP_PATH}') - elif args.firmware: - target = args.firmware - else: - parser.print_help() - return 1 - - if not os.path.exists(target): - print(f'ERROR: File not found: {target}', file=sys.stderr) - return 1 - - # CRC32 integrity check - crc_std = crc32_file(target) - size = os.path.getsize(target) - print(f'Binary : {target} ({size} bytes)') - print(f'CRC-32 : 0x{crc_std:08X} (ISO-HDLC)') - - if size > FLASH_SIZE: - print(f'ERROR: Binary ({size} bytes) exceeds flash size ' - f'({FLASH_SIZE} bytes)', file=sys.stderr) - return 1 - - # ESP32/STM32 hardware CRC (for cross-checking with firmware telemetry) - with open(target, 'rb') as fh: - bin_data = fh.read() - crc_hw = stm32_crc32(bin_data.ljust(FLASH_SIZE, b'\xff')) - print(f'CRC-32 : 0x{crc_hw:08X} (MPEG-2 / ESP32/STM32 HW, padded to {FLASH_SIZE // 1024} KB)') - - # Save backup before flashing (skip when rolling back) - if not args.rollback: - shutil.copy2(target, BACKUP_PATH) - print(f'Backup : {BACKUP_PATH}') - - # Flash - rc = flash(target, args.vid, args.pid, args.alt, args.dry_run) - if rc == 0: - print('Flash complete — device should reset into application.') - else: - print(f'ERROR: dfu-util exited with code {rc}', file=sys.stderr) - return rc - - -if __name__ == '__main__': - sys.exit(main()) diff --git a/legacy/stm32/src/audio.c b/legacy/stm32/src/audio.c deleted file mode 100644 index 053e889..0000000 --- a/legacy/stm32/src/audio.c +++ /dev/null @@ -1,352 +0,0 @@ -#include "audio.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include - -/* ================================================================ - * Buffer layout - * ================================================================ - * AUDIO_BUF_HALF samples per DMA half (config.h: 441 at 22050 Hz → 20 ms). - * DMA runs in circular mode over 2 halves; TxHalfCplt refills [0] and - * TxCplt refills [AUDIO_BUF_HALF]. Both callbacks simply call fill_half(). - */ -#define AUDIO_BUF_SIZE (AUDIO_BUF_HALF * 2u) - -/* DMA buffer: must be in non-cached SRAM or flushed before DMA access. - * Placed in SRAM1 (default .bss section on STM32F722 — below 512 KB). - * The I-cache / D-cache is on by default; DMA1 is an AHB master that - * bypasses cache, so we keep this buffer in DTCM-uncacheable SRAM. */ -static int16_t s_dma_buf[AUDIO_BUF_SIZE]; - -/* ================================================================ - * PCM FIFO — Jetson audio (main-loop writer, ISR reader) - * ================================================================ - * Single-producer / single-consumer lock-free ring buffer. - * Power-of-2 size so wrap uses bitwise AND (safe across ISR boundary). - * 4096 samples ≈ 185 ms at 22050 Hz — enough to absorb JLink jitter. - */ -#define PCM_FIFO_SIZE 4096u -#define PCM_FIFO_MASK (PCM_FIFO_SIZE - 1u) - -static int16_t s_pcm_fifo[PCM_FIFO_SIZE]; -static volatile uint16_t s_pcm_rd = 0; /* consumer (ISR advances) */ -static volatile uint16_t s_pcm_wr = 0; /* producer (main loop advances) */ - -/* ================================================================ - * Tone sequencer - * ================================================================ - * Each AudioTone maps to a const ToneStep array. Steps are played - * sequentially; a gap_ms of 0 means no silence between steps. - * audio_tick() in the main loop advances the state machine and - * writes the volatile active-tone params read by the ISR fill path. - */ -typedef struct { - uint16_t freq_hz; - uint16_t dur_ms; - uint16_t gap_ms; /* silence after this step */ -} ToneStep; - -/* ---- Tone definitions ---- */ -static const ToneStep s_def_beep_short[] = {{880, 100, 0}}; -static const ToneStep s_def_beep_long[] = {{880, 500, 0}}; -static const ToneStep s_def_startup[] = {{523, 120, 60}, - {659, 120, 60}, - {784, 200, 0}}; -static const ToneStep s_def_arm[] = {{880, 80, 60}, - {1047, 100, 0}}; -static const ToneStep s_def_disarm[] = {{880, 80, 60}, - {659, 100, 0}}; -static const ToneStep s_def_fault[] = {{200, 500, 0}}; - -typedef struct { - const ToneStep *steps; - uint8_t n_steps; -} ToneDef; - -static const ToneDef s_tone_defs[AUDIO_TONE_COUNT] = { - [AUDIO_TONE_BEEP_SHORT] = {s_def_beep_short, 1}, - [AUDIO_TONE_BEEP_LONG] = {s_def_beep_long, 1}, - [AUDIO_TONE_STARTUP] = {s_def_startup, 3}, - [AUDIO_TONE_ARM] = {s_def_arm, 2}, - [AUDIO_TONE_DISARM] = {s_def_disarm, 2}, - [AUDIO_TONE_FAULT] = {s_def_fault, 1}, -}; - -/* Active tone queue */ -#define TONE_QUEUE_DEPTH 4u - -typedef struct { - const ToneDef *def; - uint8_t step; /* current step index within def */ - bool in_gap; /* true while playing inter-step silence */ - uint32_t step_end_ms; /* abs HAL_GetTick() when step/gap expires */ -} ToneSeq; - -static ToneSeq s_tone_q[TONE_QUEUE_DEPTH]; -static uint8_t s_tq_head = 0; /* consumer (audio_tick reads) */ -static uint8_t s_tq_tail = 0; /* producer (audio_play_tone writes) */ - -/* Volatile parameters written by audio_tick(), read by ISR fill_half() */ -static volatile uint16_t s_active_freq = 0; /* 0 = silence / gap */ -static volatile uint32_t s_active_phase = 0; /* sample phase counter */ - -/* ================================================================ - * Volume - * ================================================================ */ -static uint8_t s_volume = AUDIO_VOLUME_DEFAULT; /* 0–100 */ - -/* ================================================================ - * HAL handles - * ================================================================ */ -static I2S_HandleTypeDef s_i2s; -static DMA_HandleTypeDef s_dma_tx; - -/* ================================================================ - * fill_half() — called from ISR, O(AUDIO_BUF_HALF) - * ================================================================ - * Priority: PCM FIFO (Jetson TTS) > square-wave tone > silence. - * Volume applied via integer scaling — no float in ISR. - */ -static void fill_half(int16_t *buf, uint16_t n) -{ - uint16_t rd = s_pcm_rd; - uint16_t wr = s_pcm_wr; - uint16_t avail = (uint16_t)((wr - rd) & PCM_FIFO_MASK); - - if (avail >= n) { - /* ---- Drain Jetson PCM FIFO ---- */ - for (uint16_t i = 0; i < n; i++) { - int32_t s = (int32_t)s_pcm_fifo[rd] * (int32_t)s_volume / 100; - buf[i] = (s > 32767) ? (int16_t)32767 : - (s < -32768) ? (int16_t)-32768 : (int16_t)s; - rd = (rd + 1u) & PCM_FIFO_MASK; - } - s_pcm_rd = rd; - - } else if (s_active_freq) { - /* ---- Square wave tone generator ---- */ - uint32_t half_p = (uint32_t)AUDIO_SAMPLE_RATE / (2u * s_active_freq); - int16_t amp = (int16_t)(16384u * (uint32_t)s_volume / 100u); - uint32_t ph = s_active_phase; - uint32_t period = 2u * half_p; - for (uint16_t i = 0; i < n; i++) { - buf[i] = ((ph % period) < half_p) ? amp : (int16_t)(-amp); - ph++; - } - s_active_phase = ph; - - } else { - /* ---- Silence ---- */ - memset(buf, 0, (size_t)(n * 2u)); - } -} - -/* ================================================================ - * DMA callbacks (ISR context) - * ================================================================ */ -void HAL_I2S_TxHalfCpltCallback(I2S_HandleTypeDef *hi2s) -{ - if (hi2s->Instance == SPI3) - fill_half(s_dma_buf, AUDIO_BUF_HALF); -} - -void HAL_I2S_TxCpltCallback(I2S_HandleTypeDef *hi2s) -{ - if (hi2s->Instance == SPI3) - fill_half(s_dma_buf + AUDIO_BUF_HALF, AUDIO_BUF_HALF); -} - -/* DMA1 Stream7 IRQ (SPI3/I2S3 TX) */ -void DMA1_Stream7_IRQHandler(void) -{ - HAL_DMA_IRQHandler(&s_dma_tx); -} - -/* ================================================================ - * audio_init() - * ================================================================ */ -void audio_init(void) -{ - /* ---- PLLI2S: N=192, R=2 → 96 MHz I2S clock ---- - * With SPI3/I2S3 and I2S_DATAFORMAT_16B (16-bit, 32-bit frame slot): - * FS = 96 MHz / (32 × 2 × I2SDIV) where HAL picks I2SDIV = 68 - * → 96 000 000 / (32 × 2 × 68) = 22 058 Hz (< 0.04 % error) - */ - RCC_PeriphCLKInitTypeDef pclk = {0}; - pclk.PeriphClockSelection = RCC_PERIPHCLK_I2S; - pclk.I2sClockSelection = RCC_I2SCLKSOURCE_PLLI2S; - pclk.PLLI2S.PLLI2SN = 192; /* VCO = (HSE/PLLM) × 192 = 192 MHz */ - pclk.PLLI2S.PLLI2SR = 2; /* I2S clock = 96 MHz */ - HAL_RCCEx_PeriphCLKConfig(&pclk); - - /* ---- GPIO ---- */ - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); - - GPIO_InitTypeDef gpio = {0}; - - /* PA15: I2S3_WS (LRCLK), AF6 */ - gpio.Pin = AUDIO_LRCK_PIN; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - gpio.Alternate = GPIO_AF6_SPI3; - HAL_GPIO_Init(AUDIO_LRCK_PORT, &gpio); - - /* PC10: I2S3_CK (BCLK), AF6 */ - gpio.Pin = AUDIO_BCLK_PIN; - HAL_GPIO_Init(AUDIO_BCLK_PORT, &gpio); - - /* PB5: I2S3_SD (DIN), AF6 */ - gpio.Pin = AUDIO_DOUT_PIN; - HAL_GPIO_Init(AUDIO_DOUT_PORT, &gpio); - - /* PC5: AUDIO_MUTE GPIO output — drive low (muted) initially */ - gpio.Pin = AUDIO_MUTE_PIN; - gpio.Mode = GPIO_MODE_OUTPUT_PP; - gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_LOW; - gpio.Alternate = 0; - HAL_GPIO_Init(AUDIO_MUTE_PORT, &gpio); - HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_RESET); - - /* ---- DMA1 Stream7 Channel0 (SPI3/I2S3 TX) ---- */ - __HAL_RCC_DMA1_CLK_ENABLE(); - s_dma_tx.Instance = DMA1_Stream7; - s_dma_tx.Init.Channel = DMA_CHANNEL_0; - s_dma_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; - s_dma_tx.Init.PeriphInc = DMA_PINC_DISABLE; - s_dma_tx.Init.MemInc = DMA_MINC_ENABLE; - s_dma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; - s_dma_tx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; - s_dma_tx.Init.Mode = DMA_CIRCULAR; - s_dma_tx.Init.Priority = DMA_PRIORITY_MEDIUM; - s_dma_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - HAL_DMA_Init(&s_dma_tx); - __HAL_LINKDMA(&s_i2s, hdmatx, s_dma_tx); - - HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 7, 0); - HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn); - - /* ---- SPI3 in I2S3 master TX mode ---- */ - __HAL_RCC_SPI3_CLK_ENABLE(); - s_i2s.Instance = SPI3; - s_i2s.Init.Mode = I2S_MODE_MASTER_TX; - s_i2s.Init.Standard = I2S_STANDARD_PHILIPS; - s_i2s.Init.DataFormat = I2S_DATAFORMAT_16B; - s_i2s.Init.MCLKOutput = I2S_MCLKOUTPUT_DISABLE; - s_i2s.Init.AudioFreq = I2S_AUDIOFREQ_22K; - s_i2s.Init.CPOL = I2S_CPOL_LOW; - s_i2s.Init.ClockSource = I2S_CLOCK_PLL; - HAL_I2S_Init(&s_i2s); - - /* Pre-fill with silence and start circular DMA TX */ - memset(s_dma_buf, 0, sizeof(s_dma_buf)); - HAL_I2S_Transmit_DMA(&s_i2s, (uint16_t *)s_dma_buf, AUDIO_BUF_SIZE); - - /* Unmute amp after DMA is running — avoids start-up click */ - HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_SET); -} - -/* ================================================================ - * Public API - * ================================================================ */ - -void audio_mute(bool active) -{ - HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, - active ? GPIO_PIN_SET : GPIO_PIN_RESET); -} - -void audio_set_volume(uint8_t vol) -{ - s_volume = (vol > 100u) ? 100u : vol; -} - -bool audio_play_tone(AudioTone tone) -{ - if (tone >= AUDIO_TONE_COUNT) return false; - - uint8_t next = (s_tq_tail + 1u) % TONE_QUEUE_DEPTH; - if (next == s_tq_head) return false; /* queue full */ - - s_tone_q[s_tq_tail].def = &s_tone_defs[tone]; - s_tone_q[s_tq_tail].step = 0; - s_tone_q[s_tq_tail].in_gap = false; - s_tone_q[s_tq_tail].step_end_ms = 0; /* audio_tick() sets this on first run */ - s_tq_tail = next; - return true; -} - -uint16_t audio_write_pcm(const int16_t *samples, uint16_t n) -{ - uint16_t wr = s_pcm_wr; - uint16_t rd = s_pcm_rd; - uint16_t space = (uint16_t)((rd - wr - 1u) & PCM_FIFO_MASK); - uint16_t accept = (n < space) ? n : space; - - for (uint16_t i = 0; i < accept; i++) { - s_pcm_fifo[wr] = samples[i]; - wr = (wr + 1u) & PCM_FIFO_MASK; - } - s_pcm_wr = wr; - return accept; -} - -void audio_tick(uint32_t now_ms) -{ - /* Nothing to do if queue is empty */ - if (s_tq_head == s_tq_tail) { - s_active_freq = 0; - return; - } - - ToneSeq *seq = &s_tone_q[s_tq_head]; - - /* First call for this sequence entry: arm the first step */ - if (seq->step_end_ms == 0u) { - const ToneStep *st = &seq->def->steps[0]; - seq->in_gap = false; - seq->step_end_ms = now_ms + st->dur_ms; - s_active_freq = st->freq_hz; - s_active_phase = 0; - return; - } - - /* Step / gap still running */ - if (now_ms < seq->step_end_ms) return; - - /* Current step or gap has expired */ - const ToneStep *st = &seq->def->steps[seq->step]; - - if (!seq->in_gap && st->gap_ms) { - /* Transition: tone → inter-step gap (silence) */ - seq->in_gap = true; - seq->step_end_ms = now_ms + st->gap_ms; - s_active_freq = 0; - return; - } - - /* Advance to next step */ - seq->step++; - seq->in_gap = false; - - if (seq->step >= seq->def->n_steps) { - /* Sequence complete — pop from queue */ - s_tq_head = (s_tq_head + 1u) % TONE_QUEUE_DEPTH; - s_active_freq = 0; - return; - } - - /* Start next step */ - st = &seq->def->steps[seq->step]; - seq->step_end_ms = now_ms + st->dur_ms; - s_active_freq = st->freq_hz; - s_active_phase = 0; -} - -bool audio_is_playing(void) -{ - return (s_i2s.State == HAL_I2S_STATE_BUSY_TX); -} diff --git a/legacy/stm32/src/balance.c b/legacy/stm32/src/balance.c deleted file mode 100644 index 57ea0bf..0000000 --- a/legacy/stm32/src/balance.c +++ /dev/null @@ -1,121 +0,0 @@ -#include "balance.h" -#include "slope_estimator.h" -#include "config.h" -#include - -void balance_init(balance_t *b) { - b->state = BALANCE_DISARMED; - b->pitch_deg = 0.0f; - b->pitch_rate = 0.0f; - b->integral = 0.0f; - b->prev_error = 0.0f; - b->motor_cmd = 0; - - /* Default PID — conservative starting point */ - b->kp = PID_KP; - b->ki = PID_KI; - b->kd = PID_KD; - b->setpoint = 0.0f; - - /* Safety defaults from config */ - b->max_tilt = MAX_TILT_DEG; - b->max_speed = MAX_SPEED_LIMIT; - - /* Slope estimator */ - slope_estimator_init(&b->slope); -} - -void balance_update(balance_t *b, const IMUData *imu, float dt) { - if (dt <= 0.0f || dt > 0.1f) return; /* Sanity check dt */ - - /* Consume fused angle from mpu6000 complementary filter */ - b->pitch_deg = imu->pitch; - b->pitch_rate = imu->pitch_rate; - - /* Advance slope estimator (no-op when not armed) */ - slope_estimator_update(&b->slope, b->pitch_deg, dt); - - /* Safety: tilt cutoff */ - if (b->state == BALANCE_ARMED) { - if (fabsf(b->pitch_deg) > b->max_tilt) { - b->state = BALANCE_TILT_FAULT; - b->motor_cmd = 0; - b->integral = 0.0f; - slope_estimator_reset(&b->slope); - return; - } - } - - if (b->state != BALANCE_ARMED) { - b->motor_cmd = 0; - b->integral = 0.0f; - b->prev_error = 0.0f; - return; - } - - /* PID — subtract slope estimate so controller balances on incline */ - float tilt_corrected = b->pitch_deg - slope_estimator_get_deg(&b->slope); - float error = b->setpoint - tilt_corrected; - - /* Proportional */ - float p_term = b->kp * error; - - /* Integral with anti-windup */ - b->integral += error * dt; - if (b->integral > PID_INTEGRAL_MAX) b->integral = PID_INTEGRAL_MAX; - if (b->integral < -PID_INTEGRAL_MAX) b->integral = -PID_INTEGRAL_MAX; - float i_term = b->ki * b->integral; - - /* Derivative on measurement (avoids setpoint kick) */ - float d_term = b->kd * (-b->pitch_rate); - - /* Sum and clamp */ - float output = p_term + i_term + d_term; - if (output > (float)b->max_speed) output = (float)b->max_speed; - if (output < -(float)b->max_speed) output = -(float)b->max_speed; - - b->motor_cmd = (int16_t)output; - b->prev_error = error; -} - -void balance_arm(balance_t *b) { - if (b->state == BALANCE_DISARMED) { - /* Only arm if roughly upright */ - if (fabsf(b->pitch_deg) < 10.0f) { - b->integral = 0.0f; - b->prev_error = 0.0f; - b->motor_cmd = 0; - b->state = BALANCE_ARMED; - } - } -} - -void balance_disarm(balance_t *b) { - b->state = BALANCE_DISARMED; - b->motor_cmd = 0; - b->integral = 0.0f; - slope_estimator_reset(&b->slope); -} - -void balance_park(balance_t *b) { - /* Suspend balancing from ARMED state only — keeps robot stationary on flat ground */ - if (b->state == BALANCE_ARMED) { - b->state = BALANCE_PARKED; - b->motor_cmd = 0; - b->integral = 0.0f; - b->prev_error = 0.0f; - slope_estimator_reset(&b->slope); - } -} - -void balance_unpark(balance_t *b) { - /* Quick re-arm from PARKED — only if pitch is safe (< 20 deg) */ - if (b->state == BALANCE_PARKED) { - if (fabsf(b->pitch_deg) < 20.0f) { - b->motor_cmd = 0; - b->prev_error = 0.0f; - b->state = BALANCE_ARMED; - } - /* If pitch too large, stay PARKED — caller checks resulting state */ - } -} diff --git a/legacy/stm32/src/baro.c b/legacy/stm32/src/baro.c deleted file mode 100644 index 3a401a6..0000000 --- a/legacy/stm32/src/baro.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * baro.c — BME280/BMP280 barometric pressure & ambient temperature module - * (Issue #672). - * - * Reads pressure, temperature, and (on BME280) humidity from the sensor at - * BARO_READ_HZ (1 Hz). Computes pressure altitude using bmp280_pressure_to_alt_cm() - * (ISA barometric formula, p0 = 101325 Pa). Publishes JLINK_TLM_BARO (0x8D) - * telemetry to the Orin at BARO_TLM_HZ (1 Hz). - * - * Runs entirely on the Mamba F722S. No Orin dependency. - * baro_get_alt_cm() exposes altitude for slope compensation in the balance PID. - */ - -#include "baro.h" -#include "bmp280.h" -#include "jlink.h" - -static int s_chip_id = 0; /* 0x58=BMP280, 0x60=BME280, 0=absent */ -static baro_data_t s_data; /* latest reading */ -static uint32_t s_last_read_ms; /* timestamp of last I2C read */ -static uint32_t s_last_tlm_ms; /* timestamp of last telemetry TX */ - -/* ---- baro_init() ---- */ -void baro_init(int chip_id) -{ - s_chip_id = chip_id; - s_data.pressure_pa = 0; - s_data.temp_x10 = 0; - s_data.alt_cm = 0; - s_data.humidity_pct_x10 = -1; - s_data.valid = false; - - /* - * Initialise timestamps so the first baro_tick() call fires immediately - * (same convention as slope_estimator_init and steering_pid_init). - */ - const uint32_t interval_ms = 1000u / BARO_READ_HZ; - s_last_read_ms = (uint32_t)(-(uint32_t)interval_ms); - s_last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / BARO_TLM_HZ)); -} - -/* ---- baro_tick() ---- */ -void baro_tick(uint32_t now_ms) -{ - if (s_chip_id == 0) return; - - const uint32_t read_interval_ms = 1000u / BARO_READ_HZ; - if ((now_ms - s_last_read_ms) < read_interval_ms) return; - s_last_read_ms = now_ms; - - /* Read pressure (Pa) and temperature (°C × 10) */ - bmp280_read(&s_data.pressure_pa, &s_data.temp_x10); - - /* Compute pressure altitude: ISA formula, p0 = 101325 Pa */ - s_data.alt_cm = bmp280_pressure_to_alt_cm(s_data.pressure_pa); - - /* Humidity: BME280 (0x60) only; BMP280 returns -1 */ - s_data.humidity_pct_x10 = (s_chip_id == 0x60) - ? bmp280_read_humidity() - : (int16_t)-1; - - s_data.valid = true; - - /* Publish telemetry to Orin via JLink (JLINK_TLM_BARO = 0x8D) */ - const uint32_t tlm_interval_ms = 1000u / BARO_TLM_HZ; - if ((now_ms - s_last_tlm_ms) >= tlm_interval_ms) { - s_last_tlm_ms = now_ms; - - jlink_tlm_baro_t tlm; - tlm.pressure_pa = s_data.pressure_pa; - tlm.temp_x10 = s_data.temp_x10; - tlm.alt_cm = s_data.alt_cm; - tlm.humidity_pct_x10 = s_data.humidity_pct_x10; - jlink_send_baro_tlm(&tlm); - } -} - -/* ---- baro_get() ---- */ -bool baro_get(baro_data_t *out) -{ - if (!s_data.valid) return false; - *out = s_data; - return true; -} - -/* ---- baro_get_alt_cm() ---- */ -int32_t baro_get_alt_cm(void) -{ - return s_data.valid ? s_data.alt_cm : 0; -} diff --git a/legacy/stm32/src/battery.c b/legacy/stm32/src/battery.c deleted file mode 100644 index 680a5d1..0000000 --- a/legacy/stm32/src/battery.c +++ /dev/null @@ -1,131 +0,0 @@ -/* - * battery.c — Vbat ADC reading for CRSF telemetry uplink (Issue #103) - * - * Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722S FC). - * Voltage divider: 10 kΩ (upper) / 1 kΩ (lower) → VBAT_SCALE_NUM = 11. - * - * Vbat_mV = (raw × VBAT_AREF_MV × VBAT_SCALE_NUM) >> VBAT_ADC_BITS - * = (raw × 3300 × 11) / 4096 - */ - -#include "battery.h" -#include "coulomb_counter.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include "ina219.h" -#include - -static ADC_HandleTypeDef s_hadc; -static bool s_ready = false; -static bool s_coulomb_valid = false; - -/* Default battery capacity: 2200 mAh (typical lab 3S LiPo) */ -#define DEFAULT_BATTERY_CAPACITY_MAH 2200u - -void battery_init(void) { - __HAL_RCC_ADC3_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); - - /* PC1 → analog input (no pull, no speed) */ - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_1; - gpio.Mode = GPIO_MODE_ANALOG; - gpio.Pull = GPIO_NOPULL; - HAL_GPIO_Init(GPIOC, &gpio); - - /* ADC3 — single-conversion, software trigger, 12-bit right-aligned */ - s_hadc.Instance = ADC3; - s_hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; /* APB2/8 */ - s_hadc.Init.Resolution = ADC_RESOLUTION_12B; - s_hadc.Init.ScanConvMode = DISABLE; - s_hadc.Init.ContinuousConvMode = DISABLE; - s_hadc.Init.DiscontinuousConvMode = DISABLE; - s_hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - s_hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; - s_hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - s_hadc.Init.NbrOfConversion = 1; - s_hadc.Init.DMAContinuousRequests = DISABLE; - s_hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; - if (HAL_ADC_Init(&s_hadc) != HAL_OK) return; - - /* Channel IN11 (PC1) with 480-cycle sampling for stability */ - ADC_ChannelConfTypeDef ch = {0}; - ch.Channel = ADC_CHANNEL_11; - ch.Rank = 1; - ch.SamplingTime = ADC_SAMPLETIME_480CYCLES; - if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return; - - /* Initialize coulomb counter with default battery capacity */ - coulomb_counter_init(DEFAULT_BATTERY_CAPACITY_MAH); - s_coulomb_valid = true; - - s_ready = true; -} - -uint32_t battery_read_mv(void) { - if (!s_ready) return 0u; - - HAL_ADC_Start(&s_hadc); - if (HAL_ADC_PollForConversion(&s_hadc, 2u) != HAL_OK) return 0u; - uint32_t raw = HAL_ADC_GetValue(&s_hadc); - HAL_ADC_Stop(&s_hadc); - - /* Vbat_mV = raw × (VREF_mV × scale) / ADC_counts */ - return (raw * (uint32_t)VBAT_AREF_MV * VBAT_SCALE_NUM) / - ((1u << VBAT_ADC_BITS)); -} - -/* - * Coarse SoC estimate (voltage-based fallback). - * 3S LiPo: 9.9 V (0%) – 12.6 V (100%) — detect by Vbat < 13 V - * 4S LiPo: 13.2 V (0%) – 16.8 V (100%) — detect by Vbat ≥ 13 V - */ -uint8_t battery_estimate_pct(uint32_t voltage_mv) { - uint32_t v_min_mv, v_max_mv; - - if (voltage_mv >= 13000u) { - /* 4S LiPo */ - v_min_mv = 13200u; - v_max_mv = 16800u; - } else { - /* 3S LiPo */ - v_min_mv = 9900u; - v_max_mv = 12600u; - } - - if (voltage_mv <= v_min_mv) return 0u; - if (voltage_mv >= v_max_mv) return 100u; - - return (uint8_t)(((voltage_mv - v_min_mv) * 100u) / (v_max_mv - v_min_mv)); -} - -/* - * battery_accumulate_coulombs() — call periodically (50-100 Hz) to track - * battery current and integrate coulombs. Reads motor currents via INA219. - */ -void battery_accumulate_coulombs(void) { - if (!s_coulomb_valid) return; - - /* Sum left + right motor currents as proxy for battery draw - * (simple approach; doesn't include subsystem drain like OSD, audio) */ - int16_t left_ma = 0, right_ma = 0; - ina219_read_current_ma(INA219_LEFT_MOTOR, &left_ma); - ina219_read_current_ma(INA219_RIGHT_MOTOR, &right_ma); - - /* Total battery current ≈ motors + subsystem baseline (~200 mA) */ - int16_t total_ma = left_ma + right_ma + 200; - - /* Accumulate to coulomb counter */ - coulomb_counter_accumulate(total_ma); -} - -/* - * battery_get_soc_coulomb() — get coulomb-based SoC (0-100, 255=invalid). - * Preferred over voltage-based when available. - */ -uint8_t battery_get_soc_coulomb(void) { - if (!s_coulomb_valid || !coulomb_counter_is_valid()) { - return 255; /* Invalid */ - } - return coulomb_counter_get_soc_pct(); -} diff --git a/legacy/stm32/src/battery_adc.c b/legacy/stm32/src/battery_adc.c deleted file mode 100644 index be37a31..0000000 --- a/legacy/stm32/src/battery_adc.c +++ /dev/null @@ -1,394 +0,0 @@ -/* - * battery_adc.c — DMA-based battery voltage/current ADC driver (Issue #533) - * - * Voltage divider calibration, DMA-based continuous sampling, IIR low-pass - * filter, and USART telemetry publish to Jetson via jlink (JLINK_TLM_BATTERY). - * Integrates with power_mgmt for low-battery sleep (Issue #467). - * - * DMA mapping (STM32F7 RM Table 27): - * DMA2_Stream0 Channel2 → ADC3 (no conflict: Stream2/Ch4 = USART1_RX) - * - * ADC3 scan sequence: - * Rank 1: IN11 (PC1) — Vbat - * Rank 2: IN13 (PC3) — Ibat - * Continuous mode, 480-cycle sampling → ~14 kSPS per channel @ 13.5 MHz ADC - * - * DMA circular buffer layout (8 × uint16_t, pairs repeat continuously): - * [vbat0, ibat0, vbat1, ibat1, vbat2, ibat2, vbat3, ibat3] - * - * On each battery_adc_tick() call the 4 Vbat and 4 Ibat samples are averaged - * (4× hardware oversampling) then fed into an IIR low-pass filter. - */ - -#include "battery_adc.h" -#include "jlink.h" -#include "power_mgmt.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include - -/* ---- DMA buffer ---- */ -#define ADC_DMA_BUF_LEN 8u /* 4 Vbat/Ibat pairs */ -#define ADC_OVERSAMPLE 4u /* pairs per tick average */ - -static volatile uint16_t s_dma_buf[ADC_DMA_BUF_LEN]; - -/* ---- HAL handles ---- */ -static ADC_HandleTypeDef s_hadc; -static DMA_HandleTypeDef s_hdma; - -/* ---- Filtered state (Q0 integer, units: raw ADC counts × 8 for sub-LSB) ---- */ -/* - * IIR accumulator: s_lpf_vbat = filtered Vbat in (raw_counts × 8). - * Multiply by 8 gives 3 bits of sub-LSB precision with integer arithmetic. - * On each tick: s_lpf_vbat += (raw_avg - (s_lpf_vbat >> 3)) >> LPF_SHIFT - * Simplified: s_lpf_vbat8 = s_lpf_vbat8 + (raw_avg_x8 - s_lpf_vbat8) >> SHIFT - */ -static uint32_t s_lpf_vbat8 = 0; /* Vbat accumulator × 8 */ -static int32_t s_lpf_ibat8 = 0; /* Ibat accumulator × 8 (signed) */ - -/* ---- Calibrated outputs (updated each tick) ---- */ -static uint32_t s_vbat_mv = 0; /* calibrated LPF Vbat (mV) */ -static int32_t s_ibat_ma = 0; /* calibrated LPF Ibat (mA) */ -static uint32_t s_vbat_raw_mv = 0; /* unfiltered last-tick average (mV) */ - -/* ---- Calibration ---- */ -static battery_adc_cal_t s_cal = { - .vbat_offset_mv = 0, - .ibat_offset_ma = 0, - .vbat_scale_num = 0, /* 0 = use VBAT_SCALE_NUM from config.h */ - .vbat_scale_den = 0, /* 0 = use 1 */ -}; - -/* ---- Low-voltage tracking ---- */ -static uint32_t s_low_since_ms = 0; /* tick when Vbat first went low */ -static bool s_low_active = false; /* currently below LOW_MV threshold */ -static bool s_critical_notified = false; /* pm notified for this event */ -static uint32_t s_low_mv = BATTERY_ADC_LOW_MV; -static uint32_t s_critical_mv = BATTERY_ADC_CRITICAL_MV; - -/* ---- Ready flag ---- */ -static bool s_ready = false; - -/* ---- Telemetry rate limiting ---- */ -static uint32_t s_last_publish_ms = 0; -static bool s_published_once = false; /* force send on first call */ - -/* ---- Helper: convert raw ADC count to mV ---- */ -/* - * Apply voltage divider and calibration: - * vbat_mv = (raw × Vref_mV × scale_num) / (4096 × scale_den) - * + cal.vbat_offset_mv - * Default: Vref=3300 mV, scale_num=VBAT_SCALE_NUM=11, scale_den=1. - */ -static uint32_t raw_to_vbat_mv(uint32_t raw_counts) -{ - uint16_t snum = (s_cal.vbat_scale_num != 0u) ? s_cal.vbat_scale_num - : (uint16_t)VBAT_SCALE_NUM; - uint16_t sden = (s_cal.vbat_scale_den != 0u) ? s_cal.vbat_scale_den : 1u; - - /* raw_counts × (3300 × 11) / 4096 — keep 32-bit intermediate */ - uint32_t mv = (raw_counts * (uint32_t)VBAT_AREF_MV * snum) / - ((1u << VBAT_ADC_BITS) * sden); - - int32_t cal_mv = (int32_t)mv + (int32_t)s_cal.vbat_offset_mv; - if (cal_mv < 0) cal_mv = 0; - return (uint32_t)cal_mv; -} - -/* ---- Helper: convert raw ADC count to mA ---- */ -/* - * ADC_IBAT_SCALE = 115 (betaflight ibata_scale units: mA per count × 10). - * Formula: ibat_ma = raw_counts × ADC_IBAT_SCALE / 10 + cal.ibat_offset_ma - * Betaflight ibata_scale is defined as: current = raw * scale / (4096 * 10) - * in units of 100mA per count → ibat_ma = raw × ibata_scale * 100 / 4096 - * - * With ibata_scale=115: ibat_ma = raw × 115 × 100 / 4096 - * ≈ raw × 2.808 mA/count - * For raw=4095 (full-scale): ~11,500 mA (11.5 A) — appropriate for 3S robot. - */ -static int32_t raw_to_ibat_ma(uint32_t raw_counts) -{ - int32_t ma = (int32_t)((raw_counts * (uint32_t)ADC_IBAT_SCALE * 100u) / - (1u << VBAT_ADC_BITS)); - ma += (int32_t)s_cal.ibat_offset_ma; - return ma; -} - -/* ---- battery_adc_init() ---- */ -void battery_adc_init(void) -{ - /* ---- GPIO: PC1 (Vbat) and PC3 (Ibat) as analog input ---- */ - __HAL_RCC_GPIOC_CLK_ENABLE(); - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_1 | GPIO_PIN_3; - gpio.Mode = GPIO_MODE_ANALOG; - gpio.Pull = GPIO_NOPULL; - HAL_GPIO_Init(GPIOC, &gpio); - - /* ---- DMA2_Stream0 Channel2 → ADC3 (circular, 16-bit, 8 words) ---- */ - __HAL_RCC_DMA2_CLK_ENABLE(); - s_hdma.Instance = DMA2_Stream0; - s_hdma.Init.Channel = DMA_CHANNEL_2; - s_hdma.Init.Direction = DMA_PERIPH_TO_MEMORY; - s_hdma.Init.PeriphInc = DMA_PINC_DISABLE; - s_hdma.Init.MemInc = DMA_MINC_ENABLE; - s_hdma.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; - s_hdma.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; - s_hdma.Init.Mode = DMA_CIRCULAR; - s_hdma.Init.Priority = DMA_PRIORITY_LOW; - s_hdma.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - if (HAL_DMA_Init(&s_hdma) != HAL_OK) return; - - /* ---- ADC3: continuous scan, DMA, 12-bit, 2 channels ---- */ - __HAL_RCC_ADC3_CLK_ENABLE(); - s_hadc.Instance = ADC3; - s_hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; /* 108/8 ≈ 13.5 MHz */ - s_hadc.Init.Resolution = ADC_RESOLUTION_12B; - s_hadc.Init.ScanConvMode = ENABLE; /* scan both channels */ - s_hadc.Init.ContinuousConvMode = ENABLE; /* free-running */ - s_hadc.Init.DiscontinuousConvMode = DISABLE; - s_hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - s_hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; - s_hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - s_hadc.Init.NbrOfConversion = 2; - s_hadc.Init.DMAContinuousRequests = ENABLE; /* DMA circular */ - s_hadc.Init.EOCSelection = ADC_EOC_SEQ_CONV; - __HAL_LINKDMA(&s_hadc, DMA_Handle, s_hdma); - if (HAL_ADC_Init(&s_hadc) != HAL_OK) return; - - /* ---- Rank 1: IN11 (PC1) — Vbat ---- */ - ADC_ChannelConfTypeDef ch = {0}; - ch.Channel = ADC_CHANNEL_11; - ch.Rank = 1; - ch.SamplingTime = ADC_SAMPLETIME_480CYCLES; - if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return; - - /* ---- Rank 2: IN13 (PC3) — Ibat ---- */ - ch.Channel = ADC_CHANNEL_13; - ch.Rank = 2; - ch.SamplingTime = ADC_SAMPLETIME_480CYCLES; - if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return; - - /* ---- Detect battery cell count at init for correct thresholds ---- */ - /* - * Note: at init Vbat may not be settled; thresholds are updated after - * first tick once LPF has stabilised. Set 4S thresholds if voltage - * detected ≥ 13 V in battery_adc_tick() on first valid reading. - */ - - /* ---- Start continuous DMA conversion ---- */ - if (HAL_ADC_Start_DMA(&s_hadc, (uint32_t *)s_dma_buf, ADC_DMA_BUF_LEN) - != HAL_OK) return; - - s_ready = true; -} - -/* ---- battery_adc_tick() ---- */ -void battery_adc_tick(uint32_t now_ms) -{ - if (!s_ready) return; - (void)now_ms; /* captured for caller context; may be used for future rate ctrl */ - - /* ---- Snapshot DMA buffer with IRQ guard to prevent torn reads ---- */ - uint16_t snap[ADC_DMA_BUF_LEN]; - uint32_t primask = __get_PRIMASK(); - __disable_irq(); - memcpy(snap, (void *)s_dma_buf, sizeof(snap)); - __set_PRIMASK(primask); - - /* ---- Average 4 Vbat and 4 Ibat samples (interleaved pairs) ---- */ - uint32_t vbat_sum = 0u; - uint32_t ibat_sum = 0u; - for (uint32_t i = 0u; i < ADC_DMA_BUF_LEN; i += 2u) { - vbat_sum += snap[i]; /* even indices: Vbat (rank 1) */ - ibat_sum += snap[i + 1u]; /* odd indices: Ibat (rank 2) */ - } - uint32_t vbat_avg = vbat_sum / ADC_OVERSAMPLE; /* 0–4095 */ - uint32_t ibat_avg = ibat_sum / ADC_OVERSAMPLE; - - /* ---- Raw (unfiltered) calibrated voltage — for calibration query ---- */ - s_vbat_raw_mv = raw_to_vbat_mv(vbat_avg); - - /* ---- IIR low-pass filter ---- */ - /* - * Accumulator stores value × 8 for sub-LSB precision. - * Update: acc8 += (raw × 8 - acc8) >> LPF_SHIFT - * Result: filtered_raw = acc8 >> 3 - * - * With LPF_SHIFT=3: α = 1/(1+8) ≈ 0.111 → cutoff ≈ 4 Hz at 100 Hz tick. - */ - s_lpf_vbat8 += ((vbat_avg << 3u) - s_lpf_vbat8) >> BATTERY_ADC_LPF_SHIFT; - s_lpf_ibat8 += (((int32_t)ibat_avg << 3u) - s_lpf_ibat8) - >> BATTERY_ADC_LPF_SHIFT; - - uint32_t vbat_filtered = s_lpf_vbat8 >> 3u; /* back to ADC counts */ - uint32_t ibat_filtered = (uint32_t)((s_lpf_ibat8 < 0 ? 0 : s_lpf_ibat8) >> 3u); - - /* ---- Apply calibration and convert to engineering units ---- */ - s_vbat_mv = raw_to_vbat_mv(vbat_filtered); - s_ibat_ma = raw_to_ibat_ma(ibat_filtered); - - /* ---- Auto-set thresholds based on detected cell count ---- */ - /* - * Detect 4S (Vbat ≥ 13 V) vs 3S (<13 V). Only update thresholds once - * after initial settling (first non-zero filtered reading). - */ - static bool s_thresholds_set = false; - if (!s_thresholds_set && s_vbat_mv > 500u) { - if (s_vbat_mv >= 13000u) { - s_low_mv = BATTERY_ADC_LOW_MV_4S; - s_critical_mv = BATTERY_ADC_CRITICAL_MV_4S; - } - /* else keep 3S defaults set at file scope */ - s_thresholds_set = true; - } -} - -/* ---- battery_adc_get_voltage_mv() ---- */ -uint32_t battery_adc_get_voltage_mv(void) -{ - return s_vbat_mv; -} - -/* ---- battery_adc_get_current_ma() ---- */ -int32_t battery_adc_get_current_ma(void) -{ - return s_ibat_ma; -} - -/* ---- battery_adc_get_raw_voltage_mv() ---- */ -uint32_t battery_adc_get_raw_voltage_mv(void) -{ - return s_vbat_raw_mv; -} - -/* ---- battery_adc_calibrate() ---- */ -void battery_adc_calibrate(const battery_adc_cal_t *cal) -{ - if (cal == NULL) { - /* Reset to defaults */ - s_cal.vbat_offset_mv = 0; - s_cal.ibat_offset_ma = 0; - s_cal.vbat_scale_num = 0; - s_cal.vbat_scale_den = 0; - return; - } - /* Clamp offsets to prevent runaway calibration values */ - int16_t voff = cal->vbat_offset_mv; - if (voff > 500) voff = 500; - if (voff < -500) voff = -500; - int16_t ioff = cal->ibat_offset_ma; - if (ioff > 200) ioff = 200; - if (ioff < -200) ioff = -200; - s_cal.vbat_offset_mv = voff; - s_cal.ibat_offset_ma = ioff; - s_cal.vbat_scale_num = cal->vbat_scale_num; - s_cal.vbat_scale_den = cal->vbat_scale_den; -} - -/* ---- battery_adc_get_calibration() ---- */ -void battery_adc_get_calibration(battery_adc_cal_t *out_cal) -{ - if (out_cal != NULL) { - *out_cal = s_cal; - } -} - -/* ---- battery_adc_publish() ---- */ -/* - * Sends JLINK_TLM_BATTERY (0x82) frame to Jetson at BATTERY_ADC_PUBLISH_HZ. - * - * Payload (jlink_tlm_battery_t, 10 bytes packed): - * vbat_mv uint16 — calibrated LPF voltage (mV) - * ibat_ma int16 — calibrated LPF current (mA) - * vbat_raw_mv uint16 — unfiltered last-tick voltage (mV) - * flags uint8 — bit0=low, bit1=critical, bit2=4S detected, bit3=ready - * cal_offset int8 — vbat_offset_mv / 4 (compressed, ±127 → ±508 mV) - * lpf_shift uint8 — BATTERY_ADC_LPF_SHIFT value (for Jetson info) - * soc_pct uint8 — voltage-based SoC estimate (0-100, 255=unknown) - */ -void battery_adc_publish(uint32_t now_ms) -{ - if (!s_ready) return; - - uint32_t period_ms = 1000u / BATTERY_ADC_PUBLISH_HZ; - if (s_published_once && (now_ms - s_last_publish_ms) < period_ms) return; - s_last_publish_ms = now_ms; - s_published_once = true; - - /* Build SoC estimate (voltage-based) */ - uint8_t soc = 255u; /* unknown */ - if (s_vbat_mv > 500u) { - uint32_t v_min, v_max; - if (s_vbat_mv >= 13000u) { - v_min = 13200u; v_max = 16800u; /* 4S */ - } else { - v_min = 9900u; v_max = 12600u; /* 3S */ - } - if (s_vbat_mv <= v_min) { - soc = 0u; - } else if (s_vbat_mv >= v_max) { - soc = 100u; - } else { - soc = (uint8_t)(((s_vbat_mv - v_min) * 100u) / (v_max - v_min)); - } - } - - /* Flags byte */ - uint8_t flags = 0u; - if (s_vbat_mv > 0u && s_vbat_mv < s_low_mv) flags |= 0x01u; - if (s_vbat_mv > 0u && s_vbat_mv < s_critical_mv) flags |= 0x02u; - if (s_low_mv == BATTERY_ADC_LOW_MV_4S) flags |= 0x04u; - if (s_ready) flags |= 0x08u; - - /* Build JLINK_TLM_BATTERY frame via jlink_send_battery_telemetry() */ - jlink_tlm_battery_t tlm = { - .vbat_mv = (uint16_t)(s_vbat_mv & 0xFFFFu), - .ibat_ma = (int16_t) (s_ibat_ma & 0xFFFF), - .vbat_raw_mv = (uint16_t)(s_vbat_raw_mv & 0xFFFFu), - .flags = flags, - .cal_offset = (int8_t) (s_cal.vbat_offset_mv / 4), - .lpf_shift = (uint8_t) BATTERY_ADC_LPF_SHIFT, - .soc_pct = soc, - }; - jlink_send_battery_telemetry(&tlm); -} - -/* ---- battery_adc_check_pm() ---- */ -void battery_adc_check_pm(uint32_t now_ms) -{ - if (!s_ready || s_vbat_mv == 0u) return; - - bool below_low = (s_vbat_mv < s_low_mv); - bool below_critical = (s_vbat_mv < s_critical_mv); - - /* Track first entry below low threshold */ - if (below_low && !s_low_active) { - s_low_active = true; - s_low_since_ms = now_ms; - s_critical_notified = false; - } else if (!below_low) { - s_low_active = false; - s_critical_notified = false; - } - - /* Notify power management on sustained critical voltage */ - if (below_critical && !s_critical_notified && - (now_ms - s_low_since_ms) >= BATTERY_ADC_LOW_HOLD_MS) { - s_critical_notified = true; - power_mgmt_notify_battery(s_vbat_mv); - } -} - -/* ---- battery_adc_is_low() ---- */ -bool battery_adc_is_low(void) -{ - return (s_ready && s_vbat_mv > 0u && s_vbat_mv < s_low_mv); -} - -/* ---- battery_adc_is_critical() ---- */ -bool battery_adc_is_critical(void) -{ - return (s_ready && s_vbat_mv > 0u && s_vbat_mv < s_critical_mv); -} diff --git a/legacy/stm32/src/bmp280.c b/legacy/stm32/src/bmp280.c deleted file mode 100644 index cd5dbc9..0000000 --- a/legacy/stm32/src/bmp280.c +++ /dev/null @@ -1,169 +0,0 @@ -/* - * bmp280.c — BMP280/BME280 barometer driver (I2C1, shared bus) - * - * Probes 0x76 first, then 0x77. Requires i2c1_init() before bmp280_init(). - * Returns chip_id on success (0x58=BMP280, 0x60=BME280), negative if absent. - * - * All HAL_I2C_Mem_Read/Write calls use 100ms timeouts — init cannot hang - * indefinitely even if the I2C bus is stuck or the breakout is absent. - * - * BME280 (chip_id 0x60): bmp280_read_humidity() returns %RH × 10. - * Call bmp280_read() first to refresh t_fine, then bmp280_read_humidity(). - */ -#include "bmp280.h" -#include "i2c1.h" -#include - -static uint16_t s_addr; -static int s_chip_id; /* 0x58=BMP280, 0x60=BME280, 0=none */ - -/* Shared temp/pressure calibration */ -static uint16_t dig_T1; -static int16_t dig_T2, dig_T3; -static uint16_t dig_P1; -static int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9; -static int32_t t_fine; /* updated by bmp280_read(); used by bmp280_read_humidity() */ - -/* BME280-only humidity calibration (chip_id 0x60) */ -static uint8_t dig_H1; -static int16_t dig_H2; -static uint8_t dig_H3; -static int16_t dig_H4, dig_H5; -static int8_t dig_H6; - -static uint8_t i2c_read(uint8_t reg) { - uint8_t val = 0; - HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &val, 1, 100); - return val; -} - -static void i2c_read_burst(uint8_t reg, uint8_t *buf, uint8_t len) { - HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 100); -} - -static void i2c_write(uint8_t reg, uint8_t val) { - HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100); -} - -static int try_init(uint16_t addr) { - s_addr = addr; - uint8_t id = i2c_read(0xD0); - if (id != 0x58 && id != 0x60) return -(int)id; - s_chip_id = (int)id; - - /* Temp/pressure calibration (0x88–0x9D, identical layout on BMP280 and BME280) */ - uint8_t cal[26]; - i2c_read_burst(0x88, cal, 26); - dig_T1 = (uint16_t)(cal[1] << 8 | cal[0]); - dig_T2 = (int16_t) (cal[3] << 8 | cal[2]); - dig_T3 = (int16_t) (cal[5] << 8 | cal[4]); - dig_P1 = (uint16_t)(cal[7] << 8 | cal[6]); - dig_P2 = (int16_t) (cal[9] << 8 | cal[8]); - dig_P3 = (int16_t) (cal[11] << 8 | cal[10]); - dig_P4 = (int16_t) (cal[13] << 8 | cal[12]); - dig_P5 = (int16_t) (cal[15] << 8 | cal[14]); - dig_P6 = (int16_t) (cal[17] << 8 | cal[16]); - dig_P7 = (int16_t) (cal[19] << 8 | cal[18]); - dig_P8 = (int16_t) (cal[21] << 8 | cal[20]); - dig_P9 = (int16_t) (cal[23] << 8 | cal[22]); - - if (id == 0x60) { - /* BME280: humidity calibration. - * dig_H1 : 0xA1 (uint8) - * dig_H2 : 0xE1–0xE2 (int16, LSB first) - * dig_H3 : 0xE3 (uint8) - * dig_H4 : 0xE4[7:0] | 0xE5[3:0] (int12) - * dig_H5 : 0xE5[7:4] | 0xE6[7:0] (int12) - * dig_H6 : 0xE7 (int8) - */ - dig_H1 = i2c_read(0xA1); - uint8_t hcal[7]; - i2c_read_burst(0xE1, hcal, 7); - dig_H2 = (int16_t)((hcal[1] << 8) | hcal[0]); - dig_H3 = hcal[2]; - dig_H4 = (int16_t)((hcal[3] << 4) | (hcal[4] & 0x0F)); - dig_H5 = (int16_t)((hcal[5] << 4) | (hcal[4] >> 4)); - dig_H6 = (int8_t)hcal[6]; - - /* ctrl_hum (0xF2) MUST be written before ctrl_meas (0xF4) — hardware req */ - i2c_write(0xF2, 0x05); /* osrs_h = ×16 */ - } - - i2c_write(0xF5, 0x00); /* config: standby=0.5ms, filter=off */ - i2c_write(0xF4, 0xB7); /* ctrl_meas: osrs_t=×16, osrs_p=×16, normal mode */ - - return (int)id; -} - -int bmp280_init(void) { - int ret = try_init(0x76 << 1); - if (ret > 0) return ret; - return try_init(0x77 << 1); -} - -void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) { - uint8_t buf[6]; - i2c_read_burst(0xF7, buf, 6); - - int32_t adc_P = (int32_t)((buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4)); - int32_t adc_T = (int32_t)((buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4)); - - /* Temperature compensation (BME280/BMP280 datasheet Section 4.2.3) */ - int32_t v1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11; - int32_t v2 = (((((adc_T >> 4) - (int32_t)dig_T1) * - ((adc_T >> 4) - (int32_t)dig_T1)) >> 12) * (int32_t)dig_T3) >> 14; - t_fine = v1 + v2; - *temp_x10 = (int16_t)((t_fine * 5 + 128) >> 8); /* 0.1 °C */ - - /* Pressure compensation */ - int64_t p1 = ((int64_t)t_fine) - 128000; - int64_t p2 = p1 * p1 * (int64_t)dig_P6; - p2 += (p1 * (int64_t)dig_P5) << 17; - p2 += ((int64_t)dig_P4) << 35; - p1 = ((p1 * p1 * (int64_t)dig_P3) >> 8) + ((p1 * (int64_t)dig_P2) << 12); - p1 = ((((int64_t)1 << 47) + p1)) * ((int64_t)dig_P1) >> 33; - if (p1 == 0) { *pressure_pa = 0; return; } - int64_t p = 1048576 - adc_P; - p = (((p << 31) - p2) * 3125) / p1; - p1 = ((int64_t)dig_P9 * (p >> 13) * (p >> 13)) >> 25; - p2 = ((int64_t)dig_P8 * p) >> 19; - *pressure_pa = (int32_t)(((p + p1 + p2) >> 8) + ((int64_t)dig_P7 << 4)) / 256; -} - -/* - * BME280-only humidity readout. MUST be called after bmp280_read() (uses t_fine). - * - * Compensation: BME280 datasheet section 4.2.3 integer formula. - * Result is Q22.10 fixed-point: 1024 units = 1 %RH. - * - * Returns humidity in %RH × 10 (e.g. 500 = 50.0 %RH). - * Returns -1 if chip is BMP280 (no humidity sensor). - */ -int16_t bmp280_read_humidity(void) { - if (s_chip_id != 0x60) return -1; - - uint8_t hbuf[2]; - i2c_read_burst(0xFD, hbuf, 2); - int32_t adc_H = (int32_t)((hbuf[0] << 8) | hbuf[1]); - - /* BME280 datasheet section 4.2.3 — floating-point compensation. - * Single-precision float is hardware-accelerated on STM32F7 (FPv5-SP FPU). - * Called at 50 Hz — negligible overhead. - */ - float var_H = ((float)t_fine) - 76800.0f; - var_H = (adc_H - (((float)dig_H4) * 64.0f + ((float)dig_H5) / 16384.0f * var_H)) * - (((float)dig_H2) / 65536.0f * - (1.0f + ((float)dig_H6) / 67108864.0f * var_H * - (1.0f + ((float)dig_H3) / 67108864.0f * var_H))); - var_H *= (1.0f - (float)dig_H1 * var_H / 524288.0f); - if (var_H > 100.0f) var_H = 100.0f; - if (var_H < 0.0f) var_H = 0.0f; - return (int16_t)(var_H * 10.0f + 0.5f); /* %RH × 10, rounded */ -} - -int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa) { - /* Barometric formula: h = 44330 * (1 - (p/p0)^(1/5.255)) metres */ - float ratio = (float)pressure_pa / 101325.0f; - float alt_m = 44330.0f * (1.0f - powf(ratio, 0.1902949f)); - return (int32_t)(alt_m * 100.0f); /* cm */ -} diff --git a/legacy/stm32/src/bno055.c b/legacy/stm32/src/bno055.c deleted file mode 100644 index a6a3d52..0000000 --- a/legacy/stm32/src/bno055.c +++ /dev/null @@ -1,316 +0,0 @@ -/* - * bno055.c — Bosch BNO055 NDOF IMU driver over I2C1 - * - * Issue #135: auto-detect alongside MPU6000; fallback + yaw augmentation. - * - * Calibration offsets are persisted in STM32F7 RTC backup registers - * (BKP0R–BKP6R) and restored automatically on next power-on, so the - * sensor re-enters a calibrated state immediately after reset. - * - * Internal temperature compensation: - * The BNO055 silicon compensates all three sensors (accel, gyro, mag) - * continuously against its onboard thermometer. No external correction - * is required. bno055_temperature() exposes the reading for telemetry. - */ - -#include "bno055.h" -#include "i2c1.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include -#include - -/* ---- I2C addresses (7-bit, shifted left 1 for HAL) ---- */ -#define BNO055_ADDR_LOW (0x28u << 1) -#define BNO055_ADDR_HIGH (0x29u << 1) -#define BNO055_CHIP_ID_VAL 0xA0u - -/* ---- Register map (page 0) ---- */ -#define REG_CHIP_ID 0x00u -#define REG_PAGE_ID 0x07u -#define REG_GYRO_X_LSB 0x14u /* 6 bytes: Gx, Gy, Gz */ -#define REG_EULER_H_LSB 0x1Au /* 6 bytes: H, R, P (each int16) */ -#define REG_LIA_X_LSB 0x28u /* 6 bytes: linear accel X,Y,Z */ -#define REG_GRV_X_LSB 0x2Eu /* 6 bytes: gravity X,Y,Z */ -#define REG_TEMP 0x34u -#define REG_CALIB_STAT 0x35u -#define REG_OPR_MODE 0x3Du -#define REG_PWR_MODE 0x3Eu -#define REG_SYS_TRIGGER 0x3Fu -#define REG_TEMP_SOURCE 0x40u -#define REG_OFFSET_BASE 0x55u /* 22 bytes: accel(6)+mag(6)+gyro(6)+radii(4) */ - -/* ---- Operating modes ---- */ -#define MODE_CONFIG 0x00u -#define MODE_IMUPLUS 0x08u /* 6DOF, no mag */ -#define MODE_NDOF 0x0Cu /* 9DOF with mag */ - -/* ---- RTC backup register magic for saved offsets ---- */ -/* - * Layout: BKP0R..BKP5R hold 22 bytes of calibration offsets (packed - * 4 bytes per register; BKP5R only uses its low 2 bytes). - * BKP6R holds the magic word to validate the stored data. - */ -#define BNO055_BKP_MAGIC 0xB055CA10u -#define BKP_REG(n) (*(volatile uint32_t *)(&RTC->BKP0R + (n))) - -/* ---- Scaling constants ---- */ -/* - * BNO055 default unit selection (UNIT_SEL=0x00): - * Euler angles: 1 degree = 16 LSB → divide by 16.0f - * Gyro rates: 1 °/s = 16 LSB → divide by 16.0f - * Accel/LIA/GRV: 1 m/s² = 100 LSB → divide by 100.0f - */ -#define EULER_SCALE 16.0f -#define GYRO_SCALE 16.0f -#define ACCEL_SCALE 100.0f -#define G_MS2 9.80665f /* m/s² per g */ - -/* ---- Module state ---- */ -static uint16_t s_addr = 0; -static bool s_present = false; -static bool s_offsets_ok = false; /* true if offsets restored */ -static uint8_t s_calib = 0; /* cached CALIB_STAT */ -static int8_t s_temp = 25; /* cached temperature °C */ -static uint16_t s_temp_ctr = 0; /* temp read divider */ - -/* ---- Low-level I2C helpers ---- */ -static uint8_t rd(uint8_t reg) -{ - uint8_t v = 0; - HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &v, 1, 100); - return v; -} - -static void wr(uint8_t reg, uint8_t val) -{ - HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100); -} - -static void rdblk(uint8_t reg, uint8_t *buf, uint8_t len) -{ - HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 200); -} - -static void wrblk(uint8_t reg, const uint8_t *buf, uint8_t len) -{ - HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, (uint8_t *)buf, len, 200); -} - -/* ---- bno055_restore_offsets() ---- */ -bool bno055_restore_offsets(void) -{ - __HAL_RCC_PWR_CLK_ENABLE(); - HAL_PWR_EnableBkUpAccess(); - - if (BKP_REG(6) != BNO055_BKP_MAGIC) return false; - - /* Unpack 22 bytes from BKP0R..BKP5R */ - uint8_t offsets[22]; - for (int i = 0; i < 5; i++) { - uint32_t w = BKP_REG(i); - offsets[i * 4 + 0] = (uint8_t)(w & 0xFFu); - offsets[i * 4 + 1] = (uint8_t)((w >> 8) & 0xFFu); - offsets[i * 4 + 2] = (uint8_t)((w >> 16) & 0xFFu); - offsets[i * 4 + 3] = (uint8_t)((w >> 24) & 0xFFu); - } - uint32_t w5 = BKP_REG(5); - offsets[20] = (uint8_t)(w5 & 0xFFu); - offsets[21] = (uint8_t)((w5 >> 8) & 0xFFu); - - /* Write offsets — must be in CONFIG mode */ - wr(REG_OPR_MODE, MODE_CONFIG); - HAL_Delay(25); - wr(REG_PAGE_ID, 0x00); - wrblk(REG_OFFSET_BASE, offsets, 22); - /* Caller switches back to NDOF after this returns */ - return true; -} - -/* ---- bno055_save_offsets() ---- */ -bool bno055_save_offsets(void) -{ - if (!s_present) return false; - - /* Switch to CONFIG mode to read offsets */ - wr(REG_OPR_MODE, MODE_CONFIG); - HAL_Delay(25); - wr(REG_PAGE_ID, 0x00); - - uint8_t offsets[22]; - rdblk(REG_OFFSET_BASE, offsets, 22); - - /* Restore to NDOF */ - wr(REG_OPR_MODE, MODE_NDOF); - HAL_Delay(10); - - /* Pack into RTC backup registers */ - __HAL_RCC_PWR_CLK_ENABLE(); - HAL_PWR_EnableBkUpAccess(); - - for (int i = 0; i < 5; i++) { - BKP_REG(i) = ((uint32_t)offsets[i * 4 + 0]) - | ((uint32_t)offsets[i * 4 + 1] << 8) - | ((uint32_t)offsets[i * 4 + 2] << 16) - | ((uint32_t)offsets[i * 4 + 3] << 24); - } - BKP_REG(5) = (uint32_t)offsets[20] | ((uint32_t)offsets[21] << 8); - BKP_REG(6) = BNO055_BKP_MAGIC; - - return true; -} - -/* ---- bno055_init() ---- */ -int bno055_init(void) -{ - /* Probe 0x28 (ADR=0) first, then 0x29 (ADR=1) */ - const uint16_t candidates[2] = { BNO055_ADDR_LOW, BNO055_ADDR_HIGH }; - s_present = false; - for (int i = 0; i < 2; i++) { - s_addr = candidates[i]; - if (rd(REG_CHIP_ID) == BNO055_CHIP_ID_VAL) { - s_present = true; - break; - } - } - if (!s_present) return -1; - - /* Software reset via SYS_TRIGGER */ - wr(REG_PAGE_ID, 0x00); - wr(REG_SYS_TRIGGER, 0x20u); - HAL_Delay(700); /* POR settle time from datasheet */ - - /* Confirm chip ID after reset */ - if (rd(REG_CHIP_ID) != BNO055_CHIP_ID_VAL) { - s_present = false; - return -1; - } - - wr(REG_PAGE_ID, 0x00); - - /* CONFIGMODE before any register writes */ - wr(REG_OPR_MODE, MODE_CONFIG); - HAL_Delay(25); - - /* Normal power mode */ - wr(REG_PWR_MODE, 0x00u); - HAL_Delay(10); - - /* - * Unit selection (UNIT_SEL = 0x00, default): - * Euler = degrees, Gyro = dps, Accel = m/s², Temp = °C - * No write needed — defaults match what we want. - */ - - /* Temperature source = gyro (more stable than accel) */ - wr(REG_TEMP_SOURCE, 0x01u); - - /* Try to restore saved calibration offsets */ - s_offsets_ok = bno055_restore_offsets(); - - /* Enter NDOF fusion mode */ - wr(REG_OPR_MODE, MODE_NDOF); - HAL_Delay(20); - - s_calib = 0; - s_temp = 25; - s_temp_ctr = 0; - return 0; -} - -/* ---- bno055_read() ---- */ -/* - * Performs two I2C burst reads (~3ms total at 100kHz): - * 1. REG_GYRO_X_LSB → 12 bytes: gyro(6) + euler(6) - * 2. REG_LIA_X_LSB → 12 bytes: linear_accel(6) + gravity(6) - * - * BNO055 coordinate frame (NDOF, no remapping): - * X: right, Y: forward, Z: up - * Euler H: yaw (0–360°, clockwise from north) - * Euler R: roll (–180°..+180°, right bank positive) - * Euler P: pitch (–90°..+90°, nose up positive) - * Gyro Y: pitch rate (deg/s, nose-up positive) - * - * Linear accel X (m/s²): forward-backward acceleration (gravity removed) - * Gravity Z (m/s²): gravity component on Z axis (≈9.81 when level) - */ -void bno055_read(IMUData *data) -{ - uint8_t buf[12]; - - /* ---- Burst 1: Gyro (0x14–0x19) + Euler (0x1A–0x1F) ---- */ - rdblk(REG_GYRO_X_LSB, buf, 12); - - /* Gyro: bytes 0..5 = [Gx_L, Gx_H, Gy_L, Gy_H, Gz_L, Gz_H] - * Gy = pitch rate (Y axis = pitch axis in BNO055 frame) */ - float pitch_rate = (float)(int16_t)((uint16_t)buf[3] << 8 | buf[2]) / GYRO_SCALE; - - /* Euler: bytes 6..11 = [H_L, H_H, R_L, R_H, P_L, P_H] */ - float heading = (float)(int16_t)((uint16_t)buf[7] << 8 | buf[6]) / EULER_SCALE; - float roll = (float)(int16_t)((uint16_t)buf[9] << 8 | buf[8]) / EULER_SCALE; - float pitch = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / EULER_SCALE; - - /* ---- Burst 2: Linear accel (0x28–0x2D) + Gravity (0x2E–0x33) ---- */ - rdblk(REG_LIA_X_LSB, buf, 12); - - /* Linear accel X (forward, m/s²) — gravity-compensated */ - float lia_x_ms2 = (float)(int16_t)((uint16_t)buf[1] << 8 | buf[0]) / ACCEL_SCALE; - - /* Gravity Z (up axis, m/s²) — should be ≈+9.81 when level */ - float grv_z_ms2 = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / ACCEL_SCALE; - - /* Fill IMUData in same units as mpu6000_read() */ - data->pitch = pitch; - data->pitch_rate = pitch_rate; - data->roll = roll; - data->yaw = heading; - data->accel_x = lia_x_ms2 / G_MS2; /* m/s² → g */ - data->accel_z = grv_z_ms2 / G_MS2; /* m/s² → g (≈1.0 when level) */ - - /* Periodically update calibration status and temperature (~1Hz at 250Hz loop) */ - if (++s_temp_ctr >= 250u) { - s_temp_ctr = 0; - s_calib = rd(REG_CALIB_STAT); - s_temp = (int8_t)rd(REG_TEMP); - - /* - * Auto-save offsets once when full calibration is achieved for - * the first time this session (sys=3, acc=3, gyr=3, don't wait - * for mag=3 as it is harder to achieve and not critical for balance). - */ - static bool s_saved = false; - if (!s_saved) { - uint8_t sys_cal = (s_calib & BNO055_CAL_SYS_MASK) >> 6; - uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2; - uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4; - if (sys_cal >= 1u && acc_cal >= 3u && gyr_cal >= 3u) { - bno055_save_offsets(); - s_saved = true; - } - } - } -} - -/* ---- bno055_is_ready() ---- */ -bool bno055_is_ready(void) -{ - if (!s_present) return false; - /* If we restored saved offsets, trust them immediately */ - if (s_offsets_ok) return true; - /* Otherwise require gyro ≥ 2 and accel ≥ 2 */ - uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2; - uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4; - return (acc_cal >= 2u) && (gyr_cal >= 2u); -} - -/* ---- bno055_calib_status() ---- */ -uint8_t bno055_calib_status(void) -{ - return s_calib; -} - -/* ---- bno055_temperature() ---- */ -int8_t bno055_temperature(void) -{ - return s_temp; -} diff --git a/legacy/stm32/src/buzzer.c b/legacy/stm32/src/buzzer.c deleted file mode 100644 index 48600a2..0000000 --- a/legacy/stm32/src/buzzer.c +++ /dev/null @@ -1,293 +0,0 @@ -#include "buzzer.h" -#include "stm32f7xx_hal.h" -#include "config.h" -#include - -/* ================================================================ - * Buzzer Hardware Configuration - * ================================================================ */ - -#define BUZZER_PIN GPIO_PIN_8 -#define BUZZER_PORT GPIOA -#define BUZZER_TIM TIM1 -#define BUZZER_TIM_CHANNEL TIM_CHANNEL_1 -#define BUZZER_BASE_FREQ_HZ 1000 /* Base PWM frequency (1kHz) */ - -/* ================================================================ - * Predefined Melodies - * ================================================================ */ - -/* Startup jingle: C-E-G ascending pattern */ -const MelodyNote melody_startup[] = { - {NOTE_C4, DURATION_QUARTER}, - {NOTE_E4, DURATION_QUARTER}, - {NOTE_G4, DURATION_QUARTER}, - {NOTE_C5, DURATION_HALF}, - {NOTE_REST, 0} /* Terminator */ -}; - -/* Low battery warning: two descending beeps */ -const MelodyNote melody_low_battery[] = { - {NOTE_A5, DURATION_EIGHTH}, - {NOTE_REST, DURATION_EIGHTH}, - {NOTE_A5, DURATION_EIGHTH}, - {NOTE_REST, DURATION_EIGHTH}, - {NOTE_F5, DURATION_EIGHTH}, - {NOTE_REST, DURATION_EIGHTH}, - {NOTE_F5, DURATION_EIGHTH}, - {NOTE_REST, 0} -}; - -/* Error alert: rapid repeating tone */ -const MelodyNote melody_error[] = { - {NOTE_E5, DURATION_SIXTEENTH}, - {NOTE_REST, DURATION_SIXTEENTH}, - {NOTE_E5, DURATION_SIXTEENTH}, - {NOTE_REST, DURATION_SIXTEENTH}, - {NOTE_E5, DURATION_SIXTEENTH}, - {NOTE_REST, DURATION_SIXTEENTH}, - {NOTE_REST, 0} -}; - -/* Docking complete: cheerful ascending chime */ -const MelodyNote melody_docking_complete[] = { - {NOTE_C4, DURATION_EIGHTH}, - {NOTE_E4, DURATION_EIGHTH}, - {NOTE_G4, DURATION_EIGHTH}, - {NOTE_C5, DURATION_QUARTER}, - {NOTE_REST, DURATION_QUARTER}, - {NOTE_G4, DURATION_EIGHTH}, - {NOTE_C5, DURATION_HALF}, - {NOTE_REST, 0} -}; - -/* ================================================================ - * Melody Queue - * ================================================================ */ - -#define MELODY_QUEUE_SIZE 4 - -typedef struct { - const MelodyNote *notes; /* Melody sequence pointer */ - uint16_t note_index; /* Current note in sequence */ - uint32_t note_start_ms; /* When current note started */ - uint32_t note_duration_ms; /* Duration of current note */ - uint16_t current_frequency; /* Current tone frequency (Hz) */ - bool is_custom; /* Is this a custom melody? */ -} MelodyPlayback; - -typedef struct { - MelodyPlayback queue[MELODY_QUEUE_SIZE]; - uint8_t write_index; - uint8_t read_index; - uint8_t count; -} MelodyQueue; - -static MelodyQueue s_queue = {0}; -static MelodyPlayback s_current = {0}; -static uint32_t s_last_tick_ms = 0; - -/* ================================================================ - * Hardware Initialization - * ================================================================ */ - -void buzzer_init(void) -{ - /* Enable GPIO and timer clocks */ - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_TIM1_CLK_ENABLE(); - - /* Configure PA8 as TIM1_CH1 PWM output */ - GPIO_InitTypeDef gpio_init = {0}; - gpio_init.Pin = BUZZER_PIN; - gpio_init.Mode = GPIO_MODE_AF_PP; - gpio_init.Pull = GPIO_NOPULL; - gpio_init.Speed = GPIO_SPEED_HIGH; - gpio_init.Alternate = GPIO_AF1_TIM1; - HAL_GPIO_Init(BUZZER_PORT, &gpio_init); - - /* Configure TIM1 for PWM: - * Clock: 216MHz / PSC = output frequency - * For 1kHz base frequency: PSC = 216, ARR = 1000 - * Duty cycle = CCR / ARR (e.g., 500/1000 = 50%) - */ - TIM_HandleTypeDef htim1 = {0}; - htim1.Instance = BUZZER_TIM; - htim1.Init.Prescaler = 216 - 1; /* 216MHz / 216 = 1MHz clock */ - htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = (1000000 / BUZZER_BASE_FREQ_HZ) - 1; /* 1kHz = 1000 counts */ - htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter = 0; - HAL_TIM_PWM_Init(&htim1); - - /* Configure PWM on CH1: 50% duty cycle initially (silence will be 0%) */ - TIM_OC_InitTypeDef oc_init = {0}; - oc_init.OCMode = TIM_OCMODE_PWM1; - oc_init.Pulse = 0; /* Start at 0% duty (silence) */ - oc_init.OCPolarity = TIM_OCPOLARITY_HIGH; - oc_init.OCFastMode = TIM_OCFAST_DISABLE; - HAL_TIM_PWM_ConfigChannel(&htim1, &oc_init, BUZZER_TIM_CHANNEL); - - /* Start PWM generation */ - HAL_TIM_PWM_Start(BUZZER_TIM, BUZZER_TIM_CHANNEL); - - /* Initialize queue */ - memset(&s_queue, 0, sizeof(s_queue)); - memset(&s_current, 0, sizeof(s_current)); - s_last_tick_ms = 0; -} - -/* ================================================================ - * Public API - * ================================================================ */ - -bool buzzer_play_melody(MelodyType melody_type) -{ - const MelodyNote *notes = NULL; - - switch (melody_type) { - case MELODY_STARTUP: - notes = melody_startup; - break; - case MELODY_LOW_BATTERY: - notes = melody_low_battery; - break; - case MELODY_ERROR: - notes = melody_error; - break; - case MELODY_DOCKING_COMPLETE: - notes = melody_docking_complete; - break; - default: - return false; - } - - return buzzer_play_custom(notes); -} - -bool buzzer_play_custom(const MelodyNote *notes) -{ - if (!notes || s_queue.count >= MELODY_QUEUE_SIZE) { - return false; - } - - MelodyPlayback *playback = &s_queue.queue[s_queue.write_index]; - memset(playback, 0, sizeof(*playback)); - playback->notes = notes; - playback->note_index = 0; - playback->is_custom = true; - - s_queue.write_index = (s_queue.write_index + 1) % MELODY_QUEUE_SIZE; - s_queue.count++; - - return true; -} - -bool buzzer_play_tone(uint16_t frequency, uint16_t duration_ms) -{ - if (s_queue.count >= MELODY_QUEUE_SIZE) { - return false; - } - - /* Create a simple 2-note melody: tone + rest */ - static MelodyNote temp_notes[3]; - temp_notes[0].frequency = frequency; - temp_notes[0].duration_ms = duration_ms; - temp_notes[1].frequency = NOTE_REST; - temp_notes[1].duration_ms = 0; - - MelodyPlayback *playback = &s_queue.queue[s_queue.write_index]; - memset(playback, 0, sizeof(*playback)); - playback->notes = temp_notes; - playback->note_index = 0; - playback->is_custom = true; - - s_queue.write_index = (s_queue.write_index + 1) % MELODY_QUEUE_SIZE; - s_queue.count++; - - return true; -} - -void buzzer_stop(void) -{ - /* Clear queue and current playback */ - memset(&s_queue, 0, sizeof(s_queue)); - memset(&s_current, 0, sizeof(s_current)); - - /* Silence buzzer (0% duty cycle) */ - TIM1->CCR1 = 0; -} - -bool buzzer_is_playing(void) -{ - return (s_current.notes != NULL) || (s_queue.count > 0); -} - -/* ================================================================ - * Timer Update and PWM Frequency Control - * ================================================================ */ - -static void buzzer_set_frequency(uint16_t frequency) -{ - if (frequency == 0) { - /* Silence: 0% duty cycle */ - TIM1->CCR1 = 0; - return; - } - - /* Set PWM frequency and 50% duty cycle - * TIM1 clock: 1MHz (after prescaler) - * ARR = 1MHz / frequency - * CCR1 = ARR / 2 (50% duty) - */ - uint32_t arr = (1000000 / frequency); - if (arr > 65535) arr = 65535; /* Clamp to 16-bit */ - - TIM1->ARR = arr - 1; - TIM1->CCR1 = arr / 2; /* 50% duty cycle for all tones */ -} - -void buzzer_tick(uint32_t now_ms) -{ - /* Check if current note has finished */ - if (s_current.notes != NULL) { - uint32_t elapsed = now_ms - s_current.note_start_ms; - - if (elapsed >= s_current.note_duration_ms) { - /* Move to next note */ - s_current.note_index++; - - if (s_current.notes[s_current.note_index].duration_ms == 0) { - /* End of melody sequence */ - s_current.notes = NULL; - buzzer_set_frequency(0); - - /* Start next queued melody if available */ - if (s_queue.count > 0) { - s_current = s_queue.queue[s_queue.read_index]; - s_queue.read_index = (s_queue.read_index + 1) % MELODY_QUEUE_SIZE; - s_queue.count--; - s_current.note_start_ms = now_ms; - s_current.note_duration_ms = s_current.notes[0].duration_ms; - buzzer_set_frequency(s_current.notes[0].frequency); - } - } else { - /* Play next note */ - s_current.note_start_ms = now_ms; - s_current.note_duration_ms = s_current.notes[s_current.note_index].duration_ms; - uint16_t frequency = s_current.notes[s_current.note_index].frequency; - buzzer_set_frequency(frequency); - } - } - } else if (s_queue.count > 0 && s_current.notes == NULL) { - /* Start first queued melody */ - s_current = s_queue.queue[s_queue.read_index]; - s_queue.read_index = (s_queue.read_index + 1) % MELODY_QUEUE_SIZE; - s_queue.count--; - s_current.note_start_ms = now_ms; - s_current.note_duration_ms = s_current.notes[0].duration_ms; - buzzer_set_frequency(s_current.notes[0].frequency); - } - - s_last_tick_ms = now_ms; -} diff --git a/legacy/stm32/src/can_driver.c b/legacy/stm32/src/can_driver.c deleted file mode 100644 index edc7001..0000000 --- a/legacy/stm32/src/can_driver.c +++ /dev/null @@ -1,253 +0,0 @@ -/* CAN bus driver (Issues #597, #676, #674, #694) */ -#include "can_driver.h" -#include "stm32f7xx_hal.h" -#include - -static CAN_HandleTypeDef s_can; -static volatile can_feedback_t s_feedback[CAN_NUM_MOTORS]; -static volatile can_stats_t s_stats; -static can_ext_frame_cb_t s_ext_cb = NULL; -static can_std_frame_cb_t s_std_cb = NULL; -static volatile can_wdog_t s_wdog; - -#ifdef TEST_HOST -static volatile uint32_t s_test_esr = 0u; -void can_driver_inject_esr(uint32_t v) { s_test_esr = v; } -static uint32_t _read_esr(void) { return s_test_esr; } -static HAL_StatusTypeDef _can_restart(void) { - HAL_CAN_Stop(&s_can); s_test_esr = 0u; return HAL_CAN_Start(&s_can); -} -#else -static uint32_t _read_esr(void) { return s_can.Instance->ESR; } -static HAL_StatusTypeDef _can_restart(void) { - HAL_CAN_Stop(&s_can); return HAL_CAN_Start(&s_can); -} -#endif - -void can_driver_init(void) -{ - __HAL_RCC_CAN1_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9; - gpio.Mode = GPIO_MODE_AF_PP; gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; gpio.Alternate = GPIO_AF9_CAN1; - HAL_GPIO_Init(GPIOB, &gpio); - s_can.Instance = CAN1; - s_can.Init.Prescaler = CAN_PRESCALER; - s_can.Init.Mode = CAN_MODE_NORMAL; - s_can.Init.SyncJumpWidth = CAN_SJW_1TQ; - s_can.Init.TimeSeg1 = CAN_BS1_13TQ; - s_can.Init.TimeSeg2 = CAN_BS2_4TQ; - s_can.Init.TimeTriggeredMode = DISABLE; - s_can.Init.AutoBusOff = ENABLE; - s_can.Init.AutoWakeUp = DISABLE; - s_can.Init.AutoRetransmission = ENABLE; - s_can.Init.ReceiveFifoLocked = DISABLE; - s_can.Init.TransmitFifoPriority = DISABLE; - if (HAL_CAN_Init(&s_can) != HAL_OK) { s_stats.bus_off = 1u; return; } - CAN_FilterTypeDef flt = {0}; - flt.FilterBank = 0u; flt.FilterMode = CAN_FILTERMODE_IDMASK; - flt.FilterScale = CAN_FILTERSCALE_32BIT; - flt.FilterFIFOAssignment = CAN_RX_FIFO0; - flt.FilterActivation = CAN_FILTER_ENABLE; - flt.SlaveStartFilterBank = 14u; - if (HAL_CAN_ConfigFilter(&s_can, &flt) != HAL_OK) { s_stats.bus_off = 1u; return; } - CAN_FilterTypeDef flt2 = {0}; - flt2.FilterBank = 15u; flt2.FilterMode = CAN_FILTERMODE_IDMASK; - flt2.FilterScale = CAN_FILTERSCALE_32BIT; - flt2.FilterIdLow = 0x0004u; flt2.FilterMaskIdLow = 0x0004u; - flt2.FilterFIFOAssignment = CAN_RX_FIFO1; - flt2.FilterActivation = CAN_FILTER_ENABLE; - flt2.SlaveStartFilterBank = 14u; - if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) { s_stats.bus_off = 1u; return; } - HAL_CAN_Start(&s_can); -#ifndef TEST_HOST - HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 5u, 0u); - HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn); - HAL_CAN_ActivateNotification(&s_can, CAN_IT_BUSOFF | CAN_IT_ERROR_PASSIVE | CAN_IT_ERROR_WARNING); -#endif - memset((void *)s_feedback, 0, sizeof(s_feedback)); - memset((void *)&s_stats, 0, sizeof(s_stats)); - memset((void *)&s_wdog, 0, sizeof(s_wdog)); -} - -void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd) -{ - if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) return; - uint8_t data[4]; - data[0] = (uint8_t)((uint16_t)cmd->velocity_rpm & 0xFFu); - data[1] = (uint8_t)((uint16_t)cmd->velocity_rpm >> 8u); - data[2] = (uint8_t)((uint16_t)cmd->torque_x100 & 0xFFu); - data[3] = (uint8_t)((uint16_t)cmd->torque_x100 >> 8u); - CAN_TxHeaderTypeDef hdr = {0}; - hdr.StdId = CAN_ID_VEL_CMD_BASE + (uint32_t)node_id; - hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = 4u; - uint32_t mailbox; - if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) { - if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK) - s_stats.tx_count++; - else s_stats.err_count++; - } -} - -void can_driver_send_enable(uint8_t node_id, bool enable) -{ - if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) return; - uint8_t data[1] = { enable ? 1u : 0u }; - CAN_TxHeaderTypeDef hdr = {0}; - hdr.StdId = CAN_ID_ENABLE_CMD_BASE + (uint32_t)node_id; - hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = 1u; - uint32_t mailbox; - if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) { - if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK) - s_stats.tx_count++; - else s_stats.err_count++; - } -} - -void can_driver_process(void) -{ - if (_read_esr() & CAN_ESR_BOFF) { s_stats.bus_off = 1u; return; } - s_stats.bus_off = 0u; - while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) { - CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8]; - if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO0, &rxhdr, rxdata) != HAL_OK) { - s_stats.err_count++; break; - } - if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) continue; - if (s_std_cb != NULL) s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC); - uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE; - if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) { - uint8_t nid = (uint8_t)nid_u; - s_feedback[nid].velocity_rpm = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u)); - s_feedback[nid].current_ma = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u)); - s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u)); - s_feedback[nid].temperature_c = (int8_t)rxdata[6]; - s_feedback[nid].fault = rxdata[7]; - s_feedback[nid].last_rx_ms = HAL_GetTick(); - } - s_stats.rx_count++; - } - while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) { - CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8]; - if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) { - s_stats.err_count++; break; - } - if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) continue; - if (s_ext_cb != NULL) s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC); - s_stats.rx_count++; - } -} - -void can_driver_set_ext_cb(can_ext_frame_cb_t cb) { s_ext_cb = cb; } -void can_driver_set_std_cb(can_std_frame_cb_t cb) { s_std_cb = cb; } - -void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len) -{ - if (s_stats.bus_off || len > 8u) return; - CAN_TxHeaderTypeDef hdr = {0}; - hdr.ExtId = ext_id; hdr.IDE = CAN_ID_EXT; hdr.RTR = CAN_RTR_DATA; hdr.DLC = len; - uint32_t mailbox; - if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) { - if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) - s_stats.tx_count++; - else s_stats.err_count++; - } -} - -void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len) -{ - if (s_stats.bus_off || len > 8u) return; - CAN_TxHeaderTypeDef hdr = {0}; - hdr.StdId = std_id; hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = len; - uint32_t mailbox; - if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) { - if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) - s_stats.tx_count++; - else s_stats.err_count++; - } -} - -bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out) -{ - if (node_id >= CAN_NUM_MOTORS || out == NULL) return false; - if (s_feedback[node_id].last_rx_ms == 0u) return false; - memcpy(out, (const void *)&s_feedback[node_id], sizeof(can_feedback_t)); - return true; -} - -bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms) -{ - if (node_id >= CAN_NUM_MOTORS || s_feedback[node_id].last_rx_ms == 0u) return false; - return (now_ms - s_feedback[node_id].last_rx_ms) < CAN_NODE_TIMEOUT_MS; -} - -void can_driver_get_stats(can_stats_t *out) -{ - if (out == NULL) return; - memcpy(out, (const void *)&s_stats, sizeof(can_stats_t)); -} - -/* SCE interrupt handler -- Issue #694 */ -#ifndef TEST_HOST -void CAN1_SCE_IRQHandler(void) -{ - uint32_t esr = s_can.Instance->ESR; - if (esr & CAN_ESR_BOFF) { - if (s_wdog.error_state != CAN_ERR_BUS_OFF) s_wdog.busoff_count++; - s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u; - } else if (esr & CAN_ESR_EPVF) { - if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) { - s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE; - } - } else if (esr & CAN_ESR_EWGF) { - if (s_wdog.error_state < CAN_ERR_WARNING) { - s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING; - } - } - __HAL_CAN_CLEAR_FLAG(&s_can, CAN_FLAG_ERRI); - HAL_CAN_IRQHandler(&s_can); -} -#endif - -/* Watchdog tick -- Issue #694 */ -can_error_state_t can_driver_watchdog_tick(uint32_t now_ms) -{ - uint32_t esr = _read_esr(); - if (esr & CAN_ESR_BOFF) { - if (s_wdog.error_state != CAN_ERR_BUS_OFF) { - s_wdog.busoff_count++; s_wdog.busoff_ms = now_ms; s_wdog.busoff_pending = 1u; - } - s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u; - } else if (esr & CAN_ESR_EPVF) { - if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) { - s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE; - } - } else if (esr & CAN_ESR_EWGF) { - if (s_wdog.error_state < CAN_ERR_WARNING) { - s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING; - } - } else { - if (s_wdog.error_state > CAN_ERR_NOMINAL && s_wdog.error_state < CAN_ERR_BUS_OFF) - s_wdog.error_state = CAN_ERR_NOMINAL; - } - if (s_wdog.busoff_pending && s_wdog.error_state == CAN_ERR_BUS_OFF && - (now_ms - s_wdog.busoff_ms) >= CAN_WDOG_RESTART_MS) { - if (_can_restart() == HAL_OK) { - s_wdog.restart_count++; s_wdog.error_state = CAN_ERR_NOMINAL; - s_wdog.busoff_pending = 0u; s_stats.bus_off = 0u; - } else { - s_wdog.busoff_ms = now_ms; - } - } - s_wdog.tec = (uint8_t)((esr >> CAN_ESR_TEC_Pos) & 0xFFu); - s_wdog.rec = (uint8_t)((esr >> CAN_ESR_REC_Pos) & 0xFFu); - return s_wdog.error_state; -} - -void can_driver_get_wdog(can_wdog_t *out) -{ - if (out == NULL) return; - memcpy(out, (const void *)&s_wdog, sizeof(can_wdog_t)); -} diff --git a/legacy/stm32/src/coulomb_counter.c b/legacy/stm32/src/coulomb_counter.c deleted file mode 100644 index 1be0499..0000000 --- a/legacy/stm32/src/coulomb_counter.c +++ /dev/null @@ -1,118 +0,0 @@ -/* - * coulomb_counter.c — Battery coulomb counter (Issue #325) - * - * Tracks Ah consumed from current readings, provides SoC independent of load. - * Time integration: consumed_mah += current_ma * dt_ms / 3600000 - */ - -#include "coulomb_counter.h" -#include "stm32f7xx_hal.h" - -/* State structure */ -static struct { - bool initialized; - bool valid; /* At least one measurement taken */ - uint16_t capacity_mah; /* Battery capacity in mAh */ - uint32_t accumulated_mah_x100; /* Accumulated coulombs in mAh×100 (fixed-point) */ - uint32_t last_tick_ms; /* Last update timestamp (ms) */ -} s_state = {0}; - -void coulomb_counter_init(uint16_t capacity_mah) { - if (capacity_mah == 0 || capacity_mah > 20000) { - /* Sanity check: reasonable battery is 100–20000 mAh */ - return; - } - - s_state.capacity_mah = capacity_mah; - s_state.accumulated_mah_x100 = 0; - s_state.last_tick_ms = HAL_GetTick(); - s_state.initialized = true; - s_state.valid = false; -} - -void coulomb_counter_accumulate(int16_t current_ma) { - if (!s_state.initialized) return; - - uint32_t now_ms = HAL_GetTick(); - uint32_t dt_ms = now_ms - s_state.last_tick_ms; - - /* Handle tick wraparound (~49.7 days at 32-bit ms) */ - if (dt_ms > 86400000UL) { - /* If jump > 1 day, likely wraparound; skip this sample */ - s_state.last_tick_ms = now_ms; - return; - } - - /* Prevent negative dt or dt=0 */ - if (dt_ms == 0) return; - if (dt_ms > 1000) { - /* Cap to 1 second max per call to prevent overflow */ - dt_ms = 1000; - } - - /* Accumulate: mAh += mA × dt_ms / 3600000 - * Using fixed-point (×100): accumulated_mah_x100 += mA × dt_ms / 36000 */ - int32_t coulomb_x100 = (int32_t)current_ma * (int32_t)dt_ms / 36000; - - /* Only accumulate if discharging (positive current) or realistic charging */ - if (coulomb_x100 > 0) { - s_state.accumulated_mah_x100 += (uint32_t)coulomb_x100; - } else if (coulomb_x100 < 0 && s_state.accumulated_mah_x100 > 0) { - /* Allow charging (negative current) to reduce accumulated coulombs */ - int32_t new_val = (int32_t)s_state.accumulated_mah_x100 + coulomb_x100; - if (new_val < 0) { - s_state.accumulated_mah_x100 = 0; - } else { - s_state.accumulated_mah_x100 = (uint32_t)new_val; - } - } - - /* Clamp to capacity */ - if (s_state.accumulated_mah_x100 > (uint32_t)s_state.capacity_mah * 100) { - s_state.accumulated_mah_x100 = (uint32_t)s_state.capacity_mah * 100; - } - - s_state.last_tick_ms = now_ms; - s_state.valid = true; -} - -uint8_t coulomb_counter_get_soc_pct(void) { - if (!s_state.valid) return 255; /* 255 = invalid/not measured */ - - /* SoC = 100 - (consumed_mah / capacity_mah) * 100 */ - uint32_t consumed_mah = s_state.accumulated_mah_x100 / 100; - - if (consumed_mah >= s_state.capacity_mah) { - return 0; /* Fully discharged */ - } - - uint32_t remaining_mah = s_state.capacity_mah - consumed_mah; - uint8_t soc = (uint8_t)((remaining_mah * 100u) / s_state.capacity_mah); - - return soc; -} - -uint16_t coulomb_counter_get_consumed_mah(void) { - return (uint16_t)(s_state.accumulated_mah_x100 / 100); -} - -uint16_t coulomb_counter_get_remaining_mah(void) { - if (!s_state.valid) return s_state.capacity_mah; - - uint32_t consumed = s_state.accumulated_mah_x100 / 100; - if (consumed >= s_state.capacity_mah) { - return 0; - } - return (uint16_t)(s_state.capacity_mah - consumed); -} - -void coulomb_counter_reset(void) { - if (!s_state.initialized) return; - - s_state.accumulated_mah_x100 = 0; - s_state.last_tick_ms = HAL_GetTick(); -} - -bool coulomb_counter_is_valid(void) { - return s_state.valid; -} diff --git a/legacy/stm32/src/crsf.c b/legacy/stm32/src/crsf.c deleted file mode 100644 index 71fe9ad..0000000 --- a/legacy/stm32/src/crsf.c +++ /dev/null @@ -1,361 +0,0 @@ -/* - * crsf.c — CRSF/ExpressLRS RC receiver driver - * - * Hardware: UART4, PA0=TX PA1=RX (GPIO_AF8_UART4), 420000 baud 8N1 - * DMA: DMA1 Stream2 Channel4 (UART4_RX), circular 64-byte buffer - * UART IDLE interrupt → drain; DMA half/complete callbacks → drain - * - * CRSF frame layout: - * [0xC8] [LEN] [TYPE] [PAYLOAD...] [CRC8-DVB-S2] - * LEN = bytes after itself = TYPE + PAYLOAD + CRC (max 62) - * CRC covers TYPE through last PAYLOAD byte (not SYNC, not LEN, not CRC) - * - * Supported frame types: - * 0x16 — RC channels (22 bytes, 16 ch × 11 bit packed) - * 0x14 — Link statistics (RSSI, LQ, SNR) - * - * Channel mapping (0-indexed): - * CH1 [0] = steering (-1000..+1000) - * CH2 [1] = speed/lean override (future use) - * CH5 [4] = arm switch (> CRSF_ARM_THRESHOLD = armed) - * CH6 [5] = mode (< 992 = RC, > 992 = auto) - * - * Protocol reference: ExpressLRS/CRSF_Spec, verified against Betaflight src/main/rx/crsf.c - */ - -#include "crsf.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include - -/* ------------------------------------------------------------------ */ -/* DMA circular receive buffer */ -/* ------------------------------------------------------------------ */ -#define CRSF_DMA_BUF_SIZE 64u /* must be power-of-2 >= 2× max frame (26) */ - -static uint8_t s_dma_buf[CRSF_DMA_BUF_SIZE]; -static volatile uint16_t s_dma_head = 0; /* last processed position */ - -static UART_HandleTypeDef s_uart; -static DMA_HandleTypeDef s_dma_rx; - -/* ------------------------------------------------------------------ */ -/* Frame parser state */ -/* ------------------------------------------------------------------ */ -#define CRSF_SYNC 0xC8u -#define CRSF_FRAME_RC 0x16u /* RC channels packed */ -#define CRSF_FRAME_LINK 0x14u /* Link statistics */ -#define CRSF_MAX_FRAME_LEN 64u - -typedef enum { ST_SYNC, ST_LEN, ST_DATA } parse_state_t; - -static parse_state_t s_ps = ST_SYNC; -static uint8_t s_frame[CRSF_MAX_FRAME_LEN]; -static uint8_t s_flen = 0; /* total expected frame bytes */ -static uint8_t s_fpos = 0; /* bytes received so far */ - -/* ------------------------------------------------------------------ */ -/* Public state */ -/* ------------------------------------------------------------------ */ -volatile CRSFState crsf_state = {0}; - -/* ------------------------------------------------------------------ */ -/* CRC8 DVB-S2 — polynomial 0xD5 */ -/* ------------------------------------------------------------------ */ -static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a) { - crc ^= a; - for (int i = 0; i < 8; i++) { - crc = (crc & 0x80u) ? ((crc << 1) ^ 0xD5u) : (crc << 1); - } - return crc; -} - -static uint8_t crsf_frame_crc(const uint8_t *frame, uint8_t frame_len) { - /* CRC covers frame[2] (type) .. frame[frame_len-2] (last payload byte) */ - uint8_t crc = 0; - for (uint8_t i = 2; i < frame_len - 1; i++) { - crc = crc8_dvb_s2(crc, frame[i]); - } - return crc; -} - -/* ------------------------------------------------------------------ */ -/* 11-bit channel unpacking — 16 channels from 22 bytes */ -/* ------------------------------------------------------------------ */ -static void unpack_channels(const uint8_t *payload, uint16_t *ch) { - uint32_t bits = 0; - int loaded = 0, idx = 0, src = 0; - while (idx < 16) { - while (loaded < 11 && src < 22) { - bits |= (uint32_t)payload[src++] << loaded; - loaded += 8; - } - ch[idx++] = bits & 0x7FFu; - bits >>= 11; - loaded -= 11; - } -} - -/* ------------------------------------------------------------------ */ -/* Frame processing */ -/* ------------------------------------------------------------------ */ -static void process_frame(const uint8_t *frame, uint8_t frame_len) { - /* Validate minimum length and CRC */ - if (frame_len < 4) return; - if (frame[frame_len - 1] != crsf_frame_crc(frame, frame_len)) return; - - uint8_t type = frame[2]; - const uint8_t *payload = &frame[3]; - uint8_t payload_len = frame_len - 4; /* type + payload + crc = frame[1], minus type and crc */ - - switch (type) { - case CRSF_FRAME_RC: - if (payload_len < 22) return; - unpack_channels(payload, (uint16_t *)crsf_state.channels); - crsf_state.last_rx_ms = HAL_GetTick(); - /* Update arm switch state from CH5 (index 4) */ - crsf_state.armed = (crsf_state.channels[4] > CRSF_ARM_THRESHOLD); - break; - - case CRSF_FRAME_LINK: - /* Link stats payload: - * [0] uplink RSSI ant1 (value = -dBm, so negate for dBm) - * [2] uplink link quality (0-100 %) - * [3] uplink SNR (signed dB) - */ - if (payload_len < 4) return; - crsf_state.rssi_dbm = -(int8_t)payload[0]; - crsf_state.link_quality = payload[2]; - crsf_state.snr = (int8_t)payload[3]; - break; - - default: - break; - } -} - -/* ------------------------------------------------------------------ */ -/* Byte-level parser state machine */ -/* ------------------------------------------------------------------ */ -static void parse_byte(uint8_t b) { - switch (s_ps) { - case ST_SYNC: - if (b == CRSF_SYNC) { - s_frame[0] = b; - s_ps = ST_LEN; - } - break; - - case ST_LEN: - /* LEN = bytes remaining after this field (type + payload + crc), max 62 */ - if (b >= 2 && b <= 62) { - s_frame[1] = b; - s_flen = b + 2u; /* total frame = SYNC + LEN + rest */ - s_fpos = 2; - s_ps = ST_DATA; - } else { - s_ps = ST_SYNC; /* invalid length — resync */ - } - break; - - case ST_DATA: - s_frame[s_fpos++] = b; - if (s_fpos >= s_flen) { - process_frame(s_frame, s_flen); - s_ps = ST_SYNC; - } - break; - } -} - -/* ------------------------------------------------------------------ */ -/* DMA buffer drain — called from IDLE IRQ and DMA half/complete CBs */ -/* ------------------------------------------------------------------ */ -static void dma_drain(void) { - /* DMA CNDTR counts DOWN from buf size; write position = size - CNDTR */ - uint16_t pos = (uint16_t)(CRSF_DMA_BUF_SIZE - __HAL_DMA_GET_COUNTER(&s_dma_rx)); - uint16_t head = s_dma_head; - while (head != pos) { - parse_byte(s_dma_buf[head]); - head = (head + 1u) & (CRSF_DMA_BUF_SIZE - 1u); - } - s_dma_head = pos; -} - -/* ------------------------------------------------------------------ */ -/* IRQ handlers */ -/* ------------------------------------------------------------------ */ -void UART4_IRQHandler(void) { - /* IDLE line detection — fires when bus goes quiet between frames */ - if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) { - __HAL_UART_CLEAR_IDLEFLAG(&s_uart); - dma_drain(); - } - HAL_UART_IRQHandler(&s_uart); -} - -void DMA1_Stream2_IRQHandler(void) { - HAL_DMA_IRQHandler(&s_dma_rx); -} - -/* DMA half-complete: drain first half of circular buffer */ -void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *h) { - if (h->Instance == UART4) dma_drain(); -} - -/* DMA complete: drain second half (buffer wrapped) */ -void HAL_UART_RxCpltCallback(UART_HandleTypeDef *h) { - if (h->Instance == UART4) dma_drain(); - if (h->Instance == USART6) { - extern void jetson_uart_rx_callback(UART_HandleTypeDef *huart); - jetson_uart_rx_callback(h); - } -} - -/* ------------------------------------------------------------------ */ -/* Public API */ -/* ------------------------------------------------------------------ */ - -/* - * crsf_init() — configure UART4 + DMA1 and start circular receive. - * - * UART4: PA0=TX, PA1=RX, AF8_UART4, 420000 baud 8N1, oversampling×8. - * APB1 = 54 MHz → BRR = 0x101 → actual 418604 baud (0.33% error, within CRSF spec). - * - * DMA: DMA1 Stream2 Channel4, peripheral→memory, circular, byte width. - * IDLE interrupt + DMA half/complete callbacks drain the circular buffer. - */ -void crsf_init(void) { - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_UART4_CLK_ENABLE(); - __HAL_RCC_DMA1_CLK_ENABLE(); - - /* PA0=TX, PA1=RX, AF8_UART4 */ - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_0 | GPIO_PIN_1; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_PULLUP; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; - gpio.Alternate = GPIO_AF8_UART4; - HAL_GPIO_Init(GPIOA, &gpio); - - /* DMA1 Stream2 Channel4 — UART4_RX, circular byte transfers */ - s_dma_rx.Instance = DMA1_Stream2; - s_dma_rx.Init.Channel = DMA_CHANNEL_4; - s_dma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; - s_dma_rx.Init.PeriphInc = DMA_PINC_DISABLE; - s_dma_rx.Init.MemInc = DMA_MINC_ENABLE; - s_dma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - s_dma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - s_dma_rx.Init.Mode = DMA_CIRCULAR; - s_dma_rx.Init.Priority = DMA_PRIORITY_LOW; - s_dma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - HAL_DMA_Init(&s_dma_rx); - __HAL_LINKDMA(&s_uart, hdmarx, s_dma_rx); - - /* UART4: 420000 8N1, oversampling×8 (better tolerance at high baud) */ - s_uart.Instance = UART4; - s_uart.Init.BaudRate = 420000; - s_uart.Init.WordLength = UART_WORDLENGTH_8B; - s_uart.Init.StopBits = UART_STOPBITS_1; - s_uart.Init.Parity = UART_PARITY_NONE; - s_uart.Init.Mode = UART_MODE_TX_RX; - s_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE; - s_uart.Init.OverSampling = UART_OVERSAMPLING_8; - HAL_UART_Init(&s_uart); - - /* Start circular DMA — runs indefinitely, no need to restart */ - HAL_UART_Receive_DMA(&s_uart, s_dma_buf, CRSF_DMA_BUF_SIZE); - - /* IDLE line interrupt — fires when UART bus goes quiet (end of frame) */ - __HAL_UART_ENABLE_IT(&s_uart, UART_IT_IDLE); - - HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 6, 0); - HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn); - HAL_NVIC_SetPriority(UART4_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(UART4_IRQn); -} - -/* - * crsf_parse_byte() — kept for compatibility; direct call path not used - * when DMA is active, but available for unit testing or UART-IT fallback. - */ -void crsf_parse_byte(uint8_t byte) { - parse_byte(byte); -} - -/* - * crsf_to_range() — map raw CRSF value 172–1811 to [min, max]. - * Midpoint 992 maps to (min+max)/2. - */ -int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max) { - int32_t v = (int32_t)val; - int32_t r = min + (v - 172) * (int32_t)(max - min) / (1811 - 172); - if (r < min) r = min; - if (r > max) r = max; - return (int16_t)r; -} - -/* ------------------------------------------------------------------ */ -/* Telemetry TX helpers */ -/* ------------------------------------------------------------------ */ - -/* - * Build a CRSF frame in `buf` and return the total byte count. - * buf must be at least CRSF_MAX_FRAME_LEN bytes. - * frame_type : CRSF type byte (e.g. 0x08 battery, 0x21 flight mode) - * payload : frame payload bytes (excluding type, CRC) - * plen : payload length in bytes - */ -static uint8_t crsf_build_frame(uint8_t *buf, uint8_t frame_type, - const uint8_t *payload, uint8_t plen) { - /* Total frame = SYNC + LEN + TYPE + PAYLOAD + CRC */ - uint8_t frame_len = 2u + 1u + plen + 1u; /* SYNC + LEN + TYPE + payload + CRC */ - if (frame_len > CRSF_MAX_FRAME_LEN) return 0; - - buf[0] = CRSF_SYNC; /* 0xC8 */ - buf[1] = (uint8_t)(plen + 2u); /* LEN = TYPE + payload + CRC */ - buf[2] = frame_type; - memcpy(&buf[3], payload, plen); - buf[frame_len - 1] = crsf_frame_crc(buf, frame_len); - return frame_len; -} - -/* - * crsf_send_battery() — type 0x08 battery sensor. - * voltage_mv → units of 100 mV (big-endian uint16) - * capacity_mah → remaining capacity in mAh (Issue #325, coulomb counter) - * remaining_pct→ 0–100 % (uint8) - */ -void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah, - uint8_t remaining_pct) { - uint16_t v100 = (uint16_t)(voltage_mv / 100u); /* 100 mV units */ - /* Convert capacity (mAh) to 3-byte big-endian: cap_hi, cap_mid, cap_lo */ - uint32_t cap = capacity_mah & 0xFFFFFFu; /* 24-bit cap max */ - /* Payload: [v_hi][v_lo][current_hi][current_lo][cap_hi][cap_mid][cap_lo][remaining] */ - uint8_t payload[8] = { - (uint8_t)(v100 >> 8), (uint8_t)(v100 & 0xFF), - 0, 0, /* current: not available on STM32, always 0 for now */ - (uint8_t)((cap >> 16) & 0xFF), /* cap_hi */ - (uint8_t)((cap >> 8) & 0xFF), /* cap_mid */ - (uint8_t)(cap & 0xFF), /* cap_lo */ - remaining_pct, - }; - uint8_t frame[CRSF_MAX_FRAME_LEN]; - uint8_t flen = crsf_build_frame(frame, 0x08u, payload, sizeof(payload)); - if (flen) HAL_UART_Transmit(&s_uart, frame, flen, 5u); -} - -/* - * crsf_send_flight_mode() — type 0x21 flight mode text. - * Displays on the handset's OSD/status bar. - * "ARMED\0" when armed (5 payload bytes + null) - * "DISARM\0" when not (7 payload bytes + null) - */ -void crsf_send_flight_mode(bool armed) { - const char *text = armed ? "ARMED" : "DISARM"; - uint8_t plen = (uint8_t)(strlen(text) + 1u); /* include null terminator */ - uint8_t frame[CRSF_MAX_FRAME_LEN]; - uint8_t flen = crsf_build_frame(frame, 0x21u, (const uint8_t *)text, plen); - if (flen) HAL_UART_Transmit(&s_uart, frame, flen, 5u); -} diff --git a/legacy/stm32/src/encoder_odom.c b/legacy/stm32/src/encoder_odom.c deleted file mode 100644 index f8031f3..0000000 --- a/legacy/stm32/src/encoder_odom.c +++ /dev/null @@ -1,347 +0,0 @@ -/* - * encoder_odom.c — quadrature encoder reading and differential-drive - * odometry for Issue #632. - * - * TIM2 (32-bit) = left encoder, TIM3 (16-bit) = right encoder. - * Both configured in encoder mode 3 (count on both A and B edges × 4). - * - * RPM formula: - * rpm = delta_ticks * 60 / (ticks_per_rev * dt_s) - * - * Odometry (Euler-forward differential drive): - * d_l = delta_left * meters_per_tick - * d_r = delta_right * meters_per_tick - * d_c = (d_l + d_r) / 2 - * dθ = (d_r - d_l) / wheel_base_m - * x += d_c * cos(θ) - * y += d_c * sin(θ) - * θ += dθ - */ - -#include "encoder_odom.h" -#include "jlink.h" -#include -#include - -/* ---- Platform abstraction (stubbed in TEST_HOST builds) ---- */ - -#ifndef TEST_HOST - -#include "stm32f7xx_hal.h" -#include "config.h" - -/* Read 32-bit left encoder counter (TIM2) */ -static inline uint32_t enc_read_left(void) -{ - return (uint32_t)TIM2->CNT; -} - -/* Read 16-bit right encoder counter (TIM3) */ -static inline uint16_t enc_read_right(void) -{ - return (uint16_t)TIM3->CNT; -} - -/* Configure TIM2 in encoder mode (left wheel) */ -static void enc_tim2_init(void) -{ - /* Enable peripheral clock */ - __HAL_RCC_TIM2_CLK_ENABLE(); - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); - - /* PA15 = TIM2_CH1 (AF1), PB3 = TIM2_CH2 (AF1) — floating input */ - GPIO_InitTypeDef g = {0}; - g.Mode = GPIO_MODE_AF_PP; - g.Pull = GPIO_PULLUP; - g.Speed = GPIO_SPEED_FREQ_LOW; - g.Alternate = GPIO_AF1_TIM2; - - g.Pin = GPIO_PIN_15; - HAL_GPIO_Init(GPIOA, &g); - - g.Pin = GPIO_PIN_3; - HAL_GPIO_Init(GPIOB, &g); - - /* Encoder mode 3: count on both TI1 and TI2 edges */ - TIM2->CR1 = 0; - TIM2->SMCR = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1; /* SMS = 0x3 */ - TIM2->CCMR1 = (TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0); /* IC1→TI1, IC2→TI2 */ - TIM2->CCER = 0; /* no polarity inversion (change CC1P/CC2P to reverse) */ - TIM2->ARR = 0xFFFFFFFFUL; - TIM2->CNT = 0; - TIM2->CR1 |= TIM_CR1_CEN; -} - -/* Configure TIM3 in encoder mode (right wheel) */ -static void enc_tim3_init(void) -{ - __HAL_RCC_TIM3_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); - - /* PC6 = TIM3_CH1 (AF2), PC7 = TIM3_CH2 (AF2) */ - GPIO_InitTypeDef g = {0}; - g.Mode = GPIO_MODE_AF_PP; - g.Pull = GPIO_PULLUP; - g.Speed = GPIO_SPEED_FREQ_LOW; - g.Alternate = GPIO_AF2_TIM3; - - g.Pin = GPIO_PIN_6 | GPIO_PIN_7; - HAL_GPIO_Init(GPIOC, &g); - - TIM3->CR1 = 0; - TIM3->SMCR = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1; - TIM3->CCMR1 = (TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0); - TIM3->CCER = 0; - TIM3->ARR = 0xFFFF; - TIM3->CNT = 0; - TIM3->CR1 |= TIM_CR1_CEN; -} - -/* Flash helpers */ -static bool flash_write_words(uint32_t addr, const void *data, uint32_t len) -{ - const uint32_t *src = (const uint32_t *)data; - uint32_t words = (len + 3u) / 4u; - for (uint32_t i = 0; i < words; i++) { - if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, - addr + i * 4u, src[i]) != HAL_OK) { - return false; - } - } - return true; -} - -static bool flash_erase_sector7(void) -{ - FLASH_EraseInitTypeDef erase = { - .TypeErase = FLASH_TYPEERASE_SECTORS, - .Sector = FLASH_SECTOR_7, - .NbSectors = 1, - .VoltageRange = FLASH_VOLTAGE_RANGE_3, - }; - uint32_t err = 0; - return (HAL_FLASHEx_Erase(&erase, &err) == HAL_OK) && - (err == 0xFFFFFFFFUL); -} - -#else /* TEST_HOST stubs */ - -/* Test-controlled counter values */ -static uint32_t g_enc_left_cnt = 0; -static uint16_t g_enc_right_cnt = 0; - -static inline uint32_t enc_read_left(void) { return g_enc_left_cnt; } -static inline uint16_t enc_read_right(void) { return g_enc_right_cnt; } - -static void enc_tim2_init(void) {} -static void enc_tim3_init(void) {} - -static bool g_flash_erase_ok = true; -static bool g_flash_write_ok = true; -static enc_flash_config_t g_flash_store; -static bool g_flash_has_data = false; - -static bool flash_erase_sector7(void) { return g_flash_erase_ok; } -static bool flash_write_words(uint32_t addr, const void *data, uint32_t len) -{ - (void)addr; - if (!g_flash_write_ok) return false; - memcpy(&g_flash_store, data, len < sizeof(g_flash_store) ? len : sizeof(g_flash_store)); - g_flash_has_data = true; - return true; -} - -#endif /* TEST_HOST */ - -/* ---- Config defaults ---- */ -static void cfg_set_defaults(enc_config_t *cfg) -{ - cfg->ticks_per_rev = ENC_TICKS_PER_REV_DEFAULT; - cfg->wheel_diam_mm = ENC_WHEEL_DIAM_MM_DEFAULT; - cfg->wheel_base_mm = ENC_WHEEL_BASE_MM_DEFAULT; -} - -/* ---- Pre-compute derived constants from config ---- */ -static void enc_update_precomputed(encoder_odom_t *eo) -{ - eo->meters_per_tick = (3.14159265358979f * (float)eo->cfg.wheel_diam_mm * 0.001f) - / (float)eo->cfg.ticks_per_rev; - eo->wheel_base_m = (float)eo->cfg.wheel_base_mm * 0.001f; -} - -/* ---- encoder_odom_load_config() ---- */ -bool encoder_odom_load_config(enc_config_t *cfg) -{ -#ifndef TEST_HOST - const enc_flash_config_t *p = (const enc_flash_config_t *)ENC_FLASH_ADDR; - if (p->magic == ENC_FLASH_MAGIC && - p->ticks_per_rev > 0u && - p->wheel_diam_mm > 0u && - p->wheel_base_mm > 0u) - { - cfg->ticks_per_rev = p->ticks_per_rev; - cfg->wheel_diam_mm = p->wheel_diam_mm; - cfg->wheel_base_mm = p->wheel_base_mm; - return true; - } -#else - if (g_flash_has_data && g_flash_store.magic == ENC_FLASH_MAGIC && - g_flash_store.ticks_per_rev > 0u && - g_flash_store.wheel_diam_mm > 0u && - g_flash_store.wheel_base_mm > 0u) - { - cfg->ticks_per_rev = g_flash_store.ticks_per_rev; - cfg->wheel_diam_mm = g_flash_store.wheel_diam_mm; - cfg->wheel_base_mm = g_flash_store.wheel_base_mm; - return true; - } -#endif - cfg_set_defaults(cfg); - return false; -} - -/* ---- encoder_odom_save_config() ---- */ -bool encoder_odom_save_config(const enc_config_t *cfg) -{ - enc_flash_config_t rec; - memset(&rec, 0xFF, sizeof(rec)); - rec.magic = ENC_FLASH_MAGIC; - rec.ticks_per_rev = cfg->ticks_per_rev; - rec.wheel_diam_mm = cfg->wheel_diam_mm; - rec.wheel_base_mm = cfg->wheel_base_mm; - -#ifndef TEST_HOST - HAL_FLASH_Unlock(); - bool ok = flash_erase_sector7() && - flash_write_words(ENC_FLASH_ADDR, &rec, sizeof(rec)); - HAL_FLASH_Lock(); - return ok; -#else - return flash_write_words(ENC_FLASH_ADDR, &rec, sizeof(rec)); -#endif -} - -/* ---- encoder_odom_init() ---- */ -void encoder_odom_init(encoder_odom_t *eo) -{ - memset(eo, 0, sizeof(*eo)); - - /* Load or default config */ - encoder_odom_load_config(&eo->cfg); - enc_update_precomputed(eo); - - /* Initialize timers */ - enc_tim2_init(); - enc_tim3_init(); - - /* Snapshot initial counter values (treat as zero reference) */ - eo->cnt_left = enc_read_left(); - eo->cnt_right = enc_read_right(); - - /* Initialize TLM timer so first call fires immediately */ - eo->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (ENC_TLM_HZ > 0u ? ENC_TLM_HZ : 1u))); -} - -/* ---- encoder_odom_reset_pose() ---- */ -void encoder_odom_reset_pose(encoder_odom_t *eo) -{ - eo->x_mm = 0.0f; - eo->y_mm = 0.0f; - eo->theta_rad = 0.0f; -} - -/* ---- encoder_odom_tick() ---- */ -void encoder_odom_tick(encoder_odom_t *eo, uint32_t now_ms) -{ - /* Compute dt */ - uint32_t elapsed_ms = now_ms - eo->last_tick_ms; - if (elapsed_ms == 0u) return; /* avoid divide-by-zero at high call rates */ - if (elapsed_ms > 500u) elapsed_ms = 500u; /* clamp: stale data guard */ - float dt_s = (float)elapsed_ms * 0.001f; - eo->last_tick_ms = now_ms; - - /* Read counters */ - uint32_t new_left = enc_read_left(); - uint16_t new_right = enc_read_right(); - - /* Signed delta — handles 32/16-bit wrap correctly */ - int32_t delta_left = (int32_t)(new_left - eo->cnt_left); - int16_t delta_right = (int16_t)(new_right - eo->cnt_right); - - eo->cnt_left = new_left; - eo->cnt_right = (uint16_t)new_right; - - /* RPM */ - float rpm_scale = 60.0f / ((float)eo->cfg.ticks_per_rev * dt_s); - float rpm_l = (float)delta_left * rpm_scale; - float rpm_r = (float)delta_right * rpm_scale; - - /* Clamp to int16 range */ - if (rpm_l > 32767.0f) rpm_l = 32767.0f; - if (rpm_l < -32768.0f) rpm_l = -32768.0f; - if (rpm_r > 32767.0f) rpm_r = 32767.0f; - if (rpm_r < -32768.0f) rpm_r = -32768.0f; - eo->rpm_left = (int16_t)rpm_l; - eo->rpm_right = (int16_t)rpm_r; - - /* Wheel displacements (metres) */ - float d_left = (float)delta_left * eo->meters_per_tick; - float d_right = (float)delta_right * eo->meters_per_tick; - - /* Linear / angular increments */ - float d_center = (d_left + d_right) * 0.5f; - float d_theta = (d_right - d_left) / eo->wheel_base_m; - - /* Linear speed (mm/s), clamped to int16 */ - float speed_raw = (d_center / dt_s) * 1000.0f; /* m/s → mm/s */ - if (speed_raw > 32767.0f) speed_raw = 32767.0f; - if (speed_raw < -32768.0f) speed_raw = -32768.0f; - eo->speed_mmps = (int16_t)speed_raw; - - /* Integrate pose (Euler-forward) */ - float cos_h = cosf(eo->theta_rad); - float sin_h = sinf(eo->theta_rad); - eo->x_mm += d_center * cos_h * 1000.0f; - eo->y_mm += d_center * sin_h * 1000.0f; - eo->theta_rad += d_theta; - - /* Wrap theta to (-π, π] */ - while (eo->theta_rad > 3.14159265358979f) eo->theta_rad -= 2.0f * 3.14159265358979f; - while (eo->theta_rad < -3.14159265358979f) eo->theta_rad += 2.0f * 3.14159265358979f; -} - -/* ---- encoder_odom_send_tlm() ---- */ -void encoder_odom_send_tlm(const encoder_odom_t *eo, uint32_t now_ms) -{ - if (ENC_TLM_HZ == 0u) return; - - uint32_t interval_ms = 1000u / ENC_TLM_HZ; - if ((now_ms - eo->last_tlm_ms) < interval_ms) return; - /* Cast away const for timestamp — only mutable field */ - ((encoder_odom_t *)eo)->last_tlm_ms = now_ms; - - jlink_tlm_odom_t tlm; - tlm.rpm_left = eo->rpm_left; - tlm.rpm_right = eo->rpm_right; - - /* x/y: float mm → int32_t mm (saturate at ±2147 km — more than adequate) */ - float xf = eo->x_mm; - float yf = eo->y_mm; - if (xf > 2147483647.0f) xf = 2147483647.0f; - if (xf < -2147483648.0f) xf = -2147483648.0f; - if (yf > 2147483647.0f) yf = 2147483647.0f; - if (yf < -2147483648.0f) yf = -2147483648.0f; - tlm.x_mm = (int32_t)xf; - tlm.y_mm = (int32_t)yf; - - /* theta: radians → centidegrees (0.01° steps) */ - float theta_cdeg = eo->theta_rad * (18000.0f / 3.14159265358979f); - if (theta_cdeg > 32767.0f) theta_cdeg = 32767.0f; - if (theta_cdeg < -32768.0f) theta_cdeg = -32768.0f; - tlm.theta_cdeg = (int16_t)theta_cdeg; - - tlm.speed_mmps = eo->speed_mmps; - - jlink_send_odom_tlm(&tlm); -} diff --git a/legacy/stm32/src/esc_backend.c b/legacy/stm32/src/esc_backend.c deleted file mode 100644 index 61f4cd6..0000000 --- a/legacy/stm32/src/esc_backend.c +++ /dev/null @@ -1,58 +0,0 @@ -#include "esc_backend.h" -#include - -/* Global active backend (selected at runtime or compile-time) */ -static const esc_backend_t *g_active_backend = NULL; - -void esc_backend_register(const esc_backend_t *backend) { - g_active_backend = backend; - if (backend && backend->init) { - backend->init(); - } -} - -const esc_backend_t *esc_backend_get(void) { - return g_active_backend; -} - -/* High-level convenience wrappers — call through vtable */ - -void esc_init(void) { - if (g_active_backend && g_active_backend->init) { - g_active_backend->init(); - } -} - -void esc_send(int16_t speed, int16_t steer) { - if (g_active_backend && g_active_backend->send) { - g_active_backend->send(speed, steer); - } -} - -void esc_estop(void) { - if (g_active_backend && g_active_backend->estop) { - g_active_backend->estop(); - } -} - -void esc_resume(void) { - if (g_active_backend && g_active_backend->resume) { - g_active_backend->resume(); - } -} - -void esc_get_telemetry(esc_telemetry_t *out) { - if (out) { - /* Zero-fill by default */ - out->speed = 0; - out->steer = 0; - out->voltage_mv = 0; - out->current_ma = 0; - out->temperature_c = 0; - out->fault = 0; - - if (g_active_backend && g_active_backend->get_telemetry) { - g_active_backend->get_telemetry(out); - } - } -} diff --git a/legacy/stm32/src/esc_hoverboard.c b/legacy/stm32/src/esc_hoverboard.c deleted file mode 100644 index 2f20854..0000000 --- a/legacy/stm32/src/esc_hoverboard.c +++ /dev/null @@ -1,183 +0,0 @@ -#include "esc_backend.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#ifdef DEBUG_MOTOR_TEST -#include -#endif - -/* - * Hoverboard ESC Backend Implementation - * - * Adapts the Hoverboard EFeru FOC protocol to the ESC backend vtable. - * UART2: PA2=TX, PA3=RX @ 115200 baud - * - * Packet: [0xABCD] [steer:i16] [speed:i16] [checksum:u16] - * Checksum = start ^ steer ^ speed - * Speed range: -1000 to +1000 - * Must send at >=50Hz or ESC times out. - */ - -#define HOVERBOARD_START_FRAME 0xABCD -#define HOVERBOARD_BAUD 115200 - -typedef struct __attribute__((packed)) { - uint16_t start; - int16_t steer; - int16_t speed; - uint16_t checksum; -} hoverboard_cmd_t; - -#ifdef DEBUG_MOTOR_TEST -UART_HandleTypeDef huart2; /* non-static: exposed for jetson_uart.c R command */ -#else -static UART_HandleTypeDef huart2; -#endif - -/* Backend vtable instance */ -static const esc_backend_t hoverboard_backend; - -/* - * Initialize UART2 for hoverboard communication. - * Called once at startup via backend registration. - */ -static void hoverboard_backend_init(void) { - /* Enable clocks */ - __HAL_RCC_UART5_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); - - /* PA2=TX, PA3=RX, AF7 for USART2 */ - GPIO_InitTypeDef gpio = {0}; - // UART5: PC12=TX, PD2=RX - __HAL_RCC_GPIOD_CLK_ENABLE(); - gpio.Pin = GPIO_PIN_12; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_PULLUP; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; - gpio.Alternate = GPIO_AF8_UART5; - HAL_GPIO_Init(GPIOC, &gpio); - - // RX: PD2 - gpio.Pin = GPIO_PIN_2; - gpio.Alternate = GPIO_AF8_UART5; - HAL_GPIO_Init(GPIOD, &gpio); - - /* USART2 config */ - huart2.Instance = UART5; - huart2.Init.BaudRate = HOVERBOARD_BAUD; - huart2.Init.WordLength = UART_WORDLENGTH_8B; - huart2.Init.StopBits = UART_STOPBITS_1; - huart2.Init.Parity = UART_PARITY_NONE; - huart2.Init.Mode = UART_MODE_TX_RX; - huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; - huart2.Init.OverSampling = UART_OVERSAMPLING_16; - HAL_UART_Init(&huart2); - -#ifdef DEBUG_MOTOR_TEST - /* Diagnostic: report UART5 register state on USART6 after init */ - { - extern void jetson_uart_send(const uint8_t *data, uint16_t len); - char diag[128]; - uint32_t brr = UART5->BRR; - uint32_t cr1 = UART5->CR1; - uint32_t isr = UART5->ISR; - uint32_t apb1 = HAL_RCC_GetPCLK1Freq(); - /* Also read GPIOC MODER to verify PC12 is in AF mode (bits 25:24 = 10) */ - uint32_t moder = GPIOC->MODER; - uint32_t pc12_mode = (moder >> 24) & 0x3; /* 0=input 1=output 2=AF 3=analog */ - uint32_t afr = GPIOC->AFR[1]; /* AFR high for pins 8-15 */ - uint32_t pc12_af = (afr >> 16) & 0xF; /* AF for pin 12 */ - int n = snprintf(diag, sizeof(diag), - "UART5: BRR=%lu CR1=0x%lX ISR=0x%lX APB1=%luHz PC12mode=%lu AF=%lu\n", - (unsigned long)brr, (unsigned long)cr1, (unsigned long)isr, - (unsigned long)apb1, (unsigned long)pc12_mode, (unsigned long)pc12_af); - /* Delay to let USART6 finish boot banner first */ - HAL_Delay(100); - jetson_uart_send((uint8_t*)diag, n); - } -#endif /* DEBUG_MOTOR_TEST */ -} - -/* - * Send motor command via hoverboard protocol. - * Called at ~50Hz from motor_driver_update(). - */ -#ifdef DEBUG_MOTOR_TEST -static volatile uint32_t hover_tx_count = 0; -#endif - -static void hoverboard_backend_send(int16_t speed, int16_t steer) { - hoverboard_cmd_t cmd; - cmd.start = HOVERBOARD_START_FRAME; - cmd.steer = steer; - cmd.speed = speed; - cmd.checksum = cmd.start ^ cmd.steer ^ cmd.speed; - -#ifdef DEBUG_MOTOR_TEST - HAL_StatusTypeDef rc = HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5); - hover_tx_count++; - - /* Debug: every 50th send, report status on USART6 */ - if (hover_tx_count % 50 == 1) { - extern void jetson_uart_send(const uint8_t *data, uint16_t len); - char dbg[64]; - int n = snprintf(dbg, sizeof(dbg), "ESC tx=%lu rc=%d spd=%d str=%d\n", - (unsigned long)hover_tx_count, (int)rc, speed, steer); - jetson_uart_send((uint8_t*)dbg, n); - } -#else - HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5); -#endif -} - -/* - * Emergency stop: send zero and disable motors. - * Hoverboard will disable outputs on repeated zero packets. - */ -static void hoverboard_backend_estop(void) { - hoverboard_backend_send(0, 0); -} - -/* - * Resume after estop (optional, hoverboard auto-resumes on non-zero command). - */ -static void hoverboard_backend_resume(void) { - /* No action needed — next non-zero send will resume */ -} - -/* - * Query telemetry from hoverboard. - * Hoverboard protocol is command-only (no RX in this implementation). - * Return zero telemetry stub; future: add RX feedback. - */ -static void hoverboard_backend_get_telemetry(esc_telemetry_t *out) { - if (out) { - out->speed = 0; - out->steer = 0; - out->voltage_mv = 0; - out->current_ma = 0; - out->temperature_c = 0; - out->fault = 0; - } -} - -/* Hoverboard backend vtable */ -static const esc_backend_t hoverboard_backend = { - .init = hoverboard_backend_init, - .send = hoverboard_backend_send, - .estop = hoverboard_backend_estop, - .resume = hoverboard_backend_resume, - .get_telemetry = hoverboard_backend_get_telemetry, -}; - -/* - * Public functions for backward compatibility. - * These remain for existing code that calls hoverboard_init/hoverboard_send directly. - */ - -void hoverboard_init(void) { - esc_backend_register(&hoverboard_backend); -} - -void hoverboard_send(int16_t speed, int16_t steer) { - esc_send(speed, steer); -} diff --git a/legacy/stm32/src/esc_vesc.c b/legacy/stm32/src/esc_vesc.c deleted file mode 100644 index 9d3ebfb..0000000 --- a/legacy/stm32/src/esc_vesc.c +++ /dev/null @@ -1,70 +0,0 @@ -#include "esc_backend.h" - -/* - * VESC ESC Backend Stub - * - * Placeholder implementation for FSESC 4.20 Plus (VESC-based dual ESC). - * UART: ttyTHS1 (Jetson Orin) → FC USART6 @ 921600 baud - * Protocol: pyvesc with CRC16-XModem checksum - * - * Issue #383: VESC integration (fills in this stub) - * Issue #388: ESC abstraction layer (provides this interface) - * - * TODO (Issue #383): - * - Implement vesc_init() with UART/GPIO config - * - Implement vesc_send() with pyvesc packet encoding (duty/RPM control) - * - Implement vesc_estop() to disable motor controller - * - Implement vesc_get_telemetry() to parse VESC state messages - * - Add balance mode configuration if using VESC balance app - */ - -static const esc_backend_t vesc_backend; - -/* Stub implementations — no-op until #383 fills them in */ - -static void vesc_backend_init(void) { - /* TODO (Issue #383): Initialize UART6, configure VESC balance mode */ -} - -static void vesc_backend_send(int16_t speed, int16_t steer) { - /* TODO (Issue #383): Encode speed/steer to pyvesc packet and send via UART6 */ - (void)speed; - (void)steer; -} - -static void vesc_backend_estop(void) { - /* TODO (Issue #383): Send VESC shutdown command to disable motors */ -} - -static void vesc_backend_resume(void) { - /* TODO (Issue #383): Resume from estop if needed */ -} - -static void vesc_backend_get_telemetry(esc_telemetry_t *out) { - /* TODO (Issue #383): Poll/parse VESC telemetry (voltage, current, RPM, temp, fault) */ - if (out) { - out->speed = 0; - out->steer = 0; - out->voltage_mv = 0; - out->current_ma = 0; - out->temperature_c = 0; - out->fault = 0; - } -} - -/* VESC backend vtable */ -static const esc_backend_t vesc_backend = { - .init = vesc_backend_init, - .send = vesc_backend_send, - .estop = vesc_backend_estop, - .resume = vesc_backend_resume, - .get_telemetry = vesc_backend_get_telemetry, -}; - -/* - * Public function to register VESC backend. - * Called from main.c when configured with ESC_BACKEND=VESC. - */ -void vesc_backend_register_impl(void) { - esc_backend_register(&vesc_backend); -} diff --git a/legacy/stm32/src/face_animation.c b/legacy/stm32/src/face_animation.c deleted file mode 100644 index 6a91e5b..0000000 --- a/legacy/stm32/src/face_animation.c +++ /dev/null @@ -1,307 +0,0 @@ -/* - * face_animation.c — Face Emotion Renderer for LCD Display - * - * Implements expressive face animations with smooth transitions between emotions. - * Supports idle blinking and parameterized eye/mouth shapes for each emotion. - */ - -#include "face_animation.h" -#include "face_lcd.h" -#include -#include - -/* === Configuration === */ -#define TRANSITION_FRAMES 15 /* ~0.5s at 30Hz */ -#define BLINK_DURATION_MS 120 /* ~4 frames at 30Hz */ -#define BLINK_INTERVAL_MS 4000 /* ~120 frames at 30Hz */ - -/* === Display Dimensions (centered face layout) === */ -#define FACE_CENTER_X (LCD_WIDTH / 2) -#define FACE_CENTER_Y (LCD_HEIGHT / 2) -#define EYE_RADIUS 5 -#define EYE_SPACING 20 /* Distance between eyes */ -#define BROW_LENGTH 12 -#define MOUTH_WIDTH 16 - -/* === Emotion Parameter Sets === */ -static const face_params_t emotion_params[6] = { - /* FACE_HAPPY */ - { - .eye_x = -EYE_SPACING/2, .eye_y = -10, - .eye_open_y = 5, .eye_close_y = 0, - .brow_angle = 15, .brow_y_offset = -6, - .mouth_x = 0, .mouth_y = 10, - .mouth_width = MOUTH_WIDTH, .mouth_curve = 4, /* Upturned smile */ - .blink_interval_ms = 120, - }, - /* FACE_SAD */ - { - .eye_x = -EYE_SPACING/2, .eye_y = -8, - .eye_open_y = 5, .eye_close_y = 0, - .brow_angle = -15, .brow_y_offset = -8, - .mouth_x = 0, .mouth_y = 12, - .mouth_width = MOUTH_WIDTH, .mouth_curve = -3, /* Downturned frown */ - .blink_interval_ms = 180, /* Slower blink when sad */ - }, - /* FACE_CURIOUS */ - { - .eye_x = -EYE_SPACING/2, .eye_y = -12, - .eye_open_y = 7, .eye_close_y = 0, /* Wide eyes */ - .brow_angle = 20, .brow_y_offset = -10, /* Raised brows */ - .mouth_x = 2, .mouth_y = 10, - .mouth_width = 12, .mouth_curve = 1, /* Slight smile */ - .blink_interval_ms = 150, - }, - /* FACE_ANGRY */ - { - .eye_x = -EYE_SPACING/2, .eye_y = -6, - .eye_open_y = 3, .eye_close_y = 0, /* Narrowed eyes */ - .brow_angle = -20, .brow_y_offset = -5, - .mouth_x = 0, .mouth_y = 11, - .mouth_width = 14, .mouth_curve = -5, /* Strong frown */ - .blink_interval_ms = 90, /* Angry blinks faster */ - }, - /* FACE_SLEEPING */ - { - .eye_x = -EYE_SPACING/2, .eye_y = -8, - .eye_open_y = 0, .eye_close_y = -2, /* Closed/squinted */ - .brow_angle = 5, .brow_y_offset = -4, - .mouth_x = 0, .mouth_y = 10, - .mouth_width = 10, .mouth_curve = 2, /* Peaceful smile */ - .blink_interval_ms = 60, /* Not used when sleeping */ - }, - /* FACE_NEUTRAL */ - { - .eye_x = -EYE_SPACING/2, .eye_y = -8, - .eye_open_y = 5, .eye_close_y = 0, - .brow_angle = 0, .brow_y_offset = -6, - .mouth_x = 0, .mouth_y = 10, - .mouth_width = 12, .mouth_curve = 0, /* Straight line */ - .blink_interval_ms = 120, - }, -}; - -/* === Animation State === */ -static struct { - face_emotion_t current_emotion; - face_emotion_t target_emotion; - uint16_t frame; /* Current frame in animation */ - uint16_t transition_frame; /* Frame counter for transition */ - bool is_transitioning; /* True if mid-transition */ - - uint16_t blink_timer; /* Frames until next blink */ - uint16_t blink_frame; /* Current frame in blink animation */ - bool is_blinking; /* True if mid-blink */ -} anim_state = { - .current_emotion = FACE_NEUTRAL, - .target_emotion = FACE_NEUTRAL, - .frame = 0, - .transition_frame = 0, - .is_transitioning = false, - .blink_timer = BLINK_INTERVAL_MS / 33, /* ~120 frames */ - .blink_frame = 0, - .is_blinking = false, -}; - -/* === Easing Functions === */ - -/** - * Ease-in-out cubic interpolation [0, 1]. - * Smooth acceleration/deceleration for transitions. - */ -static float ease_in_out_cubic(float t) { - if (t < 0.5f) - return 4.0f * t * t * t; - else { - float f = 2.0f * t - 2.0f; - return 0.5f * f * f * f + 1.0f; - } -} - -/** - * Interpolate two emotion parameters by factor [0, 1]. - */ -static face_params_t interpolate_params(const face_params_t *a, - const face_params_t *b, - float t) { - face_params_t result; - result.eye_x = (int16_t)(a->eye_x + (b->eye_x - a->eye_x) * t); - result.eye_y = (int16_t)(a->eye_y + (b->eye_y - a->eye_y) * t); - result.eye_open_y = (int16_t)(a->eye_open_y + (b->eye_open_y - a->eye_open_y) * t); - result.eye_close_y = (int16_t)(a->eye_close_y + (b->eye_close_y - a->eye_close_y) * t); - result.brow_angle = (int16_t)(a->brow_angle + (b->brow_angle - a->brow_angle) * t); - result.brow_y_offset = (int16_t)(a->brow_y_offset + (b->brow_y_offset - a->brow_y_offset) * t); - result.mouth_x = (int16_t)(a->mouth_x + (b->mouth_x - a->mouth_x) * t); - result.mouth_y = (int16_t)(a->mouth_y + (b->mouth_y - a->mouth_y) * t); - result.mouth_width = (int16_t)(a->mouth_width + (b->mouth_width - a->mouth_width) * t); - result.mouth_curve = (int16_t)(a->mouth_curve + (b->mouth_curve - a->mouth_curve) * t); - return result; -} - -/* === Drawing Functions === */ - -/** - * Draw an eye (circle) with optional closure (eyelid). - */ -static void draw_eye(int16_t x, int16_t y, int16_t open_y, int16_t close_y, - bool is_blinking) { - lcd_color_t color = LCD_WHITE; - - /* Eye position accounts for blink closure */ - int16_t eye_h = is_blinking ? close_y : open_y; - if (eye_h <= 0) { - /* Closed: draw horizontal line instead */ - face_lcd_line(x - EYE_RADIUS, y, x + EYE_RADIUS, y, color); - } else { - /* Open: draw circle (simplified ellipse) */ - face_lcd_circle(x, y, EYE_RADIUS, color); - /* Fill iris pupil */ - face_lcd_fill_rect(x - 2, y - 1, 4, 2, color); - } -} - -/** - * Draw an eyebrow with angle and offset. - */ -static void draw_brow(int16_t x, int16_t y, int16_t angle, int16_t y_offset) { - /* Approximate angled line by adjusting endpoints */ - int16_t brow_y = y + y_offset; - int16_t angle_offset = (angle * BROW_LENGTH) / 45; /* ~1 pixel per 45 degrees */ - - face_lcd_line(x - BROW_LENGTH/2 - angle_offset, brow_y, - x + BROW_LENGTH/2 + angle_offset, brow_y, - LCD_WHITE); -} - -/** - * Draw mouth (curved line or bezier approximation). - */ -static void draw_mouth(int16_t x, int16_t y, int16_t width, int16_t curve) { - /* Simplified mouth: two diagonal lines forming a V or inverted V */ - int16_t mouth_left = x - width / 2; - int16_t mouth_right = x + width / 2; - int16_t mouth_bottom = y + (curve > 0 ? 3 : 0); - - if (curve > 0) { - /* Smile: V shape upturned */ - face_lcd_line(mouth_left, y + 2, x, mouth_bottom, LCD_WHITE); - face_lcd_line(x, mouth_bottom, mouth_right, y + 2, LCD_WHITE); - } else if (curve < 0) { - /* Frown: ^ shape downturned */ - face_lcd_line(mouth_left, y - 2, x, y + 2, LCD_WHITE); - face_lcd_line(x, y + 2, mouth_right, y - 2, LCD_WHITE); - } else { - /* Neutral: straight line */ - face_lcd_line(mouth_left, y, mouth_right, y, LCD_WHITE); - } -} - -/* === Public API Implementation === */ - -void face_animation_init(void) { - anim_state.current_emotion = FACE_NEUTRAL; - anim_state.target_emotion = FACE_NEUTRAL; - anim_state.frame = 0; - anim_state.transition_frame = 0; - anim_state.is_transitioning = false; - anim_state.blink_timer = BLINK_INTERVAL_MS / 33; - anim_state.blink_frame = 0; - anim_state.is_blinking = false; -} - -void face_animation_set_emotion(face_emotion_t emotion) { - if (emotion < 6) { - anim_state.target_emotion = emotion; - anim_state.is_transitioning = true; - anim_state.transition_frame = 0; - } -} - -void face_animation_tick(void) { - anim_state.frame++; - - /* Handle transition */ - if (anim_state.is_transitioning) { - anim_state.transition_frame++; - if (anim_state.transition_frame >= TRANSITION_FRAMES) { - /* Transition complete */ - anim_state.current_emotion = anim_state.target_emotion; - anim_state.is_transitioning = false; - } - } - - /* Handle idle blink */ - if (!anim_state.is_blinking) { - anim_state.blink_timer--; - if (anim_state.blink_timer == 0) { - anim_state.is_blinking = true; - anim_state.blink_frame = 0; - /* Reset timer for next blink */ - anim_state.blink_timer = BLINK_INTERVAL_MS / 33; - } - } else { - /* In blink */ - anim_state.blink_frame++; - if (anim_state.blink_frame >= BLINK_DURATION_MS / 33) { - /* Blink complete */ - anim_state.is_blinking = false; - anim_state.blink_frame = 0; - } - } -} - -void face_animation_render(void) { - /* Clear display */ - face_lcd_clear(); - - /* Get current emotion parameters (interpolated if transitioning) */ - face_params_t params; - if (anim_state.is_transitioning) { - float t = ease_in_out_cubic((float)anim_state.transition_frame / - TRANSITION_FRAMES); - params = interpolate_params( - &emotion_params[anim_state.current_emotion], - &emotion_params[anim_state.target_emotion], - t); - } else { - params = emotion_params[anim_state.current_emotion]; - } - - /* Draw left eye */ - draw_eye(FACE_CENTER_X + params.eye_x, FACE_CENTER_Y + params.eye_y, - params.eye_open_y, params.eye_close_y, anim_state.is_blinking); - - /* Draw right eye */ - draw_eye(FACE_CENTER_X - params.eye_x, FACE_CENTER_Y + params.eye_y, - params.eye_open_y, params.eye_close_y, anim_state.is_blinking); - - /* Draw left brow */ - draw_brow(FACE_CENTER_X + params.eye_x, FACE_CENTER_Y + params.brow_y_offset, - params.brow_angle, 0); - - /* Draw right brow (mirrored) */ - draw_brow(FACE_CENTER_X - params.eye_x, FACE_CENTER_Y + params.brow_y_offset, - -params.brow_angle, 0); - - /* Draw mouth */ - draw_mouth(FACE_CENTER_X + params.mouth_x, FACE_CENTER_Y + params.mouth_y, - params.mouth_width, params.mouth_curve); - - /* Push framebuffer to display */ - face_lcd_flush(); -} - -face_emotion_t face_animation_get_emotion(void) { - return anim_state.is_transitioning ? anim_state.target_emotion - : anim_state.current_emotion; -} - -void face_animation_blink_now(void) { - anim_state.is_blinking = true; - anim_state.blink_frame = 0; - anim_state.blink_timer = BLINK_INTERVAL_MS / 33; -} - -bool face_animation_is_idle(void) { - return !anim_state.is_transitioning && !anim_state.is_blinking; -} diff --git a/legacy/stm32/src/face_lcd.c b/legacy/stm32/src/face_lcd.c deleted file mode 100644 index 80bd365..0000000 --- a/legacy/stm32/src/face_lcd.c +++ /dev/null @@ -1,191 +0,0 @@ -/* - * face_lcd.c — STM32 LCD Display Driver for Face Animations - * - * Implements low-level LCD framebuffer management and display control. - * Supports 1-bit monochrome displays (SSD1306, etc.) via SPI. - * - * HARDWARE: - * - SPI2 (PB13=SCK, PB14=MISO, PB15=MOSI) — already configured for OSD - * - CS (GPIO) for LCD chip select - * - DC (GPIO) for data/command mode select - * - RES (GPIO) optional reset - * - * NOTE: SPI2 is currently used by OSD (MAX7456). For face LCD, we would - * typically use a separate SPI or I2C. This implementation assumes - * a dedicated I2C or separate SPI interface. Configure LCD_INTERFACE - * in face_lcd.h to match your hardware. - */ - -#include "face_lcd.h" -#include - -/* === State Variables === */ -static uint8_t lcd_framebuffer[LCD_FBSIZE]; -static volatile uint32_t frame_counter = 0; -static volatile bool flush_requested = false; -static volatile bool transfer_busy = false; - -/* === Private Functions === */ - -/** - * Initialize hardware (SPI/I2C) and LCD controller. - * Sends initialization sequence to put display in active mode. - */ -static void lcd_hardware_init(void) { - /* TODO: Implement hardware-specific initialization - * - Configure SPI/I2C pins and clock - * - Send controller init sequence (power on, set contrast, etc.) - * - Clear display - * - * For SSD1306 (common monochrome): - * - Send 0xAE (display off) - * - Set contrast 0x81, 0x7F - * - Set clock div ratio, precharge, comdesat - * - Set address mode, column/page range - * - Send 0xAF (display on) - */ -} - -/** - * Push framebuffer to display via SPI/I2C DMA transfer. - * Handles paging for monochrome displays (8 pixels per byte, horizontal pages). - */ -static void lcd_transfer_fb(void) { - transfer_busy = true; - - /* TODO: Implement DMA/blocking transfer - * For SSD1306 (8-pixel pages): - * For each page (8 rows): - * - Send page address command - * - Send column address command - * - DMA transfer 128 bytes (1 row of page data) - * - * Can use SPI DMA for async transfer or blocking transfer. - * Set transfer_busy=false when complete (in ISR or blocking). - */ - - transfer_busy = false; -} - -/* === Public API Implementation === */ - -void face_lcd_init(void) { - memset(lcd_framebuffer, 0, LCD_FBSIZE); - frame_counter = 0; - flush_requested = false; - transfer_busy = false; - lcd_hardware_init(); -} - -void face_lcd_clear(void) { - memset(lcd_framebuffer, 0, LCD_FBSIZE); -} - -void face_lcd_pixel(uint16_t x, uint16_t y, lcd_color_t color) { - /* Bounds check */ - if (x >= LCD_WIDTH || y >= LCD_HEIGHT) - return; - -#if LCD_BPP == 1 - /* Monochrome: pack 8 pixels per byte, LSB = leftmost pixel */ - uint16_t byte_idx = (y / 8) * LCD_WIDTH + x; - uint8_t bit_pos = y % 8; - - if (color) - lcd_framebuffer[byte_idx] |= (1 << bit_pos); - else - lcd_framebuffer[byte_idx] &= ~(1 << bit_pos); -#else - /* RGB565: 2 bytes per pixel */ - uint16_t pixel_idx = y * LCD_WIDTH + x; - ((uint16_t *)lcd_framebuffer)[pixel_idx] = color; -#endif -} - -void face_lcd_line(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, - lcd_color_t color) { - /* Bresenham line algorithm */ - int16_t dx = (x1 > x0) ? (x1 - x0) : (x0 - x1); - int16_t dy = (y1 > y0) ? (y1 - y0) : (y0 - y1); - int16_t sx = (x0 < x1) ? 1 : -1; - int16_t sy = (y0 < y1) ? 1 : -1; - int16_t err = (dx > dy) ? (dx / 2) : -(dy / 2); - - int16_t x = x0, y = y0; - while (1) { - face_lcd_pixel(x, y, color); - if (x == x1 && y == y1) - break; - int16_t e2 = err; - if (e2 > -dx) { - err -= dy; - x += sx; - } - if (e2 < dy) { - err += dx; - y += sy; - } - } -} - -void face_lcd_circle(uint16_t cx, uint16_t cy, uint16_t r, lcd_color_t color) { - /* Midpoint circle algorithm */ - int16_t x = r, y = 0; - int16_t err = 0; - - while (x >= y) { - face_lcd_pixel(cx + x, cy + y, color); - face_lcd_pixel(cx + y, cy + x, color); - face_lcd_pixel(cx - y, cy + x, color); - face_lcd_pixel(cx - x, cy + y, color); - face_lcd_pixel(cx - x, cy - y, color); - face_lcd_pixel(cx - y, cy - x, color); - face_lcd_pixel(cx + y, cy - x, color); - face_lcd_pixel(cx + x, cy - y, color); - - if (err <= 0) { - y++; - err += 2 * y + 1; - } else { - x--; - err -= 2 * x + 1; - } - } -} - -void face_lcd_fill_rect(uint16_t x, uint16_t y, uint16_t w, uint16_t h, - lcd_color_t color) { - for (uint16_t row = y; row < y + h && row < LCD_HEIGHT; row++) { - for (uint16_t col = x; col < x + w && col < LCD_WIDTH; col++) { - face_lcd_pixel(col, row, color); - } - } -} - -void face_lcd_flush(void) { - flush_requested = true; -} - -bool face_lcd_is_busy(void) { - return transfer_busy; -} - -void face_lcd_tick(void) { - frame_counter++; - - /* Request flush every N frames to achieve LCD_REFRESH_HZ */ - uint32_t frames_per_flush = (1000 / LCD_REFRESH_HZ) / 33; /* ~30ms per frame */ - if (frame_counter % frames_per_flush == 0) { - flush_requested = true; - } - - /* Perform transfer if requested and not busy */ - if (flush_requested && !transfer_busy) { - flush_requested = false; - lcd_transfer_fb(); - } -} - -uint8_t *face_lcd_get_fb(void) { - return lcd_framebuffer; -} diff --git a/legacy/stm32/src/face_uart.c b/legacy/stm32/src/face_uart.c deleted file mode 100644 index 1b23b66..0000000 --- a/legacy/stm32/src/face_uart.c +++ /dev/null @@ -1,175 +0,0 @@ -/* - * face_uart.c — UART Command Interface for Face Animations - * - * Receives emotion commands from Jetson Orin and triggers face animations. - * Text-based protocol over USART3 @ 115200 baud. - */ - -#include "face_uart.h" -#include "face_animation.h" -#include -#include -#include - -/* === Ring Buffer State === */ -static struct { - uint8_t buf[FACE_UART_RX_BUF_SZ]; - uint16_t head; /* Write index (ISR) */ - uint16_t tail; /* Read index (process) */ - uint16_t count; /* Bytes in buffer */ -} rx_buf = {0}; - -/* === Forward Declarations === */ -static void uart_send_response(const char *cmd, const char *status); - -/* === Private Functions === */ - -/** - * Extract a line from RX buffer (newline-terminated). - * Returns length if found, 0 otherwise. - */ -static uint16_t extract_line(char *line, uint16_t max_len) { - uint16_t len = 0; - uint16_t idx = rx_buf.tail; - - /* Scan for newline */ - while (idx != rx_buf.head && len < max_len - 1) { - uint8_t byte = rx_buf.buf[idx]; - if (byte == '\n') { - /* Found end of line */ - for (uint16_t i = 0; i < len; i++) { - line[i] = rx_buf.buf[(rx_buf.tail + i) % FACE_UART_RX_BUF_SZ]; - } - line[len] = '\0'; - - /* Trim trailing whitespace */ - while (len > 0 && (line[len - 1] == '\r' || isspace(line[len - 1]))) { - line[--len] = '\0'; - } - - /* Update tail and count */ - rx_buf.tail = (idx + 1) % FACE_UART_RX_BUF_SZ; - rx_buf.count -= (len + 1 + 1); /* +1 for newline, +1 for any preceding data */ - - return len; - } - - len++; - idx = (idx + 1) % FACE_UART_RX_BUF_SZ; - } - - return 0; /* No complete line */ -} - -/** - * Convert string to uppercase for case-insensitive command matching. - */ -static void str_toupper(char *str) { - for (int i = 0; str[i]; i++) - str[i] = toupper((unsigned char)str[i]); -} - -/** - * Parse and execute a command. - */ -static void parse_command(const char *cmd) { - if (!cmd || !cmd[0]) - return; - - char cmd_upper[32]; - strncpy(cmd_upper, cmd, sizeof(cmd_upper) - 1); - cmd_upper[sizeof(cmd_upper) - 1] = '\0'; - str_toupper(cmd_upper); - - /* Command dispatch */ - if (strcmp(cmd_upper, "HAPPY") == 0) { - face_animation_set_emotion(FACE_HAPPY); - uart_send_response(cmd_upper, "OK"); - } else if (strcmp(cmd_upper, "SAD") == 0) { - face_animation_set_emotion(FACE_SAD); - uart_send_response(cmd_upper, "OK"); - } else if (strcmp(cmd_upper, "CURIOUS") == 0) { - face_animation_set_emotion(FACE_CURIOUS); - uart_send_response(cmd_upper, "OK"); - } else if (strcmp(cmd_upper, "ANGRY") == 0) { - face_animation_set_emotion(FACE_ANGRY); - uart_send_response(cmd_upper, "OK"); - } else if (strcmp(cmd_upper, "SLEEP") == 0 || - strcmp(cmd_upper, "SLEEPING") == 0) { - face_animation_set_emotion(FACE_SLEEPING); - uart_send_response(cmd_upper, "OK"); - } else if (strcmp(cmd_upper, "NEUTRAL") == 0) { - face_animation_set_emotion(FACE_NEUTRAL); - uart_send_response(cmd_upper, "OK"); - } else if (strcmp(cmd_upper, "BLINK") == 0) { - face_animation_blink_now(); - uart_send_response(cmd_upper, "OK"); - } else if (strcmp(cmd_upper, "STATUS") == 0) { - const char *emotion_names[] = {"HAPPY", "SAD", "CURIOUS", "ANGRY", - "SLEEPING", "NEUTRAL"}; - face_emotion_t current = face_animation_get_emotion(); - char status[64]; - snprintf(status, sizeof(status), "EMOTION=%s, IDLE=%s", - emotion_names[current], - face_animation_is_idle() ? "true" : "false"); - uart_send_response(cmd_upper, status); - } else { - uart_send_response(cmd_upper, "ERR: unknown command"); - } -} - -/** - * Send a response string to UART TX. - * Format: "CMD: status\n" - */ -static void uart_send_response(const char *cmd, const char *status) { - /* TODO: Implement UART TX - * Use HAL_UART_Transmit_IT or similar to send: - * "CMD: status\n" - */ - (void)cmd; /* Suppress unused warnings */ - (void)status; -} - -/* === Public API Implementation === */ - -void face_uart_init(void) { - /* TODO: Configure USART3 @ 115200 baud - * - Enable USART3 clock (RCC_APB1ENR) - * - Configure pins (PB10=TX, PB11=RX) - * - Set baud rate to 115200 - * - Enable RX interrupt (NVIC + USART3_IRQn) - * - Enable USART - */ - - rx_buf.head = 0; - rx_buf.tail = 0; - rx_buf.count = 0; -} - -void face_uart_process(void) { - char line[128]; - uint16_t len; - - /* Extract and process complete commands */ - while ((len = extract_line(line, sizeof(line))) > 0) { - parse_command(line); - } -} - -void face_uart_rx_isr(uint8_t byte) { - /* Push byte into ring buffer */ - if (rx_buf.count < FACE_UART_RX_BUF_SZ) { - rx_buf.buf[rx_buf.head] = byte; - rx_buf.head = (rx_buf.head + 1) % FACE_UART_RX_BUF_SZ; - rx_buf.count++; - } - /* Buffer overflow: silently discard oldest byte */ -} - -void face_uart_send(const char *str) { - /* TODO: Implement non-blocking UART TX - * Use HAL_UART_Transmit_IT() or DMA-based TX queue. - */ - (void)str; /* Suppress unused warnings */ -} diff --git a/legacy/stm32/src/fan.c b/legacy/stm32/src/fan.c deleted file mode 100644 index 33e15d3..0000000 --- a/legacy/stm32/src/fan.c +++ /dev/null @@ -1,277 +0,0 @@ -#include "fan.h" -#include "stm32f7xx_hal.h" -#include "config.h" -#include - -/* ================================================================ - * Fan Hardware Configuration - * ================================================================ */ - -#define FAN_PIN GPIO_PIN_9 -#define FAN_PORT GPIOA -#define FAN_TIM TIM1 -#define FAN_TIM_CHANNEL TIM_CHANNEL_2 -#define FAN_PWM_FREQ_HZ 25000 /* 25 kHz for brushless fan */ - -/* ================================================================ - * Temperature Curve Parameters - * ================================================================ */ - -#define TEMP_OFF 40 /* Fan off below this (°C) */ -#define TEMP_LOW 50 /* Low speed threshold (°C) */ -#define TEMP_HIGH 70 /* High speed threshold (°C) */ - -#define SPEED_OFF 0 /* Speed at TEMP_OFF (%) */ -#define SPEED_LOW 30 /* Speed at TEMP_LOW (%) */ -#define SPEED_HIGH 100 /* Speed at TEMP_HIGH (%) */ - -/* ================================================================ - * Internal State - * ================================================================ */ - -typedef struct { - uint8_t current_speed; /* Current speed 0-100% */ - uint8_t target_speed; /* Target speed 0-100% */ - int16_t last_temperature; /* Last temperature reading (°C) */ - float ramp_rate_per_ms; /* Speed change rate (%/ms) */ - uint32_t last_ramp_time_ms; /* When last ramp update occurred */ - bool is_ramping; /* Speed is transitioning */ -} FanState_t; - -static FanState_t s_fan = { - .current_speed = 0, - .target_speed = 0, - .last_temperature = 0, - .ramp_rate_per_ms = 0.05f, /* 5% per 100ms default */ - .last_ramp_time_ms = 0, - .is_ramping = false -}; - -/* ================================================================ - * Hardware Initialization - * ================================================================ */ - -void fan_init(void) -{ - /* Enable GPIO and timer clocks */ - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_TIM1_CLK_ENABLE(); - - /* Configure PA9 as TIM1_CH2 PWM output */ - GPIO_InitTypeDef gpio_init = {0}; - gpio_init.Pin = FAN_PIN; - gpio_init.Mode = GPIO_MODE_AF_PP; - gpio_init.Pull = GPIO_NOPULL; - gpio_init.Speed = GPIO_SPEED_HIGH; - gpio_init.Alternate = GPIO_AF1_TIM1; - HAL_GPIO_Init(FAN_PORT, &gpio_init); - - /* Configure TIM1 for PWM: - * Clock: 216MHz / PSC = output frequency - * For 25kHz frequency: PSC = 346, ARR = 25 - * Duty cycle = CCR / ARR (e.g., 12.5/25 = 50%) - */ - TIM_HandleTypeDef htim1 = {0}; - htim1.Instance = FAN_TIM; - htim1.Init.Prescaler = 346 - 1; /* 216MHz / 346 ≈ 624kHz clock */ - htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = 25 - 1; /* 624kHz / 25 = 25kHz */ - htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim1.Init.RepetitionCounter = 0; - HAL_TIM_PWM_Init(&htim1); - - /* Configure PWM on CH2: 0% duty initially (fan off) */ - TIM_OC_InitTypeDef oc_init = {0}; - oc_init.OCMode = TIM_OCMODE_PWM1; - oc_init.Pulse = 0; /* Start at 0% duty (off) */ - oc_init.OCPolarity = TIM_OCPOLARITY_HIGH; - oc_init.OCFastMode = TIM_OCFAST_DISABLE; - HAL_TIM_PWM_ConfigChannel(&htim1, &oc_init, FAN_TIM_CHANNEL); - - /* Start PWM generation */ - HAL_TIM_PWM_Start(FAN_TIM, FAN_TIM_CHANNEL); - - s_fan.current_speed = 0; - s_fan.target_speed = 0; - s_fan.last_ramp_time_ms = 0; -} - -/* ================================================================ - * Temperature Curve Calculation - * ================================================================ */ - -static uint8_t fan_calculate_speed_from_temp(int16_t temp_celsius) -{ - if (temp_celsius < TEMP_OFF) { - return SPEED_OFF; /* Off below 40°C */ - } - - if (temp_celsius < TEMP_LOW) { - /* Linear ramp from 0% to 30% between 40-50°C */ - int32_t temp_offset = temp_celsius - TEMP_OFF; /* 0-10 */ - int32_t temp_range = TEMP_LOW - TEMP_OFF; /* 10 */ - int32_t speed_range = SPEED_LOW - SPEED_OFF; /* 30 */ - uint8_t speed = SPEED_OFF + (temp_offset * speed_range) / temp_range; - return (speed > 100) ? 100 : speed; - } - - if (temp_celsius < TEMP_HIGH) { - /* Linear ramp from 30% to 100% between 50-70°C */ - int32_t temp_offset = temp_celsius - TEMP_LOW; /* 0-20 */ - int32_t temp_range = TEMP_HIGH - TEMP_LOW; /* 20 */ - int32_t speed_range = SPEED_HIGH - SPEED_LOW; /* 70 */ - uint8_t speed = SPEED_LOW + (temp_offset * speed_range) / temp_range; - return (speed > 100) ? 100 : speed; - } - - return SPEED_HIGH; /* 100% at 70°C and above */ -} - -/* ================================================================ - * PWM Duty Cycle Control - * ================================================================ */ - -static void fan_set_pwm_duty(uint8_t percentage) -{ - /* Clamp to 0-100% */ - if (percentage > 100) percentage = 100; - - /* Convert percentage to PWM counts - * ARR = 25 (0-24 counts for 0-96%, scale up to 25 for 100%) - * Duty = (percentage * 25) / 100 - */ - uint32_t duty = (percentage * 25) / 100; - if (duty > 25) duty = 25; - - /* Update CCR2 for TIM1_CH2 */ - TIM1->CCR2 = duty; -} - -/* ================================================================ - * Public API - * ================================================================ */ - -bool fan_set_speed(uint8_t percentage) -{ - if (percentage > 100) { - return false; - } - - s_fan.current_speed = percentage; - s_fan.target_speed = percentage; - s_fan.is_ramping = false; - fan_set_pwm_duty(percentage); - - return true; -} - -uint8_t fan_get_speed(void) -{ - return s_fan.current_speed; -} - -bool fan_set_target_speed(uint8_t percentage) -{ - if (percentage > 100) { - return false; - } - - s_fan.target_speed = percentage; - if (percentage == s_fan.current_speed) { - s_fan.is_ramping = false; - } else { - s_fan.is_ramping = true; - } - - return true; -} - -void fan_update_temperature(int16_t temp_celsius) -{ - s_fan.last_temperature = temp_celsius; - - /* Calculate target speed from temperature curve */ - uint8_t new_target = fan_calculate_speed_from_temp(temp_celsius); - fan_set_target_speed(new_target); -} - -int16_t fan_get_temperature(void) -{ - return s_fan.last_temperature; -} - -FanState fan_get_state(void) -{ - if (s_fan.current_speed == 0) return FAN_OFF; - if (s_fan.current_speed <= 30) return FAN_LOW; - if (s_fan.current_speed <= 60) return FAN_MEDIUM; - if (s_fan.current_speed <= 99) return FAN_HIGH; - return FAN_FULL; -} - -void fan_set_ramp_rate(float percentage_per_ms) -{ - if (percentage_per_ms <= 0) { - s_fan.ramp_rate_per_ms = 0.01f; /* Minimum rate */ - } else if (percentage_per_ms > 10.0f) { - s_fan.ramp_rate_per_ms = 10.0f; /* Maximum rate */ - } else { - s_fan.ramp_rate_per_ms = percentage_per_ms; - } -} - -bool fan_is_ramping(void) -{ - return s_fan.is_ramping; -} - -void fan_tick(uint32_t now_ms) -{ - if (!s_fan.is_ramping) { - return; - } - - /* Calculate time elapsed since last ramp */ - if (s_fan.last_ramp_time_ms == 0) { - s_fan.last_ramp_time_ms = now_ms; - return; - } - - uint32_t elapsed = now_ms - s_fan.last_ramp_time_ms; - if (elapsed == 0) { - return; /* No time has passed */ - } - - /* Calculate speed change allowed in this time interval */ - float speed_change = s_fan.ramp_rate_per_ms * elapsed; - int32_t new_speed; - - if (s_fan.target_speed > s_fan.current_speed) { - /* Ramp up */ - new_speed = s_fan.current_speed + (int32_t)speed_change; - if (new_speed >= s_fan.target_speed) { - s_fan.current_speed = s_fan.target_speed; - s_fan.is_ramping = false; - } else { - s_fan.current_speed = (uint8_t)new_speed; - } - } else { - /* Ramp down */ - new_speed = s_fan.current_speed - (int32_t)speed_change; - if (new_speed <= s_fan.target_speed) { - s_fan.current_speed = s_fan.target_speed; - s_fan.is_ramping = false; - } else { - s_fan.current_speed = (uint8_t)new_speed; - } - } - - /* Update PWM duty cycle */ - fan_set_pwm_duty(s_fan.current_speed); - s_fan.last_ramp_time_ms = now_ms; -} - -void fan_disable(void) -{ - fan_set_speed(0); -} diff --git a/legacy/stm32/src/fault_handler.c b/legacy/stm32/src/fault_handler.c deleted file mode 100644 index e098abe..0000000 --- a/legacy/stm32/src/fault_handler.c +++ /dev/null @@ -1,457 +0,0 @@ -#include "fault_handler.h" -#include "config.h" -#include "pid_flash.h" -#include "stm32f7xx_hal.h" -#include -#include - -/* - * fault_handler.c — STM32F7 fault detection and recovery (Issue #565) - * - * Recovery flow: - * Fault ISR (naked) → _capture_and_reset() captures registers into .noinit - * SRAM → sets FAULT_SRAM_MAGIC → NVIC_SystemReset(). - * On next boot: fault_handler_init() sees FAULT_SRAM_MAGIC → persists to - * flash log → prints CDC dump → starts LED blink code. - * - * No flash writes occur inside fault ISRs. All flash operations happen safely - * in the normal boot context, well before safety_init() / IWDG start. - */ - -/* ---- .noinit SRAM (preserved across NVIC_SystemReset) ---- */ -/* - * GCC startup code only zeroes .bss and initialises .data. Variables in - * .noinit are left untouched. The magic word guards against cold-boot garbage. - */ -#define FAULT_SRAM_MAGIC 0xFADE5A01u -#define RESET_COUNT_MAGIC 0x1234ABCDu - -static __attribute__((section(".noinit"))) volatile uint32_t s_fault_magic; -static __attribute__((section(".noinit"))) volatile fault_log_entry_t s_fault_sram; -static __attribute__((section(".noinit"))) volatile uint32_t s_reset_count_magic; -static __attribute__((section(".noinit"))) volatile uint32_t s_reset_count; - -/* ---- LED blink sequencer ---- */ -/* - * Each pattern is a 16-bit bitmask; bit 15 = first step. - * One step = period_ms milliseconds. LED2 (PC14) is active-low. - */ -typedef struct { - uint16_t pattern; /* bitmask: 1 = LED on */ - uint8_t steps; /* number of valid bits to cycle */ - uint16_t period_ms; /* ms per step */ -} LedBlink; - -/* - * Pattern table indexed by FaultType (0..8). - * NONE = silent - * HARDFAULT = 1010 1010 1010 1010 (3 fast blinks, 100 ms) - * WATCHDOG = 1111 0000 1111 0000 (2 slow pulses, 150 ms × 8 steps = 1.2 s) - * BROWNOUT = 1111 1111 0000 0000 (1 long pulse, 100 ms × 16 = 1.6 s) - * STACK_OVF = 1110 1110 1110 1110 (4 short bursts, 100 ms) - * BUS_FAULT = 1010 1111 1100 0000 (3+1 pattern) - * USAGE_FAULT = 1010 0000 0000 0000 (2 fast blinks) - * MEM_FAULT = 1010 1010 1000 0000 (3 blinks, slower tail) - * ASSERT = 1101 1011 0000 0000 (SOS-like) - */ -static const LedBlink s_blink_table[] = { - /* FAULT_NONE */ { 0x0000u, 16, 100 }, - /* FAULT_HARDFAULT */ { 0xAAAAu, 16, 100 }, - /* FAULT_WATCHDOG */ { 0xF0F0u, 16, 150 }, - /* FAULT_BROWNOUT */ { 0xFF00u, 16, 100 }, - /* FAULT_STACK_OVF */ { 0xEEEEu, 16, 100 }, - /* FAULT_BUS_FAULT */ { 0xAFC0u, 16, 100 }, - /* FAULT_USAGE_FAULT */ { 0xA000u, 16, 100 }, - /* FAULT_MEM_FAULT */ { 0xAA80u, 16, 100 }, - /* FAULT_ASSERT */ { 0xDB00u, 16, 100 }, -}; -#define BLINK_TABLE_SIZE (sizeof(s_blink_table) / sizeof(s_blink_table[0])) - -static FaultType s_led_fault = FAULT_NONE; -static uint32_t s_led_start = 0; -static uint32_t s_led_last = 0; -static uint8_t s_led_step = 0; - -/* ------------------------------------------------------------------ */ -/* Flash helpers */ -/* ------------------------------------------------------------------ */ - -static uint32_t _slot_addr(uint8_t idx) -{ - return FAULT_LOG_BASE_ADDR + (uint32_t)idx * FAULT_LOG_ENTRY_SIZE; -} - -static bool _slot_empty(uint8_t idx) -{ - /* An erased 32-bit word reads as 0xFFFFFFFF */ - const uint32_t *p = (const uint32_t *)_slot_addr(idx); - return (*p == 0xFFFFFFFFu); -} - -static int _free_slot(void) -{ - for (uint8_t i = 0; i < FAULT_LOG_MAX_ENTRIES; i++) { - if (_slot_empty(i)) return (int)i; - } - return -1; -} - -static bool _erase_sector7(void) -{ - FLASH_EraseInitTypeDef er = {0}; - er.TypeErase = FLASH_TYPEERASE_SECTORS; - er.Sector = FLASH_SECTOR_7; - er.NbSectors = 1; - er.VoltageRange = FLASH_VOLTAGE_RANGE_3; - uint32_t err = 0; - return HAL_FLASHEx_Erase(&er, &err) == HAL_OK; -} - -/* - * Write fault entry to the next free flash slot. - * When all 8 slots are occupied: erase sector 7, restore PID if valid, - * then write entry at slot 0. Sector 7 erase stalls CPU ~1 s — only - * called from fault_handler_init() before IWDG is started. - */ -static bool _fault_log_write(const fault_log_entry_t *entry) -{ - int slot = _free_slot(); - - /* ---- Handle full log: erase sector 7 ---- */ - if (slot < 0) { - float kp, ki, kd; - bool pid_ok = pid_flash_load(&kp, &ki, &kd); - - HAL_FLASH_Unlock(); - bool erased = _erase_sector7(); - HAL_FLASH_Lock(); - - if (!erased) return false; - - if (pid_ok) { - /* pid_flash_save() manages its own unlock/lock */ - pid_flash_save(kp, ki, kd); - } - slot = 0; - } - - /* ---- Write 64 bytes (16 × 32-bit words) to chosen slot ---- */ - uint32_t addr = _slot_addr((uint8_t)slot); - const uint32_t *words = (const uint32_t *)entry; - - HAL_FLASH_Unlock(); - bool ok = true; - for (uint8_t w = 0; w < FAULT_LOG_ENTRY_SIZE / 4u; w++) { - if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, - addr + (uint32_t)w * 4u, words[w]) != HAL_OK) { - ok = false; - break; - } - } - HAL_FLASH_Lock(); - return ok; -} - -/* ------------------------------------------------------------------ */ -/* LED blink */ -/* ------------------------------------------------------------------ */ - -static void _led_start(FaultType type) -{ - s_led_fault = type; - s_led_start = HAL_GetTick(); - s_led_last = s_led_start; - s_led_step = 0; -} - -/* ------------------------------------------------------------------ */ -/* Public API */ -/* ------------------------------------------------------------------ */ - -void fault_mpu_guard_init(void) -{ - /* - * Configure MPU Region 0 as a 32-byte no-access guard page at - * __stack_end (lowest address of the main stack). The stack grows - * downward; when it overflows into this region a MemManage fault fires. - * - * MPU RASR SIZE field = log2(region_bytes) - 1 = log2(32) - 1 = 4. - * AP = 0b000 → no access in any mode. - */ - extern uint32_t __stack_end; /* defined in linker script */ - - HAL_MPU_Disable(); - - MPU_Region_InitTypeDef r = {0}; - r.Enable = MPU_REGION_ENABLE; - r.Number = MPU_REGION_NUMBER0; - r.BaseAddress = (uint32_t)&__stack_end; - r.Size = MPU_REGION_SIZE_32B; - r.SubRegionDisable = 0x00u; - r.TypeExtField = MPU_TEX_LEVEL0; - r.AccessPermission = MPU_REGION_NO_ACCESS; - r.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE; - r.IsShareable = MPU_ACCESS_NOT_SHAREABLE; - r.IsCacheable = MPU_ACCESS_NOT_CACHEABLE; - r.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE; - HAL_MPU_ConfigRegion(&r); - - /* Enable MPU with default memory map for privileged access */ - HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT); - - /* Enable configurable fault handlers */ - SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk - | SCB_SHCSR_BUSFAULTENA_Msk - | SCB_SHCSR_USGFAULTENA_Msk; -} - -void fault_handler_init(void) -{ - /* ---- Maintain lifetime reset counter ---- */ - if (s_reset_count_magic != RESET_COUNT_MAGIC) { - s_reset_count_magic = RESET_COUNT_MAGIC; - s_reset_count = 0u; - } - s_reset_count++; - - /* ---- Detect brownout via RCC_CSR ---- */ - bool brownout = (RCC->CSR & RCC_CSR_BORRSTF) != 0u; - if (brownout) { - printf("[FAULT] Brownout reset detected (reset_count=%lu)\n", - (unsigned long)s_reset_count); - fault_log_entry_t e; - memset(&e, 0, sizeof(e)); - e.magic = FAULT_LOG_MAGIC; - e.fault_type = (uint8_t)FAULT_BROWNOUT; - e.reset_count = (uint8_t)(s_reset_count & 0xFFu); - _fault_log_write(&e); - _led_start(FAULT_BROWNOUT); - } - - /* ---- Clear all RCC reset source flags ---- */ - RCC->CSR |= RCC_CSR_RMVF; - - /* ---- Check for pending .noinit fault capture ---- */ - if (s_fault_magic == FAULT_SRAM_MAGIC) { - s_fault_magic = 0u; /* consume once */ - - fault_log_entry_t e; - memcpy(&e, (const void *)&s_fault_sram, sizeof(e)); - e.reset_count = (uint8_t)(s_reset_count & 0xFFu); - - /* Print register dump over CDC/UART */ - printf("[FAULT] *** FAULT RECOVERED ***\n"); - printf("[FAULT] type=%u reset_count=%u ts=%lu ms\n", - e.fault_type, e.reset_count, (unsigned long)e.timestamp_ms); - printf("[FAULT] PC=0x%08lX LR=0x%08lX SP=0x%08lX\n", - (unsigned long)e.pc, (unsigned long)e.lr, (unsigned long)e.sp); - printf("[FAULT] R0=0x%08lX R1=0x%08lX R2=0x%08lX R3=0x%08lX\n", - (unsigned long)e.r0, (unsigned long)e.r1, - (unsigned long)e.r2, (unsigned long)e.r3); - printf("[FAULT] CFSR=0x%08lX HFSR=0x%08lX MMFAR=0x%08lX BFAR=0x%08lX\n", - (unsigned long)e.cfsr, (unsigned long)e.hfsr, - (unsigned long)e.mmfar, (unsigned long)e.bfar); - - _fault_log_write(&e); - - FaultType ft = (e.fault_type < (uint8_t)BLINK_TABLE_SIZE) - ? (FaultType)e.fault_type : FAULT_HARDFAULT; - _led_start(ft); - } - - /* ---- Install MPU stack guard & enable fault handlers ---- */ - fault_mpu_guard_init(); -} - -FaultType fault_get_last_type(void) -{ - for (int i = (int)FAULT_LOG_MAX_ENTRIES - 1; i >= 0; i--) { - if (_slot_empty((uint8_t)i)) continue; - const fault_log_entry_t *e = - (const fault_log_entry_t *)_slot_addr((uint8_t)i); - if (e->magic == FAULT_LOG_MAGIC) - return (FaultType)e->fault_type; - } - return FAULT_NONE; -} - -bool fault_log_read(uint8_t idx, fault_log_entry_t *out) -{ - if (idx >= FAULT_LOG_MAX_ENTRIES) return false; - if (_slot_empty(idx)) return false; - const fault_log_entry_t *e = - (const fault_log_entry_t *)_slot_addr(idx); - if (e->magic != FAULT_LOG_MAGIC) return false; - memcpy(out, e, sizeof(*out)); - return true; -} - -uint8_t fault_log_get_count(void) -{ - uint8_t n = 0; - for (uint8_t i = 0; i < FAULT_LOG_MAX_ENTRIES; i++) { - if (!_slot_empty(i)) n++; - } - return n; -} - -void fault_log_clear(void) -{ - float kp, ki, kd; - bool pid_ok = pid_flash_load(&kp, &ki, &kd); - - HAL_FLASH_Unlock(); - _erase_sector7(); - HAL_FLASH_Lock(); - - if (pid_ok) { - pid_flash_save(kp, ki, kd); - } -} - -void fault_assert_impl(const char *file, int line) -{ - (void)file; (void)line; - s_fault_sram.magic = FAULT_LOG_MAGIC; - s_fault_sram.fault_type = (uint8_t)FAULT_ASSERT; - s_fault_sram.timestamp_ms = HAL_GetTick(); - s_fault_sram.pc = (uint32_t)__builtin_return_address(0); - s_fault_sram.lr = 0u; - s_fault_sram.r0 = (uint32_t)(uintptr_t)file; - s_fault_sram.r1 = (uint32_t)line; - s_fault_sram.cfsr = SCB->CFSR; - s_fault_sram.hfsr = 0u; - s_fault_sram.mmfar = 0u; - s_fault_sram.bfar = 0u; - s_fault_sram.sp = 0u; - s_fault_magic = FAULT_SRAM_MAGIC; - NVIC_SystemReset(); -} - -void fault_led_tick(uint32_t now_ms) -{ - if (s_led_fault == FAULT_NONE) return; - - /* Auto-disable after 10 s */ - if ((now_ms - s_led_start) > 10000u) { - s_led_fault = FAULT_NONE; - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET); /* off */ - return; - } - - uint8_t fi = (uint8_t)s_led_fault; - if (fi >= BLINK_TABLE_SIZE) return; - - const LedBlink *b = &s_blink_table[fi]; - if ((now_ms - s_led_last) >= b->period_ms) { - s_led_last = now_ms; - bool on = ((b->pattern >> (15u - s_led_step)) & 1u) != 0u; - /* LED2 is active-low (GPIO_PIN_RESET = lit) */ - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, - on ? GPIO_PIN_RESET : GPIO_PIN_SET); - s_led_step = (uint8_t)((s_led_step + 1u) % b->steps); - } -} - -/* ================================================================ - * Fault vector hooks - * ================================================================ - * - * Naked entry stubs determine whether the auto-saved stack frame is on - * MSP or PSP (bit 2 of EXC_RETURN in LR), then tail-call the C handler - * with the frame pointer in R0. - * - * Cortex-M auto-pushed stack frame layout (from [SP]): - * [0] R0 [1] R1 [2] R2 [3] R3 - * [4] R12 [5] LR [6] PC [7] xPSR - */ - -static void _capture_and_reset(FaultType type, uint32_t *frame) -{ - s_fault_sram.magic = FAULT_LOG_MAGIC; - s_fault_sram.fault_type = (uint8_t)type; - s_fault_sram.timestamp_ms = HAL_GetTick(); - s_fault_sram.r0 = frame[0]; - s_fault_sram.r1 = frame[1]; - s_fault_sram.r2 = frame[2]; - s_fault_sram.r3 = frame[3]; - /* frame[4] = R12 (unused in log), frame[5] = LR, frame[6] = PC */ - s_fault_sram.lr = frame[5]; - s_fault_sram.pc = frame[6]; - s_fault_sram.sp = (uint32_t)(uintptr_t)(frame + 8); /* SP after push */ - s_fault_sram.cfsr = SCB->CFSR; - s_fault_sram.hfsr = SCB->HFSR; - s_fault_sram.mmfar = SCB->MMFAR; - s_fault_sram.bfar = SCB->BFAR; - s_fault_magic = FAULT_SRAM_MAGIC; - - /* Brief LED flash so a scope can catch it (≈50 ms at 216 MHz) */ - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); /* on */ - for (volatile uint32_t i = 0u; i < 10800000u; i++) __NOP(); - - NVIC_SystemReset(); -} - -/* Determine if a MemManage is from stack overflow vs other memory fault */ -static FaultType _mem_fault_type(void) -{ - if ((SCB->CFSR & SCB_CFSR_MMARVALID_Msk) != 0u) { - extern uint32_t __stack_end; - uint32_t guard = (uint32_t)&__stack_end; - if (SCB->MMFAR >= guard && SCB->MMFAR < guard + 32u) - return FAULT_STACK_OVF; - } - return FAULT_MEM_FAULT; -} - -/* C-level handlers — called from naked asm stubs */ -void fault_hard_c(uint32_t *frame) { _capture_and_reset(FAULT_HARDFAULT, frame); } -void fault_mem_c(uint32_t *frame) { _capture_and_reset(_mem_fault_type(), frame); } -void fault_bus_c(uint32_t *frame) { _capture_and_reset(FAULT_BUS_FAULT, frame); } -void fault_usage_c(uint32_t *frame) { _capture_and_reset(FAULT_USAGE_FAULT, frame); } - -/* ---- Naked asm entry stubs ---- */ - -__attribute__((naked)) void HardFault_Handler(void) -{ - __asm volatile ( - "tst lr, #4 \n" /* EXC_RETURN[2]: 0=MSP, 1=PSP */ - "ite eq \n" - "mrseq r0, msp \n" - "mrsne r0, psp \n" - "b fault_hard_c \n" - ); -} - -__attribute__((naked)) void MemManage_Handler(void) -{ - __asm volatile ( - "tst lr, #4 \n" - "ite eq \n" - "mrseq r0, msp \n" - "mrsne r0, psp \n" - "b fault_mem_c \n" - ); -} - -__attribute__((naked)) void BusFault_Handler(void) -{ - __asm volatile ( - "tst lr, #4 \n" - "ite eq \n" - "mrseq r0, msp \n" - "mrsne r0, psp \n" - "b fault_bus_c \n" - ); -} - -__attribute__((naked)) void UsageFault_Handler(void) -{ - __asm volatile ( - "tst lr, #4 \n" - "ite eq \n" - "mrseq r0, msp \n" - "mrsne r0, psp \n" - "b fault_usage_c \n" - ); -} diff --git a/legacy/stm32/src/gimbal.c b/legacy/stm32/src/gimbal.c deleted file mode 100644 index be3ddb9..0000000 --- a/legacy/stm32/src/gimbal.c +++ /dev/null @@ -1,127 +0,0 @@ -#include "gimbal.h" -#include "servo_bus.h" -#include "config.h" -#include - -/* - * gimbal.c — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547) - * - * Tick rate: called every 1 ms from main loop; self-throttles to GIMBAL_TLM_HZ. - * Feedback polling alternates: pan on even ticks, tilt on odd ticks. - * This gives ~25 Hz per axis, keeping per-read latency under 2 ms total. - * - * Safety limits: - * Pan: -GIMBAL_PAN_LIMIT_DEG .. +GIMBAL_PAN_LIMIT_DEG (±180 deg) - * Tilt: -GIMBAL_TILT_LIMIT_DEG .. +GIMBAL_TILT_LIMIT_DEG (± 90 deg) - */ - -#define TICK_PERIOD_MS (1000u / GIMBAL_TLM_HZ) /* 20 ms at 50 Hz */ - -/* Clamp int16 to [lo, hi] */ -static int16_t _clamp16(int16_t v, int16_t lo, int16_t hi) -{ - if (v < lo) return lo; - if (v > hi) return hi; - return v; -} - -/* ---- gimbal_init() ---- */ - -void gimbal_init(gimbal_t *g) -{ - g->cmd_pan_x10 = 0; - g->cmd_tilt_x10 = 0; - g->cmd_speed = 0; /* 0 = max speed */ - g->fb_pan_x10 = 0; - g->fb_tilt_x10 = 0; - g->fb_pan_speed = 0; - g->fb_tilt_speed = 0; - g->rx_ok = 0; - g->rx_err = 0; - g->_last_tick_ms = 0; - g->_poll_phase = 0; - - /* Enable torque and center both servos */ - servo_bus_write_torque(GIMBAL_PAN_ID, true); - servo_bus_write_torque(GIMBAL_TILT_ID, true); - g->torque_enabled = true; - - uint16_t center = servo_bus_deg_to_raw(0.0f); - servo_bus_write_pos(GIMBAL_PAN_ID, center, 0); - servo_bus_write_pos(GIMBAL_TILT_ID, center, 0); -} - -/* ---- gimbal_set_pos() ---- */ - -void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10, - uint16_t speed) -{ - /* Clamp to hardware limits */ - pan_x10 = _clamp16(pan_x10, - -(int16_t)(GIMBAL_PAN_LIMIT_DEG * 10), - (int16_t)(GIMBAL_PAN_LIMIT_DEG * 10)); - tilt_x10 = _clamp16(tilt_x10, - -(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10), - (int16_t)(GIMBAL_TILT_LIMIT_DEG * 10)); - if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX; - - g->cmd_pan_x10 = pan_x10; - g->cmd_tilt_x10 = tilt_x10; - g->cmd_speed = speed; - - float pan_deg = (float)pan_x10 / 10.0f; - float tilt_deg = (float)tilt_x10 / 10.0f; - - servo_bus_write_pos(GIMBAL_PAN_ID, - servo_bus_deg_to_raw(pan_deg), speed); - servo_bus_write_pos(GIMBAL_TILT_ID, - servo_bus_deg_to_raw(tilt_deg), speed); -} - -/* ---- gimbal_torque() ---- */ - -void gimbal_torque(gimbal_t *g, bool enable) -{ - servo_bus_write_torque(GIMBAL_PAN_ID, enable); - servo_bus_write_torque(GIMBAL_TILT_ID, enable); - g->torque_enabled = enable; -} - -/* ---- gimbal_tick() ---- */ - -void gimbal_tick(gimbal_t *g, uint32_t now_ms) -{ - if ((now_ms - g->_last_tick_ms) < TICK_PERIOD_MS) return; - g->_last_tick_ms = now_ms; - - uint16_t raw = 0; - - if (g->_poll_phase == 0u) { - /* Poll pan position */ - if (servo_bus_read_pos(GIMBAL_PAN_ID, &raw)) { - g->fb_pan_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f); - g->rx_ok++; - } else { - g->rx_err++; - } - - /* Also refresh pan speed */ - uint16_t spd = 0; - (void)servo_bus_read_speed(GIMBAL_PAN_ID, &spd); - g->fb_pan_speed = spd; - } else { - /* Poll tilt position */ - if (servo_bus_read_pos(GIMBAL_TILT_ID, &raw)) { - g->fb_tilt_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f); - g->rx_ok++; - } else { - g->rx_err++; - } - - uint16_t spd = 0; - (void)servo_bus_read_speed(GIMBAL_TILT_ID, &spd); - g->fb_tilt_speed = spd; - } - - g->_poll_phase ^= 1u; /* toggle 0 / 1 */ -} diff --git a/legacy/stm32/src/hw_button.c b/legacy/stm32/src/hw_button.c deleted file mode 100644 index 88f0e6e..0000000 --- a/legacy/stm32/src/hw_button.c +++ /dev/null @@ -1,179 +0,0 @@ -/* hw_button.c — hardware button debounce + gesture detection (Issue #682) - * - * Debounce FSM: - * IDLE → (raw press detected) → DEBOUNCING - * DEBOUNCING → (still pressed after BTN_DEBOUNCE_MS) → HELD - * HELD → (released) → classify press type, back to IDLE - * - * Press types: - * SHORT held < BTN_LONG_MIN_MS from confirmed start - * LONG held >= BTN_LONG_MIN_MS - * - * Sequence detection (operates on classified presses): - * Buffer up to 3 presses. Recognised patterns: - * [SHORT, SHORT, LONG] -> BTN_EVENT_REARM_COMBO (fires on LONG release) - * [SHORT] + BTN_COMMIT_MS timeout -> BTN_EVENT_PARK - * Sequence reset after BTN_SEQ_TIMEOUT_MS from first press. - */ - -#include "hw_button.h" -#include "config.h" - -#ifndef TEST_HOST -#include "stm32f7xx_hal.h" -#endif - -/* ---- Timing defaults (override in config.h) ---- */ -#ifndef BTN_DEBOUNCE_MS -#define BTN_DEBOUNCE_MS 20u -#endif -#ifndef BTN_LONG_MIN_MS -#define BTN_LONG_MIN_MS 1500u -#endif -#ifndef BTN_COMMIT_MS -#define BTN_COMMIT_MS 500u -#endif -#ifndef BTN_SEQ_TIMEOUT_MS -#define BTN_SEQ_TIMEOUT_MS 3000u -#endif - -/* ---- Press type ---- */ -typedef enum { - _PT_SHORT = 1u, - _PT_LONG = 2u, -} _press_type_t; - -/* ---- Debounce state ---- */ -typedef enum { - _BTN_IDLE, - _BTN_DEBOUNCING, - _BTN_HELD, -} _btn_state_t; - -static _btn_state_t s_state = _BTN_IDLE; -static uint32_t s_trans_ms = 0u; /* timestamp of last FSM transition */ -static bool s_pressed = false; - -/* ---- Sequence buffer ---- */ -#define _SEQ_MAX 3u -static _press_type_t s_seq[_SEQ_MAX]; -static uint8_t s_seq_len = 0u; -static uint32_t s_seq_first_ms = 0u; -static uint32_t s_seq_last_ms = 0u; - -/* ---- GPIO read ---- */ -#ifdef TEST_HOST -static bool s_test_raw = false; -void hw_button_inject(bool pressed) { s_test_raw = pressed; } -static bool _read_raw(void) { return s_test_raw; } -#else -static bool _read_raw(void) -{ - return HAL_GPIO_ReadPin(BTN_PORT, BTN_PIN) == GPIO_PIN_RESET; /* active-low */ -} -#endif - -void hw_button_init(void) -{ -#ifndef TEST_HOST - __HAL_RCC_GPIOC_CLK_ENABLE(); /* BTN_PORT assumed GPIOC; adjust if needed */ - GPIO_InitTypeDef g = {0}; - g.Pin = BTN_PIN; - g.Mode = GPIO_MODE_INPUT; - g.Pull = GPIO_PULLUP; - HAL_GPIO_Init(BTN_PORT, &g); -#endif - s_state = _BTN_IDLE; - s_seq_len = 0u; - s_pressed = false; -} - -bool hw_button_is_pressed(void) -{ - return s_pressed; -} - -/* Record a classified press into the sequence buffer and check for patterns. */ -static hw_btn_event_t _record_press(_press_type_t pt, uint32_t now_ms) -{ - if (s_seq_len == 0u) { - s_seq_first_ms = now_ms; - } - s_seq_last_ms = now_ms; - - if (s_seq_len < _SEQ_MAX) { - s_seq[s_seq_len++] = pt; - } - - /* Check REARM_COMBO: SHORT + SHORT + LONG */ - if (s_seq_len == 3u && - s_seq[0] == _PT_SHORT && - s_seq[1] == _PT_SHORT && - s_seq[2] == _PT_LONG) { - s_seq_len = 0u; - return BTN_EVENT_REARM_COMBO; - } - - return BTN_EVENT_NONE; -} - -hw_btn_event_t hw_button_tick(uint32_t now_ms) -{ - bool raw = _read_raw(); - - /* ---- Debounce FSM ---- */ - switch (s_state) { - case _BTN_IDLE: - if (raw) { - s_state = _BTN_DEBOUNCING; - s_trans_ms = now_ms; - } - break; - - case _BTN_DEBOUNCING: - if (!raw) { - /* Released before debounce elapsed — bounce, ignore */ - s_state = _BTN_IDLE; - } else if ((now_ms - s_trans_ms) >= BTN_DEBOUNCE_MS) { - s_state = _BTN_HELD; - s_trans_ms = now_ms; /* record confirmed press-start time */ - s_pressed = true; - } - break; - - case _BTN_HELD: - if (!raw) { - s_pressed = false; - uint32_t held_ms = now_ms - s_trans_ms; - _press_type_t pt = (held_ms >= BTN_LONG_MIN_MS) ? _PT_LONG : _PT_SHORT; - s_state = _BTN_IDLE; - hw_btn_event_t ev = _record_press(pt, now_ms); - if (ev != BTN_EVENT_NONE) { - return ev; - } - } - break; - } - - /* ---- Sequence timeout / commit check (only when not currently held) ---- */ - if (s_state == _BTN_IDLE && s_seq_len > 0u) { - uint32_t since_first = now_ms - s_seq_first_ms; - uint32_t since_last = now_ms - s_seq_last_ms; - - /* Whole sequence window expired — abandon */ - if (since_first >= BTN_SEQ_TIMEOUT_MS) { - s_seq_len = 0u; - return BTN_EVENT_NONE; - } - - /* Single short press + BTN_COMMIT_MS of quiet -> PARK */ - if (s_seq_len == 1u && - s_seq[0] == _PT_SHORT && - since_last >= BTN_COMMIT_MS) { - s_seq_len = 0u; - return BTN_EVENT_PARK; - } - } - - return BTN_EVENT_NONE; -} diff --git a/legacy/stm32/src/i2c1.c b/legacy/stm32/src/i2c1.c deleted file mode 100644 index 8e78273..0000000 --- a/legacy/stm32/src/i2c1.c +++ /dev/null @@ -1,33 +0,0 @@ -/* - * i2c1.c — Shared I2C1 bus (PB8=SCL, PB9=SDA, 100 kHz) - * - * Used by barometer and magnetometer drivers. - * Call i2c1_init() once before any I2C probes. - */ -#include "i2c1.h" -#include "stm32f7xx_hal.h" - -I2C_HandleTypeDef hi2c1; - -int i2c1_init(void) { - __HAL_RCC_GPIOB_CLK_ENABLE(); - __HAL_RCC_I2C1_CLK_ENABLE(); - - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9; /* PB8=SCL, PB9=SDA */ - gpio.Mode = GPIO_MODE_AF_OD; - gpio.Pull = GPIO_PULLUP; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; - gpio.Alternate = GPIO_AF4_I2C1; - HAL_GPIO_Init(GPIOB, &gpio); - - hi2c1.Instance = I2C1; - hi2c1.Init.Timing = 0x20404768; /* 100 kHz @ 54 MHz APB1 */ - hi2c1.Init.OwnAddress1 = 0; - hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; - hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; - hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; - hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; - - return (HAL_I2C_Init(&hi2c1) == HAL_OK) ? 0 : -1; -} diff --git a/legacy/stm32/src/icm42688.c b/legacy/stm32/src/icm42688.c deleted file mode 100644 index 9e83861..0000000 --- a/legacy/stm32/src/icm42688.c +++ /dev/null @@ -1,191 +0,0 @@ -/* MPU6000 + ICM-42688-P dual driver — auto-detects based on WHO_AM_I */ -#include "stm32f7xx_hal.h" -#include "config.h" -#include "icm42688.h" - -static SPI_HandleTypeDef hspi1; -static uint8_t imu_type = 0; /* 0=unknown, 1=MPU6000, 2=ICM42688 */ - -/* MPU6000 registers */ -#define MPU_REG_SMPLRT_DIV 0x19 -#define MPU_REG_CONFIG 0x1A -#define MPU_REG_GYRO_CONFIG 0x1B -#define MPU_REG_ACCEL_CONFIG 0x1C -#define MPU_REG_ACCEL_XOUT_H 0x3B -#define MPU_REG_PWR_MGMT_1 0x6B -#define MPU_REG_PWR_MGMT_2 0x6C -#define MPU_REG_WHO_AM_I 0x75 -#define MPU6000_WHO 0x68 - -/* ICM-42688-P registers */ -#define ICM_REG_DEVICE_CONFIG 0x11 -#define ICM_REG_TEMP_DATA1 0x1D -#define ICM_REG_PWR_MGMT0 0x4E -#define ICM_REG_GYRO_CONFIG0 0x4F -#define ICM_REG_ACCEL_CONFIG0 0x50 -#define ICM_REG_WHO_AM_I 0x75 -#define ICM_REG_BANK_SEL 0x76 -#define ICM42688_WHO 0x47 - -static void cs_low(void) { HAL_GPIO_WritePin(MPU_CS_PORT, MPU_CS_PIN, GPIO_PIN_RESET); } -static void cs_high(void) { HAL_GPIO_WritePin(MPU_CS_PORT, MPU_CS_PIN, GPIO_PIN_SET); } - -static void wreg(uint8_t reg, uint8_t val) { - uint8_t tx[2] = { reg & 0x7F, val }; - uint8_t rx[2]; - cs_low(); - HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100); - cs_high(); - HAL_Delay(1); -} - -static uint8_t rreg(uint8_t reg) { - uint8_t tx[2] = { reg | 0x80, 0x00 }; - uint8_t rx[2] = {0, 0}; - cs_low(); - HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100); - cs_high(); - /* DCache coherency: invalidate rx so CPU reads SPI-written SRAM, not stale cache. - * No-op when DCache is disabled; required if DCache is on (e.g. SCB_EnableDCache). */ - SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx)); - return rx[1]; -} - -static uint8_t trace[16]; -static int trace_idx = 0; -static void tr(uint8_t v) { if (trace_idx < 16) trace[trace_idx++] = v; } - -static int init_mpu6000(void) { - /* Reset */ - wreg(MPU_REG_PWR_MGMT_1, 0x80); - HAL_Delay(100); - - /* Wake up, use PLL with X gyro ref */ - wreg(MPU_REG_PWR_MGMT_1, 0x01); - HAL_Delay(10); - - /* Sample rate = 1kHz (divider=0) */ - wreg(MPU_REG_SMPLRT_DIV, 0x00); - - /* DLPF = 42Hz (config=3) */ - wreg(MPU_REG_CONFIG, 0x03); - - /* Gyro: ±2000°/s (FS_SEL=3) */ - wreg(MPU_REG_GYRO_CONFIG, 0x18); - - /* Accel: ±16g (AFS_SEL=3) */ - wreg(MPU_REG_ACCEL_CONFIG, 0x18); - - /* Enable all axes */ - wreg(MPU_REG_PWR_MGMT_2, 0x00); - HAL_Delay(50); - - /* Verify */ - uint8_t pwr = rreg(MPU_REG_PWR_MGMT_1); - tr(pwr); /* Should be 0x01 */ - - return (pwr == 0x01) ? 0 : -200 - (int)pwr; -} - -int icm42688_init(void) { - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_SPI1_CLK_ENABLE(); - - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - gpio.Alternate = GPIO_AF5_SPI1; - HAL_GPIO_Init(GPIOA, &gpio); - - /* CS on PA4 for MAMBA */ - gpio.Pin = MPU_CS_PIN; - gpio.Mode = GPIO_MODE_OUTPUT_PP; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; - gpio.Pull = GPIO_PULLUP; - HAL_GPIO_Init(MPU_CS_PORT, &gpio); - cs_high(); - - /* DCache: main.c does NOT call SCB_EnableDCache(), so DCache is currently OFF. - * If DCache is ever enabled, all SPI rx buffers need SCB_InvalidateDCache_by_Addr() - * (already added in rreg() and icm42688_read()) and USB buffers must remain mapped - * non-cacheable via MPU Region 0 in usbd_conf.c. */ - - hspi1.Instance = SPI1; - hspi1.Init.Mode = SPI_MODE_MASTER; - hspi1.Init.Direction = SPI_DIRECTION_2LINES; - hspi1.Init.DataSize = SPI_DATASIZE_8BIT; - hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH; - hspi1.Init.CLKPhase = SPI_PHASE_2EDGE; - hspi1.Init.NSS = SPI_NSS_SOFT; - hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; - hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB; - hspi1.Init.TIMode = SPI_TIMODE_DISABLE; - hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - if (HAL_SPI_Init(&hspi1) != HAL_OK) return -1; - - HAL_Delay(200); - /* Wake from sleep first - MPU6000 needs this before WHO_AM_I */ - wreg(0x6B, 0x80); /* Reset */ - HAL_Delay(100); - wreg(0x6B, 0x01); /* Wake, PLL */ - HAL_Delay(50); - - /* Retry WHO_AM_I up to 3 times: a single SPI glitch returning 0x00 - * would otherwise abort init and prevent calibration from ever running. */ - uint8_t who = 0; - for (int attempt = 0; attempt < 3 && who == 0; attempt++) { - if (attempt > 0) HAL_Delay(10); - who = rreg(MPU_REG_WHO_AM_I); - } - tr(who); /* trace[0] */ - - int ret; - if (who == MPU6000_WHO) { - imu_type = 1; - ret = init_mpu6000(); - } else if (who == ICM42688_WHO) { - imu_type = 2; - ret = -99; /* TODO: ICM init */ - } else { - /* who==0 means no SPI response — must not return 0 (false success) */ - ret = (who != 0) ? -(int)who : -128; - } - - /* Speed up SPI for reads */ - hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; - HAL_SPI_Init(&hspi1); - - tr((uint8_t)imu_type); /* trace[last] */ - return ret; -} - -void icm42688_read(icm42688_data_t *d) { - if (imu_type == 1) { - /* MPU6000: ACCEL_XOUT_H (0x3B) → 14 bytes: accel(6)+temp(2)+gyro(6) */ - uint8_t tx[15] = {0}; - uint8_t rx[15] = {0}; /* zero-init: failed SPI transfers return 0, not garbage */ - tx[0] = MPU_REG_ACCEL_XOUT_H | 0x80; - cs_low(); - HAL_SPI_TransmitReceive(&hspi1, tx, rx, 15, 100); - cs_high(); - /* DCache coherency: force CPU to read SPI-written SRAM, not a stale cache line. - * No-op when DCache is disabled; critical if SCB_EnableDCache() is called. */ - SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx)); - - d->ax = (int16_t)((rx[1] << 8) | rx[2]); - d->ay = (int16_t)((rx[3] << 8) | rx[4]); - d->az = (int16_t)((rx[5] << 8) | rx[6]); - int16_t temp_raw = (int16_t)((rx[7] << 8) | rx[8]); - d->temp_x10 = (int16_t)((temp_raw + 12421) / 34); /* MPU6000 formula */ - d->gx = (int16_t)((rx[9] << 8) | rx[10]); - d->gy = (int16_t)((rx[11] << 8) | rx[12]); - d->gz = (int16_t)((rx[13] << 8) | rx[14]); - } -} - -void icm42688_get_trace(uint8_t *out, int *len) { - *len = trace_idx; - for (int i = 0; i < trace_idx; i++) out[i] = trace[i]; -} diff --git a/legacy/stm32/src/imu_cal_flash.c b/legacy/stm32/src/imu_cal_flash.c deleted file mode 100644 index 6bd9b3c..0000000 --- a/legacy/stm32/src/imu_cal_flash.c +++ /dev/null @@ -1,100 +0,0 @@ -/* imu_cal_flash.c — IMU mount angle calibration flash storage (Issue #680) - * - * Stores pitch/roll mount offsets in STM32F722 flash sector 7 at 0x0807FF00. - * Preserves existing PID records (pid_sched_flash_t + pid_flash_t) across - * the mandatory sector erase by reading them into RAM before erasing. - */ - -#include "imu_cal_flash.h" -#include "pid_flash.h" -#include "stm32f7xx_hal.h" -#include - -bool imu_cal_flash_load(float *pitch_offset, float *roll_offset) -{ - const imu_cal_flash_t *p = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR; - - if (p->magic != IMU_CAL_FLASH_MAGIC) return false; - - /* Sanity-check: mount offsets beyond ±90° indicate a corrupt record */ - if (p->pitch_offset < -90.0f || p->pitch_offset > 90.0f) return false; - if (p->roll_offset < -90.0f || p->roll_offset > 90.0f) return false; - - *pitch_offset = p->pitch_offset; - *roll_offset = p->roll_offset; - return true; -} - -/* Write 'len' bytes (multiple of 4) from 'src' to flash at 'addr'. - * Flash must be unlocked by caller. */ -static HAL_StatusTypeDef write_words(uint32_t addr, - const void *src, - uint32_t len) -{ - const uint32_t *p = (const uint32_t *)src; - for (uint32_t i = 0; i < len / 4u; i++) { - HAL_StatusTypeDef rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, - addr, p[i]); - if (rc != HAL_OK) return rc; - addr += 4u; - } - return HAL_OK; -} - -bool imu_cal_flash_save(float pitch_offset, float roll_offset) -{ - /* Snapshot PID records BEFORE erasing so we can restore them */ - pid_flash_t pid_snap; - pid_sched_flash_t sched_snap; - memcpy(&pid_snap, (const void *)PID_FLASH_STORE_ADDR, sizeof(pid_snap)); - memcpy(&sched_snap, (const void *)PID_SCHED_FLASH_ADDR, sizeof(sched_snap)); - - HAL_StatusTypeDef rc; - - rc = HAL_FLASH_Unlock(); - if (rc != HAL_OK) return false; - - /* Erase sector 7 (covers all three records) */ - FLASH_EraseInitTypeDef erase = { - .TypeErase = FLASH_TYPEERASE_SECTORS, - .Sector = PID_FLASH_SECTOR, - .NbSectors = 1, - .VoltageRange = PID_FLASH_SECTOR_VOLTAGE, - }; - uint32_t sector_error = 0; - rc = HAL_FLASHEx_Erase(&erase, §or_error); - if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) { - HAL_FLASH_Lock(); - return false; - } - - /* Write new IMU calibration record at 0x0807FF00 */ - imu_cal_flash_t cal; - memset(&cal, 0xFF, sizeof(cal)); - cal.magic = IMU_CAL_FLASH_MAGIC; - cal.pitch_offset = pitch_offset; - cal.roll_offset = roll_offset; - - rc = write_words(IMU_CAL_FLASH_ADDR, &cal, sizeof(cal)); - if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; } - - /* Restore PID gain schedule if it was valid */ - if (sched_snap.magic == PID_SCHED_MAGIC) { - rc = write_words(PID_SCHED_FLASH_ADDR, &sched_snap, sizeof(sched_snap)); - if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; } - } - - /* Restore single-PID record if it was valid */ - if (pid_snap.magic == PID_FLASH_MAGIC) { - rc = write_words(PID_FLASH_STORE_ADDR, &pid_snap, sizeof(pid_snap)); - if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; } - } - - HAL_FLASH_Lock(); - - /* Verify readback */ - const imu_cal_flash_t *v = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR; - return (v->magic == IMU_CAL_FLASH_MAGIC && - v->pitch_offset == pitch_offset && - v->roll_offset == roll_offset); -} diff --git a/legacy/stm32/src/ina219.c b/legacy/stm32/src/ina219.c deleted file mode 100644 index fc15bd7..0000000 --- a/legacy/stm32/src/ina219.c +++ /dev/null @@ -1,244 +0,0 @@ -#include "ina219.h" -#include "config.h" -#include "i2c1.h" -#include - -/* ================================================================ - * INA219 Register Definitions - * ================================================================ */ - -#define INA219_REG_CONFIG 0x00 -#define INA219_REG_SHUNT_VOLTAGE 0x01 -#define INA219_REG_BUS_VOLTAGE 0x02 -#define INA219_REG_POWER 0x03 -#define INA219_REG_CURRENT 0x04 -#define INA219_REG_CALIBRATION 0x05 - -/* Configuration Register Bits */ -#define INA219_CONFIG_RESET (1 << 15) -#define INA219_CONFIG_BRNG_16V (0 << 13) -#define INA219_CONFIG_BRNG_32V (1 << 13) -#define INA219_CONFIG_PGA_40MV (0 << 11) -#define INA219_CONFIG_PGA_80MV (1 << 11) -#define INA219_CONFIG_PGA_160MV (2 << 11) -#define INA219_CONFIG_PGA_320MV (3 << 11) -#define INA219_CONFIG_BADC_9BIT (0 << 7) -#define INA219_CONFIG_BADC_10BIT (1 << 7) -#define INA219_CONFIG_BADC_11BIT (2 << 7) -#define INA219_CONFIG_BADC_12BIT (3 << 7) -#define INA219_CONFIG_SADC_9BIT (0 << 3) -#define INA219_CONFIG_SADC_10BIT (1 << 3) -#define INA219_CONFIG_SADC_11BIT (2 << 3) -#define INA219_CONFIG_SADC_12BIT (3 << 3) -#define INA219_CONFIG_MODE_SHUNT (0 << 0) -#define INA219_CONFIG_MODE_BUSVOLT (1 << 0) -#define INA219_CONFIG_MODE_BOTH (3 << 0) - -/* I2C Addresses */ -#define INA219_ADDR_LEFT_MOTOR 0x40 /* A0=A1=GND */ -#define INA219_ADDR_RIGHT_MOTOR 0x41 /* A0=SDA, A1=GND */ - -/* ================================================================ - * Internal State - * ================================================================ */ - -typedef struct { - uint8_t i2c_addr; - uint16_t calibration_value; - uint16_t current_lsb_ua; /* Current LSB in µA */ - uint16_t power_lsb_uw; /* Power LSB in µW */ -} INA219State; - -static INA219State s_ina219[INA219_COUNT] = { - [INA219_LEFT_MOTOR] = {.i2c_addr = INA219_ADDR_LEFT_MOTOR}, - [INA219_RIGHT_MOTOR] = {.i2c_addr = INA219_ADDR_RIGHT_MOTOR} -}; - -/* ================================================================ - * I2C Helper Functions - * ================================================================ */ - -static bool i2c_write_register(uint8_t addr, uint8_t reg, uint16_t value) -{ - (void)addr; (void)reg; (void)value; - /* TODO: Implement using HAL_I2C_Master_Transmit with hi2c1 */ - return false; -} - -static bool i2c_read_register(uint8_t addr, uint8_t reg, uint16_t *value) -{ - (void)addr; (void)reg; - /* TODO: Implement using HAL_I2C_Master_Transmit/Receive with hi2c1 */ - if (value) *value = 0; - return false; -} - -/* ================================================================ - * Public API - * ================================================================ */ - -void ina219_init(void) -{ - /* Ensure I2C1 is initialized before calling this */ - /* Auto-calibrate both sensors for typical motor monitoring: - * - Max current: 5A - * - Shunt resistor: 0.1Ω - * - LSB: 160µA (5A / 32768) - */ - ina219_calibrate(INA219_LEFT_MOTOR, 5000, 100); - ina219_calibrate(INA219_RIGHT_MOTOR, 5000, 100); -} - -void ina219_calibrate(INA219Sensor sensor, uint16_t max_current_ma, uint16_t shunt_ohms_milli) -{ - if (sensor >= INA219_COUNT) return; - - INA219State *s = &s_ina219[sensor]; - - /* Calculate current LSB: max_current / 32768 (15-bit signed register) - * LSB unit: µA - * Example: 5000mA / 32768 ≈ 152.6µA → use 160µA (round up for safety) - */ - uint32_t current_lsb_ua = ((uint32_t)max_current_ma * 1000 + 32767) / 32768; - s->current_lsb_ua = (uint16_t)current_lsb_ua; - - /* Power LSB = 20 × current_lsb_ua (20µW per 1µA of current LSB) */ - s->power_lsb_uw = 20 * current_lsb_ua; - - /* Calibration register: (0.04096) / (current_lsb_ua × shunt_ohms_milli / 1000) - * Simplified: 40960 / (current_lsb_ua × shunt_ohms_milli) - */ - uint32_t calibration = 40960 / ((uint32_t)current_lsb_ua * shunt_ohms_milli / 1000); - if (calibration > 65535) calibration = 65535; - s->calibration_value = (uint16_t)calibration; - - /* Write calibration register */ - i2c_write_register(s->i2c_addr, INA219_REG_CALIBRATION, s->calibration_value); - - /* Configure for continuous conversion mode (12-bit ADC for both shunt and bus) - * Config: 32V range, 160mV PGA, 12-bit ADC, continuous mode - */ - uint16_t config = INA219_CONFIG_BRNG_32V - | INA219_CONFIG_PGA_160MV - | INA219_CONFIG_BADC_12BIT - | INA219_CONFIG_SADC_12BIT - | INA219_CONFIG_MODE_BOTH; - i2c_write_register(s->i2c_addr, INA219_REG_CONFIG, config); -} - -bool ina219_read(INA219Sensor sensor, INA219Data *data) -{ - if (sensor >= INA219_COUNT || !data) return false; - - INA219State *s = &s_ina219[sensor]; - uint8_t addr = s->i2c_addr; - uint16_t reg_value; - - /* Read shunt voltage (register 0x01) */ - if (!i2c_read_register(addr, INA219_REG_SHUNT_VOLTAGE, ®_value)) return false; - int16_t shunt_raw = (int16_t)reg_value; - data->shunt_voltage_uv = shunt_raw * 10; /* 10µV/LSB */ - - /* Read bus voltage (register 0x02) */ - if (!i2c_read_register(addr, INA219_REG_BUS_VOLTAGE, ®_value)) return false; - uint16_t bus_raw = (reg_value >> 3) & 0x1FFF; /* 13-bit voltage, 4mV/LSB */ - data->bus_voltage_mv = bus_raw * 4; - - /* Read current (register 0x04) — requires calibration */ - if (!i2c_read_register(addr, INA219_REG_CURRENT, ®_value)) return false; - int16_t current_raw = (int16_t)reg_value; - data->current_ma = (current_raw * (int32_t)s->current_lsb_ua) / 1000; - - /* Read power (register 0x03) — in units of power_lsb */ - if (!i2c_read_register(addr, INA219_REG_POWER, ®_value)) return false; - uint32_t power_raw = reg_value; - data->power_mw = (power_raw * (uint32_t)s->power_lsb_uw) / 1000; - - return true; -} - -bool ina219_read_bus_voltage_mv(INA219Sensor sensor, uint16_t *voltage_mv) -{ - if (sensor >= INA219_COUNT || !voltage_mv) return false; - - INA219State *s = &s_ina219[sensor]; - uint16_t reg_value; - - if (!i2c_read_register(s->i2c_addr, INA219_REG_BUS_VOLTAGE, ®_value)) return false; - - uint16_t bus_raw = (reg_value >> 3) & 0x1FFF; - *voltage_mv = bus_raw * 4; - return true; -} - -bool ina219_read_current_ma(INA219Sensor sensor, int16_t *current_ma) -{ - if (sensor >= INA219_COUNT || !current_ma) return false; - - INA219State *s = &s_ina219[sensor]; - uint16_t reg_value; - - if (!i2c_read_register(s->i2c_addr, INA219_REG_CURRENT, ®_value)) return false; - - int16_t current_raw = (int16_t)reg_value; - *current_ma = (current_raw * (int32_t)s->current_lsb_ua) / 1000; - return true; -} - -bool ina219_read_power_mw(INA219Sensor sensor, uint32_t *power_mw) -{ - if (sensor >= INA219_COUNT || !power_mw) return false; - - INA219State *s = &s_ina219[sensor]; - uint16_t reg_value; - - if (!i2c_read_register(s->i2c_addr, INA219_REG_POWER, ®_value)) return false; - - uint32_t power_raw = reg_value; - *power_mw = (power_raw * (uint32_t)s->power_lsb_uw) / 1000; - return true; -} - -void ina219_alert_enable(INA219Sensor sensor, uint16_t current_limit_ma) -{ - if (sensor >= INA219_COUNT) return; - - INA219State *s = &s_ina219[sensor]; - - /* Alert limit register: set to current threshold - * Current threshold = (limit_ma × 1000) / current_lsb_ua - */ - int16_t limit_raw = ((int32_t)current_limit_ma * 1000) / s->current_lsb_ua; - if (limit_raw > 32767) limit_raw = 32767; - - /* Enable alert on over-limit, latching mode */ - uint16_t alert_config = limit_raw; - i2c_write_register(s->i2c_addr, 0x06, alert_config); /* Alert register */ -} - -void ina219_alert_disable(INA219Sensor sensor) -{ - if (sensor >= INA219_COUNT) return; - - INA219State *s = &s_ina219[sensor]; - - /* Write 0 to alert register to disable */ - i2c_write_register(s->i2c_addr, 0x06, 0); -} - -void ina219_reset(INA219Sensor sensor) -{ - if (sensor >= INA219_COUNT) return; - - INA219State *s = &s_ina219[sensor]; - - /* Set reset bit in config register */ - i2c_write_register(s->i2c_addr, INA219_REG_CONFIG, INA219_CONFIG_RESET); - - /* Wait for reset to complete (~1ms) */ - uint32_t start = 0; /* In real code, use HAL_GetTick() */ - while (start < 2) start++; /* Simple delay */ - - /* Re-calibrate after reset */ - ina219_calibrate(sensor, 5000, 100); -} diff --git a/legacy/stm32/src/jetson_cmd.c b/legacy/stm32/src/jetson_cmd.c deleted file mode 100644 index 16233d4..0000000 --- a/legacy/stm32/src/jetson_cmd.c +++ /dev/null @@ -1,55 +0,0 @@ -#include "jetson_cmd.h" -#include - -/* - * Parsed drive state — updated by jetson_cmd_process() in the main loop. - * Raw fields are ints parsed from "C,\n". - */ -static volatile int16_t jcmd_speed = 0; /* -1000..+1000 */ -static volatile int16_t jcmd_steer = 0; /* -1000..+1000 */ - -/* Clamp helper (avoids including math.h) */ -static int16_t clamp16(int v, int lo, int hi) { - if (v < lo) return (int16_t)lo; - if (v > hi) return (int16_t)hi; - return (int16_t)v; -} - -/* - * Called from main loop when jetson_cmd_ready is set. - * Parses jetson_cmd_buf — safe to use sscanf here (not in ISR). - * The ISR only copies bytes and sets the ready flag. - */ -void jetson_cmd_process(void) { - int speed = 0, steer = 0; - /* buf format: "C," — skip leading 'C' */ - if (sscanf((const char *)jetson_cmd_buf + 1, "%d,%d", &speed, &steer) == 2) { - jcmd_speed = clamp16(speed, -1000, 1000); - jcmd_steer = clamp16(steer, -1000, 1000); - } - /* If parse fails, keep previous values — don't zero-out mid-motion */ -} - -/* - * Returns true if the last heartbeat (H or C command) arrived within - * JETSON_HB_TIMEOUT_MS. jetson_hb_tick is updated in the ISR. - */ -bool jetson_cmd_is_active(uint32_t now_ms) { - /* jetson_hb_tick == 0 means we've never received any command */ - if (jetson_hb_tick == 0) return false; - return (now_ms - jetson_hb_tick) < JETSON_HB_TIMEOUT_MS; -} - -/* Steer command for motor_driver_update() */ -int16_t jetson_cmd_steer(void) { - return jcmd_steer; -} - -/* - * Convert speed command to balance setpoint offset (degrees). - * Positive speed → lean forward → robot moves forward. - * Scaled linearly: speed=1000 → +JETSON_SPEED_MAX_DEG degrees. - */ -float jetson_cmd_sp_offset(void) { - return ((float)jcmd_speed / 1000.0f) * JETSON_SPEED_MAX_DEG; -} diff --git a/legacy/stm32/src/jetson_uart.c b/legacy/stm32/src/jetson_uart.c deleted file mode 100644 index 9277440..0000000 --- a/legacy/stm32/src/jetson_uart.c +++ /dev/null @@ -1,287 +0,0 @@ -/* - * jetson_uart.c — USART6 command interface for Jetson Orin - * - * Mirrors the USB CDC command protocol over UART so the Jetson can - * arm, disarm, heartbeat, drive, and e-stop via hardware UART. - * This fixes the CDC TX bug (Issue USB_CDC_BUG.md) by providing - * a reliable non-USB path. - * - * USART6: PC6=TX, PC7=RX @ 921600 baud (matches Orin ttyTHS1) - * - * Command protocol (same as CDC): - * A — arm request - * D — disarm request - * E — emergency stop - * Z — clear e-stop - * H — heartbeat (refresh timeout) - * C, — drive command: speed,steer (also refreshes heartbeat) - * G — gyro recalibration - */ - -#include "jetson_uart.h" -#include "jetson_cmd.h" -#include "config.h" -#include "safety.h" -#include "mpu6000.h" -#include "stm32f7xx_hal.h" -#include - -/* Shared flags — same ones CDC sets, main loop consumes */ -extern volatile uint8_t cdc_arm_request; -extern volatile uint8_t cdc_disarm_request; -extern volatile uint8_t cdc_recal_request; -extern volatile uint8_t cdc_estop_request; -extern volatile uint8_t cdc_estop_clear_request; -extern volatile uint8_t cdc_streaming; -extern volatile char cdc_cmd_buf[32]; -extern volatile uint8_t cdc_cmd_ready; - -/* From jetson_cmd.h */ -extern volatile uint8_t jetson_cmd_ready; -extern volatile char jetson_cmd_buf[32]; -extern volatile uint32_t jetson_hb_tick; - -static UART_HandleTypeDef huart6; -static uint8_t rx_byte; -static char line_buf[64]; -static uint8_t line_idx = 0; - -static void process_line(const char *buf, uint8_t len); - -/* Keep FC awake on Jetson activity */ -extern void power_mgmt_activity(void); - -void jetson_uart_init(void) { - __HAL_RCC_USART6_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); - - /* PC6=TX, PC7=RX, AF8 for USART6 */ - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_6 | GPIO_PIN_7; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_PULLUP; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; - gpio.Alternate = GPIO_AF8_USART6; - HAL_GPIO_Init(GPIOC, &gpio); - - huart6.Instance = USART6; - huart6.Init.BaudRate = 921600; - huart6.Init.WordLength = UART_WORDLENGTH_8B; - huart6.Init.StopBits = UART_STOPBITS_1; - huart6.Init.Parity = UART_PARITY_NONE; - huart6.Init.Mode = UART_MODE_TX_RX; - huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE; - huart6.Init.OverSampling = UART_OVERSAMPLING_16; - HAL_UART_Init(&huart6); - - HAL_NVIC_SetPriority(USART6_IRQn, 4, 0); - HAL_NVIC_EnableIRQ(USART6_IRQn); - - /* Start interrupt-driven receive */ - HAL_UART_Receive_IT(&huart6, &rx_byte, 1); - - /* Boot banner — confirms USART6 is alive */ - const char *banner = "SALTYLAB USART6 OK\n"; - HAL_UART_Transmit(&huart6, (const uint8_t *)banner, 19, 50); -} - -/* Send telemetry/status back to Jetson */ -void jetson_uart_send(const uint8_t *data, uint16_t len) { - HAL_UART_Transmit(&huart6, (uint8_t *)data, len, 10); -} - -/* ISR callback — accumulate bytes into line buffer, process on \n */ -void jetson_uart_rx_callback(UART_HandleTypeDef *huart) { - if (huart != &huart6) return; - - /* Any UART activity keeps the FC awake (prevents STOP mode) */ - power_mgmt_activity(); - - if (rx_byte == '\n' || rx_byte == '\r') { - if (line_idx > 0) { - line_buf[line_idx] = '\0'; - process_line(line_buf, line_idx); - line_idx = 0; - } - } else if (line_idx < sizeof(line_buf) - 1) { - line_buf[line_idx++] = (char)rx_byte; - } else { - line_idx = 0; /* overflow — reset */ - } - - /* Re-arm receive */ - HAL_UART_Receive_IT(&huart6, &rx_byte, 1); -} - -/* - * Process a complete line. Same command set as CDC_Receive. - * Single-char commands work without newline too (for compatibility). - */ -static void process_line(const char *buf, uint8_t len) { - if (len < 1) return; - - switch (buf[0]) { - case 'A': cdc_arm_request = 1; - /* Send arm attempt status for debugging */ - { - extern bool mpu6000_is_calibrated(void); - extern float bal_pitch_deg_get(void); /* we'll add this */ - char dbg[80]; - int n = snprintf(dbg, sizeof(dbg), "ARM_REQ cal=%d estop=%d\n", - mpu6000_is_calibrated() ? 1 : 0, - safety_remote_estop_active() ? 1 : 0); - HAL_UART_Transmit(&huart6, (uint8_t*)dbg, n, 10); - } - break; - case 'D': cdc_disarm_request = 1; break; - case 'G': cdc_recal_request = 1; break; - case 'E': cdc_estop_request = 1; break; - case 'F': cdc_estop_request = 2; break; - case 'Z': cdc_estop_clear_request = 1; break; - case 'S': cdc_streaming = !cdc_streaming; break; - - case 'H': - jetson_hb_tick = HAL_GetTick(); - break; - - case 'C': { - uint8_t copy_len = len < 31 ? len : 31; - for (uint8_t i = 0; i < copy_len; i++) - jetson_cmd_buf[i] = buf[i]; - jetson_cmd_buf[copy_len] = '\0'; - jetson_hb_tick = HAL_GetTick(); - jetson_cmd_ready = 1; - break; - } - - /* Direct motor test: W, — bypasses balance PID, - * sends directly to ESC. For bench testing and diagnostics only. - * Does NOT require arming. ESC watchdog stops motors if commands stop. */ - case 'W': { - int spd = 0, str = 0; - if (len > 1 && sscanf(buf + 1, "%d,%d", &spd, &str) >= 1) { - /* Clamp to safe bench test range */ - if (spd > 100) spd = 100; - if (spd < -100) spd = -100; - if (str > 100) str = 100; - if (str < -100) str = -100; - /* Set persistent direct test globals — main loop sends at 50Hz */ - extern volatile int16_t direct_test_speed; - extern volatile int16_t direct_test_steer; - direct_test_speed = (int16_t)spd; - direct_test_steer = (int16_t)str; - char ack[32]; - int n = snprintf(ack, sizeof(ack), "W:%d,%d\n", spd, str); - HAL_UART_Transmit(&huart6, (uint8_t*)ack, n, 10); - } - break; - } - -#ifdef DEBUG_MOTOR_TEST - /* GPIO test: reconfigure PC12 as plain GPIO output, set HIGH for 10s, then restore UART */ - case 'X': { - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_12; - gpio.Mode = GPIO_MODE_OUTPUT_PP; - gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; - HAL_GPIO_Init(GPIOC, &gpio); - HAL_GPIO_WritePin(GPIOC, GPIO_PIN_12, GPIO_PIN_SET); - char msg[] = "PC12=HIGH 10s, measure T5 pad\n"; - HAL_UART_Transmit(&huart6, (uint8_t*)msg, sizeof(msg)-1, 10); - HAL_Delay(10000); - /* Restore to UART AF */ - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_PULLUP; - gpio.Alternate = GPIO_AF8_UART5; - HAL_GPIO_Init(GPIOC, &gpio); - char done[] = "PC12 restored to UART5\n"; - HAL_UART_Transmit(&huart6, (uint8_t*)done, sizeof(done)-1, 10); - break; - } - - /* Baud rate sweep test: try multiple baud rates on UART5, 3s each */ - case 'R': { - extern UART_HandleTypeDef huart2; - uint16_t start = 0xABCD; - int16_t steer = 0, speed = 50; - uint16_t checksum = start ^ (uint16_t)steer ^ (uint16_t)speed; - uint8_t pkt[8]; - pkt[0] = start & 0xFF; pkt[1] = (start >> 8) & 0xFF; - pkt[2] = steer & 0xFF; pkt[3] = (steer >> 8) & 0xFF; - pkt[4] = speed & 0xFF; pkt[5] = (speed >> 8) & 0xFF; - pkt[6] = checksum & 0xFF; pkt[7] = (checksum >> 8) & 0xFF; - - /* Also report current BRR and APB1 freq */ - uint32_t brr = UART5->BRR; - uint32_t apb1 = HAL_RCC_GetPCLK1Freq(); - char info[80]; - int n = snprintf(info, sizeof(info), "BRR=%lu APB1=%lu\n", - (unsigned long)brr, (unsigned long)apb1); - HAL_UART_Transmit(&huart6, (uint8_t*)info, n, 20); - - const uint32_t bauds[] = {115200, 38400, 9600, 19200, 57600, 230400}; - for (int b = 0; b < 6; b++) { - /* Reconfigure baud */ - huart2.Init.BaudRate = bauds[b]; - HAL_UART_Init(&huart2); - - char msg[40]; - int mn = snprintf(msg, sizeof(msg), "BAUD %lu...\n", (unsigned long)bauds[b]); - HAL_UART_Transmit(&huart6, (uint8_t*)msg, mn, 20); - - /* Send for 3 sec */ - for (int i = 0; i < 150; i++) { - HAL_UART_Transmit(&huart2, pkt, 8, 5); - HAL_Delay(20); - } - /* Brief stop between bauds */ - HAL_Delay(500); - } - - /* Stop motors, restore 115200 */ - speed = 0; checksum = start; - pkt[4] = 0; pkt[5] = 0; - pkt[6] = checksum & 0xFF; pkt[7] = (checksum >> 8) & 0xFF; - huart2.Init.BaudRate = 115200; - HAL_UART_Init(&huart2); - for (int i = 0; i < 50; i++) { - HAL_UART_Transmit(&huart2, pkt, 8, 5); - HAL_Delay(20); - } - char ack[] = "R:done\n"; - HAL_UART_Transmit(&huart6, (uint8_t*)ack, 7, 10); - break; - } -#endif /* DEBUG_MOTOR_TEST */ - - case '?': { - /* Status dump for debugging */ - char st[128]; - int n = snprintf(st, sizeof(st), - "ST cal=%d estop=%d pitch=? hb=%lu\n", - mpu6000_is_calibrated() ? 1 : 0, - safety_remote_estop_active() ? 1 : 0, - (unsigned long)jetson_hb_tick); - HAL_UART_Transmit(&huart6, (uint8_t*)st, n, 20); - break; - } - - /* PID tuning commands */ - case 'P': case 'I': case 'K': case 'T': case 'M': { - uint8_t copy_len = len < 31 ? len : 31; - for (uint8_t i = 0; i < copy_len; i++) - cdc_cmd_buf[i] = buf[i]; - cdc_cmd_buf[copy_len] = '\0'; - cdc_cmd_ready = 1; - break; - } - - default: break; - } -} - -/* IRQ handler — call from stm32 interrupt vector */ -void USART6_IRQHandler(void) { - HAL_UART_IRQHandler(&huart6); -} diff --git a/legacy/stm32/src/jlink.c b/legacy/stm32/src/jlink.c deleted file mode 100644 index 48add99..0000000 --- a/legacy/stm32/src/jlink.c +++ /dev/null @@ -1,738 +0,0 @@ -#include "jlink.h" -#include "audio.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include - -/* ---- DMA circular RX buffer ---- */ -#define JLINK_RX_BUF_LEN 128u /* must be power-of-2 */ -static uint8_t s_rx_buf[JLINK_RX_BUF_LEN]; -static uint32_t s_rx_tail = 0; /* consumer index (byte already processed) */ - -/* ---- HAL handles ---- */ -static UART_HandleTypeDef s_uart; -static DMA_HandleTypeDef s_dma_rx; - -/* ---- TX mutex ---- */ -/* - * Issue #522: USART1 IDLE interrupt (DMA RX) fires via HAL_UART_IRQHandler - * mid-frame during polling HAL_UART_Transmit, resetting gState and causing - * truncated/null-prefixed frames on the Jetson link. - * - * Fix: disable USART1_IRQn around every blocking TX so HAL_UART_IRQHandler - * cannot modify gState while HAL_UART_Transmit is looping. s_tx_busy guards - * against any re-entrant caller (ESC debug, future paths). - */ -static volatile uint8_t s_tx_busy = 0; - -static void jlink_tx_locked(uint8_t *buf, uint16_t len) -{ - if (s_tx_busy) return; /* drop if already transmitting */ - s_tx_busy = 1u; - HAL_NVIC_DisableIRQ(USART1_IRQn); - HAL_UART_Transmit(&s_uart, buf, len, 5u); - HAL_NVIC_EnableIRQ(USART1_IRQn); - s_tx_busy = 0u; -} - -/* ---- Volatile state ---- */ -volatile JLinkState jlink_state; - -/* ---- SCHED_SET static receive buffer (Issue #550) ---- */ -static JLinkSchedSetBuf s_sched_set_buf; - -JLinkSchedSetBuf *jlink_get_sched_set(void) -{ - return &s_sched_set_buf; -} - -/* ---- CRC16-XModem (poly 0x1021, init 0x0000) ---- */ -static uint16_t crc16_xmodem(const uint8_t *data, uint16_t len) -{ - uint16_t crc = 0x0000u; - for (uint16_t i = 0; i < len; i++) { - crc ^= (uint16_t)data[i] << 8; - for (uint8_t b = 0; b < 8; b++) { - if (crc & 0x8000u) - crc = (crc << 1) ^ 0x1021u; - else - crc <<= 1; - } - } - return crc; -} - -/* ---- jlink_init() ---- */ -void jlink_init(void) -{ - /* GPIO: PB6=TX AF7 (USART1_TX), PB7=RX AF7 (USART1_RX) */ - __HAL_RCC_GPIOB_CLK_ENABLE(); - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = GPIO_PIN_6 | GPIO_PIN_7; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_PULLUP; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; - gpio.Alternate = GPIO_AF7_USART1; - HAL_GPIO_Init(GPIOB, &gpio); - - /* DMA2 Stream2 Channel4 -- USART1_RX circular */ - __HAL_RCC_DMA2_CLK_ENABLE(); - s_dma_rx.Instance = DMA2_Stream2; - s_dma_rx.Init.Channel = DMA_CHANNEL_4; - s_dma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; - s_dma_rx.Init.PeriphInc = DMA_PINC_DISABLE; - s_dma_rx.Init.MemInc = DMA_MINC_ENABLE; - s_dma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - s_dma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - s_dma_rx.Init.Mode = DMA_CIRCULAR; - s_dma_rx.Init.Priority = DMA_PRIORITY_MEDIUM; - s_dma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - HAL_DMA_Init(&s_dma_rx); - __HAL_LINKDMA(&s_uart, hdmarx, s_dma_rx); - - /* USART1 at JLINK_BAUD (921600) */ - __HAL_RCC_USART1_CLK_ENABLE(); - s_uart.Instance = USART1; - s_uart.Init.BaudRate = JLINK_BAUD; - s_uart.Init.WordLength = UART_WORDLENGTH_8B; - s_uart.Init.StopBits = UART_STOPBITS_1; - s_uart.Init.Parity = UART_PARITY_NONE; - s_uart.Init.Mode = UART_MODE_TX_RX; - s_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE; - s_uart.Init.OverSampling = UART_OVERSAMPLING_16; - HAL_UART_Init(&s_uart); - - /* Enable USART1 IDLE interrupt for circular buffer draining */ - __HAL_UART_ENABLE_IT(&s_uart, UART_IT_IDLE); - HAL_NVIC_SetPriority(USART1_IRQn, 6, 0); - HAL_NVIC_EnableIRQ(USART1_IRQn); - - /* DMA2_Stream2 IRQ (for error handling) */ - HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 7, 0); - HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn); - - /* Start circular DMA RX -- never stops */ - HAL_UART_Receive_DMA(&s_uart, s_rx_buf, JLINK_RX_BUF_LEN); - - memset((void *)&jlink_state, 0, sizeof(jlink_state)); - memset(&s_sched_set_buf, 0, sizeof(s_sched_set_buf)); - s_rx_tail = 0; -} - -/* ---- IRQ handlers ---- */ -void USART1_IRQHandler(void) -{ - /* Clear IDLE flag by reading SR then DR */ - if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) { - __HAL_UART_CLEAR_IDLEFLAG(&s_uart); - /* jlink_process() drains the buffer from main loop -- no work here */ - } - HAL_UART_IRQHandler(&s_uart); -} - -void DMA2_Stream2_IRQHandler(void) -{ - HAL_DMA_IRQHandler(&s_dma_rx); -} - -/* ---- jlink_is_active() ---- */ -bool jlink_is_active(uint32_t now_ms) -{ - if (jlink_state.last_rx_ms == 0u) return false; - return (now_ms - jlink_state.last_rx_ms) < JLINK_HB_TIMEOUT_MS; -} - -/* ---- Frame dispatch ---- */ -static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen) -{ - /* Update heartbeat timestamp on every valid frame */ - jlink_state.last_rx_ms = HAL_GetTick(); - - switch (cmd) { - - case JLINK_CMD_HEARTBEAT: - /* Heartbeat only -- no payload action needed */ - break; - - case JLINK_CMD_DRIVE: - if (plen == 4u) { - int16_t spd, str; - memcpy(&spd, payload, 2); - memcpy(&str, payload + 2, 2); - /* Clamp to +/-1000 */ - if (spd > 1000) spd = 1000; - if (spd < -1000) spd = -1000; - if (str > 1000) str = 1000; - if (str < -1000) str = -1000; - jlink_state.speed = spd; - jlink_state.steer = str; - } - break; - - case JLINK_CMD_ARM: - jlink_state.arm_req = 1u; - break; - - case JLINK_CMD_DISARM: - jlink_state.disarm_req = 1u; - break; - - case JLINK_CMD_PID_SET: - if (plen == 12u) { - float kp, ki, kd; - memcpy(&kp, payload, 4); - memcpy(&ki, payload + 4, 4); - memcpy(&kd, payload + 8, 4); - /* Sanity bounds -- same as USB CDC PID handler in main.c */ - if (kp >= 0.0f && kp <= 500.0f) jlink_state.pid_kp = kp; - if (ki >= 0.0f && ki <= 50.0f) jlink_state.pid_ki = ki; - if (kd >= 0.0f && kd <= 50.0f) jlink_state.pid_kd = kd; - jlink_state.pid_updated = 1u; - } - break; - - case JLINK_CMD_DFU_ENTER: - /* Payload-less; main loop checks armed state before calling ota_enter_dfu() */ - jlink_state.dfu_req = 1u; - break; - - case JLINK_CMD_ESTOP: - jlink_state.estop_req = 1u; - break; - - case JLINK_CMD_AUDIO: - /* Payload: int16 PCM samples, little-endian, 1..126 samples (2..252 bytes) */ - if (plen >= 2u && (plen & 1u) == 0u) { - audio_write_pcm((const int16_t *)payload, plen / 2u); - } - break; - - case JLINK_CMD_SLEEP: - /* Payload-less; main loop calls power_mgmt_request_sleep() */ - jlink_state.sleep_req = 1u; - break; - - case JLINK_CMD_PID_SAVE: - /* Payload-less; main loop calls pid_flash_save() (Issue #531) */ - jlink_state.pid_save_req = 1u; - break; - - case JLINK_CMD_GIMBAL_POS: - /* Payload: int16 pan_x10, int16 tilt_x10, uint16 speed (6 bytes) Issue #547 */ - if (plen == 6u) { - int16_t pan, tilt; - uint16_t spd; - memcpy(&pan, payload, 2); - memcpy(&tilt, payload + 2, 2); - memcpy(&spd, payload + 4, 2); - jlink_state.gimbal_pan_x10 = pan; - jlink_state.gimbal_tilt_x10 = tilt; - jlink_state.gimbal_speed = spd; - jlink_state.gimbal_updated = 1u; - } - break; - - case JLINK_CMD_SCHED_GET: - /* Payload-less; main loop calls jlink_send_sched_telemetry() (Issue #550) */ - jlink_state.sched_get_req = 1u; - break; - - case JLINK_CMD_SCHED_SET: - /* Payload: uint8 num_bands + num_bands * sizeof(pid_sched_entry_t) bytes */ - if (plen >= 1u) { - uint8_t nb = payload[0]; - if (nb == 0u) nb = 1u; - if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS; - uint8_t expected = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t); - if (plen >= expected) { - s_sched_set_buf.num_bands = nb; - memcpy(s_sched_set_buf.bands, payload + 1, - nb * sizeof(pid_sched_entry_t)); - s_sched_set_buf.ready = 1u; - } - } - break; - - case JLINK_CMD_SCHED_SAVE: - /* Payload: float kp, float ki, float kd (12 bytes) for single-PID record */ - if (plen == 12u) { - float kp, ki, kd; - memcpy(&kp, payload, 4); - memcpy(&ki, payload + 4, 4); - memcpy(&kd, payload + 8, 4); - if (kp >= 0.0f && kp <= 500.0f) jlink_state.sched_save_kp = kp; - if (ki >= 0.0f && ki <= 50.0f) jlink_state.sched_save_ki = ki; - if (kd >= 0.0f && kd <= 50.0f) jlink_state.sched_save_kd = kd; - jlink_state.sched_save_req = 1u; - } - break; - - case JLINK_CMD_FAULT_LOG_GET: /* Issue #565: request fault log telemetry */ - jlink_state.fault_log_req = 1u; - break; - - case JLINK_CMD_CAN_STATS_GET: /* Issue #597: request CAN bus statistics */ - jlink_state.can_stats_req = 1u; - break; - - default: - break; - } -} - -/* ---- jlink_process() -- call from main loop every tick ---- */ -/* - * Parser state machine. - * Frame: [STX][LEN][CMD][PAYLOAD 0..LEN-1][CRC_hi][CRC_lo][ETX] - * LEN = count of CMD + PAYLOAD bytes (1..253). - * CRC16-XModem over CMD+PAYLOAD. - * Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255, - * but we cap at 64 for safety). - */ -#define JLINK_MAX_PAYLOAD 252u /* enlarged for AUDIO chunks (126 x int16) */ - -typedef enum { - PS_WAIT_STX = 0, - PS_WAIT_LEN, - PS_WAIT_DATA, /* receiving CMD + PAYLOAD (len bytes total) */ - PS_WAIT_CRC_HI, - PS_WAIT_CRC_LO, - PS_WAIT_ETX, -} ParseState; - -void jlink_process(void) -{ - static ParseState s_state = PS_WAIT_STX; - static uint8_t s_len = 0; /* expected CMD+PAYLOAD length */ - static uint8_t s_count = 0; /* bytes received so far in PS_WAIT_DATA */ - static uint8_t s_frame[JLINK_MAX_PAYLOAD + 1u]; /* [0]=CMD, [1..]=PAYLOAD */ - static uint8_t s_crc_hi = 0; - - /* Compute how many bytes the DMA has written since last drain */ - uint32_t head = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx); - uint32_t bytes = (head - s_rx_tail) & (JLINK_RX_BUF_LEN - 1u); - - for (uint32_t i = 0; i < bytes; i++) { - uint8_t b = s_rx_buf[s_rx_tail]; - s_rx_tail = (s_rx_tail + 1u) & (JLINK_RX_BUF_LEN - 1u); - - switch (s_state) { - case PS_WAIT_STX: - if (b == JLINK_STX) s_state = PS_WAIT_LEN; - break; - - case PS_WAIT_LEN: - if (b == 0u || b > JLINK_MAX_PAYLOAD + 1u) { - /* Invalid length -- resync */ - s_state = PS_WAIT_STX; - } else { - s_len = b; - s_count = 0; - s_state = PS_WAIT_DATA; - } - break; - - case PS_WAIT_DATA: - s_frame[s_count++] = b; - if (s_count == s_len) s_state = PS_WAIT_CRC_HI; - break; - - case PS_WAIT_CRC_HI: - s_crc_hi = b; - s_state = PS_WAIT_CRC_LO; - break; - - case PS_WAIT_CRC_LO: { - uint16_t rx_crc = ((uint16_t)s_crc_hi << 8) | b; - uint16_t calc_crc = crc16_xmodem(s_frame, s_len); - if (rx_crc == calc_crc) - s_state = PS_WAIT_ETX; - else - s_state = PS_WAIT_STX; /* CRC mismatch -- drop */ - break; - } - - case PS_WAIT_ETX: - if (b == JLINK_ETX) { - /* Valid frame: s_frame[0]=CMD, s_frame[1..s_len-1]=PAYLOAD */ - dispatch(s_frame + 1, s_frame[0], s_len - 1u); - } - /* Either way, go back to idle (resync on bad ETX) */ - s_state = PS_WAIT_STX; - break; - } - } -} - -/* ---- jlink_send_telemetry() ---- */ -void jlink_send_telemetry(const jlink_tlm_status_t *status) -{ - /* - * Frame: [STX][LEN][0x80][20 bytes STATUS][CRC_hi][CRC_lo][ETX] - * LEN = 1 (CMD) + 20 (payload) = 21 - * Total frame length = 1+1+1+20+2+1 = 26 bytes - * At 921600 baud (10 bits/byte): 26x10/921600 ~0.28ms -- safe to block. - */ - static uint8_t frame[26]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_status_t); /* 20 */ - const uint8_t len = 1u + plen; /* 21 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_STATUS; - memcpy(&frame[3], status, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); /* over CMD + PAYLOAD */ - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_power_telemetry() ---- */ -void jlink_send_power_telemetry(const jlink_tlm_power_t *power) -{ - /* - * Frame: [STX][LEN][0x81][11 bytes POWER][CRC_hi][CRC_lo][ETX] - * LEN = 1 (CMD) + 11 (payload) = 12; total = 17 bytes - * At 921600 baud: 17x10/921600 ~0.18 ms -- safe to block. - */ - static uint8_t frame[17]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_power_t); /* 11 */ - const uint8_t len = 1u + plen; /* 12 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_POWER; - memcpy(&frame[3], power, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_pid_result() -- Issue #531 ---- */ -void jlink_send_pid_result(const jlink_tlm_pid_result_t *result) -{ - /* - * Frame: [STX][LEN][0x83][13 bytes PID_RESULT][CRC_hi][CRC_lo][ETX] - * LEN = 1 (CMD) + 13 (payload) = 14; total frame = 19 bytes. - * At 921600 baud: 19x10/921600 ~0.21 ms -- safe to block. - */ - static uint8_t frame_pid[19]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_pid_result_t); /* 13 */ - const uint8_t flen = 1u + plen; /* 14 */ - - frame_pid[0] = JLINK_STX; - frame_pid[1] = flen; - frame_pid[2] = JLINK_TLM_PID_RESULT; - memcpy(&frame_pid[3], result, plen); - - uint16_t crc_pid = crc16_xmodem(&frame_pid[2], flen); - frame_pid[3 + plen] = (uint8_t)(crc_pid >> 8); - frame_pid[3 + plen + 1] = (uint8_t)(crc_pid & 0xFFu); - frame_pid[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame_pid, sizeof(frame_pid)); -} - -/* ---- jlink_send_battery_telemetry() -- Issue #533 ---- */ -void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt) -{ - /* - * Frame: [STX][LEN][0x82][10 bytes BATTERY][CRC_hi][CRC_lo][ETX] - * LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes - * At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block. - */ - static uint8_t frame[16]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_battery_t); /* 10 */ - const uint8_t len = 1u + plen; /* 11 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_BATTERY; - memcpy(&frame[3], batt, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_gimbal_state() -- Issue #547 ---- */ -void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state) -{ - /* - * Frame: [STX][LEN][0x84][10 bytes GIMBAL_STATE][CRC_hi][CRC_lo][ETX] - * LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes - * At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block. - */ - static uint8_t frame[16]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_gimbal_state_t); /* 10 */ - const uint8_t len = 1u + plen; /* 11 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_GIMBAL_STATE; - memcpy(&frame[3], state, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_motor_current_tlm() -- Issue #584 ---- */ -void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm) -{ - /* - * Frame: [STX][LEN][0x86][8 bytes MOTOR_CURRENT][CRC_hi][CRC_lo][ETX] - * LEN = 1 (CMD) + 8 (payload) = 9; total frame = 14 bytes. - * At 921600 baud: 14x10/921600 ~0.15 ms -- safe to block. - */ - static uint8_t frame[14]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_motor_current_t); /* 8 */ - const uint8_t len = 1u + plen; /* 9 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_MOTOR_CURRENT; - memcpy(&frame[3], tlm, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_sched_telemetry() -- Issue #550 ---- */ -void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm) -{ - /* - * Frame: [STX][LEN][0x85][1+N*16 bytes SCHED][CRC_hi][CRC_lo][ETX] - * Actual payload = 1 (num_bands) + tlm->num_bands * 16 bytes. - * Max payload = 1 + 6*16 = 97; max frame = 103 bytes. - * At 921600 baud: 103x10/921600 ~1.1 ms -- safe to block. - */ - uint8_t nb = tlm->num_bands; - if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS; - uint8_t plen = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t); - uint8_t len = 1u + plen; /* CMD + payload */ - /* frame: STX + LEN + CMD + payload + CRC_hi + CRC_lo + ETX */ - uint8_t frame[103]; - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_SCHED; - frame[3] = nb; - memcpy(&frame[4], tlm->bands, nb * sizeof(pid_sched_entry_t)); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, (uint16_t)(3u + plen + 3u)); -} - -/* ---- jlink_send_slope_tlm() -- Issue #600 ---- */ -void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm) -{ - /* - * Frame: [STX][LEN][0x88][4 bytes SLOPE][CRC_hi][CRC_lo][ETX] - * Total: 1+1+1+4+2+1 = 10 bytes - */ - static uint8_t frame[10]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_slope_t); /* 4 */ - const uint8_t len = 1u + plen; /* CMD byte + payload */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_SLOPE; - memcpy(&frame[3], tlm, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_fault_log() -- Issue #565 ---- */ -void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl) -{ - /* - * Frame: [STX][LEN][0x87][20 bytes fault_log][CRC_hi][CRC_lo][ETX] - * LEN = 1 + 20 = 21; total = 26 bytes - * At 921600 baud: 26x10/921600 ~0.28 ms -- safe to block. - */ - static uint8_t frame[26]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_fault_log_t); /* 20 */ - const uint8_t len = 1u + plen; /* CMD byte + payload */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_FAULT_LOG; - memcpy(&frame[3], fl, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_can_stats() -- Issue #597 ---- */ -void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm) -{ - /* - * Frame: [STX][LEN][0x89][16 bytes CAN_STATS][CRC_hi][CRC_lo][ETX] - * LEN = 1 + 16 = 17; total = 22 bytes - * At 921600 baud: 22x10/921600 ~0.24 ms -- safe to block. - */ - static uint8_t frame[22]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_can_stats_t); /* 16 */ - const uint8_t len = 1u + plen; /* 17 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_CAN_STATS; - memcpy(&frame[3], tlm, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_lvc_tlm() -- Issue #613 ---- */ -void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm) -{ - /* - * Frame: [STX][LEN][0x8B][4 bytes LVC][CRC_hi][CRC_lo][ETX] - * LEN = 1 + 4 = 5; total = 10 bytes - */ - static uint8_t frame[10]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_lvc_t); /* 4 */ - const uint8_t len = 1u + plen; /* 5 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_LVC; - memcpy(&frame[3], tlm, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_odom_tlm() -- Issue #632 ---- */ -void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm) -{ - static uint8_t frame[22]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_odom_t); /* 16 */ - const uint8_t len = 1u + plen; /* 17 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_ODOM; - memcpy(&frame[3], tlm, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_baro_tlm() -- Issue #672 ---- */ -void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm) -{ - /* - * Frame: [STX][LEN][0x8D][12 bytes BARO][CRC_hi][CRC_lo][ETX] - * LEN = 1 + 12 = 13; total = 18 bytes - * At 921600 baud: 18×10/921600 ≈ 0.20 ms — safe to block. - */ - static uint8_t frame[18]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_baro_t); /* 12 */ - const uint8_t len = 1u + plen; /* 13 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_BARO; - memcpy(&frame[3], tlm, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_vesc_state_tlm() -- Issue #674 ---- */ -void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm) -{ - /* - * Frame: [STX][LEN][0x8E][22 bytes VESC_STATE][CRC_hi][CRC_lo][ETX] - * LEN = 1 (CMD) + 22 (payload) = 23; total frame = 28 bytes. - * At 921600 baud: 28×10/921600 ≈ 0.30 ms — safe to block. - */ - static uint8_t frame[28]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_vesc_state_t); /* 22 */ - const uint8_t len = 1u + plen; /* 23 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_VESC_STATE; - memcpy(&frame[3], tlm, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} - -/* ---- jlink_send_can_wdog_tlm() -- Issue #694 ---- */ -void jlink_send_can_wdog_tlm(const jlink_tlm_can_wdog_t *tlm) -{ - /* - * Frame: [STX][LEN][0x8F][16 bytes CAN_WDOG][CRC_hi][CRC_lo][ETX] - * LEN = 1 (CMD) + 16 (payload) = 17; total frame = 22 bytes. - */ - static uint8_t frame[22]; - const uint8_t plen = (uint8_t)sizeof(jlink_tlm_can_wdog_t); /* 16 */ - const uint8_t len = 1u + plen; /* 17 */ - - frame[0] = JLINK_STX; - frame[1] = len; - frame[2] = JLINK_TLM_CAN_WDOG; - memcpy(&frame[3], tlm, plen); - - uint16_t crc = crc16_xmodem(&frame[2], len); - frame[3 + plen] = (uint8_t)(crc >> 8); - frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); - frame[3 + plen + 2] = JLINK_ETX; - - jlink_tx_locked(frame, sizeof(frame)); -} diff --git a/legacy/stm32/src/led.c b/legacy/stm32/src/led.c deleted file mode 100644 index 6252484..0000000 --- a/legacy/stm32/src/led.c +++ /dev/null @@ -1,307 +0,0 @@ -#include "led.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include -#include - -/* ================================================================ - * WS2812B NeoPixel protocol via PWM - * ================================================================ - * 800 kHz PWM → 1.25 µs per cycle - * Bit encoding: - * "0": High 350 ns (40% duty) → ~3/8 of 1.25 µs - * "1": High 700 ns (56% duty) → ~7/10 of 1.25 µs - * Reset: Low > 50 µs (automatic with DMA ring and reload) - * - * Implementation: DMA copies PWM duty values from buffer. - * Each bit needs one PWM cycle; 192 bits total (24 bits/LED × 8 LEDs). - */ - -#define LED_BITS_PER_COLOR 8u -#define LED_BITS_PER_LED (LED_BITS_PER_COLOR * 3u) /* RGB */ -#define LED_TOTAL_BITS (LED_BITS_PER_LED * LED_STRIP_NUM_LEDS) -#define LED_PWM_PERIOD (216000000 / LED_STRIP_FREQ_HZ) /* 216 MHz / 800 kHz */ - -/* PWM duty values for bit encoding (out of LED_PWM_PERIOD) */ -#define LED_BIT_0_DUTY (LED_PWM_PERIOD * 40 / 100) /* ~350 ns high */ -#define LED_BIT_1_DUTY (LED_PWM_PERIOD * 56 / 100) /* ~700 ns high */ - -/* ================================================================ - * LED buffer and animation state - * ================================================================ - */ - -typedef struct { - RGBColor leds[LED_STRIP_NUM_LEDS]; - uint32_t pwm_buf[LED_TOTAL_BITS]; /* DMA buffer: PWM duty values */ -} LEDBuffer; - -/* LED state machine */ -typedef struct { - LEDState current_state; - LEDState next_state; - uint32_t state_start_ms; - uint8_t animation_phase; /* 0-255 for continuous animations */ -} LEDAnimState; - -static LEDBuffer s_led_buf = {0}; -static LEDAnimState s_anim = {0}; -static TIM_HandleTypeDef s_tim_handle = {0}; - -/* ================================================================ - * Helper functions - * ================================================================ - */ - -static void rgb_to_pwm_buffer(const RGBColor *colors, uint8_t num_leds) -{ - /* Encode LED colors into PWM duty values for WS2812B transmission. - * GRB byte order (WS2812B standard), MSB first. */ - uint32_t buf_idx = 0; - - for (uint8_t led = 0; led < num_leds; led++) { - uint8_t g = colors[led].g; - uint8_t r = colors[led].r; - uint8_t b = colors[led].b; - - /* GRB byte order */ - uint8_t bytes[3] = {g, r, b}; - - for (int byte_idx = 0; byte_idx < 3; byte_idx++) { - uint8_t byte = bytes[byte_idx]; - - /* MSB first — encode 8 bits */ - for (int bit = 7; bit >= 0; bit--) { - uint8_t bit_val = (byte >> bit) & 1; - s_led_buf.pwm_buf[buf_idx++] = bit_val ? LED_BIT_1_DUTY : LED_BIT_0_DUTY; - } - } - } -} - -static uint8_t sin_u8(uint8_t phase) -{ - /* Approximate sine wave (0-255) from phase (0-255) for breathing effect. */ - static const uint8_t sine_lut[256] = { - 128, 131, 134, 137, 140, 143, 146, 149, 152, 155, 158, 161, 164, 167, 170, 173, - 176, 179, 182, 185, 188, 191, 193, 196, 199, 201, 204, 206, 209, 211, 214, 216, - 218, 221, 223, 225, 227, 229, 231, 233, 235, 236, 238, 240, 241, 243, 244, 245, - 247, 248, 249, 250, 251, 252, 252, 253, 254, 254, 255, 255, 255, 255, 255, 254, - 254, 253, 252, 252, 251, 250, 249, 248, 247, 245, 244, 243, 241, 240, 238, 236, - 235, 233, 231, 229, 227, 225, 223, 221, 218, 216, 214, 211, 209, 206, 204, 201, - 199, 196, 193, 191, 188, 185, 182, 179, 176, 173, 170, 167, 164, 161, 158, 155, - 152, 149, 146, 143, 140, 137, 134, 131, 128, 125, 122, 119, 116, 113, 110, 107, - 104, 101, 98, 95, 92, 89, 86, 83, 80, 77, 74, 71, 68, 65, 62, 59, - 56, 53, 50, 47, 44, 41, 39, 36, 33, 31, 28, 26, 23, 21, 18, 16, - 14, 11, 9, 7, 5, 3, 1, 0, 0, 0, 0, 0, 1, 2, 3, 4, - 5, 7, 8, 10, 11, 13, 15, 17, 19, 21, 23, 26, 28, 31, 33, 36, - 39, 42, 45, 48, 51, 54, 57, 60, 63, 66, 69, 72, 75, 78, 82, 85, - 88, 92, 95, 99, 102, 105, 109, 113, 116, 120, 124, 127, 131 - }; - return sine_lut[phase]; -} - -/* ================================================================ - * Animation implementations - * ================================================================ - */ - -static void animate_boot(uint32_t elapsed_ms) -{ - /* Blue chase: rotate a single LED around the ring. */ - uint8_t led_idx = (elapsed_ms / 100) % LED_STRIP_NUM_LEDS; /* 100 ms per LED */ - - memset(s_led_buf.leds, 0, sizeof(s_led_buf.leds)); - s_led_buf.leds[led_idx].b = 255; /* Bright blue */ - - rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS); -} - -static void animate_armed(void) -{ - /* Solid green: all LEDs constant brightness. */ - for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) { - s_led_buf.leds[i].g = 200; /* Bright green */ - s_led_buf.leds[i].r = 0; - s_led_buf.leds[i].b = 0; - } - - rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS); -} - -static void animate_error(uint32_t elapsed_ms) -{ - /* Red blinking: on/off every 250 ms. */ - bool on = ((elapsed_ms / 250) % 2) == 0; - - for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) { - s_led_buf.leds[i].r = on ? 255 : 0; - s_led_buf.leds[i].g = 0; - s_led_buf.leds[i].b = 0; - } - - rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS); -} - -static void animate_low_battery(uint32_t elapsed_ms) -{ - /* Yellow pulsing: brightness varies smoothly. */ - uint8_t phase = (elapsed_ms / 20) & 0xFF; /* Cycle every 5120 ms */ - uint8_t brightness = sin_u8(phase); - - for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) { - s_led_buf.leds[i].r = (brightness * 255) >> 8; - s_led_buf.leds[i].g = (brightness * 255) >> 8; - s_led_buf.leds[i].b = 0; - } - - rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS); -} - -static void animate_charging(uint32_t elapsed_ms) -{ - /* Green breathing: smooth brightness modulation. */ - uint8_t phase = (elapsed_ms / 20) & 0xFF; /* Cycle every 5120 ms */ - uint8_t brightness = sin_u8(phase); - - for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) { - s_led_buf.leds[i].g = (brightness * 255) >> 8; - s_led_buf.leds[i].r = 0; - s_led_buf.leds[i].b = 0; - } - - rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS); -} - -static void animate_estop(uint32_t elapsed_ms) -{ - /* Red strobe: on/off every 125 ms (8 Hz). */ - bool on = ((elapsed_ms / 125) % 2) == 0; - - for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) { - s_led_buf.leds[i].r = on ? 255 : 0; - s_led_buf.leds[i].g = 0; - s_led_buf.leds[i].b = 0; - } - - rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS); -} - -/* ================================================================ - * Public API - * ================================================================ - */ - -void led_init(void) -{ - /* Initialize state machine */ - s_anim.current_state = LED_STATE_BOOT; - s_anim.next_state = LED_STATE_BOOT; - s_anim.state_start_ms = 0; - s_anim.animation_phase = 0; - - /* Configure GPIO PB4 as TIM3_CH1 output (AF2) */ - __HAL_RCC_GPIOB_CLK_ENABLE(); - GPIO_InitTypeDef gpio_init = {0}; - gpio_init.Pin = LED_STRIP_PIN; - gpio_init.Mode = GPIO_MODE_AF_PP; - gpio_init.Pull = GPIO_NOPULL; - gpio_init.Speed = GPIO_SPEED_FREQ_HIGH; - gpio_init.Alternate = LED_STRIP_AF; - HAL_GPIO_Init(LED_STRIP_PORT, &gpio_init); - - /* Configure TIM3: PWM mode, 800 kHz frequency - * STM32F722 has 216 MHz on APB1; TIM3 is on APB1 (prescaler 4×). - * APB1 clock: 216 MHz / 4 = 54 MHz - * For 800 kHz PWM: 54 MHz / 800 kHz = 67.5 → use 67 or 68 - * With ARR = 67: 54 MHz / 68 = 794 kHz ≈ 800 kHz - */ - __HAL_RCC_TIM3_CLK_ENABLE(); - - s_tim_handle.Instance = LED_STRIP_TIM; - s_tim_handle.Init.Prescaler = 0; /* No prescaler; APB1 = 54 MHz directly */ - s_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP; - s_tim_handle.Init.Period = LED_PWM_PERIOD - 1; - s_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - s_tim_handle.Init.RepetitionCounter = 0; - - HAL_TIM_PWM_Init(&s_tim_handle); - - /* Configure TIM3_CH1 for PWM */ - TIM_OC_InitTypeDef oc_init = {0}; - oc_init.OCMode = TIM_OCMODE_PWM1; - oc_init.Pulse = 0; /* Start at 0% duty */ - oc_init.OCPolarity = TIM_OCPOLARITY_HIGH; - oc_init.OCFastMode = TIM_OCFAST_DISABLE; - - HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, LED_STRIP_CHANNEL); - HAL_TIM_PWM_Start(&s_tim_handle, LED_STRIP_CHANNEL); - - /* Initialize LED buffer with boot state */ - animate_boot(0); -} - -void led_set_state(LEDState state) -{ - if (state >= LED_STATE_COUNT) { - return; - } - s_anim.next_state = state; -} - -LEDState led_get_state(void) -{ - return s_anim.current_state; -} - -void led_set_color(uint8_t r, uint8_t g, uint8_t b) -{ - for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) { - s_led_buf.leds[i].r = r; - s_led_buf.leds[i].g = g; - s_led_buf.leds[i].b = b; - } - rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS); -} - -void led_tick(uint32_t now_ms) -{ - /* State transition */ - if (s_anim.next_state != s_anim.current_state) { - s_anim.current_state = s_anim.next_state; - s_anim.state_start_ms = now_ms; - } - - uint32_t elapsed = now_ms - s_anim.state_start_ms; - - /* Run state-specific animation */ - switch (s_anim.current_state) { - case LED_STATE_BOOT: - animate_boot(elapsed); - break; - case LED_STATE_ARMED: - animate_armed(); - break; - case LED_STATE_ERROR: - animate_error(elapsed); - break; - case LED_STATE_LOW_BATT: - animate_low_battery(elapsed); - break; - case LED_STATE_CHARGING: - animate_charging(elapsed); - break; - case LED_STATE_ESTOP: - animate_estop(elapsed); - break; - default: - break; - } -} - -bool led_is_animating(void) -{ - /* Static states: ARMED (always) and ERROR (after first blink) */ - /* All others animate continuously */ - return s_anim.current_state != LED_STATE_ARMED; -} diff --git a/legacy/stm32/src/lvc.c b/legacy/stm32/src/lvc.c deleted file mode 100644 index cb09d11..0000000 --- a/legacy/stm32/src/lvc.c +++ /dev/null @@ -1,144 +0,0 @@ -/* - * lvc.c -- Low Voltage Cutoff (LVC) protection (Issue #613) - * - * State machine: - * NORMAL -> WARNING when vbat < LVC_WARNING_MV - * WARNING -> CRITICAL when vbat < LVC_CRITICAL_MV - * CRITICAL -> CUTOFF when vbat < LVC_CUTOFF_MV - * - * Recovery: step down severity only when voltage exceeds the threshold - * by LVC_HYSTERESIS_MV. CUTOFF is latched for the remainder of the session. - * - * Buzzer alerts: - * WARNING -- MELODY_LOW_BATTERY once, then every 30 s - * CRITICAL -- MELODY_LOW_BATTERY x2, then every 10 s - * CUTOFF -- MELODY_ERROR (one-shot; motor disable handled by main.c) - */ - -#include "lvc.h" -#include "buzzer.h" -#include "config.h" - -/* Periodic buzzer reminder intervals */ -#define LVC_WARN_INTERVAL_MS 30000u /* 30 s */ -#define LVC_CRIT_INTERVAL_MS 10000u /* 10 s */ - -static LvcState s_state = LVC_NORMAL; -static bool s_cutoff_latched = false; -static uint32_t s_buzzer_tick = 0u; - -/* ---- lvc_init() ---- */ -void lvc_init(void) -{ - s_state = LVC_NORMAL; - s_cutoff_latched = false; - s_buzzer_tick = 0u; -} - -/* ---- lvc_tick() ---- */ -void lvc_tick(uint32_t now_ms, uint32_t vbat_mv) -{ - if (vbat_mv == 0u) { - return; /* ADC not ready; hold current state */ - } - - /* Determine new state from raw voltage */ - LvcState new_state; - if (vbat_mv < LVC_CUTOFF_MV) { - new_state = LVC_CUTOFF; - } else if (vbat_mv < LVC_CRITICAL_MV) { - new_state = LVC_CRITICAL; - } else if (vbat_mv < LVC_WARNING_MV) { - new_state = LVC_WARNING; - } else { - new_state = LVC_NORMAL; - } - - /* Hysteresis on recovery: only decrease severity when voltage exceeds - * the threshold by LVC_HYSTERESIS_MV to prevent rapid toggling. */ - if (new_state < s_state) { - LvcState recovered; - if (vbat_mv >= LVC_CUTOFF_MV + LVC_HYSTERESIS_MV) { - recovered = LVC_CRITICAL; - } else if (vbat_mv >= LVC_CRITICAL_MV + LVC_HYSTERESIS_MV) { - recovered = LVC_WARNING; - } else if (vbat_mv >= LVC_WARNING_MV + LVC_HYSTERESIS_MV) { - recovered = LVC_NORMAL; - } else { - recovered = s_state; /* insufficient margin; stay at current level */ - } - new_state = recovered; - } - - /* CUTOFF latch: once triggered, only a reboot clears it */ - if (s_cutoff_latched) { - new_state = LVC_CUTOFF; - } - if (new_state == LVC_CUTOFF) { - s_cutoff_latched = true; - } - - /* Buzzer alerts */ - bool state_changed = (new_state != s_state); - s_state = new_state; - - switch (s_state) { - case LVC_WARNING: - if (state_changed || - (now_ms - s_buzzer_tick) >= LVC_WARN_INTERVAL_MS) { - s_buzzer_tick = now_ms; - buzzer_play_melody(MELODY_LOW_BATTERY); - } - break; - - case LVC_CRITICAL: - if (state_changed || - (now_ms - s_buzzer_tick) >= LVC_CRIT_INTERVAL_MS) { - s_buzzer_tick = now_ms; - /* Double alert to distinguish critical from warning */ - buzzer_play_melody(MELODY_LOW_BATTERY); - buzzer_play_melody(MELODY_LOW_BATTERY); - } - break; - - case LVC_CUTOFF: - if (state_changed) { - /* One-shot alarm; motors disabled by main.c */ - buzzer_play_melody(MELODY_ERROR); - } - break; - - default: - break; - } -} - -/* ---- lvc_get_state() ---- */ -LvcState lvc_get_state(void) -{ - return s_state; -} - -/* ---- lvc_get_power_scale() ---- */ -/* - * Returns the motor power scale factor (0-100): - * NORMAL / WARNING : 100% -- no reduction - * CRITICAL : 50% -- halve motor commands - * CUTOFF : 0% -- all commands zeroed - */ -uint8_t lvc_get_power_scale(void) -{ - switch (s_state) { - case LVC_NORMAL: /* fall-through */ - case LVC_WARNING: return 100u; - case LVC_CRITICAL: return 50u; - case LVC_CUTOFF: return 0u; - default: return 100u; - } -} - -/* ---- lvc_is_cutoff() ---- */ -bool lvc_is_cutoff(void) -{ - return s_cutoff_latched; -} diff --git a/legacy/stm32/src/mag.c b/legacy/stm32/src/mag.c deleted file mode 100644 index 116e26c..0000000 --- a/legacy/stm32/src/mag.c +++ /dev/null @@ -1,155 +0,0 @@ -/* - * mag.c — Auto-detect magnetometer on I2C1 - * - * Probes for QMC5883L (0x0D), HMC5883L (0x1E), IST8310 (0x0E) in that order. - * Returns detected type; mag_read_heading() returns heading in degrees×10. - * - * Requires i2c1_init() called before mag_init(). - */ -#include "mag.h" -#include "i2c1.h" -#include - -/* ------------------------------------------------------------------ */ -/* I2C helpers (7-bit addr left-shifted for HAL) */ -/* ------------------------------------------------------------------ */ -static uint16_t s_addr; -static mag_type_t s_type = MAG_NONE; - -static uint8_t mag_rreg(uint8_t reg) { - uint8_t v = 0; - HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &v, 1, 50); - return v; -} -static void mag_wreg(uint8_t reg, uint8_t val) { - HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 50); -} -static void mag_rregs(uint8_t reg, uint8_t *buf, uint8_t len) { - HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 50); -} - -/* ------------------------------------------------------------------ */ -/* QMC5883L — QUST QMC5883L (0x0D) */ -/* chip_id reg 0x0D = 0xFF */ -/* Data regs 0x00-0x05: XL XH YL YH ZL ZH */ -/* ------------------------------------------------------------------ */ -static int qmc_init(void) { - s_addr = 0x0D << 1; - uint8_t id = mag_rreg(0x0D); - if (id != 0xFF) return 0; - - /* Reset */ - mag_wreg(0x0B, 0x01); /* SET/RESET period = 0x01 */ - HAL_Delay(10); - - /* Control 1: continuous mode (0x01), 200 Hz ODR (0x0C), 512 OSR (0x00), 2G (0x00) */ - mag_wreg(0x09, 0x0D); /* MODE=cont, ODR=200Hz, RNG=2G, OSR=512 */ - HAL_Delay(10); - return 1; -} - -static int16_t qmc_heading(void) { - /* Status reg 0x06, bit0=DRDY */ - if (!(mag_rreg(0x06) & 0x01)) return -1; - - uint8_t buf[6]; - mag_rregs(0x00, buf, 6); - int16_t x = (int16_t)((buf[1] << 8) | buf[0]); - int16_t y = (int16_t)((buf[3] << 8) | buf[2]); - - float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f); - if (heading_deg < 0.0f) heading_deg += 360.0f; - return (int16_t)(heading_deg * 10.0f); -} - -/* ------------------------------------------------------------------ */ -/* HMC5883L — Honeywell HMC5883L (0x1E) */ -/* ID regs 0x0A='H', 0x0B='4', 0x0C='3' */ -/* Data regs 0x03-0x08: XH XL ZH ZL YH YL (note XZY order!) */ -/* ------------------------------------------------------------------ */ -static int hmc_init(void) { - s_addr = 0x1E << 1; - uint8_t a = mag_rreg(0x0A); - uint8_t b = mag_rreg(0x0B); - uint8_t c = mag_rreg(0x0C); - if (a != 'H' || b != '4' || c != '3') return 0; - - mag_wreg(0x00, 0x70); /* Config A: 8 samples/output, 15 Hz, normal */ - mag_wreg(0x01, 0x20); /* Config B: gain=1090 LSB/Ga */ - mag_wreg(0x02, 0x00); /* Mode: continuous measurement */ - HAL_Delay(10); - return 1; -} - -static int16_t hmc_heading(void) { - /* Status reg 0x09, bit0=RDY */ - if (!(mag_rreg(0x09) & 0x01)) return -1; - - uint8_t buf[6]; - mag_rregs(0x03, buf, 6); - int16_t x = (int16_t)((buf[0] << 8) | buf[1]); - int16_t y = (int16_t)((buf[4] << 8) | buf[5]); /* HMC order: XZY */ - - float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f); - if (heading_deg < 0.0f) heading_deg += 360.0f; - return (int16_t)(heading_deg * 10.0f); -} - -/* ------------------------------------------------------------------ */ -/* IST8310 — iSentek IST8310 (0x0E) */ -/* WHO_AM_I reg 0x00 = 0x10 */ -/* Data regs 0x03-0x08: XL XH YL YH ZL ZH */ -/* ------------------------------------------------------------------ */ -static int ist_init(void) { - s_addr = 0x0E << 1; - uint8_t id = mag_rreg(0x00); - if (id != 0x10) return 0; - - /* Soft reset via CNTL2 reg 0x0B bit[0] */ - mag_wreg(0x0B, 0x01); - HAL_Delay(20); - - /* CNTL1 reg 0x0A: single measurement mode, trigger on write */ - /* Use continuous mode bit[2]=1 if supported — trigger manually instead */ - mag_wreg(0x0A, 0x01); /* single measurement */ - HAL_Delay(10); - return 1; -} - -static int16_t ist_heading(void) { - /* Trigger a measurement */ - mag_wreg(0x0A, 0x01); - HAL_Delay(10); /* IST8310 needs ~6.6ms for single measurement */ - - /* STAT1 reg 0x02 bit0=DRDY */ - if (!(mag_rreg(0x02) & 0x01)) return -1; - - uint8_t buf[6]; - mag_rregs(0x03, buf, 6); - int16_t x = (int16_t)((buf[1] << 8) | buf[0]); - int16_t y = (int16_t)((buf[3] << 8) | buf[2]); - - float heading_deg = atan2f((float)y, (float)x) * (180.0f / 3.14159265f); - if (heading_deg < 0.0f) heading_deg += 360.0f; - return (int16_t)(heading_deg * 10.0f); -} - -/* ------------------------------------------------------------------ */ -/* Public API */ -/* ------------------------------------------------------------------ */ -mag_type_t mag_init(void) { - if (qmc_init()) { s_type = MAG_QMC5883L; return s_type; } - if (hmc_init()) { s_type = MAG_HMC5883L; return s_type; } - if (ist_init()) { s_type = MAG_IST8310; return s_type; } - s_type = MAG_NONE; - return MAG_NONE; -} - -int16_t mag_read_heading(void) { - switch (s_type) { - case MAG_QMC5883L: return qmc_heading(); - case MAG_HMC5883L: return hmc_heading(); - case MAG_IST8310: return ist_heading(); - default: return -1; - } -} diff --git a/legacy/stm32/src/main.c b/legacy/stm32/src/main.c deleted file mode 100644 index bb92be9..0000000 --- a/legacy/stm32/src/main.c +++ /dev/null @@ -1,1092 +0,0 @@ -#include "stm32f7xx_hal.h" -#include "usbd_core.h" -#include "usbd_cdc.h" -#include "usbd_cdc_if.h" -#include "usbd_desc.h" -#include "mpu6000.h" -#include "balance.h" -#include "hoverboard.h" -#include "motor_driver.h" -#include "mode_manager.h" -#include "config.h" -#include "status.h" -#include "safety.h" -#include "crsf.h" -#include "i2c1.h" -#include "bmp280.h" -#include "baro.h" -#include "mag.h" -#include "bno055.h" -#include "jetson_cmd.h" -#include "jetson_uart.h" -#include "jlink.h" -#include "ota.h" -#include "audio.h" -#include "buzzer.h" -#include "led.h" -#include "servo.h" -#include "ina219.h" -#include "ultrasonic.h" -#include "power_mgmt.h" -#include "battery.h" -#include "coulomb_counter.h" -#include "watchdog.h" -#include "pid_flash.h" -#include "fault_handler.h" -#include "can_driver.h" -#include "vesc_can.h" -#include "orin_can.h" -#include "imu_cal_flash.h" -#include "hw_button.h" -#include "servo_bus.h" -#include "gimbal.h" -#include "lvc.h" -#include "uart_protocol.h" -#include -#include -#include - -extern volatile uint8_t cdc_streaming; /* set by S command in CDC RX */ -extern volatile uint8_t cdc_arm_request; /* set by A command */ -extern volatile uint8_t cdc_disarm_request; /* set by D command */ -extern volatile uint8_t cdc_recal_request; /* set by G command */ -extern volatile uint32_t cdc_rx_count; /* total CDC packets received */ -extern volatile uint8_t cdc_estop_request; -extern volatile uint8_t cdc_estop_clear_request; -extern volatile uint8_t cdc_imu_cal_request; /* set by O command — mount cal (Issue #680) */ - -/* Direct motor test (set by W command in jetson_uart.c) */ -volatile int16_t direct_test_speed = 0; -volatile int16_t direct_test_steer = 0; - -/* BNO055 active flag (set if BNO055 initialized successfully) */ -static bool bno055_active = false; - -/* Watchdog reset flag (set if MCU was reset by IWDG timeout) */ -static bool g_watchdog_reset_detected = false; - -/* - * Apply a PID tuning command string from the USB terminal. - * Format: P I D T M ? - * Returns a short ack string written into `reply` (max reply_sz bytes). - */ -static void apply_pid_cmd(balance_t *b, const char *cmd, char *reply, int reply_sz) { - float val = 0.0f; - /* Simple ASCII float parse — avoids sscanf in hot path */ - if (cmd[0] == '?') { - snprintf(reply, reply_sz, - "{\"kp\":%d,\"ki\":%d,\"kd\":%d,\"sp\":%d,\"ms\":%d}\n", - (int)(b->kp * 1000), (int)(b->ki * 1000), (int)(b->kd * 1000), - (int)(b->setpoint * 10), (int)b->max_speed); - return; - } - /* Parse float after the command letter */ - if (sscanf(cmd + 1, "%f", &val) != 1) { - snprintf(reply, reply_sz, "{\"err\":\"bad_fmt\"}\n"); - return; - } - switch (cmd[0]) { - case 'P': if (val >= 0.0f && val <= 500.0f) b->kp = val; break; - case 'I': if (val >= 0.0f && val <= 50.0f) b->ki = val; break; - case 'K': /* alias for D */ /* fall through */ - case 'D': if (val >= 0.0f && val <= 50.0f) b->kd = val; break; - case 'T': if (val >= -20.0f && val <= 20.0f) b->setpoint = val; break; - case 'M': if (val >= 0.0f && val <= 1000.0f) b->max_speed = (int16_t)val; break; - } - snprintf(reply, reply_sz, - "{\"kp\":%d,\"ki\":%d,\"kd\":%d,\"sp\":%d,\"ms\":%d}\n", - (int)(b->kp * 1000), (int)(b->ki * 1000), (int)(b->kd * 1000), - (int)(b->setpoint * 10), (int)b->max_speed); -} - -USBD_HandleTypeDef hUsbDevice; - -static void SystemClock_Config(void) { - RCC_OscInitTypeDef osc = {0}; - RCC_ClkInitTypeDef clk = {0}; - RCC_PeriphCLKInitTypeDef pclk = {0}; - __HAL_RCC_PWR_CLK_ENABLE(); - __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); - osc.OscillatorType = RCC_OSCILLATORTYPE_HSE; - osc.HSEState = RCC_HSE_ON; - osc.PLL.PLLState = RCC_PLL_ON; - osc.PLL.PLLSource = RCC_PLLSOURCE_HSE; - osc.PLL.PLLM = 8; osc.PLL.PLLN = 432; osc.PLL.PLLP = 2; osc.PLL.PLLQ = 9; - if (HAL_RCC_OscConfig(&osc) != HAL_OK) { - osc.OscillatorType = RCC_OSCILLATORTYPE_HSI; - osc.HSIState = RCC_HSI_ON; osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; - osc.HSEState = RCC_HSE_OFF; osc.PLL.PLLSource = RCC_PLLSOURCE_HSI; osc.PLL.PLLM = 16; - if (HAL_RCC_OscConfig(&osc) != HAL_OK) while (1); - } - HAL_PWREx_EnableOverDrive(); - pclk.PeriphClockSelection = RCC_PERIPHCLK_CLK48 | RCC_PERIPHCLK_I2C1; - pclk.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP; - pclk.PLLSAI.PLLSAIN = 384; pclk.PLLSAI.PLLSAIQ = 7; pclk.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8; - pclk.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; - if (HAL_RCCEx_PeriphCLKConfig(&pclk) != HAL_OK) while (1); - clk.ClockType = RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; - clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - clk.AHBCLKDivider = RCC_SYSCLK_DIV1; - clk.APB1CLKDivider = RCC_HCLK_DIV4; clk.APB2CLKDivider = RCC_HCLK_DIV2; - HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7); -} - -extern PCD_HandleTypeDef hpcd; -void OTG_FS_IRQHandler(void) { HAL_PCD_IRQHandler(&hpcd); } -void SysTick_Handler(void) { HAL_IncTick(); } - -int main(void) { - SCB_EnableICache(); - /* DCache stays ON — MPU Region 0 in usbd_conf.c marks USB buffers non-cacheable. */ - checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */ - HAL_Init(); - SystemClock_Config(); - - /* Fault recovery handler (Issue #565) — must be first, before safety_init() */ - fault_handler_init(); - - /* Detect watchdog reset (Issue #300) — must be before safety_init() */ - g_watchdog_reset_detected = watchdog_was_reset_by_watchdog(); - - /* USB CDC */ - USBD_Init(&hUsbDevice, &SaltyLab_Desc, 0); - USBD_RegisterClass(&hUsbDevice, &USBD_CDC); - USBD_CDC_RegisterInterface(&hUsbDevice, &USBD_CDC_fops); - USBD_Start(&hUsbDevice); - - status_init(); - HAL_Delay(3000); /* Wait for USB host to enumerate */ - - /* Log watchdog reset event if it occurred (Issue #300) */ - if (g_watchdog_reset_detected) { - printf("[WATCHDOG] MCU reset by IWDG timeout detected. Main loop may have hung.\n"); - } - - /* Init IMU (MPU6000 via SPI1 — mpu6000.c wraps icm42688 + complementary filter) */ - int imu_ret = mpu6000_init() ? 0 : -1; - - /* - * Gyro bias calibration — hold board still for ~1s. - * Must run before safety_init() (IWDG not yet started). - * Blocks arming and streaming until complete. - */ - if (imu_ret == 0) mpu6000_calibrate(); - - /* Load IMU mount angle offsets from flash if previously calibrated (Issue #680) */ - { - float imu_pitch_off = 0.0f, imu_roll_off = 0.0f; - if (imu_cal_flash_load(&imu_pitch_off, &imu_roll_off)) { - mpu6000_set_mount_offset(imu_pitch_off, imu_roll_off); - printf("[IMU_CAL] Mount offsets loaded: pitch=%.1f° roll=%.1f°\n", - (double)imu_pitch_off, (double)imu_roll_off); - } - } - - /* Init hoverboard ESC UART */ - hoverboard_init(); - - /* Init balance controller */ - balance_t bal; - balance_init(&bal); - - /* Load PID from flash if previously auto-tuned (Issue #531) */ - { - float flash_kp, flash_ki, flash_kd; - if (pid_flash_load(&flash_kp, &flash_ki, &flash_kd)) { - bal.kp = flash_kp; - bal.ki = flash_ki; - bal.kd = flash_kd; - printf("[PID] Loaded from flash: Kp=%.3f Ki=%.3f Kd=%.3f\n", - (double)flash_kp, (double)flash_ki, (double)flash_kd); - } - } - - /* Init motor driver */ - motor_driver_t motors; - motor_driver_init(&motors); - - /* Init CRSF/ELRS receiver on UART4 (PA0/PA1) with DMA */ - crsf_init(); - - /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */ - jlink_init(); - - /* Init CAN1 bus — PB8/PB9, 500 kbps (Issues #597/#676/#674). - * Register callbacks BEFORE can_driver_init() so both are ready when - * filter banks activate. */ - can_driver_set_ext_cb(vesc_can_on_frame); /* VESC STATUS → FIFO1 */ - can_driver_set_std_cb(orin_can_on_frame); /* Orin cmds → FIFO0 */ - can_driver_init(); - vesc_can_init(VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT); - orin_can_init(); - - /* Init hardware button debounce/gesture driver (Issue #682) */ - hw_button_init(); - - /* Send fault log summary on boot if a prior fault was recorded (Issue #565) */ - if (fault_get_last_type() != FAULT_NONE) { - fault_log_entry_t fle; - memset(&fle, 0, sizeof(fle)); - jlink_tlm_fault_log_t ftlm; - memset(&ftlm, 0, sizeof(ftlm)); - ftlm.entry_count = fault_log_get_count(); - if (fault_log_read(0u, &fle)) { - ftlm.fault_type = fle.fault_type; - ftlm.reset_count = fle.reset_count; - ftlm.timestamp_ms = fle.timestamp_ms; - ftlm.pc = fle.pc; - ftlm.lr = fle.lr; - ftlm.cfsr = fle.cfsr; - ftlm.hfsr = fle.hfsr; - } - jlink_send_fault_log(&ftlm); - printf("[FAULT] Prior fault type=%u count=%u PC=0x%08lX\n", - (unsigned)ftlm.fault_type, (unsigned)ftlm.entry_count, - (unsigned long)ftlm.pc); - } - - /* Init Jetson UART command interface on USART6 (PC6/PC7) at 921600 baud. - * Mirrors CDC command protocol over hardware UART (fixes USB CDC TX bug). */ - jetson_uart_init(); - - /* Init I2S3 audio amplifier (PC10/PA15/PB5, mute=PC5) */ - audio_init(); - audio_play_tone(AUDIO_TONE_STARTUP); - - /* Init piezo buzzer driver (TIM4_CH3 PWM on PB2, Issue #189) */ - buzzer_init(); - buzzer_play_melody(MELODY_STARTUP); - - /* Init WS2812B NeoPixel LED ring (TIM3_CH1 PWM on PB4, Issue #193) */ - led_init(); - led_set_state(LED_STATE_BOOT); - - /* Init power management — STOP-mode sleep/wake, wake EXTIs configured */ - power_mgmt_init(); - - /* Init servo pan-tilt driver for camera head (TIM4 PWM on PB6/PB7, Issue #206) */ - servo_init(); - - /* Init ST3215 serial bus servo driver (USART3 half-duplex PB10, Issue #547) */ - servo_bus_init(); - - /* Init pan/tilt gimbal (ST3215 servos, centers both axes on boot) */ - gimbal_t gimbal; - gimbal_init(&gimbal); - - /* Init HC-SR04 ultrasonic distance sensor (TIM1 input capture on PA1, Issue #243) */ - ultrasonic_init(); - - /* Init mode manager (RC/autonomous blend; CH6 mode switch) */ - mode_manager_t mode; - mode_manager_init(&mode); - - /* Init battery ADC (PC1/ADC3 — Vbat divider 11:1) for CRSF telemetry */ - battery_init(); - - /* Init LVC: low voltage cutoff state machine (Issue #613) */ - lvc_init(); - - /* Init UART command protocol for Jetson (UART5 PC12/PD2, 115200 baud, Issue #629) */ - uart_protocol_init(); - - /* Probe I2C1 for optional sensors — skip gracefully if not found */ - int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */ - mag_type_t mag_type = MAG_NONE; - if (i2c1_init() == 0) { - int chip = bmp280_init(); - baro_chip = (chip > 0) ? chip : 0; - baro_init(baro_chip); /* Issue #672: 1 Hz baro read + JLink telemetry */ - mag_type = mag_init(); - ina219_init(); /* Init INA219 dual motor current monitoring (Issue #214) */ - } - - /* - * IWDG starts AFTER all peripheral inits — avoids reset during mpu6000_init() - * which takes ~510ms (well above the 50ms WATCHDOG_TIMEOUT_MS). - * Once started, safety_refresh() MUST be called every WATCHDOG_TIMEOUT_MS - * or MCU resets. IWDG cannot be stopped once started (hardware enforced). - */ - safety_init(); - - char buf[320]; - int len; - uint32_t tx_count = 0; /* telemetry frames sent to host */ - - IMUData imu; - uint32_t send_tick = 0; - uint32_t balance_tick = 0; - uint32_t esc_tick = 0; - uint32_t crsf_telem_tick = 0; /* CRSF uplink telemetry TX timer */ - uint32_t jlink_tlm_tick = 0; /* Jetson binary telemetry TX timer */ - uint32_t pm_tlm_tick = 0; /* JLINK_TLM_POWER transmit timer */ - uint32_t can_cmd_tick = 0; /* CAN velocity command TX timer (Issue #597) */ - uint32_t can_tlm_tick = 0; /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */ - uint32_t wdog_tlm_tick = 0; /* JLINK_TLM_CAN_WDOG transmit timer (Issue #694) */ - uint32_t lvc_tlm_tick = 0; /* JLINK_TLM_LVC transmit timer (Issue #613) */ - uint32_t uart_status_tick = 0; /* UART protocol STATUS frame timer (Issue #629) */ - uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */ - const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */ - - while (1) { - if (imu_ret == 0) mpu6000_read(&imu); - - uint32_t now = HAL_GetTick(); - - /* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */ - safety_refresh(); - - /* Advance audio tone sequencer (non-blocking, call every tick) */ - audio_tick(now); - - /* Advance buzzer pattern sequencer (non-blocking, call every tick) */ - buzzer_tick(now); - - /* Orin LED override (Issue #685): apply pattern from CAN 0x304 if updated. - * Safety states (estop, tilt fault) are applied later and always win. */ - if (orin_can_led_updated) { - orin_can_led_updated = 0u; - /* pattern: 0=auto, 1=solid-green, 2=slow-blink, 3=fast-blink, 4=pulse */ - switch (orin_can_led_override.pattern) { - case 1u: led_set_state(LED_STATE_ARMED); break; - case 2u: led_set_state(LED_STATE_LOW_BATT); break; /* slow blink */ - case 3u: led_set_state(LED_STATE_ERROR); break; /* fast blink */ - case 4u: led_set_state(LED_STATE_CHARGING); break; /* pulse */ - default: break; /* 0=auto — let state machine below decide */ - } - } - - /* Advance LED animation sequencer (non-blocking, call every tick) */ - led_tick(now); - - /* Fault recovery LED blink code (Issue #565; self-disables after 10 s) */ - fault_led_tick(now); - - /* Servo pan-tilt animation tick — updates smooth sweeps */ - servo_tick(now); - - /* Gimbal ST3215 feedback polling tick (self-throttles to GIMBAL_TLM_HZ) */ - gimbal_tick(&gimbal, now); - - /* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */ - battery_accumulate_coulombs(); - - /* LVC: update low-voltage protection state machine (Issue #613) */ - lvc_tick(now, battery_read_mv()); - - /* Baro: 1 Hz BME280 read + JLink telemetry (Issue #672) */ - baro_tick(now); - - /* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness. - * pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */ - pm_pwm_phase++; - { - uint8_t pm_bright = power_mgmt_led_brightness(); - if (pm_bright > 0u) { - bool led_on = (pm_pwm_phase < pm_bright); - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, - led_on ? GPIO_PIN_RESET : GPIO_PIN_SET); - } - } - - /* Power manager tick — may block in WFI (STOP mode) when disarmed */ - if (bal.state != BALANCE_ARMED) { - power_mgmt_tick(now); - } - - /* Mode manager: update RC liveness, CH6 mode selection, blend ramp */ - mode_manager_update(&mode, now); - - if (cdc_estop_request) { - EstopSource src = (cdc_estop_request == 1) ? ESTOP_REMOTE : ESTOP_CELLULAR_TIMEOUT; - cdc_estop_request = 0; - safety_remote_estop(src); - safety_arm_cancel(); - balance_disarm(&bal); - motor_driver_estop(&motors); - } - if (cdc_estop_clear_request) { - cdc_estop_clear_request = 0; - if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED) - safety_remote_estop_clear(); - } - - /* Drain JLink DMA circular buffer and parse frames */ - jlink_process(); - - /* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */ - can_driver_process(); - - /* CAN watchdog: detect bus errors, auto-restart (Issue #694) */ - can_driver_watchdog_tick(now); - - /* UART command protocol RX: parse Jetson frames (Issue #629) */ - uart_protocol_process(); - - /* Handle JLink one-shot flags from Jetson binary protocol */ - if (jlink_state.estop_req) { - jlink_state.estop_req = 0u; - safety_remote_estop(ESTOP_REMOTE); - safety_arm_cancel(); - balance_disarm(&bal); - motor_driver_estop(&motors); - } - if (jlink_state.arm_req) { - jlink_state.arm_req = 0u; - if (!safety_remote_estop_active() && - mpu6000_is_calibrated() && - bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) { - safety_arm_start(now); - } - } - if (jlink_state.disarm_req) { - jlink_state.disarm_req = 0u; - safety_arm_cancel(); - balance_disarm(&bal); - } - if (jlink_state.pid_updated) { - jlink_state.pid_updated = 0u; - bal.kp = jlink_state.pid_kp; - bal.ki = jlink_state.pid_ki; - bal.kd = jlink_state.pid_kd; - } - if (jlink_state.dfu_req) { - jlink_state.dfu_req = 0u; - /* ota_enter_dfu() is a no-op (returns false) when armed; - * never returns when disarmed — MCU resets into DFU mode. */ - ota_enter_dfu(bal.state == BALANCE_ARMED); - } - if (jlink_state.sleep_req) { - jlink_state.sleep_req = 0u; - power_mgmt_request_sleep(); - } - /* GIMBAL_POS: forward new pan/tilt command to gimbal (Issue #547) */ - if (jlink_state.gimbal_updated) { - jlink_state.gimbal_updated = 0u; - gimbal_set_pos(&gimbal, - (int16_t)jlink_state.gimbal_pan_x10, - (int16_t)jlink_state.gimbal_tilt_x10, - jlink_state.gimbal_speed); - } - - /* PID_SAVE: persist current gains to flash, reply with result (Issue #531). - * Only allowed while disarmed -- flash sector erase stalls CPU for ~1 s. */ - if (jlink_state.pid_save_req && bal.state != BALANCE_ARMED) { - jlink_state.pid_save_req = 0u; - bool save_ok = pid_flash_save(bal.kp, bal.ki, bal.kd); - jlink_tlm_pid_result_t pid_res; - pid_res.kp = bal.kp; - pid_res.ki = bal.ki; - pid_res.kd = bal.kd; - pid_res.saved_ok = save_ok ? 1u : 0u; - jlink_send_pid_result(&pid_res); - printf("[PID] Flash save %s: Kp=%.3f Ki=%.3f Kd=%.3f\n", - save_ok ? "OK" : "FAIL", - (double)bal.kp, (double)bal.ki, (double)bal.kd); - } - - /* UART protocol: handle commands from Jetson (Issue #629) */ - { - if (uart_prot_state.vel_updated) { - uart_prot_state.vel_updated = 0u; - if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) { - can_cmd_t ucmd_l = { uart_prot_state.left_rpm, 0 }; - can_cmd_t ucmd_r = { uart_prot_state.right_rpm, 0 }; - can_driver_send_cmd(CAN_NODE_LEFT, &ucmd_l); - can_driver_send_cmd(CAN_NODE_RIGHT, &ucmd_r); - } - } - if (uart_prot_state.pid_updated) { - uart_prot_state.pid_updated = 0u; - bal.kp = uart_prot_state.pid_kp; - bal.ki = uart_prot_state.pid_ki; - bal.kd = uart_prot_state.pid_kd; - } - if (uart_prot_state.estop_req) { - uart_prot_state.estop_req = 0u; - safety_remote_estop(ESTOP_REMOTE); - safety_arm_cancel(); - balance_disarm(&bal); - motor_driver_estop(&motors); - } - if (uart_prot_state.estop_clear_req) { - uart_prot_state.estop_clear_req = 0u; - if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED) - safety_remote_estop_clear(); - } - } - - /* Orin CAN: handle commands from Orin over CAN bus (Issue #674). - * Estop and clear are latching; drive/mode are consumed each tick. - * Balance independence: if orin_can_is_alive() == false the loop - * simply does not inject any commands and holds upright in-place. */ - if (orin_can_state.estop_req) { - orin_can_state.estop_req = 0u; - safety_remote_estop(ESTOP_REMOTE); - safety_arm_cancel(); - balance_disarm(&bal); - motor_driver_estop(&motors); - } - if (orin_can_state.estop_clear_req) { - orin_can_state.estop_clear_req = 0u; - if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED) - safety_remote_estop_clear(); - } - /* PID_SET (Issue #693): apply new gains from Orin immediately. - * Gains persist in RAM until reboot; not saved to flash. */ - if (orin_can_state.pid_updated) { - orin_can_state.pid_updated = 0u; - float new_kp = (float)orin_can_state.pid_kp_x100 / 100.0f; - float new_ki = (float)orin_can_state.pid_ki_x100 / 100.0f; - float new_kd = (float)orin_can_state.pid_kd_x100 / 100.0f; - if (new_kp > 500.0f) new_kp = 500.0f; - if (new_ki > 50.0f) new_ki = 50.0f; - if (new_kd > 50.0f) new_kd = 50.0f; - bal.kp = new_kp; - bal.ki = new_ki; - bal.kd = new_kd; - orin_can_send_pid_ack(bal.kp, bal.ki, bal.kd); - printf("[PID] Orin set Kp=%.3f Ki=%.3f Kd=%.3f\n", - (double)bal.kp, (double)bal.ki, (double)bal.kd); - } - - /* Hardware button park/re-arm (Issue #682). - * Short press -> park (ARMED only): freeze PID, stop motors, amber LED. - * SHORT+SHORT+LONG combo -> unpark (PARKED only): resume if upright. */ - { - hw_btn_event_t btn_ev = hw_button_tick(now); - if (btn_ev == BTN_EVENT_PARK) { - if (bal.state == BALANCE_ARMED) { - balance_park(&bal); - led_set_state(LED_STATE_LOW_BATT); - orin_can_send_btn_event(1u, (uint8_t)bal.state); - } - } else if (btn_ev == BTN_EVENT_REARM_COMBO) { - if (bal.state == BALANCE_PARKED) { - balance_unpark(&bal); - if (bal.state == BALANCE_ARMED) { - led_set_state(LED_STATE_ARMED); - orin_can_send_btn_event(2u, (uint8_t)bal.state); - } else { - /* Pitch too large — unpark refused, stay parked */ - orin_can_send_btn_event(3u, (uint8_t)bal.state); - } - } - } - } - - /* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */ - if (jlink_state.fault_log_req) { - jlink_state.fault_log_req = 0u; - fault_log_entry_t fle; - memset(&fle, 0, sizeof(fle)); - jlink_tlm_fault_log_t ftlm; - memset(&ftlm, 0, sizeof(ftlm)); - ftlm.entry_count = fault_log_get_count(); - if (fault_log_read(0u, &fle)) { - ftlm.fault_type = fle.fault_type; - ftlm.reset_count = fle.reset_count; - ftlm.timestamp_ms = fle.timestamp_ms; - ftlm.pc = fle.pc; - ftlm.lr = fle.lr; - ftlm.cfsr = fle.cfsr; - ftlm.hfsr = fle.hfsr; - } - jlink_send_fault_log(&ftlm); - } - - /* CAN_STATS: send telemetry on demand or at CAN_TLM_HZ (1 Hz) (Issue #597) */ - { - bool send_can = (jlink_state.can_stats_req != 0u); - if (!send_can && (now - can_tlm_tick >= (1000u / CAN_TLM_HZ))) { - send_can = true; - } - if (send_can) { - jlink_state.can_stats_req = 0u; - can_tlm_tick = now; - can_stats_t cs; - can_driver_get_stats(&cs); - can_feedback_t fb0, fb1; - jlink_tlm_can_stats_t ctlm; - memset(&ctlm, 0, sizeof(ctlm)); - ctlm.tx_count = cs.tx_count; - ctlm.rx_count = cs.rx_count; - ctlm.err_count = cs.err_count; - ctlm.bus_off = cs.bus_off; - if (can_driver_get_feedback(CAN_NODE_LEFT, &fb0)) { - ctlm.vel0_rpm = fb0.velocity_rpm; - ctlm.node_faults |= (fb0.fault ? 1u : 0u); - } - if (can_driver_get_feedback(CAN_NODE_RIGHT, &fb1)) { - ctlm.vel1_rpm = fb1.velocity_rpm; - ctlm.node_faults |= (fb1.fault ? 2u : 0u); - } - jlink_send_can_stats(&ctlm); - } - } - - /* CAN_WDOG: send watchdog telemetry at 1 Hz (Issue #694) */ - if (now - wdog_tlm_tick >= 1000u) { - wdog_tlm_tick = now; - can_wdog_t wd; - can_driver_get_wdog(&wd); - jlink_tlm_can_wdog_t wtlm; - memset(&wtlm, 0, sizeof(wtlm)); - wtlm.restart_count = wd.restart_count; - wtlm.busoff_count = wd.busoff_count; - wtlm.errpassive_count = wd.errpassive_count; - wtlm.errwarn_count = wd.errwarn_count; - wtlm.error_state = (uint8_t)wd.error_state; - wtlm.tec = wd.tec; - wtlm.rec = wd.rec; - jlink_send_can_wdog_tlm(&wtlm); - } - - /* Power management: CRSF/JLink activity or armed state resets idle timer */ - if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) || - jlink_is_active(now) || - bal.state == BALANCE_ARMED) { - power_mgmt_activity(); - } - - /* RC CH5 kill switch: emergency stop if RC is alive and CH5 off. - * Issue #512: RC becomes optional override — kill switch triggers estop, - * not disarm, so Jetson-armed robots remain armed when RC disconnects. - * Emergency stop kills motors immediately but allows re-arm. */ - if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) { - motor_driver_estop(&motors); - } - /* RC failsafe: disarm if signal lost AFTER RC was previously alive. - * Prevents auto-disarm in USB-only mode (crsf_state.last_rx_ms == 0). */ - if (bal.state == BALANCE_ARMED && - crsf_state.last_rx_ms != 0 && - (now - crsf_state.last_rx_ms) > CRSF_FAILSAFE_MS) { - safety_arm_cancel(); - balance_disarm(&bal); - } - - /* Tilt fault buzzer alert (one-shot on fault edge) */ - safety_alert_tilt_fault(bal.state == BALANCE_TILT_FAULT); - - /* RC arm/disarm via CH5 switch (CRSF) — edge detect with hold interlock. - * Issue #512: RC becomes optional override. Falling edge only disarms if RC - * explicitly requested it (CH5 off while RC alive). RC disconnect doesn't - * disarm Jetson-controlled robots; Jetson timeout disarm (in main loop) handles it. */ - { - static bool s_rc_armed_prev = false; - bool rc_armed_now = safety_rc_alive(now) && crsf_state.armed; - if (rc_armed_now && !s_rc_armed_prev) { - /* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */ - if (!safety_remote_estop_active() && - mpu6000_is_calibrated() && - bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) { - safety_arm_start(now); - } - } - if (!rc_armed_now && s_rc_armed_prev && safety_rc_alive(now)) { - /* Falling edge with RC still alive: RC explicitly de-armed. - * Cancel pending arm or disarm if already armed. */ - safety_arm_cancel(); - if (bal.state == BALANCE_ARMED) balance_disarm(&bal); - } - /* Note: RC disconnect (crsf_state.last_rx_ms == 0 after being alive) is handled - * by failsafe timer below, NOT by this edge detector. */ - s_rc_armed_prev = rc_armed_now; - } - - /* Handle arm/disarm requests from USB with arm-hold interlock */ - if (cdc_arm_request) { - cdc_arm_request = 0; - if (!safety_remote_estop_active() && - mpu6000_is_calibrated() && - bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) { - safety_arm_start(now); - } - } - /* Complete arming once hold timer expires */ - if (safety_arm_ready(now) && bal.state == BALANCE_DISARMED) { - safety_arm_cancel(); - balance_arm(&bal); - audio_play_tone(AUDIO_TONE_ARM); - } - if (cdc_disarm_request) { - cdc_disarm_request = 0; - safety_arm_cancel(); - balance_disarm(&bal); - } - - /* Gyro recalibration — disarm first, then re-sample bias (~1s blocked) */ - if (cdc_recal_request) { - cdc_recal_request = 0; - safety_arm_cancel(); - balance_disarm(&bal); - if (imu_ret == 0) mpu6000_calibrate(); - } - - /* IMU mount angle calibration (Issue #680): capture current pitch/roll as - * mount offsets and save to flash. Disarmed only — flash erase stalls ~1s. */ - if (cdc_imu_cal_request && bal.state != BALANCE_ARMED) { - cdc_imu_cal_request = 0; - float off_p = bal.pitch_deg; - float off_r = imu.roll; - mpu6000_set_mount_offset(off_p, off_r); - bool cal_ok = imu_cal_flash_save(off_p, off_r); - char reply[64]; - snprintf(reply, sizeof(reply), - "{\"imu_cal\":%s,\"pitch_off\":%d,\"roll_off\":%d}\n", - cal_ok ? "ok" : "fail", - (int)(off_p * 10), (int)(off_r * 10)); - CDC_Transmit((uint8_t *)reply, strlen(reply)); - printf("[IMU_CAL] Mount offset saved: pitch=%.1f° roll=%.1f° (%s)\n", - (double)off_p, (double)off_r, cal_ok ? "OK" : "FAIL"); - } - - /* Handle PID tuning commands from USB (P/I/D/T/M/?) */ - if (cdc_cmd_ready) { - cdc_cmd_ready = 0; - char reply[96]; - apply_pid_cmd(&bal, (const char *)cdc_cmd_buf, reply, sizeof(reply)); - CDC_Transmit((uint8_t *)reply, strlen(reply)); - } - - /* Handle Jetson C, command (parsed here, not in ISR) */ - if (jetson_cmd_ready) { - jetson_cmd_ready = 0; - jetson_cmd_process(); - } - - /* - * Balance PID — 1kHz with MPU6000/SPI, ~250Hz with BNO055/I2C. - * BNO055 read (~3ms) is placed inside the gate so it doesn't block - * the full main loop; the gap between ticks is naturally longer. - * jlink takes priority for Jetson speed offset; falls back to CDC. - */ - if (imu_ret == 0 && now - balance_tick >= 1) { - if (bno055_active) bno055_read(&imu); /* I2C read inside gate */ - balance_tick = now; - float base_sp = bal.setpoint; - if (jlink_is_active(now)) - bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG; - else if (orin_can_is_alive(now)) - bal.setpoint += ((float)orin_can_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG; - else if (jetson_cmd_is_active(now)) - bal.setpoint += jetson_cmd_sp_offset(); - balance_update(&bal, &imu, dt); - bal.setpoint = base_sp; - } - - /* Latch estop on tilt fault or disarm */ - { - static uint8_t s_prev_tilt_fault = 0; - uint8_t tilt_now = (bal.state == BALANCE_TILT_FAULT) ? 1u : 0u; - if (tilt_now && !s_prev_tilt_fault) - audio_play_tone(AUDIO_TONE_FAULT); - s_prev_tilt_fault = tilt_now; - } - if (bal.state == BALANCE_TILT_FAULT) { - motor_driver_estop(&motors); - } else if (bal.state == BALANCE_DISARMED && motors.estop && - !safety_remote_estop_active()) { - motor_driver_estop_clear(&motors); - } - - /* LVC cutoff: disarm and estop on undervoltage latch (Issue #613) */ - if (lvc_is_cutoff() && bal.state == BALANCE_ARMED) { - safety_arm_cancel(); - balance_disarm(&bal); - motor_driver_estop(&motors); - } - - /* Feed autonomous steer from Jetson into mode manager. - * jlink takes priority over legacy CDC jetson_cmd. - * mode_manager_get_steer() blends it with RC steer per active mode. */ - if (jlink_is_active(now)) - mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0); - else if (orin_can_is_alive(now)) - mode_manager_set_auto_cmd(&mode, orin_can_state.steer, 0); - else if (jetson_cmd_is_active(now)) - mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0); - - /* Send to hoverboard ESC at 50Hz (every 20ms) */ - if (now - esc_tick >= 20) { - esc_tick = now; - if (bal.state == BALANCE_ARMED) { - /* Blended steer (RC ↔ auto per mode) + RC speed bias */ - int16_t steer = mode_manager_get_steer(&mode); - int16_t spd_bias = mode_manager_get_speed_bias(&mode); - int32_t speed = (int32_t)bal.motor_cmd + spd_bias; - /* LVC power scaling: 100% normal, 50% critical, 0% cutoff (Issue #613) */ - uint8_t lvc_scale = lvc_get_power_scale(); - if (lvc_scale < 100u) { - speed = (speed * (int32_t)lvc_scale) / 100; - } - if (speed > 1000) speed = 1000; - if (speed < -1000) speed = -1000; - motor_driver_update(&motors, (int16_t)speed, steer, now); - } else if (direct_test_speed != 0 || direct_test_steer != 0) { - /* Direct motor test mode — set by W command */ - esc_send(direct_test_speed, direct_test_steer); - } else { - /* Always send zero while disarmed to prevent ESC timeout */ - motor_driver_update(&motors, 0, 0, now); - } - } - - /* VESC: send RPM commands at CAN_TX_RATE_HZ (100 Hz) via 29-bit extended CAN. - * VESC does not need enable/disable frames — RPM=0 while disarmed holds brakes. - * Left wheel: speed_rpm + steer_rpm (differential drive) - * Right wheel: speed_rpm − steer_rpm (Issue #674) */ - if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) { - can_cmd_tick = now; - int32_t speed_rpm = 0; - int32_t steer_rpm = 0; - if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) { - int16_t can_steer = mode_manager_get_steer(&mode); - speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE; - steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2; - } - vesc_can_send_rpm(VESC_CAN_ID_LEFT, speed_rpm + steer_rpm); - vesc_can_send_rpm(VESC_CAN_ID_RIGHT, speed_rpm - steer_rpm); - } - - /* VESC telemetry: send JLINK_TLM_VESC_STATE at 1 Hz (Issue #674) */ - vesc_can_send_tlm(now); - - /* Orin CAN broadcast: FC_STATUS + FC_VESC at ORIN_TLM_HZ (10 Hz) (Issue #674) */ - { - orin_can_fc_status_t fc_st; - fc_st.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f); - fc_st.motor_cmd = bal.motor_cmd; - uint32_t _vbat = battery_read_mv(); - fc_st.vbat_mv = (_vbat > 65535u) ? 65535u : (uint16_t)_vbat; - fc_st.balance_state = (uint8_t)bal.state; - fc_st.flags = (safety_remote_estop_active() ? 0x01u : 0u) | - (bal.state == BALANCE_ARMED ? 0x02u : 0u); - - orin_can_fc_vesc_t fc_vesc; - vesc_state_t vl, vr; - bool vl_ok = vesc_can_get_state(VESC_CAN_ID_LEFT, &vl); - bool vr_ok = vesc_can_get_state(VESC_CAN_ID_RIGHT, &vr); - fc_vesc.left_rpm_x10 = vl_ok ? (int16_t)(vl.rpm / 10) : 0; - fc_vesc.right_rpm_x10 = vr_ok ? (int16_t)(vr.rpm / 10) : 0; - fc_vesc.left_current_x10 = vl_ok ? vl.current_x10 : 0; - fc_vesc.right_current_x10 = vr_ok ? vr.current_x10 : 0; - - orin_can_broadcast(now, &fc_st, &fc_vesc); - } - - /* FC_IMU (0x402): full IMU angles + calibration status at 50 Hz (Issue #680) */ - { - orin_can_fc_imu_t fc_imu; - fc_imu.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f); - fc_imu.roll_x10 = (int16_t)(imu.roll * 10.0f); - fc_imu.yaw_x10 = (int16_t)(imu.yaw * 10.0f); - /* cal_status: 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */ - if (!mpu6000_is_calibrated()) fc_imu.cal_status = 0u; - else if (mpu6000_has_mount_offset()) fc_imu.cal_status = 2u; - else fc_imu.cal_status = 1u; - fc_imu.balance_state = (uint8_t)bal.state; - orin_can_broadcast_imu(now, &fc_imu); - } - - /* FC_BARO (0x403): barometer at 1 Hz (Issue #672) */ - if (baro_chip) { - int32_t _pres_pa; int16_t _temp_x10; - bmp280_read(&_pres_pa, &_temp_x10); - int32_t _alt_cm = bmp280_pressure_to_alt_cm(_pres_pa); - orin_can_fc_baro_t fc_baro; - fc_baro.pressure_pa = _pres_pa; - fc_baro.temp_x10 = _temp_x10; - fc_baro.alt_cm = (_alt_cm > 32767) ? 32767 : - (_alt_cm < -32768) ? -32768 : (int16_t)_alt_cm; - orin_can_broadcast_baro(now, &fc_baro); - } - - /* CRSF telemetry uplink — battery voltage + arm state at 1 Hz. - * Sends battery sensor frame (0x08) and flight mode frame (0x21) - * back to ELRS TX module so the pilot's handset OSD shows Vbat + state. */ - if (now - crsf_telem_tick >= (1000u / CRSF_TELEMETRY_HZ)) { - crsf_telem_tick = now; - uint32_t vbat_mv = battery_read_mv(); - /* Use coulomb-based SoC if available, fallback to voltage-based */ - uint8_t soc_pct = battery_get_soc_coulomb(); - if (soc_pct == 255) { - soc_pct = battery_estimate_pct(vbat_mv); - } - crsf_send_battery(vbat_mv, coulomb_counter_get_remaining_mah(), soc_pct); - crsf_send_flight_mode(bal.state == BALANCE_ARMED); - } - - /* JLink STATUS telemetry at JLINK_TLM_HZ (50Hz) back to Jetson */ - if (now - jlink_tlm_tick >= (1000u / JLINK_TLM_HZ)) { - jlink_tlm_tick = now; - jlink_tlm_status_t tlm; - tlm.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f); - tlm.roll_x10 = (int16_t)(imu.roll * 10.0f); - tlm.yaw_x10 = (int16_t)(imu.yaw * 10.0f); - tlm.motor_cmd = bal.motor_cmd; - uint32_t vbat = battery_read_mv(); - tlm.vbat_mv = (vbat > 65535u) ? 65535u : (uint16_t)vbat; - tlm.rssi_dbm = crsf_state.rssi_dbm; - tlm.link_quality = crsf_state.link_quality; - tlm.balance_state = (uint8_t)bal.state; - tlm.rc_armed = crsf_state.armed ? 1u : 0u; - tlm.mode = (uint8_t)mode_manager_active(&mode); - EstopSource _es = safety_get_estop(); - tlm.estop = (uint8_t)_es; - /* Use coulomb-based SoC if available, fallback to voltage-based */ - uint8_t soc = battery_get_soc_coulomb(); - tlm.soc_pct = (soc == 255) ? battery_estimate_pct(vbat) : soc; - tlm.fw_major = FW_MAJOR; - tlm.fw_minor = FW_MINOR; - tlm.fw_patch = FW_PATCH; - jlink_send_telemetry(&tlm); - - /* Send gimbal state at same 50 Hz cadence (Issue #547) */ - jlink_tlm_gimbal_state_t gtlm; - gtlm.pan_x10 = gimbal.fb_pan_x10; - gtlm.tilt_x10 = gimbal.fb_tilt_x10; - gtlm.pan_speed_raw = gimbal.fb_pan_speed; - gtlm.tilt_speed_raw = gimbal.fb_tilt_speed; - gtlm.torque_en = gimbal.torque_enabled ? 1u : 0u; - uint32_t rx_total = gimbal.rx_ok + gimbal.rx_err; - gtlm.rx_err_pct = (rx_total > 0u) - ? (uint8_t)(gimbal.rx_err * 100u / rx_total) - : 0u; - jlink_send_gimbal_state(>lm); - } - - /* JLINK_TLM_POWER telemetry at PM_TLM_HZ (1 Hz) */ - if (now - pm_tlm_tick >= (1000u / PM_TLM_HZ)) { - pm_tlm_tick = now; - jlink_tlm_power_t pow; - pow.power_state = (uint8_t)power_mgmt_state(); - pow.est_total_ma = power_mgmt_current_ma(); - pow.est_audio_ma = (uint16_t)(power_mgmt_state() == PM_SLEEPING ? 0u : PM_CURRENT_AUDIO_MA); - pow.est_osd_ma = (uint16_t)(power_mgmt_state() == PM_SLEEPING ? 0u : PM_CURRENT_OSD_MA); - pow.idle_ms = power_mgmt_idle_ms(); - jlink_send_power_telemetry(&pow); - } - - /* JLINK_TLM_LVC telemetry at LVC_TLM_HZ (1 Hz) -- battery voltage + protection state (Issue #613) */ - if (now - lvc_tlm_tick >= (1000u / LVC_TLM_HZ)) { - lvc_tlm_tick = now; - uint32_t lvc_vbat = battery_read_mv(); - jlink_tlm_lvc_t ltlm; - ltlm.voltage_mv = (lvc_vbat > 65535u) ? 65535u : (uint16_t)lvc_vbat; - ltlm.protection_state = (uint8_t)lvc_get_state(); - if (lvc_vbat == 0u) { - ltlm.percent = 255u; - } else if (lvc_vbat <= LVC_CUTOFF_MV) { - ltlm.percent = 0u; - } else if (lvc_vbat >= LVC_WARNING_MV) { - ltlm.percent = 100u; - } else { - ltlm.percent = (uint8_t)(((lvc_vbat - LVC_CUTOFF_MV) * 100u) / - (LVC_WARNING_MV - LVC_CUTOFF_MV)); - } - jlink_send_lvc_tlm(<lm); - } - - /* UART protocol: send STATUS to Jetson at 10 Hz (Issue #629) */ - if (now - uart_status_tick >= 100u) { - uart_status_tick = now; - uart_prot_status_t ups; - ups.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f); - ups.motor_cmd = bal.motor_cmd; - uint32_t _uv = battery_read_mv(); - ups.vbat_mv = (_uv > 65535u) ? 65535u : (uint16_t)_uv; - ups.balance_state = (uint8_t)bal.state; - ups.estop_active = safety_remote_estop_active() ? 1u : 0u; - uart_protocol_send_status(&ups); - } - - /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */ - if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) { - send_tick = now; - if (imu_ret == 0) { - float err = bal.setpoint - bal.pitch_deg; - /* Build JSON incrementally; append optional sensor fields */ - char *p = buf; - int rem = (int)sizeof(buf); - int n; - n = snprintf(p, rem, - "{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d,\"y\":%d", - (int)(bal.pitch_deg * 10), /* pitch degrees x10 */ - (int)(imu.roll * 10), /* roll degrees x10 */ - (int)(err * 10), /* PID error x10 */ - (int)(bal.integral * 10), /* integral x10 (windup monitor) */ - (int)bal.motor_cmd, /* ESC command -1000..+1000 */ - (int)bal.state, - (int)(imu.yaw * 10)); /* yaw degrees x10 (gyro-integrated) */ - p += n; rem -= n; - n = snprintf(p, rem, ",\"md\":%d", - (int)mode_manager_active(&mode)); /* 0=MANUAL,1=ASSISTED,2=AUTO */ - p += n; rem -= n; - if (mag_type != MAG_NONE) { - int16_t hd = mag_read_heading(); - if (hd >= 0) - n = snprintf(p, rem, ",\"hd\":%d", hd); /* heading deg×10 */ - else - n = snprintf(p, rem, ",\"hd\":-1"); /* not ready */ - p += n; rem -= n; - } - if (baro_chip) { - int32_t pres_pa; int16_t temp_x10; - bmp280_read(&pres_pa, &temp_x10); - int32_t alt_cm = bmp280_pressure_to_alt_cm(pres_pa); - /* alt cm, temp °C×10, pressure hPa×10 (Pa÷10 = hPa×10) */ - n = snprintf(p, rem, ",\"alt\":%ld,\"t\":%d,\"pa\":%ld", - (long)alt_cm, (int)temp_x10, (long)(pres_pa / 10)); - p += n; rem -= n; - if (baro_chip == 0x60) { /* BME280: add humidity %RH×10 */ - int16_t hum_x10 = bmp280_read_humidity(); - if (hum_x10 >= 0) { - n = snprintf(p, rem, ",\"h\":%d", (int)hum_x10); - p += n; rem -= n; - } - } - } - if (safety_rc_alive(now)) { - /* RSSI, link quality, and CH1–CH4 mapped to µs (1000–2000) */ - n = snprintf(p, rem, ",\"rssi\":%d,\"lq\":%d" - ",\"ch1\":%d,\"ch2\":%d,\"ch3\":%d,\"ch4\":%d", - (int)crsf_state.rssi_dbm, (int)crsf_state.link_quality, - (int)crsf_to_range(crsf_state.channels[0], 1000, 2000), - (int)crsf_to_range(crsf_state.channels[1], 1000, 2000), - (int)crsf_to_range(crsf_state.channels[2], 1000, 2000), - (int)crsf_to_range(crsf_state.channels[3], 1000, 2000)); - p += n; rem -= n; - } - /* Jetson active flag, USB TX/RX packet counters */ - int es; EstopSource _rs = safety_get_estop(); - if (_rs >= ESTOP_REMOTE) es = (int)_rs; - else if (bal.state == BALANCE_TILT_FAULT) es = ESTOP_TILT; - else es = ESTOP_CLEAR; - /* BNO055 calibration status and temperature when active */ - if (bno055_active) { - uint8_t cs = bno055_calib_status(); - n = snprintf(p, rem, ",\"bno_cs\":%d,\"bno_t\":%d", - (int)cs, (int)bno055_temperature()); - p += n; rem -= n; - } - n = snprintf(p, rem, ",\"ja\":%d,\"jl\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n", - jetson_cmd_is_active(now) ? 1 : 0, - jlink_is_active(now) ? 1 : 0, - (unsigned)tx_count, - (unsigned)cdc_rx_count, es); - len = (int)(p + n - buf); - } else { - len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret); - } - CDC_Transmit((uint8_t *)buf, len); - tx_count++; - } - - status_update(now, (imu_ret == 0), - (bal.state == BALANCE_ARMED), - (bal.state == BALANCE_TILT_FAULT), - safety_remote_estop_active()); - HAL_Delay(1); - } -} diff --git a/legacy/stm32/src/main.c.bak b/legacy/stm32/src/main.c.bak deleted file mode 100644 index 5f66b9f..0000000 --- a/legacy/stm32/src/main.c.bak +++ /dev/null @@ -1,147 +0,0 @@ -#include "stm32f7xx_hal.h" -#include "usbd_core.h" -#include "usbd_cdc.h" -#include "usbd_cdc_if.h" -#include "usbd_desc.h" -#include "icm42688.h" -#include "bmp280.h" -#include "balance.h" -#include "hoverboard.h" -#include "config.h" -#include "status.h" -#include -#include - -extern volatile uint8_t cdc_streaming; /* set by S command in CDC RX */ -extern volatile uint8_t cdc_arm_request; /* set by A command */ -extern volatile uint8_t cdc_disarm_request; /* set by D command */ - -USBD_HandleTypeDef hUsbDevice; - -static void SystemClock_Config(void) { - RCC_OscInitTypeDef osc = {0}; - RCC_ClkInitTypeDef clk = {0}; - RCC_PeriphCLKInitTypeDef pclk = {0}; - __HAL_RCC_PWR_CLK_ENABLE(); - __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); - osc.OscillatorType = RCC_OSCILLATORTYPE_HSE; - osc.HSEState = RCC_HSE_ON; - osc.PLL.PLLState = RCC_PLL_ON; - osc.PLL.PLLSource = RCC_PLLSOURCE_HSE; - osc.PLL.PLLM = 8; osc.PLL.PLLN = 432; osc.PLL.PLLP = 2; osc.PLL.PLLQ = 9; - if (HAL_RCC_OscConfig(&osc) != HAL_OK) { - osc.OscillatorType = RCC_OSCILLATORTYPE_HSI; - osc.HSIState = RCC_HSI_ON; osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; - osc.HSEState = RCC_HSE_OFF; osc.PLL.PLLSource = RCC_PLLSOURCE_HSI; osc.PLL.PLLM = 16; - if (HAL_RCC_OscConfig(&osc) != HAL_OK) while (1); - } - HAL_PWREx_EnableOverDrive(); - pclk.PeriphClockSelection = RCC_PERIPHCLK_CLK48 | RCC_PERIPHCLK_I2C1; - pclk.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP; - pclk.PLLSAI.PLLSAIN = 384; pclk.PLLSAI.PLLSAIQ = 7; pclk.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8; - pclk.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; - if (HAL_RCCEx_PeriphCLKConfig(&pclk) != HAL_OK) while (1); - clk.ClockType = RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; - clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - clk.AHBCLKDivider = RCC_SYSCLK_DIV1; - clk.APB1CLKDivider = RCC_HCLK_DIV4; clk.APB2CLKDivider = RCC_HCLK_DIV2; - HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7); -} - -extern PCD_HandleTypeDef hpcd; -void OTG_FS_IRQHandler(void) { HAL_PCD_IRQHandler(&hpcd); } -void SysTick_Handler(void) { HAL_IncTick(); } - -int main(void) { - SCB_EnableICache(); - checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */ - HAL_Init(); - SystemClock_Config(); - - /* USB CDC */ - USBD_Init(&hUsbDevice, &SaltyLab_Desc, 0); - USBD_RegisterClass(&hUsbDevice, &USBD_CDC); - USBD_CDC_RegisterInterface(&hUsbDevice, &USBD_CDC_fops); - USBD_Start(&hUsbDevice); - - status_init(); - HAL_Delay(1000); - - /* Init IMU */ - int imu_ret = icm42688_init(); - - /* Init hoverboard ESC UART */ - hoverboard_init(); - - /* Init balance controller */ - balance_t bal; - balance_init(&bal); - - /* Send init status over USB */ - uint8_t tr[16]; int tlen; - icm42688_get_trace(tr, &tlen); - char buf[256]; - int len = snprintf(buf, sizeof(buf), - "{\"init\":1,\"imu\":%d,\"who\":%d}\n", - imu_ret, tr[0]); - CDC_Transmit((uint8_t *)buf, len); - - icm42688_data_t imu; - uint32_t send_tick = 0; - uint32_t balance_tick = 0; - uint32_t esc_tick = 0; - const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */ - - while (1) { - if (imu_ret == 0) icm42688_read(&imu); - - uint32_t now = HAL_GetTick(); - - /* Handle arm/disarm requests from USB */ - if (cdc_arm_request) { - cdc_arm_request = 0; - balance_arm(&bal); - } - if (cdc_disarm_request) { - cdc_disarm_request = 0; - balance_disarm(&bal); - } - - /* Balance PID at 1kHz */ - if (imu_ret == 0 && now - balance_tick >= 1) { - balance_tick = now; - balance_update(&bal, &imu, dt); - } - - /* Send to hoverboard ESC at 50Hz (every 20ms) - * Both wheels get same speed for balance, steer=0 for now */ - if (now - esc_tick >= 20) { - esc_tick = now; - if (bal.state == BALANCE_ARMED) { - hoverboard_send(bal.motor_cmd, 0); - } else { - hoverboard_send(0, 0); /* Always send to prevent ESC timeout */ - } - } - - /* USB telemetry at 50Hz (only when streaming enabled via S command) */ - if (cdc_streaming && now - send_tick >= 20) { - send_tick = now; - if (imu_ret == 0) { - len = snprintf(buf, sizeof(buf), - "{\"ax\":%d,\"ay\":%d,\"az\":%d,\"gx\":%d,\"gy\":%d,\"gz\":%d," - "\"p\":%d,\"m\":%d,\"s\":%d}\n", - imu.ax, imu.ay, imu.az, imu.gx, imu.gy, imu.gz, - (int)(bal.pitch_deg * 10), /* pitch x10 for precision without %f */ - (int)bal.motor_cmd, - (int)bal.state); - } else { - len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret); - } - CDC_Transmit((uint8_t *)buf, len); - } - - status_update(now, (imu_ret == 0), (bal.state == BALANCE_ARMED)); - HAL_Delay(1); - } -} diff --git a/legacy/stm32/src/mode_manager.c b/legacy/stm32/src/mode_manager.c deleted file mode 100644 index 81be333..0000000 --- a/legacy/stm32/src/mode_manager.c +++ /dev/null @@ -1,129 +0,0 @@ -#include "mode_manager.h" -#include "crsf.h" -#include "config.h" - -/* ----------------------------------------------------------------------- - * Internal helpers - * --------------------------------------------------------------------- */ - -static int16_t clamp16(int32_t v, int16_t lo, int16_t hi) { - if (v < lo) return lo; - if (v > hi) return hi; - return (int16_t)v; -} - -static float clampf(float v, float lo, float hi) { - if (v < lo) return lo; - if (v > hi) return hi; - return v; -} - -/* - * Map a CRSF raw value to [-out_max, +out_max] with a symmetric deadband - * around center (992). Within ±CRSF_DEADBAND counts of center → returns 0. - * Outside deadband the remaining range is rescaled linearly to ±out_max. - */ -static int16_t crsf_stick(uint16_t raw, int16_t out_max) { - int32_t centered = (int32_t)raw - 992; - if (centered > CRSF_DEADBAND) centered -= CRSF_DEADBAND; - else if (centered < -CRSF_DEADBAND) centered += CRSF_DEADBAND; - else return 0; - /* CRSF half-range from centre ≈ 820 counts; subtract deadband */ - const int32_t half_range = 820 - CRSF_DEADBAND; - int32_t out = centered * out_max / half_range; - return clamp16(out, -out_max, out_max); -} - -/* Blend target values for each mode (0=pure RC, 1=pure autonomous) */ -static const float k_blend_target[3] = { - [MODE_RC_MANUAL] = 0.0f, - [MODE_RC_ASSISTED] = 0.5f, - [MODE_AUTONOMOUS] = 1.0f, -}; - -/* Blend advance rate: 1/MODE_BLEND_MS per ms → full 0..1 transition in - * MODE_BLEND_MS. Adjacent mode steps (covering 0.5 of range) take 250ms. */ -#define BLEND_RATE (1.0f / (float)MODE_BLEND_MS) - -/* ----------------------------------------------------------------------- - * Public API - * --------------------------------------------------------------------- */ - -void mode_manager_init(mode_manager_t *m) { - m->target = MODE_RC_MANUAL; - m->blend = 0.0f; - m->rc_alive = false; - m->auto_steer = 0; - m->auto_speed_bias = 0; -} - -void mode_manager_update(mode_manager_t *m, uint32_t now) { - static uint32_t s_last_tick = 0; - - /* Delta-time (cap at 100ms for first call / resume after gap) */ - int32_t dt_ms = (int32_t)(now - s_last_tick); - if (dt_ms > 100) dt_ms = 100; - s_last_tick = now; - - /* Cache RC liveness — checked by main loop too, but needed by getters */ - m->rc_alive = (crsf_state.last_rx_ms != 0) && - ((now - crsf_state.last_rx_ms) < RC_TIMEOUT_MS); - - /* Determine mode target from CH6 */ - if (m->rc_alive) { - uint16_t ch6 = crsf_state.channels[CRSF_CH_MODE]; - if (ch6 <= CRSF_MODE_LOW_THRESH) - m->target = MODE_RC_MANUAL; - else if (ch6 >= CRSF_MODE_HIGH_THRESH) - m->target = MODE_AUTONOMOUS; - else - m->target = MODE_RC_ASSISTED; - } - /* If RC is not alive, keep existing target — don't flap to MANUAL just - * because the stub returns zeros; kill authority is a separate concern. */ - - /* Advance blend toward target value */ - float target_blend = k_blend_target[m->target]; - float step = BLEND_RATE * (float)dt_ms; - if (m->blend < target_blend) - m->blend = clampf(m->blend + step, 0.0f, target_blend); - else - m->blend = clampf(m->blend - step, target_blend, 1.0f); -} - -void mode_manager_set_auto_cmd(mode_manager_t *m, - int16_t steer, - int16_t speed_bias) { - m->auto_steer = clamp16(steer, -1000, 1000); - m->auto_speed_bias = clamp16(speed_bias, - -MOTOR_RC_SPEED_MAX, - MOTOR_RC_SPEED_MAX); -} - -int16_t mode_manager_get_steer(const mode_manager_t *m) { - int16_t rc_steer = 0; - if (m->rc_alive) - rc_steer = crsf_stick(crsf_state.channels[CRSF_CH_STEER], 1000); - - /* lerp: rc_steer → auto_steer over blend */ - int32_t mixed = (int32_t)rc_steer + - (int32_t)((float)(m->auto_steer - rc_steer) * m->blend); - return clamp16(mixed, -1000, 1000); -} - -int16_t mode_manager_get_speed_bias(const mode_manager_t *m) { - int16_t rc_bias = 0; - if (m->rc_alive) - rc_bias = crsf_stick(crsf_state.channels[CRSF_CH_SPEED], - MOTOR_RC_SPEED_MAX); - - int32_t mixed = (int32_t)rc_bias + - (int32_t)((float)(m->auto_speed_bias - rc_bias) * m->blend); - return clamp16(mixed, -MOTOR_RC_SPEED_MAX, MOTOR_RC_SPEED_MAX); -} - -robot_mode_t mode_manager_active(const mode_manager_t *m) { - if (m->blend < 0.25f) return MODE_RC_MANUAL; - if (m->blend > 0.75f) return MODE_AUTONOMOUS; - return MODE_RC_ASSISTED; -} diff --git a/legacy/stm32/src/motor_current.c b/legacy/stm32/src/motor_current.c deleted file mode 100644 index 828e307..0000000 --- a/legacy/stm32/src/motor_current.c +++ /dev/null @@ -1,183 +0,0 @@ -/* - * motor_current.c — ADC-based motor current monitoring and overload protection - * (Issue #584). - * - * Reads battery discharge current from battery_adc_get_current_ma() (ADC3 IN13, - * PC3), which is already DMA-sampled by battery_adc.c. Implements: - * - * 1. Soft current limiting: linear PWM reduction when current exceeds - * MOTOR_CURR_SOFT_MA (4 A, 80% of hard threshold). - * - * 2. Hard cutoff: if current stays above MOTOR_CURR_HARD_MA (5 A) for - * MOTOR_CURR_OVERLOAD_MS (2 s), output is zeroed. A fault event is - * signalled via motor_current_fault_pending() for one tick so the main - * loop can append a fault log entry. - * - * 3. Auto-recovery: after MOTOR_CURR_COOLDOWN_MS (10 s) in MC_COOLDOWN, - * state returns to MC_NORMAL and normal PWM authority is restored. - * - * 4. Telemetry: JLINK_TLM_MOTOR_CURRENT (0x86) published at - * MOTOR_CURR_TLM_HZ (5 Hz) via jlink_send_motor_current_tlm(). - */ - -#include "motor_current.h" -#include "battery_adc.h" -#include "jlink.h" -#include - -/* ---- Module state ---- */ -static MotorCurrentState s_state = MC_NORMAL; -static int32_t s_current_ma = 0; -static uint32_t s_overload_start = 0; /* ms when current first ≥ HARD_MA */ -static uint32_t s_cooldown_start = 0; /* ms when cooldown began */ -static uint8_t s_fault_count = 0; /* lifetime trip counter */ -static uint8_t s_fault_pending = 0; /* cleared after one read */ -static uint32_t s_last_tlm_ms = 0; /* rate-limit TLM TX */ - -/* Soft-limit scale factor in 0..256 fixed-point (256 = 1.0) */ -static uint16_t s_scale256 = 256u; - -/* ---- motor_current_init() ---- */ -void motor_current_init(void) -{ - s_state = MC_NORMAL; - s_current_ma = 0; - s_overload_start = 0; - s_cooldown_start = 0; - s_fault_count = 0; - s_fault_pending = 0; - s_last_tlm_ms = 0; - s_scale256 = 256u; -} - -/* ---- motor_current_tick() ---- */ -void motor_current_tick(uint32_t now_ms) -{ - /* Snapshot current from battery ADC (mA, positive = discharge) */ - s_current_ma = battery_adc_get_current_ma(); - - /* Use absolute value: protect in both forward and regen braking */ - int32_t abs_ma = s_current_ma; - if (abs_ma < 0) abs_ma = -abs_ma; - - switch (s_state) { - - case MC_NORMAL: - s_scale256 = 256u; - if (abs_ma >= (int32_t)MOTOR_CURR_SOFT_MA) { - s_state = MC_SOFT_LIMIT; - /* Track overload onset if already above hard threshold */ - s_overload_start = (abs_ma >= (int32_t)MOTOR_CURR_HARD_MA) - ? now_ms : 0u; - } - break; - - case MC_SOFT_LIMIT: - if (abs_ma < (int32_t)MOTOR_CURR_SOFT_MA) { - /* Recovered below soft threshold */ - s_state = MC_NORMAL; - s_overload_start = 0u; - s_scale256 = 256u; - } else { - /* Compute linear scale: 256 at SOFT_MA, 0 at HARD_MA */ - int32_t range = (int32_t)MOTOR_CURR_HARD_MA - - (int32_t)MOTOR_CURR_SOFT_MA; - int32_t over = abs_ma - (int32_t)MOTOR_CURR_SOFT_MA; - if (over >= range) { - s_scale256 = 0u; - } else { - /* scale256 = (range - over) * 256 / range */ - s_scale256 = (uint16_t)(((range - over) * 256u) / range); - } - - /* Track sustained hard-threshold overload */ - if (abs_ma >= (int32_t)MOTOR_CURR_HARD_MA) { - if (s_overload_start == 0u) { - s_overload_start = now_ms; - } else if ((now_ms - s_overload_start) >= MOTOR_CURR_OVERLOAD_MS) { - /* Hard cutoff — trip the fault */ - if (s_fault_count < 255u) s_fault_count++; - s_fault_pending = 1u; - s_cooldown_start = now_ms; - s_overload_start = 0u; - s_scale256 = 0u; - s_state = MC_COOLDOWN; - } - } else { - /* Current dipped back below HARD_MA — reset overload timer */ - s_overload_start = 0u; - } - } - break; - - case MC_COOLDOWN: - s_scale256 = 0u; - if ((now_ms - s_cooldown_start) >= MOTOR_CURR_COOLDOWN_MS) { - s_state = MC_NORMAL; - s_scale256 = 256u; - } - break; - } -} - -/* ---- motor_current_apply_limit() ---- */ -int16_t motor_current_apply_limit(int16_t cmd) -{ - if (s_scale256 >= 256u) return cmd; - if (s_scale256 == 0u) return 0; - return (int16_t)(((int32_t)cmd * s_scale256) / 256); -} - -/* ---- Accessors ---- */ -bool motor_current_is_faulted(void) -{ - return s_state == MC_COOLDOWN; -} - -MotorCurrentState motor_current_state(void) -{ - return s_state; -} - -int32_t motor_current_ma(void) -{ - return s_current_ma; -} - -uint8_t motor_current_fault_count(void) -{ - return s_fault_count; -} - -bool motor_current_fault_pending(void) -{ - if (!s_fault_pending) return false; - s_fault_pending = 0u; - return true; -} - -/* ---- motor_current_send_tlm() ---- */ -void motor_current_send_tlm(uint32_t now_ms) -{ - if (MOTOR_CURR_TLM_HZ == 0u) return; - - uint32_t interval_ms = 1000u / MOTOR_CURR_TLM_HZ; - if ((now_ms - s_last_tlm_ms) < interval_ms) return; - s_last_tlm_ms = now_ms; - - jlink_tlm_motor_current_t tlm; - - tlm.current_ma = s_current_ma; - - /* limit_pct: 0 = no limiting, 100 = full cutoff */ - if (s_scale256 >= 256u) { - tlm.limit_pct = 0u; - } else { - tlm.limit_pct = (uint8_t)(((256u - s_scale256) * 100u) / 256u); - } - - tlm.state = (uint8_t)s_state; - tlm.fault_count = s_fault_count; - - jlink_send_motor_current_tlm(&tlm); -} diff --git a/legacy/stm32/src/motor_driver.c b/legacy/stm32/src/motor_driver.c deleted file mode 100644 index c4a3860..0000000 --- a/legacy/stm32/src/motor_driver.c +++ /dev/null @@ -1,58 +0,0 @@ -#include "motor_driver.h" -#include "esc_backend.h" -#include "config.h" -#include - -void motor_driver_init(motor_driver_t *m) { - m->steer_actual = 0; - m->estop = false; -} - -void motor_driver_update(motor_driver_t *m, - int16_t balance_cmd, - int16_t steer_cmd, - uint32_t now) { - static uint32_t s_last_tick = 0; - - /* Delta-time for ramp (cap at 100ms to handle first call / long gaps). - * Always update tick so ramp starts fresh when estop clears. */ - int32_t dt_ms = (int32_t)(now - s_last_tick); - if (dt_ms > 100) dt_ms = 100; - s_last_tick = now; - - /* Emergency stop: send zero, hold latch */ - if (m->estop) { - esc_send(0, 0); - return; - } - - /* Ramp steer toward target at MOTOR_STEER_RAMP_RATE counts/ms */ - int32_t max_step = (int32_t)MOTOR_STEER_RAMP_RATE * dt_ms; - int32_t steer_error = (int32_t)steer_cmd - (int32_t)m->steer_actual; - if (steer_error > max_step) steer_error = max_step; - else if (steer_error < -max_step) steer_error = -max_step; - m->steer_actual = (int16_t)((int32_t)m->steer_actual + steer_error); - - /* Headroom clamp: ensure |speed| + |steer| <= MOTOR_CMD_MAX - * Reduce steer (not balance) to preserve PID authority. */ - int32_t speed = (int32_t)balance_cmd; - int32_t steer = (int32_t)m->steer_actual; - int32_t headroom = MOTOR_CMD_MAX - abs((int)speed); - if (headroom < 0) headroom = 0; - if (steer > headroom) steer = headroom; - if (steer < -headroom) steer = -headroom; - - esc_send((int16_t)speed, (int16_t)steer); -} - -void motor_driver_estop(motor_driver_t *m) { - m->estop = true; - m->steer_actual = 0; - /* Don't call hoverboard_send here — caller must drive sends via - * motor_driver_update() at the normal 50Hz ESC rate to avoid - * flooding the UART with 1kHz zero packets. */ -} - -void motor_driver_estop_clear(motor_driver_t *m) { - m->estop = false; -} diff --git a/legacy/stm32/src/mpu6000.c b/legacy/stm32/src/mpu6000.c deleted file mode 100644 index e89bf6c..0000000 --- a/legacy/stm32/src/mpu6000.c +++ /dev/null @@ -1,172 +0,0 @@ -/* - * mpu6000.c — IMU Sensor Fusion for MPU6000 - * - * Wraps the icm42688 SPI driver (which auto-detects MPU6000) and applies - * a complementary filter to produce a stable pitch estimate. - * - * Hardware: MAMBA F722S, MPU6000 on SPI1, CW270 alignment - * Config: Gyro ±2000°/s (init_mpu6000 sets FS_SEL=3) - * Accel ±16g (init_mpu6000 sets AFS_SEL=3) - */ - -#include "mpu6000.h" -#include "icm42688.h" -#include "config.h" -#include "safety.h" -#include "stm32f7xx_hal.h" -#include - -/* Scale factors matching init_mpu6000() config in icm42688.c */ -#define GYRO_SCALE (1.0f / 16.384f) /* LSB to °/s — ±2000°/s range */ -#define ACCEL_SCALE (1.0f / 2048.0f) /* LSB to g — ±16g range */ - -/* - * Complementary filter coefficient. - * 0.98 = trust gyro integration, 0.02 = accel correction for drift. - * Tune higher (0.99) for noisier environments. - */ -#define COMP_ALPHA 0.98f - -/* Filter state */ -static float s_pitch = 0.0f; -static float s_roll = 0.0f; -static float s_yaw = 0.0f; -static uint32_t s_last_tick = 0; - -/* Gyro bias offsets (raw sensor LSBs, sensor frame) */ -static float s_bias_gx = 0.0f; -static float s_bias_gy = 0.0f; -static float s_bias_gz = 0.0f; -static bool s_calibrated = false; - -/* Mount angle offsets (degrees, Issue #680) — set via mpu6000_set_mount_offset() */ -static float s_pitch_offset = 0.0f; -static float s_roll_offset = 0.0f; - -bool mpu6000_init(void) { - int ret = icm42688_init(); - if (ret == 0) { - s_pitch = 0.0f; - s_roll = 0.0f; - s_yaw = 0.0f; - s_last_tick = HAL_GetTick(); - } - return (ret == 0); -} - -void mpu6000_calibrate(void) { - /* LED1 + LED2 solid ON (active low) — visual indicator: calibrating */ - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); - - /* - * Accumulate raw gyro readings in sensor frame. - * Use int32 to avoid float accumulation error over 1000 samples. - * Max raw value ~32768, 1000 samples → max sum ~32.7M < INT32_MAX. - */ - int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0; - for (int i = 0; i < GYRO_CAL_SAMPLES; i++) { - icm42688_data_t raw = {0}; /* zero-init: guard against icm42688_read no-op on imu_type mismatch */ - icm42688_read(&raw); - sum_gx += raw.gx; - sum_gy += raw.gy; - sum_gz += raw.gz; - HAL_Delay(1); - /* Refresh IWDG every 40ms, starting immediately (i=0) — the gap between - * safety_refresh() at the top of the main loop and entry here can be - * ~10ms, so we must refresh on i=0 to avoid the 50ms IWDG window. */ - if (i % 40 == 0) safety_refresh(); - } - - s_bias_gx = (float)sum_gx / GYRO_CAL_SAMPLES; - s_bias_gy = (float)sum_gy / GYRO_CAL_SAMPLES; - s_bias_gz = (float)sum_gz / GYRO_CAL_SAMPLES; - - /* Reset filter state so angles start from zero with clean bias */ - s_pitch = 0.0f; - s_roll = 0.0f; - s_yaw = 0.0f; - s_last_tick = HAL_GetTick(); - - s_calibrated = true; -} - -bool mpu6000_is_calibrated(void) { - return s_calibrated; -} - -void mpu6000_set_mount_offset(float pitch_deg, float roll_deg) -{ - s_pitch_offset = pitch_deg; - s_roll_offset = roll_deg; -} - -bool mpu6000_has_mount_offset(void) -{ - return (s_pitch_offset != 0.0f || s_roll_offset != 0.0f); -} - -void mpu6000_read(IMUData *data) { - icm42688_data_t raw; - icm42688_read(&raw); - - /* Compute dt from wall clock — robust to loop jitter */ - uint32_t now = HAL_GetTick(); - uint32_t elapsed_ms = now - s_last_tick; - if (elapsed_ms == 0) elapsed_ms = 1; /* min 1ms to avoid divide-by-zero */ - if (elapsed_ms > 100) elapsed_ms = 100; /* clamp: don't integrate stale data */ - float dt = elapsed_ms * 0.001f; - s_last_tick = now; - - /* Convert raw to physical units */ - float ax = raw.ax * ACCEL_SCALE; /* g */ - float ay = raw.ay * ACCEL_SCALE; /* g */ - float az = raw.az * ACCEL_SCALE; /* g */ - - /* - * CW270 alignment transform (Betaflight convention, R_CW270): - * - * R = [[0, 1, 0], [-1, 0, 0], [0, 0, 1]] - * board_forward = sensor_Y → pitch accel uses ay - * board_right = -sensor_X → roll accel uses -ax - * board_gx (roll rate) = sensor_gy (board_gx = R[0,·]·omega_sensor) - * board_gy (pitch rate) = -sensor_gx (board_gy = R[1,·]·omega_sensor) - * board_gz (yaw rate) = sensor_gz (unchanged) - * - * Convention: pitch+ = nose up, roll+ = right bank, yaw+ = CW from above. - */ - /* Subtract bias (sensor-frame raw LSBs) before axis transform + scale */ - float gyro_pitch_rate = -(raw.gx - s_bias_gx) * GYRO_SCALE; /* °/s board_gy = -sensor_gx */ - float gyro_roll_rate = (raw.gy - s_bias_gy) * GYRO_SCALE; /* °/s board_gx = sensor_gy */ - float gyro_yaw_rate = (raw.gz - s_bias_gz) * GYRO_SCALE; /* °/s unchanged */ - - /* - * Accel-derived angles after CW270 transform. - * board_ax (forward) = sensor_ay → pitch = atan2(ay, az) - * board_ay (right) = -sensor_ax → roll = atan2(-ax, az) - * Valid while total accel ≈ 1g (low linear acceleration). - */ - float accel_pitch = atan2f( ay, az) * (180.0f / 3.14159265358979f); - float accel_roll = atan2f(-ax, az) * (180.0f / 3.14159265358979f); - - /* - * Complementary filter for pitch and roll: - * angle = α * (angle + ω*dt) + (1−α) * accel_angle - * Gyro integration tracks fast dynamics; accel corrects drift. - */ - s_pitch = COMP_ALPHA * (s_pitch + gyro_pitch_rate * dt) - + (1.0f - COMP_ALPHA) * accel_pitch; - s_roll = COMP_ALPHA * (s_roll + gyro_roll_rate * dt) - + (1.0f - COMP_ALPHA) * accel_roll; - - /* Yaw: pure gyro integration — no accel correction, drifts over time */ - s_yaw += gyro_yaw_rate * dt; - - data->pitch = s_pitch - s_pitch_offset; - data->pitch_rate = gyro_pitch_rate; - data->roll = s_roll - s_roll_offset; - data->yaw = s_yaw; - data->yaw_rate = gyro_yaw_rate; /* board_gz: raw bias-corrected gyro Z (Issue #616) */ - data->accel_x = ax; - data->accel_z = az; -} diff --git a/legacy/stm32/src/orin_can.c b/legacy/stm32/src/orin_can.c deleted file mode 100644 index 37d1e63..0000000 --- a/legacy/stm32/src/orin_can.c +++ /dev/null @@ -1,175 +0,0 @@ -/* orin_can.c — Orin↔FC CAN protocol driver (Issue #674) - * - * Receives high-level drive/mode/estop commands from Orin over CAN. - * Broadcasts FC status and VESC telemetry back to Orin at ORIN_TLM_HZ. - * - * Registered as the standard-ID callback with can_driver via - * can_driver_set_std_cb(orin_can_on_frame). - * - * Balance independence: if Orin link drops (orin_can_is_alive() == false), - * main loop stops injecting Orin commands and the balance PID holds - * upright in-place. No action required here — the safety is in main.c. - */ - -#include "orin_can.h" -#include "can_driver.h" -#include "stm32f7xx_hal.h" -#include - -volatile OrinCanState orin_can_state; -volatile orin_can_led_cmd_t orin_can_led_override; -volatile uint8_t orin_can_led_updated; - -static uint32_t s_tlm_tick; -static uint32_t s_imu_tick; -static uint32_t s_baro_tick; - -void orin_can_init(void) -{ - memset((void *)&orin_can_state, 0, sizeof(orin_can_state)); - memset((void *)&orin_can_led_override, 0, sizeof(orin_can_led_override)); - orin_can_led_updated = 0u; - /* Pre-wind so first broadcasts fire on the first eligible tick */ - s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_TLM_HZ)); - s_imu_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_IMU_TLM_HZ)); - s_baro_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_BARO_TLM_HZ)); - can_driver_set_std_cb(orin_can_on_frame); -} - -void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len) -{ - /* Any frame from Orin refreshes the heartbeat */ - orin_can_state.last_rx_ms = HAL_GetTick(); - - switch (std_id) { - case ORIN_CAN_ID_HEARTBEAT: - /* Heartbeat payload (sequence counter) ignored — timestamp is enough */ - break; - - case ORIN_CAN_ID_DRIVE: - /* int16 speed (BE), int16 steer (BE) */ - if (len < 4u) { break; } - orin_can_state.speed = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]); - orin_can_state.steer = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]); - orin_can_state.drive_updated = 1u; - break; - - case ORIN_CAN_ID_MODE: - /* uint8 mode */ - if (len < 1u) { break; } - orin_can_state.mode = data[0]; - orin_can_state.mode_updated = 1u; - break; - - case ORIN_CAN_ID_ESTOP: - /* uint8: 1 = assert estop, 0 = clear estop */ - if (len < 1u) { break; } - if (data[0] != 0u) { - orin_can_state.estop_req = 1u; - } else { - orin_can_state.estop_clear_req = 1u; - } - break; - - case ORIN_CAN_ID_LED_CMD: - /* pattern(u8), brightness(u8), duration_ms(u16 LE) — Issue #685 */ - if (len < 4u) { break; } - orin_can_led_override.pattern = data[0]; - orin_can_led_override.brightness = data[1]; - orin_can_led_override.duration_ms = (uint16_t)((uint16_t)data[2] | - ((uint16_t)data[3] << 8u)); - orin_can_led_updated = 1u; - break; - - case ORIN_CAN_ID_PID_SET: - /* kp_x100(u16 BE), ki_x100(u16 BE), kd_x100(u16 BE) -- Issue #693 */ - if (len < 6u) { break; } - orin_can_state.pid_kp_x100 = (uint16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]); - orin_can_state.pid_ki_x100 = (uint16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]); - orin_can_state.pid_kd_x100 = (uint16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]); - orin_can_state.pid_updated = 1u; - break; - - default: - break; - } -} - -void orin_can_send_pid_ack(float kp, float ki, float kd) -{ - orin_can_fc_pid_ack_t ack; - if (kp < 0.0f) kp = 0.0f; - if (ki < 0.0f) ki = 0.0f; - if (kd < 0.0f) kd = 0.0f; - ack.kp_x100 = (uint16_t)(kp * 100.0f + 0.5f); - ack.ki_x100 = (uint16_t)(ki * 100.0f + 0.5f); - ack.kd_x100 = (uint16_t)(kd * 100.0f + 0.5f); - uint8_t buf[6]; - memcpy(buf, &ack, sizeof(ack)); - can_driver_send_std(ORIN_CAN_ID_FC_PID_ACK, buf, (uint8_t)sizeof(ack)); -} - -bool orin_can_is_alive(uint32_t now_ms) -{ - if (orin_can_state.last_rx_ms == 0u) { - return false; - } - return (now_ms - orin_can_state.last_rx_ms) < ORIN_HB_TIMEOUT_MS; -} - -void orin_can_broadcast(uint32_t now_ms, - const orin_can_fc_status_t *status, - const orin_can_fc_vesc_t *vesc) -{ - if ((now_ms - s_tlm_tick) < (1000u / ORIN_TLM_HZ)) { - return; - } - s_tlm_tick = now_ms; - - uint8_t buf[8]; - - /* FC_STATUS (0x400): 8 bytes */ - memcpy(buf, status, sizeof(orin_can_fc_status_t)); - can_driver_send_std(ORIN_CAN_ID_FC_STATUS, buf, (uint8_t)sizeof(orin_can_fc_status_t)); - - /* FC_VESC (0x401): 8 bytes */ - memcpy(buf, vesc, sizeof(orin_can_fc_vesc_t)); - can_driver_send_std(ORIN_CAN_ID_FC_VESC, buf, (uint8_t)sizeof(orin_can_fc_vesc_t)); -} - -void orin_can_broadcast_imu(uint32_t now_ms, - const orin_can_fc_imu_t *imu_tlm) -{ - if ((now_ms - s_imu_tick) < (1000u / ORIN_IMU_TLM_HZ)) { - return; - } - s_imu_tick = now_ms; - - uint8_t buf[8]; - memcpy(buf, imu_tlm, sizeof(orin_can_fc_imu_t)); - can_driver_send_std(ORIN_CAN_ID_FC_IMU, buf, (uint8_t)sizeof(orin_can_fc_imu_t)); -} - -void orin_can_broadcast_baro(uint32_t now_ms, - const orin_can_fc_baro_t *baro_tlm) -{ - if (baro_tlm == NULL) return; - if ((now_ms - s_baro_tick) < (1000u / ORIN_BARO_TLM_HZ)) { - return; - } - s_baro_tick = now_ms; - - uint8_t buf[8]; - memcpy(buf, baro_tlm, sizeof(orin_can_fc_baro_t)); - can_driver_send_std(ORIN_CAN_ID_FC_BARO, buf, (uint8_t)sizeof(orin_can_fc_baro_t)); -} - -void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state) -{ - orin_can_fc_btn_t btn; - btn.event_id = event_id; - btn.balance_state = balance_state; - uint8_t buf[2]; - memcpy(buf, &btn, sizeof(orin_can_fc_btn_t)); - can_driver_send_std(ORIN_CAN_ID_FC_BTN, buf, (uint8_t)sizeof(orin_can_fc_btn_t)); -} diff --git a/legacy/stm32/src/ota.c b/legacy/stm32/src/ota.c deleted file mode 100644 index 28c6038..0000000 --- a/legacy/stm32/src/ota.c +++ /dev/null @@ -1,57 +0,0 @@ -#include "ota.h" -#include "stm32f7xx_hal.h" - -/* ---- ota_enter_dfu() ---- */ -bool ota_enter_dfu(bool is_armed) -{ - if (is_armed) return false; - - /* Enable backup domain access */ - __HAL_RCC_PWR_CLK_ENABLE(); - HAL_PWR_EnableBkUpAccess(); - __HAL_RCC_RTC_ENABLE(); - - /* - * Write DFU magic to BKP15R. - * checkForBootloader() runs on next boot and jumps to the ST system - * bootloader at 0x1FF00000 when it finds this magic in BKP15R. - * BKP15R avoids the BNO055 calibration range (BKP0R–BKP6R). - * - * RTC->BKP0R through BKP31R are laid out consecutively in memory, - * so (&RTC->BKP0R)[OTA_DFU_BKP_IDX] reaches BKP15R. - */ - (&RTC->BKP0R)[OTA_DFU_BKP_IDX] = OTA_DFU_MAGIC; - - __disable_irq(); - NVIC_SystemReset(); - - return true; /* never reached */ -} - -/* ---- ota_fw_crc32() ---- */ -uint32_t ota_fw_crc32(void) -{ - /* - * STM32F7 hardware CRC unit: - * Polynomial : 0x04C11DB7 (CRC-32/MPEG-2) - * Initial : 0xFFFFFFFF (CRC_INIT register default) - * Width : 32 bits - * Reflection : none - * - * The unit processes one 32-bit word at a time. Feeding the full - * 512 KB flash (128 K words) takes ~0.5 ms at 216 MHz. - */ - __HAL_RCC_CRC_CLK_ENABLE(); - - /* Reset CRC state; keep default 32-bit polynomial and no reversal */ - CRC->CR = CRC_CR_RESET; - - const uint32_t *p = (const uint32_t *)OTA_FLASH_BASE; - uint32_t words = OTA_FLASH_SIZE / 4u; - - while (words--) { - CRC->DR = *p++; - } - - return CRC->DR; -} diff --git a/legacy/stm32/src/pid_flash.c b/legacy/stm32/src/pid_flash.c deleted file mode 100644 index 6a5f412..0000000 --- a/legacy/stm32/src/pid_flash.c +++ /dev/null @@ -1,176 +0,0 @@ -#include "pid_flash.h" -#include "stm32f7xx_hal.h" -#include - -bool pid_flash_load(float *kp, float *ki, float *kd) -{ - const pid_flash_t *p = (const pid_flash_t *)PID_FLASH_STORE_ADDR; - - if (p->magic != PID_FLASH_MAGIC) return false; - - /* Basic sanity bounds — same as JLINK_CMD_PID_SET handler */ - if (p->kp < 0.0f || p->kp > 500.0f) return false; - if (p->ki < 0.0f || p->ki > 50.0f) return false; - if (p->kd < 0.0f || p->kd > 50.0f) return false; - - *kp = p->kp; - *ki = p->ki; - *kd = p->kd; - return true; -} - -bool pid_flash_save(float kp, float ki, float kd) -{ - HAL_StatusTypeDef rc; - - /* Unlock flash */ - rc = HAL_FLASH_Unlock(); - if (rc != HAL_OK) return false; - - /* Erase sector 7 */ - FLASH_EraseInitTypeDef erase = { - .TypeErase = FLASH_TYPEERASE_SECTORS, - .Sector = PID_FLASH_SECTOR, - .NbSectors = 1, - .VoltageRange = PID_FLASH_SECTOR_VOLTAGE, - }; - uint32_t sector_error = 0; - rc = HAL_FLASHEx_Erase(&erase, §or_error); - if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) { - HAL_FLASH_Lock(); - return false; - } - - /* Build record */ - pid_flash_t rec; - memset(&rec, 0xFF, sizeof(rec)); - rec.magic = PID_FLASH_MAGIC; - rec.kp = kp; - rec.ki = ki; - rec.kd = kd; - - /* Write 64 bytes as 16 × 32-bit words */ - const uint32_t *src = (const uint32_t *)&rec; - uint32_t addr = PID_FLASH_STORE_ADDR; - for (uint8_t i = 0; i < sizeof(rec) / 4u; i++) { - rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, src[i]); - if (rc != HAL_OK) { - HAL_FLASH_Lock(); - return false; - } - addr += 4u; - } - - HAL_FLASH_Lock(); - - /* Verify readback */ - const pid_flash_t *stored = (const pid_flash_t *)PID_FLASH_STORE_ADDR; - return (stored->magic == PID_FLASH_MAGIC && - stored->kp == kp && - stored->ki == ki && - stored->kd == kd); -} - -/* ---- Helper: write arbitrary bytes as 32-bit words ---- */ -/* - * Writes 'len' bytes from 'src' to flash at 'addr'. - * len must be a multiple of 4. Flash must already be unlocked. - * Returns HAL_OK on success, or first failure status. - */ -static HAL_StatusTypeDef flash_write_words(uint32_t addr, - const void *src, - uint32_t len) -{ - const uint32_t *p = (const uint32_t *)src; - HAL_StatusTypeDef rc = HAL_OK; - for (uint32_t i = 0; i < len / 4u; i++) { - rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, p[i]); - if (rc != HAL_OK) return rc; - addr += 4u; - } - return HAL_OK; -} - -/* ---- pid_flash_load_schedule() ---- */ -bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n) -{ - const pid_sched_flash_t *p = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR; - - if (p->magic != PID_SCHED_MAGIC) return false; - if (p->num_bands == 0u || p->num_bands > PID_SCHED_MAX_BANDS) return false; - - *out_n = p->num_bands; - for (uint8_t i = 0; i < p->num_bands; i++) { - out_entries[i] = p->bands[i]; - } - return true; -} - -/* ---- pid_flash_save_all() ---- */ -bool pid_flash_save_all(float kp_single, float ki_single, float kd_single, - const pid_sched_entry_t *entries, uint8_t num_bands) -{ - if (num_bands == 0u || num_bands > PID_SCHED_MAX_BANDS) return false; - - HAL_StatusTypeDef rc; - - rc = HAL_FLASH_Unlock(); - if (rc != HAL_OK) return false; - - /* Single erase of sector 7 covers both records */ - FLASH_EraseInitTypeDef erase = { - .TypeErase = FLASH_TYPEERASE_SECTORS, - .Sector = PID_FLASH_SECTOR, - .NbSectors = 1, - .VoltageRange = PID_FLASH_SECTOR_VOLTAGE, - }; - uint32_t sector_error = 0; - rc = HAL_FLASHEx_Erase(&erase, §or_error); - if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) { - HAL_FLASH_Lock(); - return false; - } - - /* Build and write schedule record at PID_SCHED_FLASH_ADDR */ - pid_sched_flash_t srec; - memset(&srec, 0xFF, sizeof(srec)); - srec.magic = PID_SCHED_MAGIC; - srec.num_bands = num_bands; - srec.flags = 0u; - for (uint8_t i = 0; i < num_bands; i++) { - srec.bands[i] = entries[i]; - } - - rc = flash_write_words(PID_SCHED_FLASH_ADDR, &srec, sizeof(srec)); - if (rc != HAL_OK) { - HAL_FLASH_Lock(); - return false; - } - - /* Build and write single-PID record at PID_FLASH_STORE_ADDR */ - pid_flash_t prec; - memset(&prec, 0xFF, sizeof(prec)); - prec.magic = PID_FLASH_MAGIC; - prec.kp = kp_single; - prec.ki = ki_single; - prec.kd = kd_single; - - rc = flash_write_words(PID_FLASH_STORE_ADDR, &prec, sizeof(prec)); - if (rc != HAL_OK) { - HAL_FLASH_Lock(); - return false; - } - - HAL_FLASH_Lock(); - - /* Verify both records */ - const pid_sched_flash_t *sv = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR; - const pid_flash_t *pv = (const pid_flash_t *)PID_FLASH_STORE_ADDR; - - return (sv->magic == PID_SCHED_MAGIC && - sv->num_bands == num_bands && - pv->magic == PID_FLASH_MAGIC && - pv->kp == kp_single && - pv->ki == ki_single && - pv->kd == kd_single); -} diff --git a/legacy/stm32/src/pid_schedule.c b/legacy/stm32/src/pid_schedule.c deleted file mode 100644 index 261a5ae..0000000 --- a/legacy/stm32/src/pid_schedule.c +++ /dev/null @@ -1,174 +0,0 @@ -#include "pid_schedule.h" -#include "pid_flash.h" -#include -#include /* fabsf */ - -/* ---- Default 3-band table ---- */ -static const pid_sched_entry_t k_default_table[3] = { - { .speed_mps = 0.00f, .kp = 40.0f, .ki = 1.5f, .kd = 1.2f }, - { .speed_mps = 0.30f, .kp = 35.0f, .ki = 1.0f, .kd = 1.0f }, - { .speed_mps = 0.80f, .kp = 28.0f, .ki = 0.5f, .kd = 0.8f }, -}; - -/* ---- Active table ---- */ -static pid_sched_entry_t s_bands[PID_SCHED_MAX_BANDS]; -static uint8_t s_num_bands = 0u; -static uint8_t s_active_band = 0u; /* lower-bracket index of last call */ -static uint8_t s_prev_band = 0xFFu; /* sentinel: forces integrator reset on first apply */ - -/* ---- sort helper (insertion sort — table is small, ≤6 entries) ---- */ -static void sort_bands(void) -{ - for (uint8_t i = 1u; i < s_num_bands; i++) { - pid_sched_entry_t key = s_bands[i]; - int8_t j = (int8_t)(i - 1u); - while (j >= 0 && s_bands[j].speed_mps > key.speed_mps) { - s_bands[j + 1] = s_bands[j]; - j--; - } - s_bands[j + 1] = key; - } -} - -/* ---- pid_schedule_init() ---- */ -void pid_schedule_init(void) -{ - pid_sched_entry_t tmp[PID_SCHED_MAX_BANDS]; - uint8_t n = 0u; - - if (pid_flash_load_schedule(tmp, &n)) { - /* Validate entries minimally */ - bool ok = true; - for (uint8_t i = 0u; i < n; i++) { - if (tmp[i].kp < 0.0f || tmp[i].kp > 500.0f || - tmp[i].ki < 0.0f || tmp[i].ki > 50.0f || - tmp[i].kd < 0.0f || tmp[i].kd > 50.0f || - tmp[i].speed_mps < 0.0f) { - ok = false; - break; - } - } - if (ok) { - memcpy(s_bands, tmp, n * sizeof(pid_sched_entry_t)); - s_num_bands = n; - sort_bands(); - s_active_band = 0u; - return; - } - } - - /* Fall back to built-in default */ - memcpy(s_bands, k_default_table, sizeof(k_default_table)); - s_num_bands = 3u; - s_active_band = 0u; - s_prev_band = 0xFFu; -} - -/* ---- pid_schedule_get_gains() ---- */ -void pid_schedule_get_gains(float speed_mps, float *kp, float *ki, float *kd) -{ - float spd = fabsf(speed_mps); - - if (s_num_bands == 0u) { - *kp = k_default_table[0].kp; - *ki = k_default_table[0].ki; - *kd = k_default_table[0].kd; - return; - } - - /* Clamp below first entry */ - if (spd <= s_bands[0].speed_mps) { - s_active_band = 0u; - *kp = s_bands[0].kp; - *ki = s_bands[0].ki; - *kd = s_bands[0].kd; - return; - } - - /* Clamp above last entry */ - uint8_t last = s_num_bands - 1u; - if (spd >= s_bands[last].speed_mps) { - s_active_band = last; - *kp = s_bands[last].kp; - *ki = s_bands[last].ki; - *kd = s_bands[last].kd; - return; - } - - /* Find bracket [i-1, i] where bands[i-1].speed <= spd < bands[i].speed */ - uint8_t i = 1u; - while (i < s_num_bands && s_bands[i].speed_mps <= spd) i++; - /* Now bands[i-1].speed_mps <= spd < bands[i].speed_mps */ - - s_active_band = (uint8_t)(i - 1u); - - float dv = s_bands[i].speed_mps - s_bands[i - 1u].speed_mps; - float t = (dv > 0.0f) ? (spd - s_bands[i - 1u].speed_mps) / dv : 0.0f; - - *kp = s_bands[i - 1u].kp + t * (s_bands[i].kp - s_bands[i - 1u].kp); - *ki = s_bands[i - 1u].ki + t * (s_bands[i].ki - s_bands[i - 1u].ki); - *kd = s_bands[i - 1u].kd + t * (s_bands[i].kd - s_bands[i - 1u].kd); -} - -/* ---- pid_schedule_apply() ---- */ -void pid_schedule_apply(balance_t *b, float speed_mps) -{ - float kp, ki, kd; - pid_schedule_get_gains(speed_mps, &kp, &ki, &kd); - - b->kp = kp; - b->ki = ki; - b->kd = kd; - - /* Reset integrator on band transition to prevent windup spike */ - if (s_active_band != s_prev_band) { - b->integral = 0.0f; - s_prev_band = s_active_band; - } -} - -/* ---- pid_schedule_set_table() ---- */ -void pid_schedule_set_table(const pid_sched_entry_t *entries, uint8_t n) -{ - if (n == 0u) n = 1u; - if (n > PID_SCHED_MAX_BANDS) n = PID_SCHED_MAX_BANDS; - - memcpy(s_bands, entries, n * sizeof(pid_sched_entry_t)); - s_num_bands = n; - s_active_band = 0u; - s_prev_band = 0xFFu; - sort_bands(); -} - -/* ---- pid_schedule_get_table() ---- */ -void pid_schedule_get_table(pid_sched_entry_t *out_entries, uint8_t *out_n) -{ - memcpy(out_entries, s_bands, s_num_bands * sizeof(pid_sched_entry_t)); - *out_n = s_num_bands; -} - -/* ---- pid_schedule_get_num_bands() ---- */ -uint8_t pid_schedule_get_num_bands(void) -{ - return s_num_bands; -} - -/* ---- pid_schedule_flash_save() ---- */ -bool pid_schedule_flash_save(float kp_single, float ki_single, float kd_single) -{ - return pid_flash_save_all(kp_single, ki_single, kd_single, - s_bands, s_num_bands); -} - -/* ---- pid_schedule_active_band_idx() ---- */ -uint8_t pid_schedule_active_band_idx(void) -{ - return s_active_band; -} - -/* ---- pid_schedule_get_default_table() ---- */ -void pid_schedule_get_default_table(pid_sched_entry_t *out_entries, uint8_t *out_n) -{ - memcpy(out_entries, k_default_table, sizeof(k_default_table)); - *out_n = 3u; -} diff --git a/legacy/stm32/src/power_mgmt.c b/legacy/stm32/src/power_mgmt.c deleted file mode 100644 index ccb76a1..0000000 --- a/legacy/stm32/src/power_mgmt.c +++ /dev/null @@ -1,265 +0,0 @@ -#include "power_mgmt.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include - -/* ---- Internal state ---- */ -static PowerState s_state = PM_ACTIVE; -static uint32_t s_last_active = 0; -static uint32_t s_fade_start = 0; -static bool s_sleep_req = false; -static bool s_peripherals_gated = false; - -/* ---- EXTI wake-source configuration ---- */ -/* - * EXTI1 → PA1 (UART4_RX / CRSF): falling edge (UART start bit) - * EXTI7 → PB7 (USART1_RX / JLink): falling edge - * EXTI4 → PC4 (MPU6000 INT): already configured by mpu6000_init(); - * we just ensure IMR bit is set. - * - * GPIO pins remain in their current AF mode; EXTI is pad-level and - * fires independently of the AF setting. - */ -static void enable_wake_exti(void) -{ - __HAL_RCC_SYSCFG_CLK_ENABLE(); - - /* EXTI1: PA1 (UART4_RX) — SYSCFG EXTICR1[7:4] = 0000 (PA) */ - SYSCFG->EXTICR[0] = (SYSCFG->EXTICR[0] & ~(0xFu << 4)) | (0x0u << 4); - EXTI->FTSR |= (1u << 1); - EXTI->RTSR &= ~(1u << 1); - EXTI->PR = (1u << 1); /* clear pending */ - EXTI->IMR |= (1u << 1); - HAL_NVIC_SetPriority(EXTI1_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(EXTI1_IRQn); - - /* EXTI7: PC7 (USART6_RX / Jetson UART) — SYSCFG EXTICR2[15:12] = 0010 (PC) - * Changed from PB7 (JLink) to PC7 (Jetson) — Jetson is primary interface. - * JLink wake is handled by Jetson timeout instead. */ - SYSCFG->EXTICR[1] = (SYSCFG->EXTICR[1] & ~(0xFu << 12)) | (0x2u << 12); - EXTI->FTSR |= (1u << 7); - EXTI->RTSR &= ~(1u << 7); - EXTI->PR = (1u << 7); - EXTI->IMR |= (1u << 7); - HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); - - /* EXTI4: PC4 (MPU6000 INT) — handler in mpu6000.c; just ensure IMR set */ - EXTI->IMR |= (1u << 4); -} - -static void disable_wake_exti(void) -{ - /* Mask UART RX wake EXTIs now that UART peripherals handle traffic */ - EXTI->IMR &= ~(1u << 1); - EXTI->IMR &= ~(1u << 7); - /* Leave EXTI4 (IMU data-ready) always unmasked */ -} - -/* ---- Peripheral clock gating ---- */ -/* - * Clock-only gate (no force-reset): peripheral register state is preserved. - * On re-enable, DMA circular transfers resume without reinitialisation. - */ -static void gate_peripherals(void) -{ - if (s_peripherals_gated) return; - __HAL_RCC_SPI3_CLK_DISABLE(); /* I2S3 / audio amplifier */ - __HAL_RCC_SPI2_CLK_DISABLE(); /* OSD MAX7456 */ - // __HAL_RCC_USART6_CLK_DISABLE(); // kept active for Jetson UART /* legacy Jetson CDC */ - /* UART5 kept active — hoverboard ESC needs continuous comms */ - /* __HAL_RCC_UART5_CLK_DISABLE(); */ - s_peripherals_gated = true; -} - -static void ungate_peripherals(void) -{ - if (!s_peripherals_gated) return; - __HAL_RCC_SPI3_CLK_ENABLE(); - __HAL_RCC_SPI2_CLK_ENABLE(); - __HAL_RCC_USART6_CLK_ENABLE(); - __HAL_RCC_UART5_CLK_ENABLE(); - s_peripherals_gated = false; -} - -/* ---- PLL clock restore after STOP mode ---- */ -/* - * After STOP wakeup SYSCLK = HSI (16 MHz). Re-lock PLL for 216 MHz. - * PLLM=8, PLLN=216, PLLP=2, PLLQ=9 — STM32F722 @ 216 MHz, HSI source. - * - * HAL_RCC_ClockConfig() calls HAL_InitTick() which resets uwTick to 0; - * save and restore it so existing timeouts remain valid across sleep. - */ -extern volatile uint32_t uwTick; - -static void restore_clocks(void) -{ - uint32_t saved_tick = uwTick; - - RCC_OscInitTypeDef osc = {0}; - osc.OscillatorType = RCC_OSCILLATORTYPE_HSI; - osc.HSIState = RCC_HSI_ON; - osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; - osc.PLL.PLLState = RCC_PLL_ON; - osc.PLL.PLLSource = RCC_PLLSOURCE_HSI; - osc.PLL.PLLM = 8; - osc.PLL.PLLN = 216; - osc.PLL.PLLP = RCC_PLLP_DIV2; - osc.PLL.PLLQ = 9; - HAL_RCC_OscConfig(&osc); - - RCC_ClkInitTypeDef clk = {0}; - clk.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | - RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; - clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - clk.AHBCLKDivider = RCC_SYSCLK_DIV1; - clk.APB1CLKDivider = RCC_HCLK_DIV4; /* 54 MHz */ - clk.APB2CLKDivider = RCC_HCLK_DIV2; /* 108 MHz */ - HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7); - - uwTick = saved_tick; /* restore — HAL_InitTick() reset it to 0 */ -} - -/* ---- EXTI IRQ handlers (wake-only: clear pending bit and return) ---- */ -/* - * These handlers fire once on wakeup. After restore_clocks() the respective - * UART peripherals resume normal DMA/IDLE-interrupt operation. - * - * NOTE: If EXTI9_5_IRQHandler is already defined elsewhere in the project, - * merge that handler with this one. - */ -void EXTI1_IRQHandler(void) -{ - if (EXTI->PR & (1u << 1)) EXTI->PR = (1u << 1); -} - -void EXTI9_5_IRQHandler(void) -{ - /* Clear any pending EXTI5-9 lines (PB7 = EXTI7 is our primary wake) */ - uint32_t pr = EXTI->PR & 0x3E0u; - if (pr) EXTI->PR = pr; -} - -/* ---- LED brightness (integer arithmetic, no float, called from main loop) ---- */ -/* - * Triangle wave: 0→255→0 over PM_LED_PERIOD_MS. - * Only active during PM_SLEEP_PENDING; returns 0 otherwise. - */ -uint8_t power_mgmt_led_brightness(void) -{ - if (s_state != PM_SLEEP_PENDING) return 0u; - - uint32_t phase = (HAL_GetTick() - s_fade_start) % PM_LED_PERIOD_MS; - uint32_t half = PM_LED_PERIOD_MS / 2u; - if (phase < half) - return (uint8_t)(phase * 255u / half); - else - return (uint8_t)((PM_LED_PERIOD_MS - phase) * 255u / half); -} - -/* ---- Current estimate ---- */ -uint16_t power_mgmt_current_ma(void) -{ - if (s_state == PM_SLEEPING) - return (uint16_t)PM_CURRENT_STOP_MA; - uint16_t ma = (uint16_t)PM_CURRENT_BASE_MA; - if (!s_peripherals_gated) { - ma += (uint16_t)(PM_CURRENT_AUDIO_MA + PM_CURRENT_OSD_MA + - PM_CURRENT_DEBUG_MA); - } - return ma; -} - -/* ---- Idle elapsed ---- */ -uint32_t power_mgmt_idle_ms(void) -{ - return HAL_GetTick() - s_last_active; -} - -/* ---- Public API ---- */ -void power_mgmt_init(void) -{ - s_state = PM_ACTIVE; - s_last_active = HAL_GetTick(); - s_fade_start = 0; - s_sleep_req = false; - s_peripherals_gated = false; - enable_wake_exti(); -} - -void power_mgmt_activity(void) -{ - s_last_active = HAL_GetTick(); - if (s_state != PM_ACTIVE) { - s_sleep_req = false; - s_state = PM_WAKING; /* resolved to PM_ACTIVE on next tick() */ - } -} - -void power_mgmt_request_sleep(void) -{ - s_sleep_req = true; -} - -PowerState power_mgmt_state(void) -{ - return s_state; -} - -PowerState power_mgmt_tick(uint32_t now_ms) -{ - switch (s_state) { - - case PM_ACTIVE: - if (s_sleep_req || (now_ms - s_last_active) >= PM_IDLE_TIMEOUT_MS) { - s_sleep_req = false; - s_fade_start = now_ms; - s_state = PM_SLEEP_PENDING; - } - break; - - case PM_SLEEP_PENDING: - if ((now_ms - s_fade_start) >= PM_FADE_MS) { - gate_peripherals(); - enable_wake_exti(); - s_state = PM_SLEEPING; - - /* Feed IWDG: wakeup <10 ms << WATCHDOG_TIMEOUT_MS (50 ms) */ - IWDG->KR = 0xAAAAu; - - /* === STOP MODE ENTRY — execution resumes here on EXTI wake === */ - HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI); - /* === WAKEUP POINT (< 10 ms latency) === */ - - restore_clocks(); - ungate_peripherals(); - disable_wake_exti(); - s_last_active = HAL_GetTick(); - s_state = PM_ACTIVE; - } - break; - - case PM_SLEEPING: - /* Unreachable: WFI is inline in PM_SLEEP_PENDING above */ - break; - - case PM_WAKING: - /* Set by power_mgmt_activity() during SLEEP_PENDING/SLEEPING */ - ungate_peripherals(); - s_state = PM_ACTIVE; - break; - } - - return s_state; -} - -/* Issue #467: battery low-voltage emergency sleep integration */ -static uint32_t s_batt_critical_mv = 0u; - -void power_mgmt_notify_battery(uint32_t vbat_mv) -{ - s_batt_critical_mv = vbat_mv; - if (s_state == PM_SLEEPING || s_state == PM_SLEEP_PENDING) return; - s_sleep_req = true; - s_fade_start = HAL_GetTick(); -} diff --git a/legacy/stm32/src/rgb_fsm.c b/legacy/stm32/src/rgb_fsm.c deleted file mode 100644 index fb3f6e4..0000000 --- a/legacy/stm32/src/rgb_fsm.c +++ /dev/null @@ -1,332 +0,0 @@ -#include "rgb_fsm.h" -#include "stm32f7xx_hal.h" -#include "config.h" -#include -#include - -/* ================================================================ - * WS2812 NeoPixel LED Strip Configuration - * ================================================================ */ - -#define NUM_LEDS 8 -#define LED_STRIP_BITS (NUM_LEDS * 24) /* 24 bits per LED (RGB) */ - -/* ================================================================ - * State Machine Internal State - * ================================================================ */ - -typedef struct { - LedState current_state; /* Current operational state */ - LedState previous_state; /* Previous state for transition detection */ - uint32_t state_start_time_ms; /* When current state started */ - uint32_t last_tick_ms; /* Last tick time */ - uint8_t animation_frame; /* Current animation frame (0-255) */ - RgbColor led_colors[NUM_LEDS]; /* Current color of each LED */ -} RgbFsm; - -static RgbFsm s_rgb = { - .current_state = LED_STATE_BOOT, - .previous_state = LED_STATE_BOOT, - .state_start_time_ms = 0, - .last_tick_ms = 0, - .animation_frame = 0 -}; - -/* ================================================================ - * Color Definitions for Each State - * ================================================================ */ - -static const RgbColor COLOR_BLUE = { 0, 0, 255 }; -static const RgbColor COLOR_GREEN = { 0, 255, 0 }; -static const RgbColor COLOR_CYAN = { 0, 255, 255 }; -static const RgbColor COLOR_RED = {255, 0, 0 }; -static const RgbColor COLOR_ORANGE = {255, 165, 0 }; -static const RgbColor COLOR_OFF = { 0, 0, 0 }; - -/* ================================================================ - * Hardware Initialization - * ================================================================ */ - -void rgb_fsm_init(void) -{ - /* Enable GPIO and timer clocks */ - __HAL_RCC_GPIOB_CLK_ENABLE(); - __HAL_RCC_TIM3_CLK_ENABLE(); - - /* Configure PB4 as TIM3_CH1 PWM output */ - GPIO_InitTypeDef gpio_init = {0}; - gpio_init.Pin = LED_STRIP_PIN; - gpio_init.Mode = GPIO_MODE_AF_PP; - gpio_init.Pull = GPIO_NOPULL; - gpio_init.Speed = GPIO_SPEED_HIGH; - gpio_init.Alternate = LED_STRIP_AF; - HAL_GPIO_Init(LED_STRIP_PORT, &gpio_init); - - /* Configure TIM3 for 800 kHz PWM - * Clock: 216MHz / PSC = output frequency - * For 800 kHz: PSC = 270, ARR = 100 - * Duty cycle = CCR / ARR - */ - TIM_HandleTypeDef htim3 = {0}; - htim3.Instance = LED_STRIP_TIM; - htim3.Init.Prescaler = 270 - 1; /* 216MHz / 270 = 800kHz clock */ - htim3.Init.CounterMode = TIM_COUNTERMODE_UP; - htim3.Init.Period = 100 - 1; /* 800kHz / 100 = 8 kHz PWM */ - htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim3.Init.RepetitionCounter = 0; - HAL_TIM_PWM_Init(&htim3); - - /* Configure PWM on CH1 for WS2812 signal */ - TIM_OC_InitTypeDef oc_init = {0}; - oc_init.OCMode = TIM_OCMODE_PWM1; - oc_init.Pulse = 0; /* Start at 0% duty (off) */ - oc_init.OCPolarity = TIM_OCPOLARITY_HIGH; - oc_init.OCFastMode = TIM_OCFAST_DISABLE; - HAL_TIM_PWM_ConfigChannel(&htim3, &oc_init, LED_STRIP_CHANNEL); - - /* Start PWM generation */ - HAL_TIM_PWM_Start(LED_STRIP_TIM, LED_STRIP_CHANNEL); - - /* Initialize state */ - s_rgb.current_state = LED_STATE_BOOT; - s_rgb.previous_state = LED_STATE_BOOT; - s_rgb.state_start_time_ms = 0; - s_rgb.animation_frame = 0; - memset(s_rgb.led_colors, 0, sizeof(s_rgb.led_colors)); -} - -/* ================================================================ - * LED Update Function - * ================================================================ */ - -static void rgb_update_led_strip(void) -{ - /* Calculate total brightness from LED colors */ - uint32_t total_brightness = 0; - for (int i = 0; i < NUM_LEDS; i++) { - total_brightness += s_rgb.led_colors[i].r; - total_brightness += s_rgb.led_colors[i].g; - total_brightness += s_rgb.led_colors[i].b; - } - /* Normalize to 0-100 (PWM duty cycle) */ - uint32_t duty = (total_brightness / (NUM_LEDS * 3)) * 100 / 255; - if (duty > 100) duty = 100; - - TIM3->CCR1 = (duty * 100) / 100; /* Set duty cycle */ -} - -/* ================================================================ - * Animation Functions - * ================================================================ */ - -static void animate_boot(uint32_t elapsed_ms) -{ - /* Blue pulse at 0.5 Hz (2 second period) */ - uint32_t period_ms = 2000; - uint8_t phase = (elapsed_ms % period_ms) * 255 / period_ms; - uint8_t brightness = (uint8_t)(128 + 127 * sin(2 * 3.14159 * phase / 256)); - - RgbColor color = COLOR_BLUE; - color.b = (color.b * brightness) / 255; - - for (int i = 0; i < NUM_LEDS; i++) { - s_rgb.led_colors[i] = color; - } - s_rgb.animation_frame = phase; -} - -static void animate_idle(uint32_t elapsed_ms) -{ - /* Green breathe at 0.5 Hz (2 second period) */ - uint32_t period_ms = 2000; - uint8_t phase = (elapsed_ms % period_ms) * 255 / period_ms; - uint8_t brightness = (uint8_t)(128 + 127 * sin(2 * 3.14159 * phase / 256)); - - RgbColor color = COLOR_GREEN; - color.g = (color.g * brightness) / 255; - - for (int i = 0; i < NUM_LEDS; i++) { - s_rgb.led_colors[i] = color; - } - s_rgb.animation_frame = phase; -} - -static void animate_armed(uint32_t elapsed_ms) -{ - /* Solid green (no animation) */ - (void)elapsed_ms; - for (int i = 0; i < NUM_LEDS; i++) { - s_rgb.led_colors[i] = COLOR_GREEN; - } - s_rgb.animation_frame = 255; -} - -static void animate_nav(uint32_t elapsed_ms) -{ - /* Cyan spin (rotating pattern at 1 Hz) */ - uint32_t period_ms = 1000; - uint8_t phase = (elapsed_ms % period_ms) * 8 / period_ms; - - for (int i = 0; i < NUM_LEDS; i++) { - if (i == phase) { - s_rgb.led_colors[i] = COLOR_CYAN; - } else if (i == (phase + 7) % 8) { - s_rgb.led_colors[i] = (RgbColor){0, 128, 128}; - } else { - s_rgb.led_colors[i] = COLOR_OFF; - } - } - s_rgb.animation_frame = (uint8_t)phase * 32; -} - -static void animate_error(uint32_t elapsed_ms) -{ - /* Red flash at 2 Hz (500ms period) */ - uint32_t period_ms = 500; - uint8_t brightness = ((elapsed_ms % period_ms) < 250) ? 255 : 0; - - RgbColor color = COLOR_RED; - color.r = (color.r * brightness) / 255; - - for (int i = 0; i < NUM_LEDS; i++) { - s_rgb.led_colors[i] = color; - } - s_rgb.animation_frame = brightness; -} - -static void animate_low_batt(uint32_t elapsed_ms) -{ - /* Orange blink at 1 Hz (1000ms period) */ - uint32_t period_ms = 1000; - uint8_t brightness = ((elapsed_ms % period_ms) < 500) ? 255 : 0; - - RgbColor color = COLOR_ORANGE; - color.r = (color.r * brightness) / 255; - color.g = (color.g * brightness) / 255; - - for (int i = 0; i < NUM_LEDS; i++) { - s_rgb.led_colors[i] = color; - } - s_rgb.animation_frame = brightness; -} - -static void animate_charging(uint32_t elapsed_ms) -{ - /* Green fill (progressive LEDs lighting up at 1 Hz) */ - uint32_t period_ms = 1000; - uint8_t phase = (elapsed_ms % period_ms) * 8 / period_ms; - - for (int i = 0; i < NUM_LEDS; i++) { - if (i < phase) { - s_rgb.led_colors[i] = COLOR_GREEN; - } else { - s_rgb.led_colors[i] = COLOR_OFF; - } - } - s_rgb.animation_frame = phase * 32; -} - -static void animate_estop(uint32_t elapsed_ms) -{ - /* Red solid (full intensity, no animation) */ - (void)elapsed_ms; - for (int i = 0; i < NUM_LEDS; i++) { - s_rgb.led_colors[i] = COLOR_RED; - } - s_rgb.animation_frame = 255; -} - -/* ================================================================ - * Public API - * ================================================================ */ - -bool rgb_fsm_set_state(LedState state) -{ - if (state >= LED_STATE_COUNT) { - return false; - } - - if (state == s_rgb.current_state) { - return false; - } - - s_rgb.previous_state = s_rgb.current_state; - s_rgb.current_state = state; - s_rgb.state_start_time_ms = 0; - s_rgb.animation_frame = 0; - - return true; -} - -LedState rgb_fsm_get_state(void) -{ - return s_rgb.current_state; -} - -void rgb_fsm_tick(uint32_t now_ms) -{ - if (s_rgb.state_start_time_ms == 0) { - s_rgb.state_start_time_ms = now_ms; - s_rgb.last_tick_ms = now_ms; - return; - } - - uint32_t elapsed = now_ms - s_rgb.state_start_time_ms; - - switch (s_rgb.current_state) { - case LED_STATE_BOOT: - animate_boot(elapsed); - break; - case LED_STATE_IDLE: - animate_idle(elapsed); - break; - case LED_STATE_ARMED: - animate_armed(elapsed); - break; - case LED_STATE_NAV: - animate_nav(elapsed); - break; - case LED_STATE_ERROR: - animate_error(elapsed); - break; - case LED_STATE_LOW_BATT: - animate_low_batt(elapsed); - break; - case LED_STATE_CHARGING: - animate_charging(elapsed); - break; - case LED_STATE_ESTOP: - animate_estop(elapsed); - break; - default: - rgb_fsm_all_off(); - break; - } - - rgb_update_led_strip(); - s_rgb.last_tick_ms = now_ms; -} - -bool rgb_fsm_set_color(uint8_t led_index, RgbColor color) -{ - if (led_index >= NUM_LEDS) { - return false; - } - - s_rgb.led_colors[led_index] = color; - rgb_update_led_strip(); - return true; -} - -void rgb_fsm_all_off(void) -{ - for (int i = 0; i < NUM_LEDS; i++) { - s_rgb.led_colors[i] = COLOR_OFF; - } - rgb_update_led_strip(); -} - -uint8_t rgb_fsm_get_animation_frame(void) -{ - return s_rgb.animation_frame; -} diff --git a/legacy/stm32/src/safety.c b/legacy/stm32/src/safety.c deleted file mode 100644 index ac4b197..0000000 --- a/legacy/stm32/src/safety.c +++ /dev/null @@ -1,72 +0,0 @@ -/* - * safety.c — SaltyLab Safety Systems - * - * IWDG: 40kHz LSI, prescaler 32 → tick = 0.8ms. - * WATCHDOG_TIMEOUT_MS from config.h (default 50ms → reload = 62). - * Formula: reload = (timeout_ms / (prescaler / 40000)) - 1 - * reload = (50 * 40000 / 32) - 1 = 62499 → clamp to 4095 (12-bit max) - * At PSC=256: tick = 6.4ms, reload = ceil(50/6.4)-1 = 7 → ~57.6ms - * Using PSC=32: tick = 0.8ms, reload = ceil(50/0.8)-1 = 62 → 50ms ✓ - */ - -#include "safety.h" -#include "config.h" -#include "crsf.h" -#include "watchdog.h" -#include "stm32f7xx_hal.h" - -/* Arm interlock */ -static uint32_t s_arm_start_ms = 0; -static bool s_arm_pending = false; - -/* Tilt fault alert state — edge-detect to fire buzzer once */ -static bool s_was_faulted = false; - -static EstopSource s_estop_source = ESTOP_CLEAR; - -void safety_init(void) { - /* Initialize IWDG via watchdog module (Issue #300) with ~2s timeout */ - watchdog_init(2000); -} - -void safety_refresh(void) { - /* Feed the watchdog timer */ - watchdog_kick(); -} - -bool safety_rc_alive(uint32_t now) { - /* If crsf_state has never received a frame, last_rx_ms == 0 */ - if (crsf_state.last_rx_ms == 0) return false; - return (now - crsf_state.last_rx_ms) < RC_TIMEOUT_MS; -} - -void safety_alert_tilt_fault(bool faulted) { - if (faulted && !s_was_faulted) { - /* Rising edge: single buzzer burst to alert rider */ - HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET); - HAL_Delay(200); - HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); - } - s_was_faulted = faulted; -} - -void safety_arm_start(uint32_t now) { - if (!s_arm_pending) { - s_arm_start_ms = now; - s_arm_pending = true; - } -} - -bool safety_arm_ready(uint32_t now) { - if (!s_arm_pending) return false; - return (now - s_arm_start_ms) >= ARMING_HOLD_MS; -} - -void safety_arm_cancel(void) { - s_arm_pending = false; -} - -void safety_remote_estop(EstopSource src) { s_estop_source = src; } -void safety_remote_estop_clear(void) { s_estop_source = ESTOP_CLEAR; } -EstopSource safety_get_estop(void) { return s_estop_source; } -bool safety_remote_estop_active(void) { return s_estop_source >= ESTOP_REMOTE; } diff --git a/legacy/stm32/src/servo.c b/legacy/stm32/src/servo.c deleted file mode 100644 index 6eca806..0000000 --- a/legacy/stm32/src/servo.c +++ /dev/null @@ -1,229 +0,0 @@ -#include "servo.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include - -/* ================================================================ - * Servo PWM Protocol - * ================================================================ - * TIM4 at 50 Hz (20 ms period) - * APB1 clock: 54 MHz - * Prescaler: 53 (54 MHz / 54 = 1 MHz) - * ARR: 19999 (1 MHz / 20000 = 50 Hz) - * CCR: 500-2500 (0.5-2.5 ms out of 20 ms) - * - * Servo pulse mapping: - * 500 µs → 0° (full left/down) - * 1500 µs → 90° (center) - * 2500 µs → 180° (full right/up) - */ - -#define SERVO_PWM_FREQ 50u /* 50 Hz */ -#define SERVO_PERIOD_MS 20u /* 20 ms = 1/50 Hz */ -#define SERVO_CLOCK_HZ 1000000u /* 1 MHz timer clock */ -#define SERVO_PRESCALER 53u /* APB1 54 MHz / 54 = 1 MHz */ -#define SERVO_ARR 19999u /* 1 MHz / 20000 = 50 Hz */ - -static ServoState s_servo = {0}; -static TIM_HandleTypeDef s_tim_handle = {0}; - -/* ================================================================ - * Helper functions - * ================================================================ - */ - -static uint16_t angle_to_pulse_us(uint16_t degrees) -{ - /* Linear interpolation: 0° → 500µs, 180° → 2500µs */ - if (degrees > 180) degrees = 180; - - uint32_t pulse = SERVO_MIN_US + (uint32_t)degrees * (SERVO_MAX_US - SERVO_MIN_US) / 180; - return (uint16_t)pulse; -} - -static uint16_t pulse_us_to_angle(uint16_t pulse_us) -{ - /* Inverse mapping: 500µs → 0°, 2500µs → 180° */ - if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US; - if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US; - - uint32_t angle = (uint32_t)(pulse_us - SERVO_MIN_US) * 180 / (SERVO_MAX_US - SERVO_MIN_US); - return (uint16_t)angle; -} - -static void update_pwm(ServoChannel channel) -{ - /* Convert pulse width (500-2500 µs) to CCR value */ - /* At 1 MHz timer clock: 1 µs = 1 count */ - uint32_t ccr_value = s_servo.pulse_us[channel]; - - if (channel == SERVO_PAN) { - __HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_PAN_CHANNEL, ccr_value); - } else { - __HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_TILT_CHANNEL, ccr_value); - } -} - -/* ================================================================ - * Public API - * ================================================================ - */ - -void servo_init(void) -{ - /* Initialize state */ - memset(&s_servo, 0, sizeof(s_servo)); - - /* Center both servos at 90° */ - for (int i = 0; i < SERVO_COUNT; i++) { - s_servo.current_angle_deg[i] = 90; - s_servo.target_angle_deg[i] = 90; - s_servo.pulse_us[i] = SERVO_CENTER_US; - } - - /* Configure GPIO PB6 (CH1) and PB7 (CH2) as TIM4 PWM */ - __HAL_RCC_GPIOB_CLK_ENABLE(); - - GPIO_InitTypeDef gpio_init = {0}; - gpio_init.Mode = GPIO_MODE_AF_PP; - gpio_init.Pull = GPIO_NOPULL; - gpio_init.Speed = GPIO_SPEED_FREQ_HIGH; - gpio_init.Alternate = SERVO_AF; - - /* Configure PB6 (pan) */ - gpio_init.Pin = SERVO_PAN_PIN; - HAL_GPIO_Init(SERVO_PAN_PORT, &gpio_init); - - /* Configure PB7 (tilt) */ - gpio_init.Pin = SERVO_TILT_PIN; - HAL_GPIO_Init(SERVO_TILT_PORT, &gpio_init); - - /* Configure TIM4: 50 Hz PWM */ - __HAL_RCC_TIM4_CLK_ENABLE(); - - s_tim_handle.Instance = SERVO_TIM; - s_tim_handle.Init.Prescaler = SERVO_PRESCALER; - s_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP; - s_tim_handle.Init.Period = SERVO_ARR; - s_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - s_tim_handle.Init.RepetitionCounter = 0; - - HAL_TIM_PWM_Init(&s_tim_handle); - - /* Configure TIM4_CH1 (pan) for PWM */ - TIM_OC_InitTypeDef oc_init = {0}; - oc_init.OCMode = TIM_OCMODE_PWM1; - oc_init.Pulse = SERVO_CENTER_US; - oc_init.OCPolarity = TIM_OCPOLARITY_HIGH; - oc_init.OCFastMode = TIM_OCFAST_DISABLE; - - HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_PAN_CHANNEL); - HAL_TIM_PWM_Start(&s_tim_handle, SERVO_PAN_CHANNEL); - - /* Configure TIM4_CH2 (tilt) for PWM */ - oc_init.Pulse = SERVO_CENTER_US; - HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_TILT_CHANNEL); - HAL_TIM_PWM_Start(&s_tim_handle, SERVO_TILT_CHANNEL); -} - -void servo_set_angle(ServoChannel channel, uint16_t degrees) -{ - if (channel >= SERVO_COUNT) return; - if (degrees > 180) degrees = 180; - - s_servo.current_angle_deg[channel] = degrees; - s_servo.target_angle_deg[channel] = degrees; - s_servo.pulse_us[channel] = angle_to_pulse_us(degrees); - - /* Stop any sweep in progress */ - s_servo.is_sweeping[channel] = false; - - /* Update PWM immediately */ - update_pwm(channel); -} - -uint16_t servo_get_angle(ServoChannel channel) -{ - if (channel >= SERVO_COUNT) return 0; - return s_servo.current_angle_deg[channel]; -} - -void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us) -{ - if (channel >= SERVO_COUNT) return; - if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US; - if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US; - - s_servo.pulse_us[channel] = pulse_us; - s_servo.current_angle_deg[channel] = pulse_us_to_angle(pulse_us); - s_servo.target_angle_deg[channel] = s_servo.current_angle_deg[channel]; - - /* Stop any sweep in progress */ - s_servo.is_sweeping[channel] = false; - - /* Update PWM immediately */ - update_pwm(channel); -} - -void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms) -{ - if (channel >= SERVO_COUNT) return; - if (duration_ms == 0) return; - if (start_deg > 180) start_deg = 180; - if (end_deg > 180) end_deg = 180; - - s_servo.sweep_start_deg[channel] = start_deg; - s_servo.sweep_end_deg[channel] = end_deg; - s_servo.sweep_duration_ms[channel] = duration_ms; - s_servo.sweep_start_ms[channel] = 0; /* Will be set on first tick */ - s_servo.is_sweeping[channel] = true; -} - -void servo_tick(uint32_t now_ms) -{ - for (int ch = 0; ch < SERVO_COUNT; ch++) { - if (!s_servo.is_sweeping[ch]) continue; - - /* Initialize start time on first call */ - if (s_servo.sweep_start_ms[ch] == 0) { - s_servo.sweep_start_ms[ch] = now_ms; - } - - uint32_t elapsed = now_ms - s_servo.sweep_start_ms[ch]; - uint32_t duration = s_servo.sweep_duration_ms[ch]; - - if (elapsed >= duration) { - /* Sweep complete */ - s_servo.is_sweeping[ch] = false; - s_servo.current_angle_deg[ch] = s_servo.sweep_end_deg[ch]; - s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.sweep_end_deg[ch]); - } else { - /* Linear interpolation */ - int16_t start = (int16_t)s_servo.sweep_start_deg[ch]; - int16_t end = (int16_t)s_servo.sweep_end_deg[ch]; - int32_t delta = end - start; - - /* angle = start + (delta * elapsed / duration) */ - int32_t angle_i32 = start + (delta * (int32_t)elapsed / (int32_t)duration); - s_servo.current_angle_deg[ch] = (uint16_t)angle_i32; - s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.current_angle_deg[ch]); - } - - /* Update PWM */ - update_pwm((ServoChannel)ch); - } -} - -bool servo_is_sweeping(void) -{ - for (int i = 0; i < SERVO_COUNT; i++) { - if (s_servo.is_sweeping[i]) return true; - } - return false; -} - -void servo_stop_sweep(ServoChannel channel) -{ - if (channel >= SERVO_COUNT) return; - s_servo.is_sweeping[channel] = false; -} diff --git a/legacy/stm32/src/servo_bus.c b/legacy/stm32/src/servo_bus.c deleted file mode 100644 index ab1f650..0000000 --- a/legacy/stm32/src/servo_bus.c +++ /dev/null @@ -1,189 +0,0 @@ -#include "servo_bus.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include - -/* - * servo_bus.c — Feetech/ST3215 serial bus servo driver (Issue #547) - * - * Half-duplex on USART3 PB10 (USART3_TX, AF7) at 1 Mbps. - * HDSEL bit (CR3.3) routes TX and RX to the TX pin. - * TX direction: TX active, RX gated. - * RX direction: TX idle, RX active — enabled after TC interrupt. - * - * All operations are blocking with short timeouts; the servo bus - * is polled at 50 Hz from the main loop (not from an ISR). - */ - -static UART_HandleTypeDef s_uart; - -/* ---- Init ---- */ - -void servo_bus_init(void) -{ - /* GPIO: PB10 open-drain AF7 (USART3_TX) with external pull-up on bus */ - __HAL_RCC_GPIOB_CLK_ENABLE(); - GPIO_InitTypeDef gpio = {0}; - gpio.Pin = SERVO_BUS_PIN; - gpio.Mode = GPIO_MODE_AF_OD; /* open-drain for half-duplex bus */ - gpio.Pull = GPIO_PULLUP; - gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - gpio.Alternate = GPIO_AF7_USART3; - HAL_GPIO_Init(SERVO_BUS_PORT, &gpio); - - /* USART3 at SERVO_BUS_BAUD (1 Mbps), 8N1 */ - __HAL_RCC_USART3_CLK_ENABLE(); - s_uart.Instance = USART3; - s_uart.Init.BaudRate = SERVO_BUS_BAUD; - s_uart.Init.WordLength = UART_WORDLENGTH_8B; - s_uart.Init.StopBits = UART_STOPBITS_1; - s_uart.Init.Parity = UART_PARITY_NONE; - s_uart.Init.Mode = UART_MODE_TX_RX; - s_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE; - s_uart.Init.OverSampling = UART_OVERSAMPLING_8; /* required for 1 Mbps @ APB1 54 MHz */ - HAL_UART_Init(&s_uart); - - /* Enable half-duplex: HDSEL bit in CR3 routes TX/RX to TX pin */ - SET_BIT(s_uart.Instance->CR3, USART_CR3_HDSEL); -} - -/* ---- Checksum ---- */ - -static uint8_t _cksum(uint8_t id, uint8_t len, uint8_t instr, - const uint8_t *params, uint8_t nparams) -{ - uint16_t sum = (uint16_t)id + len + instr; - for (uint8_t i = 0; i < nparams; i++) sum += params[i]; - return (uint8_t)(~sum & 0xFFu); -} - -/* ---- Transmit a raw packet ---- */ - -static bool _tx(uint8_t id, uint8_t instr, const uint8_t *params, uint8_t nparams) -{ - uint8_t len = nparams + 2u; /* instr + params + cksum */ - uint8_t buf[32]; - uint8_t n = 0; - - buf[n++] = 0xFFu; - buf[n++] = 0xFFu; - buf[n++] = id; - buf[n++] = len; - buf[n++] = instr; - for (uint8_t i = 0; i < nparams; i++) buf[n++] = params[i]; - buf[n++] = _cksum(id, len, instr, params, nparams); - - return HAL_UART_Transmit(&s_uart, buf, n, 10u) == HAL_OK; -} - -/* ---- Receive response packet ---- */ - -/* - * Read one byte with timeout. In half-duplex the TX line is still driving - * after the last stop bit; we must wait ~1 bit-time before the servo begins - * its response. HAL_UART_Receive handles the timing via the timeout. - */ -static bool _rx(uint8_t id, uint8_t *data, uint8_t ndata) -{ - uint8_t hdr[4]; /* 0xFF 0xFF ID LEN */ - uint8_t tail[2]; /* ERROR CKSUM (or just CKSUM if ndata==0) */ - - /* Read header */ - if (HAL_UART_Receive(&s_uart, hdr, 4u, SB_RX_TIMEOUT_MS) != HAL_OK) - return false; - - /* Validate header */ - if (hdr[0] != 0xFFu || hdr[1] != 0xFFu || hdr[2] != id) - return false; - - uint8_t pkt_len = hdr[3]; /* ERROR + DATA... + CKSUM */ - if (pkt_len < 2u || pkt_len > 20u) /* sanity */ - return false; - - /* Read payload: ERROR + DATA bytes + CKSUM */ - uint8_t payload[20]; - uint8_t payload_len = pkt_len; /* includes error byte and cksum */ - if (HAL_UART_Receive(&s_uart, payload, payload_len, SB_RX_TIMEOUT_MS) != HAL_OK) - return false; - - /* Verify checksum: ~(ID + LEN + ERROR + DATA) */ - uint16_t sum = (uint16_t)id + pkt_len; - for (uint8_t i = 0; i < (uint8_t)(pkt_len - 1u); i++) sum += payload[i]; - uint8_t expected_cksum = (uint8_t)(~sum & 0xFFu); - if (payload[pkt_len - 1u] != expected_cksum) - return false; - - /* payload[0] = ERROR; payload[1..] = data */ - if (payload[0] != 0u) return false; /* servo reported error */ - - uint8_t n_data_bytes = pkt_len - 2u; /* minus ERROR and CKSUM */ - if (n_data_bytes < ndata) return false; - - for (uint8_t i = 0; i < ndata; i++) data[i] = payload[1u + i]; - return true; -} - -/* ---- Public API ---- */ - -bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed) -{ - if (raw_pos > SB_POS_MAX) raw_pos = SB_POS_MAX; - if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX; - - /* Write 4 bytes starting at SB_REG_GOAL_POS_L: - * [addr][POS_L][POS_H][SPD_L][SPD_H] */ - uint8_t params[5] = { - SB_REG_GOAL_POS_L, - (uint8_t)(raw_pos & 0xFFu), - (uint8_t)(raw_pos >> 8), - (uint8_t)(speed & 0xFFu), - (uint8_t)(speed >> 8), - }; - return _tx(id, SB_INSTR_WRITE, params, 5u); - /* WRITE does not generate a status packet by default */ -} - -bool servo_bus_write_torque(uint8_t id, bool enable) -{ - uint8_t params[2] = { SB_REG_TORQUE_EN, enable ? 1u : 0u }; - return _tx(id, SB_INSTR_WRITE, params, 2u); -} - -bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos) -{ - /* READ instruction: [addr][count] */ - uint8_t params[2] = { SB_REG_PRES_POS_L, 2u }; - if (!_tx(id, SB_INSTR_READ, params, 2u)) return false; - - uint8_t data[2]; - if (!_rx(id, data, 2u)) return false; - - *raw_pos = (uint16_t)data[0] | ((uint16_t)data[1] << 8); - return true; -} - -bool servo_bus_read_speed(uint8_t id, uint16_t *speed) -{ - uint8_t params[2] = { SB_REG_PRES_SPD_L, 2u }; - if (!_tx(id, SB_INSTR_READ, params, 2u)) return false; - - uint8_t data[2]; - if (!_rx(id, data, 2u)) return false; - - *speed = (uint16_t)data[0] | ((uint16_t)data[1] << 8); - return true; -} - -uint16_t servo_bus_deg_to_raw(float deg) -{ - /* Center = 0 deg = 2048 raw; 1 deg = 4096/360 raw */ - int32_t raw = (int32_t)(SB_POS_CENTER + (int32_t)(deg * (4096.0f / 360.0f))); - if (raw < 0) raw = 0; - if (raw > (int32_t)SB_POS_MAX) raw = (int32_t)SB_POS_MAX; - return (uint16_t)raw; -} - -float servo_bus_raw_to_deg(uint16_t raw) -{ - return ((float)raw - (float)SB_POS_CENTER) * (360.0f / 4096.0f); -} diff --git a/legacy/stm32/src/slope_estimator.c b/legacy/stm32/src/slope_estimator.c deleted file mode 100644 index 959869f..0000000 --- a/legacy/stm32/src/slope_estimator.c +++ /dev/null @@ -1,82 +0,0 @@ -/* - * slope_estimator.c — slow-adapting terrain slope estimator (Issue #600). - * - * First-order IIR low-pass filter on fused IMU pitch with tau = SLOPE_TAU_S (5 s). - * The long time constant means fast balance corrections and transients are - * ignored; only sustained pitch bias (i.e., genuine slope) accumulates. - * - * Estimate is clamped to ±SLOPE_MAX_DEG (15°) per the safety requirement. - */ - -#include "slope_estimator.h" -#include "jlink.h" - -/* ---- slope_estimator_init() ---- */ -void slope_estimator_init(slope_estimator_t *se) -{ - se->estimate_deg = 0.0f; - se->enabled = true; - /* Initialize so first send_tlm call fires immediately */ - se->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (SLOPE_TLM_HZ > 0u ? SLOPE_TLM_HZ : 1u))); -} - -/* ---- slope_estimator_reset() ---- */ -void slope_estimator_reset(slope_estimator_t *se) -{ - se->estimate_deg = 0.0f; -} - -/* ---- slope_estimator_update() ---- */ -void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt) -{ - if (!se->enabled || dt <= 0.0f) return; - - /* - * First-order IIR: alpha = dt / (tau + dt) - * At 1 kHz (dt=0.001): alpha ≈ 0.0002 → time to ~63% ≈ 5 s - * At 100 Hz (dt=0.01): alpha ≈ 0.002 → time to ~63% ≈ 5 s (same tau) - */ - float alpha = dt / (SLOPE_TAU_S + dt); - float raw = se->estimate_deg * (1.0f - alpha) + pitch_deg * alpha; - - /* Clamp to ±SLOPE_MAX_DEG */ - if (raw > SLOPE_MAX_DEG) raw = SLOPE_MAX_DEG; - if (raw < -SLOPE_MAX_DEG) raw = -SLOPE_MAX_DEG; - - se->estimate_deg = raw; -} - -/* ---- slope_estimator_get_deg() ---- */ -float slope_estimator_get_deg(const slope_estimator_t *se) -{ - if (!se->enabled) return 0.0f; - return se->estimate_deg; -} - -/* ---- slope_estimator_set_enabled() ---- */ -void slope_estimator_set_enabled(slope_estimator_t *se, bool en) -{ - se->enabled = en; -} - -/* ---- slope_estimator_send_tlm() ---- */ -void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms) -{ - if (SLOPE_TLM_HZ == 0u) return; - - uint32_t interval_ms = 1000u / SLOPE_TLM_HZ; - if ((now_ms - se->last_tlm_ms) < interval_ms) return; - /* Cast away const for timestamp update -- only mutable field */ - ((slope_estimator_t *)se)->last_tlm_ms = now_ms; - - jlink_tlm_slope_t tlm; - /* Scale to ×100 for 0.01° resolution, clamped to int16 range */ - float scaled = se->estimate_deg * 100.0f; - if (scaled > 32767.0f) scaled = 32767.0f; - if (scaled < -32768.0f) scaled = -32768.0f; - tlm.slope_x100 = (int16_t)scaled; - tlm.active = se->enabled ? 1u : 0u; - tlm._pad = 0u; - - jlink_send_slope_tlm(&tlm); -} diff --git a/legacy/stm32/src/status.c b/legacy/stm32/src/status.c deleted file mode 100644 index 33c9960..0000000 --- a/legacy/stm32/src/status.c +++ /dev/null @@ -1,69 +0,0 @@ -#include "stm32f7xx_hal.h" -#include "config.h" -#include "status.h" - -void status_init(void) { - __HAL_RCC_GPIOB_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); - - GPIO_InitTypeDef gpio = {0}; - gpio.Mode = GPIO_MODE_OUTPUT_PP; - gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_LOW; - - /* LED1 - PC15 */ - gpio.Pin = LED1_PIN; - HAL_GPIO_Init(LED1_PORT, &gpio); - - /* LED2 - PC14 */ - gpio.Pin = LED2_PIN; - HAL_GPIO_Init(LED2_PORT, &gpio); - - /* Buzzer - PB2 (inverted: HIGH = off) */ - gpio.Pin = BEEPER_PIN; - HAL_GPIO_Init(BEEPER_PORT, &gpio); - - /* Start with both LEDs ON (active low) */ - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); - - /* Buzzer off */ - HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); -} - -void status_boot_beep(void) { - /* Two quick beeps */ - HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET); - HAL_Delay(80); - HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); - HAL_Delay(80); - HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET); - HAL_Delay(80); - HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET); -} - -void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault, int remote_estop) { - GPIO_PinState fast_blink = ((tick / 200) % 2) ? GPIO_PIN_RESET : GPIO_PIN_SET; - GPIO_PinState blink = ((tick / 500) % 2) ? GPIO_PIN_RESET /* ON half */ - : GPIO_PIN_SET; /* OFF half */ - if (remote_estop) { - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, fast_blink); - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, fast_blink); - } else if (!imu_ok) { - /* IMU error: LED1 blinking (attention), LED2 solid ON */ - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink); - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); - } else if (tilt_fault) { - /* Tilt fault: both LEDs slow blink */ - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink); - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, blink); - } else if (armed) { - /* Armed: both LEDs solid ON */ - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET); - } else { - /* Normal disarmed: LED1 solid ON, LED2 off */ - HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); - HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET); - } -} diff --git a/legacy/stm32/src/steering_pid.c b/legacy/stm32/src/steering_pid.c deleted file mode 100644 index 825bfbd..0000000 --- a/legacy/stm32/src/steering_pid.c +++ /dev/null @@ -1,140 +0,0 @@ -/* - * steering_pid.c — closed-loop yaw-rate PID for differential drive (Issue #616). - * - * Converts Jetson Twist.angular.z (encoded as steer * STEER_OMEGA_SCALE deg/s) - * into a differential wheel speed offset using IMU gyro Z as feedback. - * - * Anti-windup: integral clamped to ±STEER_INTEGRAL_MAX before Ki multiply. - * Rate limiter: output changes at most STEER_RAMP_RATE_PER_MS per ms so that - * a step in steering demand cannot disturb the balance PID. - */ - -#include "steering_pid.h" -#include "jlink.h" - -/* ---- steering_pid_init() ---- */ -void steering_pid_init(steering_pid_t *s) -{ - s->target_omega_dps = 0.0f; - s->actual_omega_dps = 0.0f; - s->integral = 0.0f; - s->prev_error = 0.0f; - s->output = 0; - s->enabled = true; - /* Initialize so first send_tlm call fires immediately */ - s->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (STEER_TLM_HZ > 0u ? STEER_TLM_HZ : 1u))); -} - -/* ---- steering_pid_reset() ---- */ -void steering_pid_reset(steering_pid_t *s) -{ - s->target_omega_dps = 0.0f; - s->integral = 0.0f; - s->prev_error = 0.0f; - s->output = 0; -} - -/* ---- steering_pid_set_target() ---- */ -void steering_pid_set_target(steering_pid_t *s, float omega_dps) -{ - if (!s->enabled) return; - s->target_omega_dps = omega_dps; -} - -/* ---- steering_pid_update() ---- */ -int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt) -{ - if (!s->enabled || dt <= 0.0f) { - s->output = 0; - return 0; - } - - s->actual_omega_dps = actual_omega_dps; - - /* PID error */ - float error = s->target_omega_dps - actual_omega_dps; - - /* Proportional */ - float p_term = STEER_KP * error; - - /* Integral with anti-windup clamp */ - s->integral += error * dt; - if (s->integral > STEER_INTEGRAL_MAX) s->integral = STEER_INTEGRAL_MAX; - if (s->integral < -STEER_INTEGRAL_MAX) s->integral = -STEER_INTEGRAL_MAX; - float i_term = STEER_KI * s->integral; - - /* Derivative on error (avoids setpoint kick for smooth yaw changes) */ - float d_term = 0.0f; - if (dt > 0.0f) { - d_term = STEER_KD * (error - s->prev_error) / dt; - } - s->prev_error = error; - - /* Sum and clamp raw output */ - float raw = p_term + i_term + d_term; - if (raw > (float)STEER_OUTPUT_MAX) raw = (float)STEER_OUTPUT_MAX; - if (raw < -(float)STEER_OUTPUT_MAX) raw = -(float)STEER_OUTPUT_MAX; - - /* Rate limiter: bound change per step by STEER_RAMP_RATE_PER_MS * dt */ - float max_step = (float)STEER_RAMP_RATE_PER_MS * (dt * 1000.0f); - float delta = raw - (float)s->output; - if (delta > max_step) delta = max_step; - if (delta < -max_step) delta = -max_step; - - float limited = (float)s->output + delta; - /* Final clamp after rate limit */ - if (limited > (float)STEER_OUTPUT_MAX) limited = (float)STEER_OUTPUT_MAX; - if (limited < -(float)STEER_OUTPUT_MAX) limited = -(float)STEER_OUTPUT_MAX; - - s->output = (int16_t)limited; - return s->output; -} - -/* ---- steering_pid_get_output() ---- */ -int16_t steering_pid_get_output(const steering_pid_t *s) -{ - return s->output; -} - -/* ---- steering_pid_set_enabled() ---- */ -void steering_pid_set_enabled(steering_pid_t *s, bool en) -{ - if (!en && s->enabled) { - /* Disabling: zero out state */ - s->target_omega_dps = 0.0f; - s->integral = 0.0f; - s->prev_error = 0.0f; - s->output = 0; - } - s->enabled = en; -} - -/* ---- steering_pid_send_tlm() ---- */ -void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms) -{ - if (STEER_TLM_HZ == 0u) return; - - uint32_t interval_ms = 1000u / STEER_TLM_HZ; - if ((now_ms - s->last_tlm_ms) < interval_ms) return; - /* Cast away const for timestamp update — only mutable field */ - ((steering_pid_t *)s)->last_tlm_ms = now_ms; - - jlink_tlm_steering_t tlm; - - /* Scale to ×10 for 0.1 deg/s resolution, clamped to int16 range */ - float t = s->target_omega_dps * 10.0f; - if (t > 32767.0f) t = 32767.0f; - if (t < -32768.0f) t = -32768.0f; - tlm.target_x10 = (int16_t)t; - - float a = s->actual_omega_dps * 10.0f; - if (a > 32767.0f) a = 32767.0f; - if (a < -32768.0f) a = -32768.0f; - tlm.actual_x10 = (int16_t)a; - - tlm.output = s->output; - tlm.enabled = s->enabled ? 1u : 0u; - tlm._pad = 0u; - - jlink_send_steering_tlm(&tlm); -} diff --git a/legacy/stm32/src/uart_protocol.c b/legacy/stm32/src/uart_protocol.c deleted file mode 100644 index 04ea675..0000000 --- a/legacy/stm32/src/uart_protocol.c +++ /dev/null @@ -1,270 +0,0 @@ -/* - * uart_protocol.c — UART command protocol for Jetson-STM32 communication (Issue #629) - * - * Physical: UART5, PC12 (TX, AF8) / PD2 (RX, AF8), 115200 baud, 8N1, no flow control. - * NOTE: Spec requested USART1 @ 115200, but USART1 is already used by JLink @ 921600. - * Implemented on UART5 (PC12/PD2) instead. - * - * RX: DMA1_Stream0 (Channel 4), 256-byte circular buffer, no interrupt needed. - * TX: Polled (HAL_UART_Transmit), frames are short (<20 B) so blocking is acceptable. - * - * CRC: CRC8-SMBUS — poly 0x07, init 0x00, computed over CMD+PAYLOAD bytes only. - */ - -#include "uart_protocol.h" -#include "config.h" -#include "stm32f7xx_hal.h" -#include - -/* ── Configuration ─────────────────────────────────────────────────────────── */ -#define RX_BUF_SIZE 256u /* must be power-of-two for wrap math */ -#define TX_TIMEOUT 5u /* HAL_UART_Transmit timeout ms */ - -/* ── Peripheral handles ───────────────────────────────────────────────────── */ -static UART_HandleTypeDef huart5; -static DMA_HandleTypeDef hdma_rx; - -/* ── DMA ring buffer ──────────────────────────────────────────────────────── */ -static uint8_t rx_buf[RX_BUF_SIZE]; -static uint32_t rx_head = 0u; /* next byte to consume */ - -/* ── Shared state (read by main.c) ───────────────────────────────────────── */ -UartProtState uart_prot_state; - -/* ── Parser state machine ─────────────────────────────────────────────────── */ -typedef enum { - PS_IDLE, - PS_LEN, - PS_CMD, - PS_PAYLOAD, - PS_CRC, - PS_ETX -} ParseState; - -static ParseState ps = PS_IDLE; -static uint8_t ps_len = 0u; /* expected payload bytes */ -static uint8_t ps_cmd = 0u; /* command byte */ -static uint8_t ps_payload[12]; /* max payload = SET_PID = 12 B */ -static uint8_t ps_pi = 0u; /* payload index */ -static uint8_t ps_crc = 0u; /* received CRC byte */ - -/* ── CRC8-SMBUS ───────────────────────────────────────────────────────────── */ -static uint8_t crc8(const uint8_t *data, uint8_t len) -{ - uint8_t crc = 0x00u; - while (len--) { - crc ^= *data++; - for (uint8_t i = 0u; i < 8u; i++) { - if (crc & 0x80u) - crc = (uint8_t)((crc << 1) ^ 0x07u); - else - crc <<= 1; - } - } - return crc; -} - -/* ── TX helper ────────────────────────────────────────────────────────────── */ -static void tx_frame(uint8_t cmd, const uint8_t *payload, uint8_t plen) -{ - uint8_t frame[20]; - uint8_t fi = 0u; - frame[fi++] = UPROT_STX; - frame[fi++] = plen; - frame[fi++] = cmd; - for (uint8_t i = 0u; i < plen; i++) - frame[fi++] = payload[i]; - /* CRC over CMD + PAYLOAD */ - frame[fi++] = crc8(&frame[2], (uint8_t)(1u + plen)); - frame[fi++] = UPROT_ETX; - HAL_UART_Transmit(&huart5, frame, fi, TX_TIMEOUT); -} - -static void send_ack(uint8_t cmd) -{ - tx_frame(URESP_ACK, &cmd, 1u); -} - -static void send_nack(uint8_t cmd, uint8_t err) -{ - uint8_t p[2] = { cmd, err }; - tx_frame(URESP_NACK, p, 2u); -} - -/* ── Command dispatcher ───────────────────────────────────────────────────── */ -static void dispatch(uint8_t cmd, const uint8_t *payload, uint8_t plen) -{ - /* Validate CRC (computed over cmd + payload) */ - uint8_t buf[13]; - buf[0] = cmd; - memcpy(&buf[1], payload, plen); - if (crc8(buf, (uint8_t)(1u + plen)) != ps_crc) { - send_nack(cmd, UERR_BAD_CRC); - return; - } - - uart_prot_state.last_rx_ms = HAL_GetTick(); - - switch (cmd) { - case UCMD_SET_VELOCITY: - if (plen != 4u) { send_nack(cmd, UERR_BAD_LEN); break; } - { - int16_t lrpm, rrpm; - memcpy(&lrpm, &payload[0], 2u); - memcpy(&rrpm, &payload[2], 2u); - uart_prot_state.left_rpm = lrpm; - uart_prot_state.right_rpm = rrpm; - uart_prot_state.vel_updated = 1u; - send_ack(cmd); - } - break; - - case UCMD_GET_STATUS: - if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; } - /* ACK immediately; main.c sends URESP_STATUS at next 10 Hz tick */ - send_ack(cmd); - break; - - case UCMD_SET_PID: - if (plen != 12u) { send_nack(cmd, UERR_BAD_LEN); break; } - { - float kp, ki, kd; - memcpy(&kp, &payload[0], 4u); - memcpy(&ki, &payload[4], 4u); - memcpy(&kd, &payload[8], 4u); - uart_prot_state.pid_kp = kp; - uart_prot_state.pid_ki = ki; - uart_prot_state.pid_kd = kd; - uart_prot_state.pid_updated = 1u; - send_ack(cmd); - } - break; - - case UCMD_ESTOP: - if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; } - uart_prot_state.estop_req = 1u; - send_ack(cmd); - break; - - case UCMD_CLEAR_ESTOP: - if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; } - uart_prot_state.estop_clear_req = 1u; - send_ack(cmd); - break; - - default: - send_nack(cmd, UERR_BAD_LEN); - break; - } -} - -/* ── Parser byte handler ──────────────────────────────────────────────────── */ -static void parse_byte(uint8_t b) -{ - switch (ps) { - case PS_IDLE: - if (b == UPROT_STX) ps = PS_LEN; - break; - - case PS_LEN: - if (b > 12u) { ps = PS_IDLE; break; } /* sanity: max payload 12 B */ - ps_len = b; - ps = PS_CMD; - break; - - case PS_CMD: - ps_cmd = b; - ps_pi = 0u; - ps = (ps_len == 0u) ? PS_CRC : PS_PAYLOAD; - break; - - case PS_PAYLOAD: - ps_payload[ps_pi++] = b; - if (ps_pi >= ps_len) ps = PS_CRC; - break; - - case PS_CRC: - ps_crc = b; - ps = PS_ETX; - break; - - case PS_ETX: - if (b == UPROT_ETX) - dispatch(ps_cmd, ps_payload, ps_len); - else - send_nack(ps_cmd, UERR_BAD_ETX); - ps = PS_IDLE; - break; - - default: - ps = PS_IDLE; - break; - } -} - -/* ── Public API ───────────────────────────────────────────────────────────── */ - -void uart_protocol_init(void) -{ - memset(&uart_prot_state, 0, sizeof(uart_prot_state)); - ps = PS_IDLE; - - /* GPIO: PC12 (TX, AF8) and PD2 (RX, AF8) */ - __HAL_RCC_GPIOC_CLK_ENABLE(); - __HAL_RCC_GPIOD_CLK_ENABLE(); - GPIO_InitTypeDef gpio = {0}; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_HIGH; - gpio.Alternate = GPIO_AF8_UART5; - gpio.Pin = GPIO_PIN_12; - HAL_GPIO_Init(GPIOC, &gpio); - gpio.Pin = GPIO_PIN_2; - HAL_GPIO_Init(GPIOD, &gpio); - - /* UART5 */ - __HAL_RCC_UART5_CLK_ENABLE(); - huart5.Instance = UART5; - huart5.Init.BaudRate = UART_PROT_BAUD; - huart5.Init.WordLength = UART_WORDLENGTH_8B; - huart5.Init.StopBits = UART_STOPBITS_1; - huart5.Init.Parity = UART_PARITY_NONE; - huart5.Init.Mode = UART_MODE_TX_RX; - huart5.Init.HwFlowCtl = UART_HWCONTROL_NONE; - huart5.Init.OverSampling = UART_OVERSAMPLING_16; - if (HAL_UART_Init(&huart5) != HAL_OK) return; - - /* DMA1_Stream0, Channel 4 — UART5_RX */ - __HAL_RCC_DMA1_CLK_ENABLE(); - hdma_rx.Instance = DMA1_Stream0; - hdma_rx.Init.Channel = DMA_CHANNEL_4; - hdma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; - hdma_rx.Init.PeriphInc = DMA_PINC_DISABLE; - hdma_rx.Init.MemInc = DMA_MINC_ENABLE; - hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - hdma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - hdma_rx.Init.Mode = DMA_CIRCULAR; - hdma_rx.Init.Priority = DMA_PRIORITY_LOW; - hdma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; - HAL_DMA_Init(&hdma_rx); - __HAL_LINKDMA(&huart5, hdmarx, hdma_rx); - - /* Start circular DMA receive */ - HAL_UART_Receive_DMA(&huart5, rx_buf, RX_BUF_SIZE); -} - -void uart_protocol_process(void) -{ - /* DMA writes forward; NDTR counts down from RX_BUF_SIZE */ - uint32_t ndtr = __HAL_DMA_GET_COUNTER(&hdma_rx); - uint32_t tail = (RX_BUF_SIZE - ndtr) & (RX_BUF_SIZE - 1u); - while (rx_head != tail) { - parse_byte(rx_buf[rx_head]); - rx_head = (rx_head + 1u) & (RX_BUF_SIZE - 1u); - } -} - -void uart_protocol_send_status(const uart_prot_status_t *s) -{ - tx_frame(URESP_STATUS, (const uint8_t *)s, (uint8_t)sizeof(*s)); -} diff --git a/legacy/stm32/src/ultrasonic.c b/legacy/stm32/src/ultrasonic.c deleted file mode 100644 index e9c4d3f..0000000 --- a/legacy/stm32/src/ultrasonic.c +++ /dev/null @@ -1,247 +0,0 @@ -#include "ultrasonic.h" -#include "stm32f7xx_hal.h" -#include "config.h" - -/* ================================================================ - * HC-SR04 Ultrasonic Sensor Parameters - * ================================================================ */ - -#define TRIGGER_PIN GPIO_PIN_0 -#define TRIGGER_PORT GPIOA -#define ECHO_PIN GPIO_PIN_1 -#define ECHO_PORT GPIOA -#define ECHO_TIM TIM1 -#define ECHO_TIM_CHANNEL TIM_CHANNEL_2 - -/* Trigger pulse duration (10µs recommended) */ -#define TRIGGER_PULSE_US 10 - -/* Echo timeout: max 30ms (corresponds to ~5m distance) */ -#define ECHO_TIMEOUT_MS 30 - -/* Speed of sound: ~343 m/s @ 20°C - * Distance = (pulse_time_us / 2) / (1000000 / 343000) mm - * Distance = (pulse_time_us / 2) / 2.914 mm ≈ pulse_time_us / 5.828 mm - * Or: distance_mm = (pulse_us * 343) / 2000000 = pulse_us / 5.828 - * Using approximation: distance_mm ≈ (pulse_us * 1000) / 5830 - */ -#define US_TO_MM_NUMERATOR 1000 -#define US_TO_MM_DENOMINATOR 5830 - -/* ================================================================ - * Internal State Machine - * ================================================================ */ - -typedef struct { - UltrasonicState state; - uint32_t trigger_time_ms; /* When trigger pulse was sent */ - uint32_t echo_start_ticks; /* TIM1 counter at rising edge */ - uint32_t echo_end_ticks; /* TIM1 counter at falling edge */ - uint32_t echo_width_us; /* Calculated pulse width in µs */ - uint16_t distance_mm; /* Last measured distance */ - bool last_valid; /* Was last measurement valid */ - ultrasonic_callback_t callback; /* Result callback (optional) */ -} UltrasonicState_t; - -static UltrasonicState_t s_ultrasonic = { - .state = ULTRASONIC_IDLE, - .callback = NULL -}; - -static TIM_HandleTypeDef s_htim1 = {0}; /* Timer handle for IRQ handler */ - -/* ================================================================ - * Hardware Initialization - * ================================================================ */ - -void ultrasonic_init(void) -{ - /* Enable GPIO and timer clocks */ - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_TIM1_CLK_ENABLE(); - - /* Configure PA0 as trigger output (push-pull, fast slew) */ - GPIO_InitTypeDef gpio_init = {0}; - gpio_init.Pin = TRIGGER_PIN; - gpio_init.Mode = GPIO_MODE_OUTPUT_PP; - gpio_init.Pull = GPIO_NOPULL; - gpio_init.Speed = GPIO_SPEED_HIGH; - HAL_GPIO_Init(TRIGGER_PORT, &gpio_init); - HAL_GPIO_WritePin(TRIGGER_PORT, TRIGGER_PIN, GPIO_PIN_RESET); - - /* Configure PA1 as alternate function (TIM1_CH2) */ - gpio_init.Pin = ECHO_PIN; - gpio_init.Mode = GPIO_MODE_AF_PP; - gpio_init.Pull = GPIO_PULLDOWN; - gpio_init.Speed = GPIO_SPEED_HIGH; - gpio_init.Alternate = GPIO_AF1_TIM1; - HAL_GPIO_Init(ECHO_PORT, &gpio_init); - - /* Configure TIM1 for input capture on PA1 (TIM1_CH2) - * Clock: 216MHz / PSC = 216 counts/µs (PSC=1 gives 1 count per ~4.6ns) - * Use PSC=216 to get 1MHz clock → 1 count = 1µs - * ARR=0xFFFF for 16-bit capture (max 65535µs ≈ 9.6m) - */ - s_htim1.Instance = ECHO_TIM; - s_htim1.Init.Prescaler = 216 - 1; /* 216MHz / 216 = 1MHz (1µs per count) */ - s_htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - s_htim1.Init.Period = 0xFFFF; /* 16-bit counter */ - s_htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - s_htim1.Init.RepetitionCounter = 0; - HAL_TIM_IC_Init(&s_htim1); - - /* Configure input capture: CH2 on PA1, both rising and falling edges - * TIM1_CH2 captures on both edges to measure echo pulse width - */ - TIM_IC_InitTypeDef ic_init = {0}; - ic_init.ICPolarity = TIM_ICPOLARITY_RISING; /* Start with rising edge */ - ic_init.ICSelection = TIM_ICSELECTION_DIRECTTI; - ic_init.ICPrescaler = TIM_ICPSC_DIV1; /* No prescaler */ - ic_init.ICFilter = 0; /* No filter */ - HAL_TIM_IC_ConfigChannel(&s_htim1, &ic_init, ECHO_TIM_CHANNEL); - HAL_TIM_IC_Start_IT(&s_htim1, ECHO_TIM_CHANNEL); - - /* Enable input capture interrupt */ - HAL_NVIC_SetPriority(TIM1_CC_IRQn, 6, 0); - HAL_NVIC_EnableIRQ(TIM1_CC_IRQn); - - /* Start the timer */ - HAL_TIM_Base_Start(&s_htim1); - - s_ultrasonic.state = ULTRASONIC_IDLE; -} - -/* ================================================================ - * Public API - * ================================================================ */ - -bool ultrasonic_trigger(void) -{ - /* Only trigger if not currently measuring */ - if (s_ultrasonic.state != ULTRASONIC_IDLE && s_ultrasonic.state != ULTRASONIC_COMPLETE) { - return false; - } - - /* Send 10µs trigger pulse on PA0 */ - HAL_GPIO_WritePin(TRIGGER_PORT, TRIGGER_PIN, GPIO_PIN_SET); - - /* Busy-wait for 10µs (non-ideal but simple and precise) - * At 216MHz: 1 clock = ~4.6ns, so ~2160 clocks for 10µs - */ - uint32_t start = HAL_GetTick(); - while ((HAL_GetTick() - start) < 1) { - /* Wait at least 10µs — use sysclock counter for precision */ - for (volatile int i = 0; i < 2200; i++) __NOP(); - } - HAL_GPIO_WritePin(TRIGGER_PORT, TRIGGER_PIN, GPIO_PIN_RESET); - - s_ultrasonic.state = ULTRASONIC_TRIGGERED; - s_ultrasonic.trigger_time_ms = HAL_GetTick(); - s_ultrasonic.echo_start_ticks = 0; - s_ultrasonic.echo_end_ticks = 0; - - return true; -} - -void ultrasonic_set_callback(ultrasonic_callback_t callback) -{ - s_ultrasonic.callback = callback; -} - -UltrasonicState ultrasonic_get_state(void) -{ - return s_ultrasonic.state; -} - -bool ultrasonic_get_result(uint16_t *distance_mm, bool *is_valid) -{ - if (s_ultrasonic.state != ULTRASONIC_COMPLETE) { - return false; - } - - if (distance_mm) *distance_mm = s_ultrasonic.distance_mm; - if (is_valid) *is_valid = s_ultrasonic.last_valid; - - s_ultrasonic.state = ULTRASONIC_IDLE; - return true; -} - -void ultrasonic_tick(uint32_t now_ms) -{ - /* Timeout detection: if measurement takes too long, mark as error */ - if (s_ultrasonic.state == ULTRASONIC_MEASURING) { - if ((now_ms - s_ultrasonic.trigger_time_ms) > ECHO_TIMEOUT_MS) { - s_ultrasonic.state = ULTRASONIC_COMPLETE; - s_ultrasonic.distance_mm = 0; - s_ultrasonic.last_valid = false; - - if (s_ultrasonic.callback) { - s_ultrasonic.callback(0, false); - } - } - } -} - -/* ================================================================ - * TIM1 Input Capture Interrupt Handler - * ================================================================ */ - -void TIM1_CC_IRQHandler(void) -{ - /* Check if capture interrupt on CH2 */ - if (__HAL_TIM_GET_FLAG(&s_htim1, TIM_FLAG_CC2) != RESET) { - __HAL_TIM_CLEAR_FLAG(&s_htim1, TIM_FLAG_CC2); - - uint32_t capture_value = HAL_TIM_ReadCapturedValue(&s_htim1, ECHO_TIM_CHANNEL); - - if (s_ultrasonic.state == ULTRASONIC_TRIGGERED || s_ultrasonic.state == ULTRASONIC_MEASURING) { - if (s_ultrasonic.echo_start_ticks == 0) { - /* Rising edge: mark start of echo pulse */ - s_ultrasonic.echo_start_ticks = capture_value; - s_ultrasonic.state = ULTRASONIC_MEASURING; - - /* Switch to falling edge detection for end of echo */ - TIM_IC_InitTypeDef ic_init = {0}; - ic_init.ICPolarity = TIM_ICPOLARITY_FALLING; - ic_init.ICSelection = TIM_ICSELECTION_DIRECTTI; - ic_init.ICPrescaler = TIM_ICPSC_DIV1; - ic_init.ICFilter = 0; - HAL_TIM_IC_ConfigChannel(&s_htim1, &ic_init, ECHO_TIM_CHANNEL); - } else { - /* Falling edge: mark end of echo pulse and calculate distance */ - s_ultrasonic.echo_end_ticks = capture_value; - - /* Calculate pulse width (accounting for timer overflow) */ - uint32_t width = (s_ultrasonic.echo_end_ticks >= s_ultrasonic.echo_start_ticks) - ? (s_ultrasonic.echo_end_ticks - s_ultrasonic.echo_start_ticks) - : (0xFFFF - s_ultrasonic.echo_start_ticks + s_ultrasonic.echo_end_ticks + 1); - - s_ultrasonic.echo_width_us = width; - - /* Convert pulse width to distance: distance_mm = pulse_us / 5.828 - * Using integer arithmetic: (pulse_us * 1000) / 5830 - */ - s_ultrasonic.distance_mm = (width * US_TO_MM_NUMERATOR) / US_TO_MM_DENOMINATOR; - - /* Validate range: 20-5000mm */ - s_ultrasonic.last_valid = (s_ultrasonic.distance_mm >= 20 && s_ultrasonic.distance_mm <= 5000); - if (!s_ultrasonic.last_valid) { - s_ultrasonic.distance_mm = 0; - } - - s_ultrasonic.state = ULTRASONIC_COMPLETE; - - /* Call callback if registered */ - if (s_ultrasonic.callback) { - s_ultrasonic.callback(s_ultrasonic.distance_mm, s_ultrasonic.last_valid); - } - - /* Reset for next measurement */ - s_ultrasonic.echo_start_ticks = 0; - s_ultrasonic.echo_end_ticks = 0; - } - } - } - - HAL_TIM_IRQHandler(&s_htim1); -} diff --git a/legacy/stm32/src/vesc_can.c b/legacy/stm32/src/vesc_can.c deleted file mode 100644 index df1b62b..0000000 --- a/legacy/stm32/src/vesc_can.c +++ /dev/null @@ -1,141 +0,0 @@ -/* vesc_can.c — VESC CAN protocol driver (Issue #674) - * - * Registers vesc_can_on_frame as the extended-frame callback with can_driver. - * VESC uses 29-bit arb IDs: (pkt_type << 8) | vesc_node_id. - * All wire values are big-endian (VESC FW 6.x). - */ - -#include "vesc_can.h" -#include "can_driver.h" -#include "jlink.h" -#include "stm32f7xx_hal.h" -#include -#include - -static uint8_t s_id_left; -static uint8_t s_id_right; -static vesc_state_t s_state[2]; /* [0] = left, [1] = right */ -static uint32_t s_tlm_tick; - -static vesc_state_t *state_for_id(uint8_t vesc_id) -{ - if (vesc_id == s_id_left) return &s_state[0]; - if (vesc_id == s_id_right) return &s_state[1]; - return NULL; -} - -void vesc_can_init(uint8_t id_left, uint8_t id_right) -{ - s_id_left = id_left; - s_id_right = id_right; - memset(s_state, 0, sizeof(s_state)); - /* Pre-wind so first send_tlm call fires immediately */ - s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / VESC_TLM_HZ)); - can_driver_set_ext_cb(vesc_can_on_frame); -} - -void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm) -{ - /* arb_id = (VESC_PKT_SET_RPM << 8) | vesc_id */ - uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | (uint32_t)vesc_id; - - /* Payload: int32 RPM, big-endian */ - uint32_t urpm = (uint32_t)rpm; - uint8_t data[4]; - data[0] = (uint8_t)(urpm >> 24u); - data[1] = (uint8_t)(urpm >> 16u); - data[2] = (uint8_t)(urpm >> 8u); - data[3] = (uint8_t)(urpm); - - can_driver_send_ext(ext_id, data, 4u); -} - -void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len) -{ - uint8_t pkt_type = (uint8_t)(ext_id >> 8u); - uint8_t vesc_id = (uint8_t)(ext_id & 0xFFu); - vesc_state_t *s = state_for_id(vesc_id); - - if (s == NULL) { - return; - } - - switch (pkt_type) { - case VESC_PKT_STATUS: /* 9: int32 RPM, int16 I×10, int16 duty×1000 (8 bytes) */ - if (len < 8u) { break; } - s->rpm = (int32_t)(((uint32_t)data[0] << 24u) | - ((uint32_t)data[1] << 16u) | - ((uint32_t)data[2] << 8u) | - (uint32_t)data[3]); - s->current_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]); - s->duty_x1000 = (int16_t)(((uint16_t)data[6] << 8u) | (uint16_t)data[7]); - s->last_rx_ms = HAL_GetTick(); - break; - - case VESC_PKT_STATUS_4: /* 16: int16 T_fet×10, T_mot×10, I_in×10 (6 bytes) */ - if (len < 6u) { break; } - s->temp_fet_x10 = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]); - s->temp_motor_x10 = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]); - s->current_in_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]); - break; - - case VESC_PKT_STATUS_5: /* 27: int32 tacho (ignored), int16 V_in×10 (6 bytes) */ - if (len < 6u) { break; } - /* bytes [0..3] = odometer tachometer — not stored */ - s->voltage_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]); - break; - - default: - break; - } -} - -bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out) -{ - if (out == NULL) { - return false; - } - vesc_state_t *s = state_for_id(vesc_id); - if (s == NULL || s->last_rx_ms == 0u) { - return false; - } - memcpy(out, s, sizeof(vesc_state_t)); - return true; -} - -bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms) -{ - vesc_state_t *s = state_for_id(vesc_id); - if (s == NULL || s->last_rx_ms == 0u) { - return false; - } - return (now_ms - s->last_rx_ms) < VESC_ALIVE_TIMEOUT_MS; -} - -void vesc_can_send_tlm(uint32_t now_ms) -{ - if ((now_ms - s_tlm_tick) < (1000u / VESC_TLM_HZ)) { - return; - } - s_tlm_tick = now_ms; - - jlink_tlm_vesc_state_t tlm; - memset(&tlm, 0, sizeof(tlm)); - - tlm.left_rpm = s_state[0].rpm; - tlm.right_rpm = s_state[1].rpm; - tlm.left_current_x10 = s_state[0].current_x10; - tlm.right_current_x10 = s_state[1].current_x10; - tlm.left_temp_x10 = s_state[0].temp_fet_x10; - tlm.right_temp_x10 = s_state[1].temp_fet_x10; - /* Use left voltage; fall back to right if left not yet received */ - tlm.voltage_x10 = s_state[0].voltage_x10 - ? s_state[0].voltage_x10 - : s_state[1].voltage_x10; - tlm.left_fault = s_state[0].fault_code; - tlm.right_fault = s_state[1].fault_code; - tlm.left_alive = vesc_can_is_alive(s_id_left, now_ms) ? 1u : 0u; - tlm.right_alive = vesc_can_is_alive(s_id_right, now_ms) ? 1u : 0u; - - jlink_send_vesc_state_tlm(&tlm); -} diff --git a/legacy/stm32/src/watchdog.c b/legacy/stm32/src/watchdog.c deleted file mode 100644 index ca1597f..0000000 --- a/legacy/stm32/src/watchdog.c +++ /dev/null @@ -1,148 +0,0 @@ -#include "watchdog.h" -#include "stm32f7xx_hal.h" -#include - -/* ================================================================ - * IWDG Hardware Configuration - * ================================================================ */ - -/* LSI frequency: approximately 32 kHz (typical, 20-40 kHz) */ -#define LSI_FREQUENCY_HZ 32000 - -/* IWDG prescaler values */ -#define IWDG_PSC_4 0 /* Divider = 4 */ -#define IWDG_PSC_8 1 /* Divider = 8 */ -#define IWDG_PSC_16 2 /* Divider = 16 */ -#define IWDG_PSC_32 3 /* Divider = 32 */ -#define IWDG_PSC_64 4 /* Divider = 64 */ -#define IWDG_PSC_128 5 /* Divider = 128 */ -#define IWDG_PSC_256 6 /* Divider = 256 */ - -/* Reload register range: 0-4095 */ -#define IWDG_RELOAD_MIN 1 -#define IWDG_RELOAD_MAX 4095 - -/* ================================================================ - * Watchdog State - * ================================================================ */ - -typedef struct { - bool is_initialized; /* Whether watchdog has been initialized */ - bool is_running; /* Whether watchdog is currently active */ - uint32_t timeout_ms; /* Configured timeout in milliseconds */ - uint8_t prescaler; /* IWDG prescaler value */ - uint16_t reload_value; /* IWDG reload register value */ -} WatchdogState; - -static WatchdogState s_watchdog = { - .is_initialized = false, - .is_running = false, - .timeout_ms = 0, - .prescaler = IWDG_PSC_32, - .reload_value = 0 -}; - -static IWDG_HandleTypeDef s_iwdg_handle = {0}; - -/* ================================================================ - * Helper Functions - * ================================================================ */ - -/* Calculate prescaler and reload values for desired timeout */ -static bool watchdog_calculate_config(uint32_t timeout_ms, - uint8_t *out_prescaler, - uint16_t *out_reload) -{ - if (timeout_ms < 1 || timeout_ms > 32000) { - return false; /* Out of reasonable range */ - } - - /* Try prescalers from smallest to largest */ - const uint8_t prescalers[] = {IWDG_PSC_4, IWDG_PSC_8, IWDG_PSC_16, - IWDG_PSC_32, IWDG_PSC_64, IWDG_PSC_128, - IWDG_PSC_256}; - const uint16_t dividers[] = {4, 8, 16, 32, 64, 128, 256}; - - for (int i = 0; i < 7; i++) { - uint16_t divider = dividers[i]; - /* timeout_ms = (reload * divider * 1000) / LSI_FREQUENCY_HZ */ - uint32_t reload = (timeout_ms * LSI_FREQUENCY_HZ) / (divider * 1000); - - if (reload >= IWDG_RELOAD_MIN && reload <= IWDG_RELOAD_MAX) { - *out_prescaler = prescalers[i]; - *out_reload = (uint16_t)reload; - return true; - } - } - - return false; /* No suitable prescaler found */ -} - -/* ================================================================ - * Public API - * ================================================================ */ - -bool watchdog_init(uint32_t timeout_ms) -{ - if (s_watchdog.is_initialized) { - return false; /* Already initialized */ - } - - /* Validate and calculate prescaler/reload values */ - uint8_t prescaler; - uint16_t reload; - if (!watchdog_calculate_config(timeout_ms, &prescaler, &reload)) { - return false; - } - - s_watchdog.prescaler = prescaler; - s_watchdog.reload_value = reload; - s_watchdog.timeout_ms = timeout_ms; - - /* Configure and start IWDG */ - s_iwdg_handle.Instance = IWDG; - s_iwdg_handle.Init.Prescaler = prescaler; - s_iwdg_handle.Init.Reload = reload; - s_iwdg_handle.Init.Window = reload; /* Window == Reload means full timeout */ - - HAL_IWDG_Init(&s_iwdg_handle); - - s_watchdog.is_initialized = true; - s_watchdog.is_running = true; - - return true; -} - -void watchdog_kick(void) -{ - if (s_watchdog.is_running) { - HAL_IWDG_Refresh(&s_iwdg_handle); /* Reset IWDG counter */ - } -} - -uint32_t watchdog_get_timeout(void) -{ - return s_watchdog.timeout_ms; -} - -bool watchdog_is_running(void) -{ - return s_watchdog.is_running; -} - -bool watchdog_was_reset_by_watchdog(void) -{ - /* Check RCC reset source flags */ - /* IWDG reset sets the IWDGRSTF flag in RCC_CSR */ - uint32_t reset_flags = RCC->CSR; - - /* IWDGRSTF is bit 29 of RCC_CSR */ - bool was_iwdg_reset = (reset_flags & RCC_CSR_IWDGRSTF) != 0; - - /* Clear the flag by writing to RMVF (Bit 24) */ - if (was_iwdg_reset) { - RCC->CSR |= RCC_CSR_RMVF; /* Clear reset flags */ - } - - return was_iwdg_reset; -} diff --git a/legacy/stm32/test/stubs/stm32f7xx_hal.h b/legacy/stm32/test/stubs/stm32f7xx_hal.h deleted file mode 100644 index 2b1a115..0000000 --- a/legacy/stm32/test/stubs/stm32f7xx_hal.h +++ /dev/null @@ -1,101 +0,0 @@ -/* test/stubs/stm32f7xx_hal.h - minimal HAL stub for host-side unit tests. */ -#ifndef STM32F7XX_HAL_H -#define STM32F7XX_HAL_H -#include -#include -#include -#define HAL_OK 0 -#define HAL_ERROR 1 -#define HAL_BUSY 2 -#define HAL_TIMEOUT 3 -typedef uint32_t HAL_StatusTypeDef; -#define ENABLE 1 -#define DISABLE 0 -uint32_t HAL_GetTick(void); -typedef struct { uint32_t dummy; uint32_t ESR; } CAN_TypeDef; -typedef struct { - uint32_t Prescaler; uint32_t Mode; uint32_t SyncJumpWidth; - uint32_t TimeSeg1; uint32_t TimeSeg2; uint32_t TimeTriggeredMode; - uint32_t AutoBusOff; uint32_t AutoWakeUp; uint32_t AutoRetransmission; - uint32_t ReceiveFifoLocked; uint32_t TransmitFifoPriority; -} CAN_InitTypeDef; -typedef struct { CAN_TypeDef *Instance; CAN_InitTypeDef Init; } CAN_HandleTypeDef; -typedef struct { - uint32_t StdId; uint32_t ExtId; uint32_t IDE; uint32_t RTR; - uint32_t DLC; uint32_t Timestamp; uint32_t FilterMatchIndex; -} CAN_RxHeaderTypeDef; -typedef struct { - uint32_t StdId; uint32_t ExtId; uint32_t IDE; uint32_t RTR; - uint32_t DLC; uint32_t TransmitGlobalTime; -} CAN_TxHeaderTypeDef; -typedef struct { - uint32_t FilterIdHigh; uint32_t FilterIdLow; - uint32_t FilterMaskIdHigh; uint32_t FilterMaskIdLow; - uint32_t FilterFIFOAssignment; uint32_t FilterBank; - uint32_t FilterMode; uint32_t FilterScale; - uint32_t FilterActivation; uint32_t SlaveStartFilterBank; -} CAN_FilterTypeDef; -#define CAN_ID_STD 0x00000000u -#define CAN_ID_EXT 0x00000004u -#define CAN_RTR_DATA 0x00000000u -#define CAN_RTR_REMOTE 0x00000002u -#define CAN_FILTERMODE_IDMASK 0u -#define CAN_FILTERSCALE_32BIT 1u -#define CAN_RX_FIFO0 0u -#define CAN_RX_FIFO1 1u -#define CAN_FILTER_ENABLE 1u -#define CAN_MODE_NORMAL 0u -#define CAN_SJW_1TQ 0u -#define CAN_BS1_13TQ 0u -#define CAN_BS2_4TQ 0u -#define CAN_ESR_EWGF 0x00000001u -#define CAN_ESR_EPVF 0x00000002u -#define CAN_ESR_BOFF 0x00000004u -#define CAN_ESR_TEC_Pos 16u -#define CAN_ESR_REC_Pos 24u -#define CAN_IT_ERROR_WARNING 0x00000100u -#define CAN_IT_ERROR_PASSIVE 0x00000200u -#define CAN_IT_BUSOFF 0x00000400u -#define CAN_FLAG_ERRI 0x00060000u -typedef uint32_t IRQn_Type; -#define CAN1_SCE_IRQn ((IRQn_Type)22u) -static inline void HAL_NVIC_SetPriority(IRQn_Type i,uint32_t p,uint32_t s){(void)i;(void)p;(void)s;} -static inline void HAL_NVIC_EnableIRQ(IRQn_Type i){(void)i;} -#define CAN1 ((CAN_TypeDef *)0) -static inline HAL_StatusTypeDef HAL_CAN_Init(CAN_HandleTypeDef *h){(void)h;return HAL_OK;} -static inline HAL_StatusTypeDef HAL_CAN_ConfigFilter(CAN_HandleTypeDef *h,CAN_FilterTypeDef *f){(void)h;(void)f;return HAL_OK;} -static inline HAL_StatusTypeDef HAL_CAN_Start(CAN_HandleTypeDef *h){(void)h;return HAL_OK;} -static inline HAL_StatusTypeDef HAL_CAN_Stop(CAN_HandleTypeDef *h){(void)h;return HAL_OK;} -static inline HAL_StatusTypeDef HAL_CAN_ActivateNotification(CAN_HandleTypeDef *h,uint32_t it){(void)h;(void)it;return HAL_OK;} -static inline void HAL_CAN_IRQHandler(CAN_HandleTypeDef *h){(void)h;} -static inline uint32_t HAL_CAN_GetTxMailboxesFreeLevel(CAN_HandleTypeDef *h){(void)h;return 3u;} -static inline HAL_StatusTypeDef HAL_CAN_AddTxMessage(CAN_HandleTypeDef *h,CAN_TxHeaderTypeDef *hdr,uint8_t *d,uint32_t *mb){(void)h;(void)hdr;(void)d;(void)mb;return HAL_OK;} -static inline uint32_t HAL_CAN_GetRxFifoFillLevel(CAN_HandleTypeDef *h,uint32_t f){(void)h;(void)f;return 0u;} -static inline HAL_StatusTypeDef HAL_CAN_GetRxMessage(CAN_HandleTypeDef *h,uint32_t f,CAN_RxHeaderTypeDef *hdr,uint8_t *d){(void)h;(void)f;(void)hdr;(void)d;return HAL_OK;} -#define __HAL_CAN_CLEAR_FLAG(h,f) ((void)(h),(void)(f)) -typedef struct { uint32_t dummy; } GPIO_TypeDef; -typedef struct { uint32_t Pin; uint32_t Mode; uint32_t Pull; uint32_t Speed; uint32_t Alternate; } GPIO_InitTypeDef; -typedef uint32_t GPIO_PinState; -#define GPIO_PIN_RESET 0u -#define GPIO_PIN_SET 1u -static inline void HAL_GPIO_Init(GPIO_TypeDef *p,GPIO_InitTypeDef *g){(void)p;(void)g;} -static inline GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *p,uint32_t pin){(void)p;(void)pin;return GPIO_PIN_SET;} -#define GPIOB ((GPIO_TypeDef *)0) -#define GPIOC ((GPIO_TypeDef *)0) -#define GPIO_PIN_2 (1u<<2) -#define GPIO_PIN_8 (1u<<8) -#define GPIO_PIN_9 (1u<<9) -#define GPIO_PIN_12 (1u<<12) -#define GPIO_PIN_13 (1u<<13) -#define GPIO_MODE_AF_PP 0u -#define GPIO_MODE_INPUT 1u -#define GPIO_NOPULL 0u -#define GPIO_PULLUP 1u -#define GPIO_SPEED_FREQ_HIGH 0u -#define GPIO_AF9_CAN1 9u -#define GPIO_AF9_CAN2 9u -#define __HAL_RCC_CAN1_CLK_ENABLE() ((void)0) -#define __HAL_RCC_CAN2_CLK_ENABLE() ((void)0) -#define __HAL_RCC_GPIOB_CLK_ENABLE() ((void)0) -#define __HAL_RCC_GPIOC_CLK_ENABLE() ((void)0) -#endif /* STM32F7XX_HAL_H */ diff --git a/legacy/stm32/test/test_battery_adc.c b/legacy/stm32/test/test_battery_adc.c deleted file mode 100644 index 01a4d68..0000000 --- a/legacy/stm32/test/test_battery_adc.c +++ /dev/null @@ -1,476 +0,0 @@ -/* - * test_battery_adc.c — Unit tests for battery_adc driver (Issue #533) - * - * Tests run on host (Linux/macOS) with stub HAL functions. - * Build: gcc -I../include -DTEST_HOST -o test_battery_adc test_battery_adc.c - * - * Coverage: - * 1. raw_to_vbat_mv — default scale (VBAT_SCALE_NUM=11, VBAT_AREF_MV=3300) - * 2. raw_to_vbat_mv — calibration offset (+100 mV, -100 mV) - * 3. raw_to_vbat_mv — calibration scale override - * 4. IIR LPF convergence — step input settles within N ticks - * 5. IIR LPF — LPF_SHIFT=3 gives α ≈ 1/8 per tick - * 6. SoC estimate — 3S LiPo 0% / 100% / midpoint - * 7. SoC estimate — 4S LiPo 0% / 100% - * 8. battery_adc_is_low() / is_critical() flags - * 9. battery_adc_check_pm() — below threshold for < HOLD_MS: no notify - * 10. battery_adc_check_pm() — below threshold for >= HOLD_MS: notify fires - * 11. battery_adc_check_pm() — recovery clears state - * 12. calibrate(NULL) resets to defaults - * 13. calibrate() clamps offset to ±500 mV - * 14. publish() rate-limit — not sent before PUBLISH_HZ period - * 15. publish() rate-limit — sent after period elapses - */ - -#include -#include -#include -#include -#include - -/* ---- Minimal HAL stubs (guards real header, provides needed types) ---- */ -/* Must appear before any #include that could pull in stm32f7xx_hal.h */ -#define STM32F7XX_HAL_H -#define HAL_OK 0 -#define ENABLE 1 -#define DISABLE 0 - -typedef uint32_t GPIO_TypeDef; -typedef struct { uint32_t Pin, Mode, Pull, Speed; } GPIO_InitTypeDef; -typedef uint32_t DMA_Stream_TypeDef; -typedef struct { - uint32_t Channel, Direction, PeriphInc, MemInc; - uint32_t PeriphDataAlignment, MemDataAlignment, Mode, Priority, FIFOMode; -} DMA_InitTypeDef; -typedef struct { DMA_Stream_TypeDef *Instance; DMA_InitTypeDef Init; } DMA_HandleTypeDef; -typedef uint32_t ADC_TypeDef; -typedef struct { - uint32_t ClockPrescaler, Resolution, ScanConvMode, ContinuousConvMode; - uint32_t DiscontinuousConvMode, ExternalTrigConvEdge, ExternalTrigConv; - uint32_t DataAlign, NbrOfConversion, DMAContinuousRequests, EOCSelection; -} ADC_InitTypeDef; -typedef struct { ADC_TypeDef *Instance; ADC_InitTypeDef Init; DMA_HandleTypeDef *DMA_Handle; } ADC_HandleTypeDef; -typedef struct { uint32_t Channel, Rank, SamplingTime; } ADC_ChannelConfTypeDef; - -#define GPIO_PIN_1 (1u<<1) -#define GPIO_PIN_3 (1u<<3) -#define GPIO_MODE_ANALOG 0 -#define GPIO_NOPULL 0 -#define GPIO_SPEED_FREQ_HIGH 0 -#define DMA_CHANNEL_2 0 -#define DMA_PERIPH_TO_MEMORY 0 -#define DMA_PINC_DISABLE 0 -#define DMA_MINC_ENABLE 0 -#define DMA_PDATAALIGN_HALFWORD 0 -#define DMA_MDATAALIGN_HALFWORD 0 -#define DMA_CIRCULAR 0 -#define DMA_PRIORITY_LOW 0 -#define DMA_FIFOMODE_DISABLE 0 -#define ADC_CLOCK_SYNC_PCLK_DIV8 0 -#define ADC_RESOLUTION_12B 0 -#define ADC_EXTERNALTRIGCONVEDGE_NONE 0 -#define ADC_SOFTWARE_START 0 -#define ADC_DATAALIGN_RIGHT 0 -#define ADC_EOC_SEQ_CONV 0 -#define ADC_CHANNEL_11 11 -#define ADC_CHANNEL_13 13 -#define ADC_SAMPLETIME_480CYCLES 0 - -static uint32_t _gpioc_s, _dma2s0_s, _adc3_s; -#define GPIOC ((GPIO_TypeDef *)&_gpioc_s) -#define DMA2_Stream0 ((DMA_Stream_TypeDef *)&_dma2s0_s) -#define ADC3 ((ADC_TypeDef *)&_adc3_s) - -#define __HAL_RCC_GPIOC_CLK_ENABLE() ((void)0) -#define __HAL_RCC_DMA2_CLK_ENABLE() ((void)0) -#define __HAL_RCC_ADC3_CLK_ENABLE() ((void)0) -#define __HAL_LINKDMA(hadc, field, hdma) ((hadc)->DMA_Handle = &(hdma)) - -static inline int HAL_GPIO_Init(void *p, GPIO_InitTypeDef *g){(void)p;(void)g;return 0;} -static inline int HAL_DMA_Init(DMA_HandleTypeDef *h){(void)h;return HAL_OK;} -static inline int HAL_ADC_Init(ADC_HandleTypeDef *h){(void)h;return HAL_OK;} -static inline int HAL_ADC_ConfigChannel(ADC_HandleTypeDef *h,ADC_ChannelConfTypeDef *c){(void)h;(void)c;return HAL_OK;} -static inline int HAL_ADC_Start_DMA(ADC_HandleTypeDef *h,uint32_t *b,uint32_t n){(void)h;(void)b;(void)n;return HAL_OK;} -static inline uint32_t __get_PRIMASK(void){return 0;} -static inline void __disable_irq(void){} -static inline void __set_PRIMASK(uint32_t v){(void)v;} - -/* Config constants (mirror config.h) */ -#define VBAT_SCALE_NUM 11 -#define VBAT_AREF_MV 3300 -#define VBAT_ADC_BITS 12 -#define ADC_IBAT_SCALE 115 - -/* power_mgmt stub */ -static uint32_t g_pm_notify_vbat = 0; -static int g_pm_notify_count = 0; -void power_mgmt_notify_battery(uint32_t vbat_mv) { - g_pm_notify_vbat = vbat_mv; - g_pm_notify_count++; -} - -/* jlink stubs — must appear before battery_adc.c which calls these */ -static int g_jlink_send_count = 0; -/* jlink_tlm_battery_t is defined in jlink.h; replicate minimal version here */ -typedef struct __attribute__((packed)) { - uint16_t vbat_mv; int16_t ibat_ma; uint16_t vbat_raw_mv; - uint8_t flags; int8_t cal_offset; uint8_t lpf_shift; uint8_t soc_pct; -} jlink_tlm_battery_t; -void jlink_send_battery_telemetry(const jlink_tlm_battery_t *b) { - (void)b; g_jlink_send_count++; -} - -/* ---- Include driver source directly for white-box testing ---- */ -/* jlink.h and power_mgmt.h includes are bypassed by the stubs above */ -#define JLINK_H /* block jlink.h inclusion in battery_adc.c */ -#define POWER_MGMT_H /* block power_mgmt.h inclusion in battery_adc.c */ -#include "../src/battery_adc.c" - -/* ---- Test helpers ---- */ -static int g_pass = 0, g_fail = 0; - -#define TEST(name) do { printf(" %-55s", name); } while(0) -#define PASS() do { g_pass++; printf("PASS\n"); } while(0) -#define FAIL(msg) do { g_fail++; printf("FAIL — %s\n", msg); } while(0) -#define ASSERT_EQ(a,b,msg) do { if ((a)==(b)) PASS(); else FAIL(msg); } while(0) -#define ASSERT_NEAR(a,b,tol,msg) do { \ - int32_t _d = (int32_t)(a) - (int32_t)(b); \ - if (_d < 0) _d = -_d; \ - if (_d <= (int32_t)(tol)) PASS(); else FAIL(msg); } while(0) -#define ASSERT_TRUE(x,msg) ASSERT_EQ(!!(x), 1, msg) -#define ASSERT_FALSE(x,msg) ASSERT_EQ(!!(x), 0, msg) - -/* ---- Reset driver private state for each test ---- */ -static void reset_driver(void) -{ - s_vbat_mv = 0; - s_ibat_ma = 0; - s_vbat_raw_mv = 0; - s_lpf_vbat8 = 0; - s_lpf_ibat8 = 0; - s_low_active = false; - s_low_since_ms = 0; - s_critical_notified = false; - s_last_publish_ms = 0; - s_published_once = false; - s_low_mv = BATTERY_ADC_LOW_MV; - s_critical_mv = BATTERY_ADC_CRITICAL_MV; - s_ready = true; /* simulate successful init */ - battery_adc_calibrate(NULL); - - g_pm_notify_vbat = 0; - g_pm_notify_count = 0; - g_jlink_send_count = 0; - - /* Reset thresholds_set flag — hack: zero the static inside battery_adc_tick */ - /* Re-init will re-detect 3S/4S on next tick */ -} - -/* ---- Helper: directly inject filtered values (bypass DMA) ---- */ -static void inject_voltage_mv(uint32_t mv) -{ - /* Back-calculate raw count from mV = raw × (3300 × 11) / 4096 */ - uint32_t raw = (mv * (1u << VBAT_ADC_BITS)) / - (VBAT_AREF_MV * VBAT_SCALE_NUM); - /* Set LPF accumulators to fully settled value */ - s_lpf_vbat8 = raw << 3u; - /* Force output update */ - s_vbat_mv = raw_to_vbat_mv(raw); - s_vbat_raw_mv = s_vbat_mv; - /* Auto-set 4S thresholds if needed */ - if (s_vbat_mv >= 13000u) { - s_low_mv = BATTERY_ADC_LOW_MV_4S; - s_critical_mv = BATTERY_ADC_CRITICAL_MV_4S; - } else { - s_low_mv = BATTERY_ADC_LOW_MV; - s_critical_mv = BATTERY_ADC_CRITICAL_MV; - } -} - -/* ---- Tests ---- */ - -static void test_raw_to_vbat_default(void) -{ - TEST("raw_to_vbat_mv: 0 count → 0 mV"); - ASSERT_EQ(raw_to_vbat_mv(0), 0u, "zero raw should be 0 mV"); - - TEST("raw_to_vbat_mv: 4095 count → 3300×11 mV"); - uint32_t expected = (4095u * 3300u * 11u) / 4096u; - ASSERT_NEAR(raw_to_vbat_mv(4095), expected, 2u, "full-scale raw"); - - TEST("raw_to_vbat_mv: midpoint 2048 → ~8812 mV (3S half-charge)"); - uint32_t mid = (2048u * 3300u * 11u) / 4096u; - ASSERT_NEAR(raw_to_vbat_mv(2048), mid, 2u, "midpoint raw"); -} - -static void test_calibration_offset(void) -{ - reset_driver(); - battery_adc_cal_t cal = {0}; - - TEST("cal offset +100 mV increases output by ~100 mV"); - cal.vbat_offset_mv = 100; - battery_adc_calibrate(&cal); - uint32_t base = raw_to_vbat_mv(1000); - cal.vbat_offset_mv = 0; - battery_adc_calibrate(&cal); - uint32_t no_offset = raw_to_vbat_mv(1000); - ASSERT_NEAR(base - no_offset, 100u, 2u, "+100 mV offset delta"); - - TEST("cal offset -100 mV decreases output by ~100 mV"); - cal.vbat_offset_mv = -100; - battery_adc_calibrate(&cal); - uint32_t neg_offset = raw_to_vbat_mv(1000); - cal.vbat_offset_mv = 0; - battery_adc_calibrate(&cal); - uint32_t delta = no_offset - neg_offset; - ASSERT_NEAR(delta, 100u, 2u, "-100 mV offset delta"); - - TEST("cal offset clamp: +600 clamped to +500 mV"); - cal.vbat_offset_mv = 600; - battery_adc_calibrate(&cal); - battery_adc_get_calibration(&cal); - ASSERT_EQ(cal.vbat_offset_mv, 500, "clamp to +500"); -} - -static void test_calibration_scale_override(void) -{ - reset_driver(); - battery_adc_cal_t cal = {0}; - - TEST("scale override: num=22, den=2 same as default (11/1)"); - cal.vbat_scale_num = 22; - cal.vbat_scale_den = 2; - battery_adc_calibrate(&cal); - uint32_t scaled = raw_to_vbat_mv(2000); - cal.vbat_scale_num = 0; cal.vbat_scale_den = 0; - battery_adc_calibrate(&cal); - uint32_t dflt = raw_to_vbat_mv(2000); - ASSERT_NEAR(scaled, dflt, 2u, "22/2 == 11/1 scale"); -} - -static void test_lpf_convergence(void) -{ - reset_driver(); - - /* Step from 0 to a target: check convergence after N ticks */ - /* Target raw = 3000 counts ≈ 27,000 mV @ 11:1 divider */ - uint16_t target_raw = 3000u; - /* Fill DMA buffer with target value */ - uint16_t fake_buf[8]; - for (int i = 0; i < 8; i += 2) { fake_buf[i] = target_raw; fake_buf[i+1] = 0; } - - /* Manually tick IIR 80 times (should be ~99% settled in 36 ticks) */ - for (int i = 0; i < 80; i++) { - uint32_t avg = target_raw; - s_lpf_vbat8 += ((avg << 3u) - s_lpf_vbat8) >> BATTERY_ADC_LPF_SHIFT; - } - uint32_t filtered = s_lpf_vbat8 >> 3u; - - TEST("IIR LPF 80-tick convergence within 2% of target"); - uint32_t diff = (filtered > target_raw) ? (filtered - target_raw) : (target_raw - filtered); - ASSERT_TRUE(diff <= target_raw / 50u, "LPF settled to within 2%"); -} - -static void test_lpf_alpha(void) -{ - TEST("IIR LPF_SHIFT=3 → α ≈ 1/8 per step"); - /* After 1 tick from 0 to 8000 (×8): delta = 8000/8 = 1000 */ - uint32_t acc = 0; - uint32_t step = 8000u; - acc += (step - acc) >> BATTERY_ADC_LPF_SHIFT; - /* Expected: acc = 8000/8 = 1000 */ - ASSERT_NEAR(acc, 1000u, 2u, "one-step IIR response"); -} - -static void test_soc_3s(void) -{ - reset_driver(); - - TEST("SoC 3S: 9900 mV → 0%"); - inject_voltage_mv(9900u); - s_vbat_mv = 9900u; - /* Run publish logic inline */ - uint8_t soc = 255u; - { uint32_t v = 9900u, vmin = 9900u, vmax = 12600u; - soc = (v <= vmin) ? 0u : (v >= vmax ? 100u : - (uint8_t)(((v - vmin) * 100u) / (vmax - vmin))); } - ASSERT_EQ(soc, 0u, "9900 mV = 0%"); - - TEST("SoC 3S: 12600 mV → 100%"); - { uint32_t v = 12600u, vmin = 9900u, vmax = 12600u; - soc = (v <= vmin) ? 0u : (v >= vmax ? 100u : - (uint8_t)(((v - vmin) * 100u) / (vmax - vmin))); } - ASSERT_EQ(soc, 100u, "12600 mV = 100%"); - - TEST("SoC 3S: 11250 mV → ~50%"); - { uint32_t v = 11250u, vmin = 9900u, vmax = 12600u; - soc = (uint8_t)(((v - vmin) * 100u) / (vmax - vmin)); } - ASSERT_NEAR(soc, 50u, 2u, "11250 mV ≈ 50%"); -} - -static void test_soc_4s(void) -{ - TEST("SoC 4S: 13200 mV → 0%"); - uint8_t soc; - { uint32_t v = 13200u, vmin = 13200u, vmax = 16800u; - soc = (v <= vmin) ? 0u : 100u; } - ASSERT_EQ(soc, 0u, "13200 mV 4S = 0%"); - - TEST("SoC 4S: 16800 mV → 100%"); - { uint32_t v = 16800u, vmin = 13200u, vmax = 16800u; - soc = (v >= vmax) ? 100u : 0u; } - ASSERT_EQ(soc, 100u, "16800 mV 4S = 100%"); -} - -static void test_is_low_critical_flags(void) -{ - reset_driver(); - - TEST("is_low(): false when Vbat above LOW_MV threshold"); - inject_voltage_mv(11000u); /* well above 10200 mV LOW_MV */ - ASSERT_FALSE(battery_adc_is_low(), "11 V not low"); - - TEST("is_low(): true when Vbat below LOW_MV"); - inject_voltage_mv(9800u); - ASSERT_TRUE(battery_adc_is_low(), "9.8 V is low"); - - TEST("is_critical(): false when above CRITICAL_MV"); - inject_voltage_mv(10000u); /* above 9600 */ - ASSERT_FALSE(battery_adc_is_critical(), "10 V not critical"); - - TEST("is_critical(): true when below CRITICAL_MV"); - inject_voltage_mv(9400u); - ASSERT_TRUE(battery_adc_is_critical(), "9.4 V is critical"); -} - -static void test_check_pm_no_notify_short(void) -{ - reset_driver(); - - TEST("check_pm: no notify when below critical < HOLD_MS"); - inject_voltage_mv(9400u); /* below CRITICAL_MV=9600 */ - - /* Call check_pm at t=0 */ - s_low_active = false; - s_low_since_ms = 0; - s_critical_notified = false; - - battery_adc_check_pm(0u); - battery_adc_check_pm(1000u); /* 1 second — not yet 5 s */ - battery_adc_check_pm(4000u); /* 4 seconds */ - - ASSERT_EQ(g_pm_notify_count, 0, "no notify before HOLD_MS"); -} - -static void test_check_pm_notify_after_hold(void) -{ - reset_driver(); - - TEST("check_pm: notify fires after HOLD_MS sustained critical voltage"); - inject_voltage_mv(9400u); - - battery_adc_check_pm(0u); - battery_adc_check_pm(5001u); /* just past HOLD_MS=5000 */ - - ASSERT_EQ(g_pm_notify_count, 1, "notify fires once after hold"); -} - -static void test_check_pm_notify_only_once(void) -{ - reset_driver(); - inject_voltage_mv(9400u); - - battery_adc_check_pm(0u); - battery_adc_check_pm(5001u); - battery_adc_check_pm(6000u); /* still critical */ - battery_adc_check_pm(7000u); - - TEST("check_pm: notify fires only once per low event"); - ASSERT_EQ(g_pm_notify_count, 1, "notify not repeated"); -} - -static void test_check_pm_recovery(void) -{ - reset_driver(); - inject_voltage_mv(9400u); - - battery_adc_check_pm(0u); - battery_adc_check_pm(5001u); /* first event fires */ - - /* Voltage recovers */ - inject_voltage_mv(11000u); - battery_adc_check_pm(6000u); /* recovery tick */ - - /* Voltage drops again */ - inject_voltage_mv(9400u); - battery_adc_check_pm(7000u); /* new low event starts */ - battery_adc_check_pm(12001u); /* 5 s after new event */ - - TEST("check_pm: recovery + re-trigger fires notify again"); - ASSERT_EQ(g_pm_notify_count, 2, "two events = two notifies"); -} - -static void test_calibrate_null_reset(void) -{ - reset_driver(); - battery_adc_cal_t cal = { .vbat_offset_mv = 200, .ibat_offset_ma = 50, - .vbat_scale_num = 10, .vbat_scale_den = 1 }; - battery_adc_calibrate(&cal); - battery_adc_calibrate(NULL); - - battery_adc_get_calibration(&cal); - - TEST("calibrate(NULL): resets vbat_offset to 0"); - ASSERT_EQ(cal.vbat_offset_mv, 0, "offset reset"); - - TEST("calibrate(NULL): resets scale_num to 0 (use default)"); - ASSERT_EQ(cal.vbat_scale_num, 0, "scale_num reset"); -} - -static void test_publish_rate_limit(void) -{ - reset_driver(); - inject_voltage_mv(11000u); - s_ready = true; - g_jlink_send_count = 0; - - uint32_t period_ms = 1000u / BATTERY_ADC_PUBLISH_HZ; - - battery_adc_publish(0u); - - TEST("publish: first call always sends"); - ASSERT_EQ(g_jlink_send_count, 1, "first send"); - - battery_adc_publish(period_ms - 1u); - TEST("publish: no send before period elapses"); - ASSERT_EQ(g_jlink_send_count, 1, "no premature send"); - - battery_adc_publish(period_ms + 1u); - TEST("publish: send after period elapses"); - ASSERT_EQ(g_jlink_send_count, 2, "send after period"); -} - -/* ---- main ---- */ -int main(void) -{ - printf("=== test_battery_adc (Issue #533) ===\n\n"); - - test_raw_to_vbat_default(); - test_calibration_offset(); - test_calibration_scale_override(); - test_lpf_convergence(); - test_lpf_alpha(); - test_soc_3s(); - test_soc_4s(); - test_is_low_critical_flags(); - test_check_pm_no_notify_short(); - test_check_pm_notify_after_hold(); - test_check_pm_notify_only_once(); - test_check_pm_recovery(); - test_calibrate_null_reset(); - test_publish_rate_limit(); - - printf("\n=== Results: %d passed, %d failed ===\n", g_pass, g_fail); - return (g_fail == 0) ? 0 : 1; -} diff --git a/legacy/stm32/test/test_can_watchdog.c b/legacy/stm32/test/test_can_watchdog.c deleted file mode 100644 index f5e31ae..0000000 --- a/legacy/stm32/test/test_can_watchdog.c +++ /dev/null @@ -1,224 +0,0 @@ -/* - * test_can_watchdog.c — Unit tests for CAN bus watchdog and error recovery (Issue #694). - * - * Build (host, no hardware): - * gcc -I include -I test/stubs -DTEST_HOST \ - * -o /tmp/test_can_watchdog src/can_driver.c test/test_can_watchdog.c - * - * All timing driven through now_ms argument passed to can_driver_watchdog_tick(). - * ESR state injected via can_driver_inject_esr(). - */ - -/* Include implementation directly (pulls stm32 stub via -I test/stubs) */ -#include "../src/can_driver.c" - -/* ---- Test framework ---- */ -#include -#include - -static int g_pass = 0; -static int g_fail = 0; - -#define ASSERT(cond, msg) \ - do { \ - if (cond) { g_pass++; } \ - else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \ - } while (0) - -/* HAL_GetTick stub */ -uint32_t HAL_GetTick(void) { return 0u; } - -/* Reset driver state between tests */ -static void _reset(void) -{ - can_driver_inject_esr(0u); - can_driver_init(); -} - -/* ---- Tests ---- */ - -static void test_nominal_no_errors(void) -{ - _reset(); - can_error_state_t st = can_driver_watchdog_tick(0u); - ASSERT(st == CAN_ERR_NOMINAL, "clean ESR: state is NOMINAL"); - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.restart_count == 0u, "nominal: restart_count 0"); - ASSERT(wd.busoff_count == 0u, "nominal: busoff_count 0"); -} - -static void test_errwarn_detection(void) -{ - _reset(); - can_driver_inject_esr(CAN_ESR_EWGF); - can_error_state_t st = can_driver_watchdog_tick(1000u); - ASSERT(st == CAN_ERR_WARNING, "EWGF set: state is WARNING"); - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.errwarn_count == 1u, "errwarn_count incremented"); -} - -static void test_errwarn_cleared_returns_nominal(void) -{ - _reset(); - /* Enter warning state */ - can_driver_inject_esr(CAN_ESR_EWGF); - can_driver_watchdog_tick(100u); - - /* Clear ESR - should de-escalate */ - can_driver_inject_esr(0u); - can_error_state_t st = can_driver_watchdog_tick(200u); - ASSERT(st == CAN_ERR_NOMINAL, "ESR cleared: de-escalate to NOMINAL"); -} - -static void test_errpassive_detection(void) -{ - _reset(); - can_driver_inject_esr(CAN_ESR_EPVF); - can_error_state_t st = can_driver_watchdog_tick(500u); - ASSERT(st == CAN_ERR_ERROR_PASSIVE, "EPVF set: state is ERROR_PASSIVE"); - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.errpassive_count == 1u, "errpassive_count incremented"); -} - -static void test_busoff_detection(void) -{ - _reset(); - can_driver_inject_esr(CAN_ESR_BOFF); - can_error_state_t st = can_driver_watchdog_tick(1000u); - ASSERT(st == CAN_ERR_BUS_OFF, "BOFF set: state is BUS_OFF"); - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.busoff_count == 1u, "busoff_count incremented"); - ASSERT(wd.busoff_ms == 1000u, "busoff_ms recorded"); -} - -static void test_busoff_sets_stats_bus_off(void) -{ - _reset(); - can_driver_inject_esr(CAN_ESR_BOFF); - can_driver_watchdog_tick(500u); - - can_stats_t cs; - can_driver_get_stats(&cs); - ASSERT(cs.bus_off == 1u, "bus-off: stats.bus_off = 1"); -} - -static void test_busoff_no_restart_before_timeout(void) -{ - _reset(); - can_driver_inject_esr(CAN_ESR_BOFF); - can_driver_watchdog_tick(0u); - - /* Call again just before CAN_WDOG_RESTART_MS */ - can_error_state_t st = can_driver_watchdog_tick(CAN_WDOG_RESTART_MS - 1u); - ASSERT(st == CAN_ERR_BUS_OFF, "before restart timeout: still BUS_OFF"); - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.restart_count == 0u, "no restart yet"); -} - -static void test_busoff_auto_restart_after_timeout(void) -{ - _reset(); - /* Inject bus-off at t=0 */ - can_driver_inject_esr(CAN_ESR_BOFF); - can_driver_watchdog_tick(0u); - - /* After restart timeout, HAL_CAN_Stop/Start clears s_test_esr internally */ - can_error_state_t st = can_driver_watchdog_tick(CAN_WDOG_RESTART_MS); - ASSERT(st == CAN_ERR_NOMINAL, "after restart timeout: state is NOMINAL"); - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.restart_count == 1u, "restart_count incremented to 1"); - ASSERT(wd.busoff_ms == 0u, "busoff_ms cleared after restart"); -} - -static void test_busoff_stats_cleared_after_restart(void) -{ - _reset(); - can_driver_inject_esr(CAN_ESR_BOFF); - can_driver_watchdog_tick(0u); - can_driver_watchdog_tick(CAN_WDOG_RESTART_MS); - - can_stats_t cs; - can_driver_get_stats(&cs); - ASSERT(cs.bus_off == 0u, "after restart: stats.bus_off cleared"); -} - -static void test_busoff_count_only_increments_once_per_event(void) -{ - _reset(); - can_driver_inject_esr(CAN_ESR_BOFF); - /* Call tick multiple times while still in bus-off */ - can_driver_watchdog_tick(0u); - can_driver_watchdog_tick(10u); - can_driver_watchdog_tick(50u); - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.busoff_count == 1u, "repeated BOFF ticks: count stays 1"); -} - -static void test_multiple_restart_cycles(void) -{ - _reset(); - /* Cycle 1 */ - can_driver_inject_esr(CAN_ESR_BOFF); - can_driver_watchdog_tick(0u); - can_driver_watchdog_tick(CAN_WDOG_RESTART_MS); /* restart */ - - /* Cycle 2: inject bus-off again */ - can_driver_inject_esr(CAN_ESR_BOFF); - can_driver_watchdog_tick(CAN_WDOG_RESTART_MS + 1u); - can_driver_watchdog_tick(CAN_WDOG_RESTART_MS * 2u + 1u); /* restart */ - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.restart_count == 2u, "two restart cycles: restart_count = 2"); - ASSERT(wd.busoff_count == 2u, "two restart cycles: busoff_count = 2"); -} - -static void test_tec_rec_extracted(void) -{ - _reset(); - /* Inject ESR with TEC=0xAB (bits 23:16) and REC=0xCD (bits 31:24) */ - uint32_t fake_esr = (0xABu << 16) | (0xCDu << 24); - can_driver_inject_esr(fake_esr); - can_driver_watchdog_tick(100u); - - can_wdog_t wd; - can_driver_get_wdog(&wd); - ASSERT(wd.tec == 0xABu, "TEC extracted from ESR correctly"); - ASSERT(wd.rec == 0xCDu, "REC extracted from ESR correctly"); -} - -/* ---- main ---- */ -int main(void) -{ - printf("=== test_can_watchdog ===\n"); - - test_nominal_no_errors(); - test_errwarn_detection(); - test_errwarn_cleared_returns_nominal(); - test_errpassive_detection(); - test_busoff_detection(); - test_busoff_sets_stats_bus_off(); - test_busoff_no_restart_before_timeout(); - test_busoff_auto_restart_after_timeout(); - test_busoff_stats_cleared_after_restart(); - test_busoff_count_only_increments_once_per_event(); - test_multiple_restart_cycles(); - test_tec_rec_extracted(); - - printf("\nResults: %d passed, %d failed\n", g_pass, g_fail); - return g_fail ? 1 : 0; -} diff --git a/legacy/stm32/test/test_hw_button.c b/legacy/stm32/test/test_hw_button.c deleted file mode 100644 index fca5507..0000000 --- a/legacy/stm32/test/test_hw_button.c +++ /dev/null @@ -1,283 +0,0 @@ -/* - * test_hw_button.c — Unit tests for hw_button debounce/gesture driver (Issue #682). - * - * Build (host, no hardware): - * gcc -I include -I test/stubs -DTEST_HOST \ - * -o /tmp/test_hw_button src/hw_button.c test/test_hw_button.c - * - * All tests use hw_button_inject() to drive the simulated GPIO pin. - * All timing is driven through the now_ms argument passed to hw_button_tick(). - */ - -/* ---- Stubs: prevent STM32 HAL from being included in non-TEST_HOST path ---- */ -#define STM32F7XX_HAL_H -#include -#include - -/* ---- Include implementation directly ---- */ -#include "../src/hw_button.c" - -/* ---- Test framework ---- */ -#include -#include - -static int g_pass = 0; -static int g_fail = 0; - -#define ASSERT(cond, msg) \ - do { \ - if (cond) { g_pass++; } \ - else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \ - } while (0) - -/* ---- Helpers ---- */ - -/* Advance time by `ms` ticks with the pin held at `pressed`. - * Returns the last non-NONE event seen, or BTN_EVENT_NONE. */ -static hw_btn_event_t _tick_n(uint32_t *now, uint32_t ms, bool pressed) -{ - hw_btn_event_t last = BTN_EVENT_NONE; - hw_button_inject(pressed); - for (uint32_t i = 0; i < ms; i++) { - (*now)++; - hw_btn_event_t ev = hw_button_tick(*now); - if (ev != BTN_EVENT_NONE) last = ev; - } - return last; -} - -/* Reset driver state between tests. */ -static void _reset(uint32_t *now) -{ - *now = 0; - hw_button_inject(false); - hw_button_init(); - /* Drain any pending sequence state */ - for (int i = 0; i < 100; i++) hw_button_tick((*now)++); -} - -/* ---- Tests ---- */ - -static void test_idle_no_event(void) -{ - uint32_t now = 0; - hw_button_init(); - hw_button_inject(false); - for (int i = 0; i < 1000; i++) { - hw_btn_event_t ev = hw_button_tick(now++); - ASSERT(ev == BTN_EVENT_NONE, "idle: no event while pin released"); - } -} - -static void test_debounce_bounce_ignored(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* Press for only 10 ms (< BTN_DEBOUNCE_MS=20) then release */ - hw_btn_event_t ev = _tick_n(&now, 10, true); - ASSERT(ev == BTN_EVENT_NONE, "bounce < debounce: no event during press"); - ev = _tick_n(&now, BTN_COMMIT_MS + 50, false); - ASSERT(ev == BTN_EVENT_NONE, "bounce < debounce: no event after release"); -} - -static void test_debounce_confirmed(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* Press for BTN_DEBOUNCE_MS + 1 ms — debounce should confirm */ - _tick_n(&now, BTN_DEBOUNCE_MS + 1, true); - ASSERT(hw_button_is_pressed(), "debounce passed: is_pressed() == true"); -} - -static void test_short_press_park(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* Press for BTN_DEBOUNCE_MS + 10 ms (short: well < BTN_LONG_MIN_MS) */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); - /* Release */ - _tick_n(&now, 1, false); - ASSERT(!hw_button_is_pressed(), "short press: released"); - - /* Wait BTN_COMMIT_MS quiet -> should fire PARK */ - hw_btn_event_t ev = BTN_EVENT_NONE; - hw_button_inject(false); - for (uint32_t i = 0; i < BTN_COMMIT_MS + 10; i++) { - now++; - hw_btn_event_t e = hw_button_tick(now); - if (e != BTN_EVENT_NONE) ev = e; - } - ASSERT(ev == BTN_EVENT_PARK, "short press + quiet: BTN_EVENT_PARK"); -} - -static void test_long_press_no_park(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* Press for BTN_LONG_MIN_MS + 100 ms (long press) */ - _tick_n(&now, BTN_DEBOUNCE_MS + BTN_LONG_MIN_MS + 100, true); - /* Release and wait */ - hw_btn_event_t ev = _tick_n(&now, BTN_COMMIT_MS + 10, false); - /* Lone LONG press has no defined gesture */ - ASSERT(ev == BTN_EVENT_NONE, "lone long press: no event"); -} - -static void test_rearm_combo_fires(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* Press 1: SHORT (30 ms hold + release) */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); - _tick_n(&now, 1, false); - /* Brief gap between presses (< BTN_COMMIT_MS so no PARK fires) */ - _tick_n(&now, 100, false); - - /* Press 2: SHORT */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); - _tick_n(&now, 1, false); - _tick_n(&now, 100, false); - - /* Press 3: LONG — combo fires on release */ - _tick_n(&now, BTN_DEBOUNCE_MS + BTN_LONG_MIN_MS + 50, true); - hw_btn_event_t ev = BTN_EVENT_NONE; - hw_button_inject(false); - for (int i = 0; i < 5; i++) { - hw_btn_event_t e = hw_button_tick(now++); - if (e != BTN_EVENT_NONE) ev = e; - } - ASSERT(ev == BTN_EVENT_REARM_COMBO, "SHORT+SHORT+LONG: BTN_EVENT_REARM_COMBO"); -} - -static void test_rearm_combo_resets_buffer(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* Fire combo once */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); _tick_n(&now, 100, false); - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); _tick_n(&now, 100, false); - _tick_n(&now, BTN_DEBOUNCE_MS + BTN_LONG_MIN_MS + 50, true); - hw_button_inject(false); - hw_btn_event_t ev = BTN_EVENT_NONE; - for (int i = 0; i < 5; i++) { - hw_btn_event_t e = hw_button_tick(now++); - if (e != BTN_EVENT_NONE) ev = e; - } - ASSERT(ev == BTN_EVENT_REARM_COMBO, "first combo fires"); - - /* After combo, a lone short press should produce PARK */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); - _tick_n(&now, 1, false); - ev = BTN_EVENT_NONE; - hw_button_inject(false); - for (uint32_t i = 0; i < BTN_COMMIT_MS + 10; i++) { - hw_btn_event_t e = hw_button_tick(now++); - if (e != BTN_EVENT_NONE) ev = e; - } - ASSERT(ev == BTN_EVENT_PARK, "after combo: short press fires PARK"); -} - -static void test_seq_timeout_clears_buffer(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* TWO short presses — seq_len=2 (no PARK since it's not a lone SHORT). - * Then wait BTN_SEQ_TIMEOUT_MS — buffer should be abandoned silently. */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); - _tick_n(&now, 1, false); - _tick_n(&now, 100, false); /* gap between presses (< BTN_COMMIT_MS) */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); - _tick_n(&now, 1, false); - - /* Wait BTN_SEQ_TIMEOUT_MS — seq_len=2 buffer expires silently */ - hw_btn_event_t ev = _tick_n(&now, BTN_SEQ_TIMEOUT_MS + 10, false); - ASSERT(ev == BTN_EVENT_NONE, "seq timeout (seq_len=2): buffer cleared, no event"); - - /* Fresh lone short press should produce PARK as a new sequence */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); - _tick_n(&now, 1, false); - ev = BTN_EVENT_NONE; - hw_button_inject(false); - for (uint32_t i = 0; i < BTN_COMMIT_MS + 10; i++) { - hw_btn_event_t e = hw_button_tick(now++); - if (e != BTN_EVENT_NONE) ev = e; - } - ASSERT(ev == BTN_EVENT_PARK, "after timeout: fresh short press fires PARK"); -} - -static void test_is_pressed_tracks_held(void) -{ - uint32_t now = 0; - hw_button_init(); - - ASSERT(!hw_button_is_pressed(), "initially not pressed"); - _tick_n(&now, BTN_DEBOUNCE_MS + 1, true); - ASSERT(hw_button_is_pressed(), "after debounce: is_pressed true"); - _tick_n(&now, 1, false); - ASSERT(!hw_button_is_pressed(), "after release: is_pressed false"); -} - -static void test_short_press_before_commit_no_park(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* Press and release */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); - _tick_n(&now, 1, false); - - /* Wait less than BTN_COMMIT_MS — no PARK yet */ - hw_btn_event_t ev = BTN_EVENT_NONE; - hw_button_inject(false); - for (uint32_t i = 0; i < BTN_COMMIT_MS - 50; i++) { - hw_btn_event_t e = hw_button_tick(now++); - if (e != BTN_EVENT_NONE) ev = e; - } - ASSERT(ev == BTN_EVENT_NONE, "before commit window: no PARK yet"); -} - -static void test_two_shorts_no_park(void) -{ - uint32_t now = 0; - hw_button_init(); - - /* Two short presses close together — seq_len=2 does not trigger PARK */ - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); _tick_n(&now, 1, false); - _tick_n(&now, 100, false); - _tick_n(&now, BTN_DEBOUNCE_MS + 10, true); _tick_n(&now, 1, false); - - hw_btn_event_t ev = BTN_EVENT_NONE; - hw_button_inject(false); - for (uint32_t i = 0; i < BTN_COMMIT_MS + 10; i++) { - hw_btn_event_t e = hw_button_tick(now++); - if (e != BTN_EVENT_NONE) ev = e; - } - ASSERT(ev == BTN_EVENT_NONE, "two shorts + quiet: no PARK (seq_len=2, not lone SHORT)"); -} - -/* ---- main ---- */ -int main(void) -{ - printf("=== test_hw_button ===\n"); - - test_idle_no_event(); - test_debounce_bounce_ignored(); - test_debounce_confirmed(); - test_short_press_park(); - test_long_press_no_park(); - test_rearm_combo_fires(); - test_rearm_combo_resets_buffer(); - test_seq_timeout_clears_buffer(); - test_is_pressed_tracks_held(); - test_short_press_before_commit_no_park(); - test_two_shorts_no_park(); - - printf("\nResults: %d passed, %d failed\n", g_pass, g_fail); - return g_fail ? 1 : 0; -} diff --git a/legacy/stm32/test/test_pid_schedule.c b/legacy/stm32/test/test_pid_schedule.c deleted file mode 100644 index d54d258..0000000 --- a/legacy/stm32/test/test_pid_schedule.c +++ /dev/null @@ -1,441 +0,0 @@ -/* - * test_pid_schedule.c -- host-side unit tests for pid_schedule (Issue #550) - * - * Build: - * gcc -I /tmp/stub_hal -I include -DTEST_HOST -lm \ - * -o test_pid_schedule test/test_pid_schedule.c - * - * Run: - * ./test_pid_schedule - */ - -/* ---- Minimal HAL stub (no hardware) ---- */ -#ifndef STM32F7XX_HAL_H -#define STM32F7XX_HAL_H -#include -#include -typedef enum { HAL_OK = 0 } HAL_StatusTypeDef; -typedef struct { uint32_t TypeErase; uint32_t Sector; uint32_t NbSectors; uint32_t VoltageRange; } FLASH_EraseInitTypeDef; -#define FLASH_TYPEERASE_SECTORS 0 -#define FLASH_SECTOR_7 7 -#define VOLTAGE_RANGE_3 3 -#define FLASH_TYPEPROGRAM_WORD 0 -static inline HAL_StatusTypeDef HAL_FLASH_Unlock(void) { return HAL_OK; } -static inline HAL_StatusTypeDef HAL_FLASH_Lock(void) { return HAL_OK; } -static inline HAL_StatusTypeDef HAL_FLASHEx_Erase(FLASH_EraseInitTypeDef *e, uint32_t *err) { (void)e; *err = 0xFFFFFFFFUL; return HAL_OK; } -static inline HAL_StatusTypeDef HAL_FLASH_Program(uint32_t t, uint32_t addr, uint64_t data) { (void)t; (void)addr; (void)data; return HAL_OK; } -static inline uint32_t HAL_GetTick(void) { return 0; } -#endif - -/* ---- Block flash/jlink/balance headers (not needed) ---- */ -/* pid_flash.h is included via pid_schedule.h -- stub flash functions */ - -/* Forward-declare stubs for pid_flash functions (used by pid_schedule.c) */ -#include -#include -#include -#include -#include - -/* Minimal pid_sched_entry_t and pid_flash stubs before pulling in schedule */ -#define PID_FLASH_H /* prevent pid_flash.h from being re-included */ - -/* Replicate types from pid_flash.h */ -#define PID_SCHED_MAX_BANDS 6u -#define PID_SCHED_FLASH_ADDR 0x0807FF40UL -#define PID_SCHED_MAGIC 0x534C5402UL -#define PID_FLASH_STORE_ADDR 0x0807FFC0UL -#define PID_FLASH_MAGIC 0x534C5401UL -#define PID_FLASH_SECTOR 7 -#define PID_FLASH_SECTOR_VOLTAGE 3 - -typedef struct __attribute__((packed)) { - float speed_mps; - float kp; - float ki; - float kd; -} pid_sched_entry_t; - -typedef struct __attribute__((packed)) { - uint32_t magic; - uint8_t num_bands; - uint8_t flags; - uint8_t _pad0[2]; - pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; - uint8_t _pad1[24]; -} pid_sched_flash_t; - -typedef struct __attribute__((packed)) { - uint32_t magic; - float kp; - float ki; - float kd; - uint8_t _pad[48]; -} pid_flash_t; - -/* Stub flash storage (simulated in RAM) */ -static pid_sched_flash_t g_sched_flash; -static pid_flash_t g_pid_flash; -static bool g_sched_flash_valid = false; -static bool g_pid_flash_valid = false; - -bool pid_flash_load(float *kp, float *ki, float *kd) -{ - if (!g_pid_flash_valid || g_pid_flash.magic != PID_FLASH_MAGIC) return false; - *kp = g_pid_flash.kp; *ki = g_pid_flash.ki; *kd = g_pid_flash.kd; - return true; -} - -bool pid_flash_save(float kp, float ki, float kd) -{ - g_pid_flash.magic = PID_FLASH_MAGIC; - g_pid_flash.kp = kp; g_pid_flash.ki = ki; g_pid_flash.kd = kd; - g_pid_flash_valid = true; - return true; -} - -bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n) -{ - if (!g_sched_flash_valid || g_sched_flash.magic != PID_SCHED_MAGIC) return false; - if (g_sched_flash.num_bands == 0 || g_sched_flash.num_bands > PID_SCHED_MAX_BANDS) return false; - memcpy(out_entries, g_sched_flash.bands, g_sched_flash.num_bands * sizeof(pid_sched_entry_t)); - *out_n = g_sched_flash.num_bands; - return true; -} - -bool pid_flash_save_all(float kp_s, float ki_s, float kd_s, - const pid_sched_entry_t *entries, uint8_t num_bands) -{ - if (num_bands == 0 || num_bands > PID_SCHED_MAX_BANDS) return false; - g_sched_flash.magic = PID_SCHED_MAGIC; - g_sched_flash.num_bands = num_bands; - memcpy(g_sched_flash.bands, entries, num_bands * sizeof(pid_sched_entry_t)); - g_sched_flash_valid = true; - g_pid_flash.magic = PID_FLASH_MAGIC; - g_pid_flash.kp = kp_s; g_pid_flash.ki = ki_s; g_pid_flash.kd = kd_s; - g_pid_flash_valid = true; - return true; -} - -/* Stub mpu6000.h and balance.h so pid_schedule.h doesn't pull in hardware types */ -#define MPU6000_H -typedef struct { float ax, ay, az, gx, gy, gz, pitch, pitch_rate; } IMUData; - -#define BALANCE_H -typedef enum { BALANCE_DISARMED=0, BALANCE_ARMED, BALANCE_TILT_FAULT } balance_state_t; -typedef struct { - balance_state_t state; - float pitch_deg, pitch_rate; - float integral, prev_error; - int16_t motor_cmd; - float kp, ki, kd, setpoint, max_tilt; - int16_t max_speed; -} balance_t; - -/* Include the implementation directly */ -#include "../src/pid_schedule.c" - -/* ============================================================ - * Test framework - * ============================================================ */ -static int g_pass = 0, g_fail = 0; - -#define ASSERT(cond, msg) do { \ - if (cond) { g_pass++; } \ - else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \ -} while (0) - -#define ASSERT_NEAR(a, b, eps, msg) ASSERT(fabsf((a)-(b)) < (eps), msg) - -static void reset_flash(void) -{ - g_sched_flash_valid = false; - g_pid_flash_valid = false; - memset(&g_sched_flash, 0xFF, sizeof(g_sched_flash)); - memset(&g_pid_flash, 0xFF, sizeof(g_pid_flash)); -} - -/* ============================================================ - * Tests - * ============================================================ */ - -static void test_init_loads_default_when_flash_empty(void) -{ - reset_flash(); - pid_schedule_init(); - ASSERT(pid_schedule_get_num_bands() == 3u, "default 3 bands"); - pid_sched_entry_t tbl[PID_SCHED_MAX_BANDS]; - uint8_t n; - pid_schedule_get_table(tbl, &n); - ASSERT(n == 3u, "get_table returns 3"); - ASSERT_NEAR(tbl[0].speed_mps, 0.00f, 1e-5f, "band0 speed=0.00"); - ASSERT_NEAR(tbl[0].kp, 40.0f, 1e-4f, "band0 kp=40"); - ASSERT_NEAR(tbl[2].speed_mps, 0.80f, 1e-5f, "band2 speed=0.80"); - ASSERT_NEAR(tbl[2].kp, 28.0f, 1e-4f, "band2 kp=28"); -} - -static void test_init_loads_from_flash_when_valid(void) -{ - reset_flash(); - pid_sched_entry_t entries[2] = { - { .speed_mps = 0.0f, .kp = 10.0f, .ki = 0.5f, .kd = 0.2f }, - { .speed_mps = 1.0f, .kp = 20.0f, .ki = 0.8f, .kd = 0.4f }, - }; - pid_flash_save_all(1.0f, 0.1f, 0.1f, entries, 2u); - pid_schedule_init(); - ASSERT(pid_schedule_get_num_bands() == 2u, "init loads 2 bands from flash"); - pid_sched_entry_t tbl[PID_SCHED_MAX_BANDS]; - uint8_t n; - pid_schedule_get_table(tbl, &n); - ASSERT_NEAR(tbl[1].kp, 20.0f, 1e-4f, "flash band1 kp=20"); -} - -static void test_get_gains_below_first_band(void) -{ - reset_flash(); - pid_schedule_init(); /* default table: 0.0, 0.3, 0.8 m/s */ - float kp, ki, kd; - pid_schedule_get_gains(0.0f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 40.0f, 1e-4f, "speed=0 -> kp=40 (clamp low)"); - /* abs(-0.1)=0.1 m/s: between band0(0.0) and band1(0.3), t=1/3 -> kp=40+(35-40)/3 */ - pid_schedule_get_gains(-0.1f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 40.0f + (35.0f - 40.0f) * (0.1f / 0.3f), 0.01f, - "speed=-0.1 interpolates via abs(speed)"); -} - -static void test_get_gains_above_last_band(void) -{ - reset_flash(); - pid_schedule_init(); - float kp, ki, kd; - pid_schedule_get_gains(2.0f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 28.0f, 1e-4f, "speed=2.0 -> kp=28 (clamp high)"); -} - -static void test_get_gains_at_band_boundary(void) -{ - reset_flash(); - pid_schedule_init(); - float kp, ki, kd; - pid_schedule_get_gains(0.30f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 35.0f, 1e-4f, "speed=0.30 exactly -> kp=35"); - pid_schedule_get_gains(0.80f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 28.0f, 1e-4f, "speed=0.80 exactly -> kp=28"); -} - -static void test_interpolation_midpoint(void) -{ - reset_flash(); - pid_schedule_init(); - /* Between band0 (0.0,kp=40) and band1 (0.3,kp=35): at t=0.5 -> kp=37.5 */ - float kp, ki, kd; - pid_schedule_get_gains(0.15f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 37.5f, 0.01f, "interp midpoint kp=37.5"); - /* Between band1 (0.3,kp=35) and band2 (0.8,kp=28): at t=0.2 -> 35+(28-35)*0.2=33.6 */ - pid_schedule_get_gains(0.40f, &kp, &ki, &kd); - float expected = 35.0f + (28.0f - 35.0f) * ((0.40f - 0.30f) / (0.80f - 0.30f)); - ASSERT_NEAR(kp, expected, 0.01f, "interp band1->2 kp"); -} - -static void test_interpolation_ki_kd(void) -{ - reset_flash(); - pid_schedule_init(); - float kp, ki, kd; - pid_schedule_get_gains(0.15f, &kp, &ki, &kd); - /* ki: band0=1.5, band1=1.0, t=0.5 -> 1.25 */ - ASSERT_NEAR(ki, 1.25f, 0.01f, "interp midpoint ki=1.25"); - /* kd: band0=1.2, band1=1.0, t=0.5 -> 1.1 */ - ASSERT_NEAR(kd, 1.1f, 0.01f, "interp midpoint kd=1.1"); -} - -static void test_set_table_and_sort(void) -{ - pid_sched_entry_t tbl[3] = { - { .speed_mps = 0.8f, .kp = 5.0f, .ki = 0.1f, .kd = 0.1f }, - { .speed_mps = 0.0f, .kp = 9.0f, .ki = 0.3f, .kd = 0.3f }, - { .speed_mps = 0.4f, .kp = 7.0f, .ki = 0.2f, .kd = 0.2f }, - }; - pid_schedule_set_table(tbl, 3u); - ASSERT(pid_schedule_get_num_bands() == 3u, "set_table 3 bands"); - pid_sched_entry_t out[PID_SCHED_MAX_BANDS]; - uint8_t n; - pid_schedule_get_table(out, &n); - /* After sort: 0.0, 0.4, 0.8 */ - ASSERT_NEAR(out[0].speed_mps, 0.0f, 1e-5f, "sorted[0]=0.0"); - ASSERT_NEAR(out[1].speed_mps, 0.4f, 1e-5f, "sorted[1]=0.4"); - ASSERT_NEAR(out[2].speed_mps, 0.8f, 1e-5f, "sorted[2]=0.8"); -} - -static void test_set_table_clamps_n(void) -{ - pid_sched_entry_t big[8]; - memset(big, 0, sizeof(big)); - for (int i = 0; i < 8; i++) big[i].speed_mps = (float)i * 0.1f; - pid_schedule_set_table(big, 8u); - ASSERT(pid_schedule_get_num_bands() == PID_SCHED_MAX_BANDS, "clamp to MAX_BANDS"); -} - -static void test_set_table_min_1(void) -{ - pid_sched_entry_t one = { .speed_mps = 0.5f, .kp = 30.0f, .ki = 1.0f, .kd = 0.8f }; - pid_schedule_set_table(&one, 0u); /* n=0 clamped to 1 */ - ASSERT(pid_schedule_get_num_bands() == 1u, "min 1 band"); -} - -static void test_active_band_idx_clamp_low(void) -{ - reset_flash(); - pid_schedule_init(); - float kp, ki, kd; - pid_schedule_get_gains(0.0f, &kp, &ki, &kd); - ASSERT(pid_schedule_active_band_idx() == 0u, "active=0 when clamped low"); -} - -static void test_active_band_idx_interpolating(void) -{ - reset_flash(); - pid_schedule_init(); - float kp, ki, kd; - pid_schedule_get_gains(0.5f, &kp, &ki, &kd); /* between band1 and band2 */ - ASSERT(pid_schedule_active_band_idx() == 1u, "active=1 between band1-2"); -} - -static void test_active_band_idx_clamp_high(void) -{ - reset_flash(); - pid_schedule_init(); - float kp, ki, kd; - pid_schedule_get_gains(5.0f, &kp, &ki, &kd); - ASSERT(pid_schedule_active_band_idx() == 2u, "active=2 when clamped high"); -} - -static void test_apply_writes_gains(void) -{ - reset_flash(); - pid_schedule_init(); - balance_t b; - memset(&b, 0, sizeof(b)); - pid_schedule_apply(&b, 0.0f); - ASSERT_NEAR(b.kp, 40.0f, 1e-4f, "apply: kp written"); - ASSERT_NEAR(b.ki, 1.5f, 1e-4f, "apply: ki written"); - ASSERT_NEAR(b.kd, 1.2f, 1e-4f, "apply: kd written"); -} - -static void test_apply_resets_integral_on_band_change(void) -{ - reset_flash(); - pid_schedule_init(); - balance_t b; - memset(&b, 0, sizeof(b)); - b.integral = 99.0f; - - /* First call: sets s_prev_band from sentinel -> band 0 (integral reset) */ - pid_schedule_apply(&b, 0.0f); - ASSERT_NEAR(b.integral, 0.0f, 1e-6f, "apply: integral reset on first call"); - - b.integral = 77.0f; - pid_schedule_apply(&b, 0.0f); /* same band -- no reset */ - ASSERT_NEAR(b.integral, 77.0f, 1e-6f, "apply: integral preserved same band"); - - b.integral = 55.0f; - pid_schedule_apply(&b, 0.5f); /* band changes 0->1 -- reset */ - ASSERT_NEAR(b.integral, 0.0f, 1e-6f, "apply: integral reset on band change"); -} - -static void test_flash_save_and_reload(void) -{ - reset_flash(); - pid_sched_entry_t tbl[2] = { - { .speed_mps = 0.0f, .kp = 15.0f, .ki = 0.6f, .kd = 0.3f }, - { .speed_mps = 0.5f, .kp = 10.0f, .ki = 0.4f, .kd = 0.2f }, - }; - pid_schedule_set_table(tbl, 2u); - bool ok = pid_schedule_flash_save(25.0f, 1.1f, 0.9f); - ASSERT(ok, "flash_save returns true"); - ASSERT(g_sched_flash_valid, "flash_save wrote sched record"); - ASSERT(g_pid_flash_valid, "flash_save wrote pid record"); - ASSERT_NEAR(g_pid_flash.kp, 25.0f, 1e-4f, "pid kp saved"); - - /* Now reload */ - pid_schedule_init(); - ASSERT(pid_schedule_get_num_bands() == 2u, "reload 2 bands"); - float kp, ki, kd; - pid_schedule_get_gains(0.0f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 15.0f, 1e-4f, "reload kp at speed=0"); -} - -static void test_get_default_table(void) -{ - pid_sched_entry_t def[PID_SCHED_MAX_BANDS]; - uint8_t n; - pid_schedule_get_default_table(def, &n); - ASSERT(n == 3u, "default table has 3 entries"); - ASSERT_NEAR(def[0].kp, 40.0f, 1e-4f, "default[0] kp=40"); - ASSERT_NEAR(def[1].kp, 35.0f, 1e-4f, "default[1] kp=35"); - ASSERT_NEAR(def[2].kp, 28.0f, 1e-4f, "default[2] kp=28"); -} - -static void test_init_discards_invalid_flash(void) -{ - reset_flash(); - /* Write a valid record but with out-of-range gain */ - pid_sched_entry_t bad[1] = {{ .speed_mps=0.0f, .kp=999.0f, .ki=0.1f, .kd=0.1f }}; - pid_flash_save_all(1.0f, 0.1f, 0.1f, bad, 1u); - pid_schedule_init(); - /* Should fall back to default */ - ASSERT(pid_schedule_get_num_bands() == 3u, "invalid flash -> default 3 bands"); -} - -static void test_single_band_clamps_both_ends(void) -{ - pid_sched_entry_t one = { .speed_mps = 0.5f, .kp = 50.0f, .ki = 2.0f, .kd = 1.5f }; - pid_schedule_set_table(&one, 1u); - float kp, ki, kd; - pid_schedule_get_gains(0.0f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 50.0f, 1e-4f, "single band: clamp low -> kp=50"); - pid_schedule_get_gains(9.9f, &kp, &ki, &kd); - ASSERT_NEAR(kp, 50.0f, 1e-4f, "single band: clamp high -> kp=50"); -} - -static void test_negative_speed_symmetric(void) -{ - reset_flash(); - pid_schedule_init(); - float kp_fwd, ki_fwd, kd_fwd; - float kp_rev, ki_rev, kd_rev; - pid_schedule_get_gains( 0.5f, &kp_fwd, &ki_fwd, &kd_fwd); - pid_schedule_get_gains(-0.5f, &kp_rev, &ki_rev, &kd_rev); - ASSERT_NEAR(kp_fwd, kp_rev, 1e-5f, "symmetric: kp same for +/-speed"); - ASSERT_NEAR(ki_fwd, ki_rev, 1e-5f, "symmetric: ki same for +/-speed"); - ASSERT_NEAR(kd_fwd, kd_rev, 1e-5f, "symmetric: kd same for +/-speed"); -} - -int main(void) -{ - printf("=== test_pid_schedule ===\n"); - - test_init_loads_default_when_flash_empty(); - test_init_loads_from_flash_when_valid(); - test_get_gains_below_first_band(); - test_get_gains_above_last_band(); - test_get_gains_at_band_boundary(); - test_interpolation_midpoint(); - test_interpolation_ki_kd(); - test_set_table_and_sort(); - test_set_table_clamps_n(); - test_set_table_min_1(); - test_active_band_idx_clamp_low(); - test_active_band_idx_interpolating(); - test_active_band_idx_clamp_high(); - test_apply_writes_gains(); - test_apply_resets_integral_on_band_change(); - test_flash_save_and_reload(); - test_get_default_table(); - test_init_discards_invalid_flash(); - test_single_band_clamps_both_ends(); - test_negative_speed_symmetric(); - - printf("PASSED: %d FAILED: %d\n", g_pass, g_fail); - return (g_fail == 0) ? 0 : 1; -} diff --git a/legacy/stm32/test/test_vesc_can.c b/legacy/stm32/test/test_vesc_can.c deleted file mode 100644 index 8557a17..0000000 --- a/legacy/stm32/test/test_vesc_can.c +++ /dev/null @@ -1,518 +0,0 @@ -/* - * test_vesc_can.c — Unit tests for VESC CAN protocol driver (Issue #674). - * - * Build (host, no hardware): - * gcc -I include -I test/stubs -DTEST_HOST -lm \ - * -o /tmp/test_vesc_can test/test_vesc_can.c - * - * All tests are self-contained; no HAL, no CAN peripheral required. - * vesc_can.c calls can_driver_send_ext / can_driver_set_ext_cb and - * jlink_send_vesc_state_tlm — all stubbed below. - */ - -/* ---- Block HAL and board-specific headers ---- */ -/* Must appear before any board include is transitively pulled */ -#define STM32F7XX_HAL_H /* skip stm32f7xx_hal.h */ -#define STM32F722xx /* satisfy any chip guard */ -#define JLINK_H /* skip jlink.h (pid_flash / HAL deps) */ -#define CAN_DRIVER_H /* skip can_driver.h body (we stub functions below) */ - -#include -#include -#include - -/* Minimal HAL types needed by vesc_can.c (none for this module, but keep HAL_OK) */ -#define HAL_OK 0 - -/* ---- Minimal type replicas (must match the real packed structs) ---- */ - -typedef struct __attribute__((packed)) { - int32_t left_rpm; - int32_t right_rpm; - int16_t left_current_x10; - int16_t right_current_x10; - int16_t left_temp_x10; - int16_t right_temp_x10; - int16_t voltage_x10; - uint8_t left_fault; - uint8_t right_fault; - uint8_t left_alive; - uint8_t right_alive; -} jlink_tlm_vesc_state_t; /* 22 bytes */ - -/* ---- Stubs ---- */ - -/* Simulated tick counter */ -static uint32_t g_tick_ms = 0; -uint32_t HAL_GetTick(void) { return g_tick_ms; } - -/* Capture last extended CAN TX */ -static uint32_t g_last_ext_id = 0; -static uint8_t g_last_ext_data[8]; -static uint8_t g_last_ext_len = 0; -static int g_ext_tx_count = 0; - -void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len) -{ - g_last_ext_id = ext_id; - if (len > 8u) len = 8u; - for (uint8_t i = 0; i < len; i++) g_last_ext_data[i] = data[i]; - g_last_ext_len = len; - g_ext_tx_count++; -} - -/* Replicate types from can_driver.h (header is blocked by #define CAN_DRIVER_H) */ -typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len); -typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len); - -/* Capture registered ext callback */ -static can_ext_frame_cb_t g_registered_cb = NULL; -void can_driver_set_ext_cb(can_ext_frame_cb_t cb) { g_registered_cb = cb; } - -/* Capture last TLM sent to JLink */ -static jlink_tlm_vesc_state_t g_last_tlm; -static int g_tlm_count = 0; -void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm) -{ - g_last_tlm = *tlm; - g_tlm_count++; -} - -/* ---- Include implementation directly ---- */ -#include "../src/vesc_can.c" - -/* ---- Test framework ---- */ -#include -#include -#include - -static int g_pass = 0; -static int g_fail = 0; - -#define ASSERT(cond, msg) do { \ - if (cond) { g_pass++; } \ - else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \ -} while(0) - -/* ---- Helpers ---- */ - -static void reset_stubs(void) -{ - g_tick_ms = 0; - g_last_ext_id = 0; - g_last_ext_len = 0; - g_ext_tx_count = 0; - g_tlm_count = 0; - g_registered_cb = NULL; - memset(g_last_ext_data, 0, sizeof(g_last_ext_data)); - memset(&g_last_tlm, 0, sizeof(g_last_tlm)); -} - -/* Build a STATUS frame for vesc_id with given RPM, current_x10, duty_x1000 */ -static void make_status(uint8_t buf[8], int32_t rpm, int16_t cur_x10, int16_t duty) -{ - uint32_t urpm = (uint32_t)rpm; - buf[0] = (uint8_t)(urpm >> 24u); - buf[1] = (uint8_t)(urpm >> 16u); - buf[2] = (uint8_t)(urpm >> 8u); - buf[3] = (uint8_t)(urpm); - buf[4] = (uint8_t)((uint16_t)cur_x10 >> 8u); - buf[5] = (uint8_t)((uint16_t)cur_x10 & 0xFFu); - buf[6] = (uint8_t)((uint16_t)duty >> 8u); - buf[7] = (uint8_t)((uint16_t)duty & 0xFFu); -} - -/* ---- Tests ---- */ - -static void test_init_stores_ids(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - ASSERT(s_id_left == 56u, "init stores left ID"); - ASSERT(s_id_right == 68u, "init stores right ID"); -} - -static void test_init_zeroes_state(void) -{ - reset_stubs(); - /* Dirty the state first */ - s_state[0].rpm = 9999; - s_state[1].rpm = -9999; - vesc_can_init(56u, 68u); - ASSERT(s_state[0].rpm == 0, "init zeroes left RPM"); - ASSERT(s_state[1].rpm == 0, "init zeroes right RPM"); - ASSERT(s_state[0].last_rx_ms == 0u, "init zeroes left last_rx_ms"); -} - -static void test_init_registers_ext_callback(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - ASSERT(g_registered_cb == vesc_can_on_frame, "init registers vesc_can_on_frame as ext_cb"); -} - -static void test_send_rpm_ext_id_left(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_ext_tx_count = 0; - vesc_can_send_rpm(56u, 1000); - /* ext_id = (VESC_PKT_SET_RPM << 8) | vesc_id = (3 << 8) | 56 = 0x0338 */ - ASSERT(g_last_ext_id == 0x0338u, "send_rpm left: correct ext_id"); - ASSERT(g_ext_tx_count == 1, "send_rpm: one TX frame"); - ASSERT(g_last_ext_len == 4u, "send_rpm: DLC=4"); -} - -static void test_send_rpm_ext_id_right(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - vesc_can_send_rpm(68u, 2000); - /* ext_id = (3 << 8) | 68 = 0x0344 */ - ASSERT(g_last_ext_id == 0x0344u, "send_rpm right: correct ext_id"); -} - -static void test_send_rpm_payload_positive(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - vesc_can_send_rpm(56u, 0x01020304); - ASSERT(g_last_ext_data[0] == 0x01u, "send_rpm payload byte0"); - ASSERT(g_last_ext_data[1] == 0x02u, "send_rpm payload byte1"); - ASSERT(g_last_ext_data[2] == 0x03u, "send_rpm payload byte2"); - ASSERT(g_last_ext_data[3] == 0x04u, "send_rpm payload byte3"); -} - -static void test_send_rpm_payload_negative(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - /* -1 as int32 = 0xFFFFFFFF */ - vesc_can_send_rpm(56u, -1); - ASSERT(g_last_ext_data[0] == 0xFFu, "send_rpm -1 byte0"); - ASSERT(g_last_ext_data[1] == 0xFFu, "send_rpm -1 byte1"); - ASSERT(g_last_ext_data[2] == 0xFFu, "send_rpm -1 byte2"); - ASSERT(g_last_ext_data[3] == 0xFFu, "send_rpm -1 byte3"); -} - -static void test_send_rpm_zero(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - vesc_can_send_rpm(56u, 0); - ASSERT(g_last_ext_data[0] == 0u, "send_rpm 0 byte0"); - ASSERT(g_last_ext_data[3] == 0u, "send_rpm 0 byte3"); - ASSERT(g_ext_tx_count == 1, "send_rpm 0: one TX"); -} - -static void test_on_frame_status_rpm(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - uint8_t buf[8]; - make_status(buf, 12345, 150, 500); - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; - vesc_can_on_frame(ext_id, buf, 8u); - ASSERT(s_state[0].rpm == 12345, "on_frame STATUS: RPM parsed"); -} - -static void test_on_frame_status_current(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - uint8_t buf[8]; - make_status(buf, 0, 250, 0); - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; - vesc_can_on_frame(ext_id, buf, 8u); - ASSERT(s_state[0].current_x10 == 250, "on_frame STATUS: current_x10 parsed"); -} - -static void test_on_frame_status_duty(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - uint8_t buf[8]; - make_status(buf, 0, 0, -300); - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; - vesc_can_on_frame(ext_id, buf, 8u); - ASSERT(s_state[0].duty_x1000 == -300, "on_frame STATUS: duty_x1000 parsed"); -} - -static void test_on_frame_status_updates_timestamp(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_tick_ms = 5000u; - uint8_t buf[8]; - make_status(buf, 100, 0, 0); - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; - vesc_can_on_frame(ext_id, buf, 8u); - ASSERT(s_state[0].last_rx_ms == 5000u, "on_frame STATUS: last_rx_ms updated"); -} - -static void test_on_frame_status_right_node(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - uint8_t buf[8]; - make_status(buf, -9999, 0, 0); - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 68u; - vesc_can_on_frame(ext_id, buf, 8u); - ASSERT(s_state[1].rpm == -9999, "on_frame STATUS: right node RPM"); - ASSERT(s_state[0].rpm == 0, "on_frame STATUS: left unaffected"); -} - -static void test_on_frame_status4_temps(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - uint8_t buf[8] = {0x00, 0xF0, 0x01, 0x2C, 0x00, 0x64, 0, 0}; - /* T_fet = 0x00F0 = 240 (24.0°C), T_mot = 0x012C = 300 (30.0°C), I_in = 0x0064 = 100 */ - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS_4 << 8u) | 56u; - vesc_can_on_frame(ext_id, buf, 6u); - ASSERT(s_state[0].temp_fet_x10 == 240, "on_frame STATUS_4: temp_fet_x10"); - ASSERT(s_state[0].temp_motor_x10 == 300, "on_frame STATUS_4: temp_motor_x10"); - ASSERT(s_state[0].current_in_x10 == 100, "on_frame STATUS_4: current_in_x10"); -} - -static void test_on_frame_status5_voltage(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - /* tacho at [0..3], V_in×10 at [4..5] = 0x0100 = 256 (25.6 V) */ - uint8_t buf[8] = {0, 0, 0, 0, 0x01, 0x00, 0, 0}; - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS_5 << 8u) | 56u; - vesc_can_on_frame(ext_id, buf, 6u); - ASSERT(s_state[0].voltage_x10 == 256, "on_frame STATUS_5: voltage_x10"); -} - -static void test_on_frame_unknown_pkt_type_ignored(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - uint8_t buf[8] = {0}; - uint32_t ext_id = (99u << 8u) | 56u; /* unknown pkt type 99 */ - vesc_can_on_frame(ext_id, buf, 8u); - /* No crash, state unmodified (last_rx_ms stays 0) */ - ASSERT(s_state[0].last_rx_ms == 0u, "on_frame: unknown pkt_type ignored"); -} - -static void test_on_frame_unknown_vesc_id_ignored(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - uint8_t buf[8]; - make_status(buf, 9999, 0, 0); - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 99u; /* unknown ID */ - vesc_can_on_frame(ext_id, buf, 8u); - ASSERT(s_state[0].rpm == 0 && s_state[1].rpm == 0, "on_frame: unknown vesc_id ignored"); -} - -static void test_on_frame_short_status_ignored(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - uint8_t buf[8]; - make_status(buf, 1234, 0, 0); - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; - vesc_can_on_frame(ext_id, buf, 7u); /* too short: need 8 */ - ASSERT(s_state[0].rpm == 0, "on_frame STATUS: short frame ignored"); -} - -static void test_get_state_unknown_id_returns_false(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - vesc_state_t out; - bool ok = vesc_can_get_state(99u, &out); - ASSERT(!ok, "get_state: unknown id returns false"); -} - -static void test_get_state_no_frame_returns_false(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - vesc_state_t out; - bool ok = vesc_can_get_state(56u, &out); - ASSERT(!ok, "get_state: no frame yet returns false"); -} - -static void test_get_state_after_status_returns_true(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_tick_ms = 1000u; - uint8_t buf[8]; - make_status(buf, 4321, 88, -100); - uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; - vesc_can_on_frame(ext_id, buf, 8u); - - vesc_state_t out; - bool ok = vesc_can_get_state(56u, &out); - ASSERT(ok, "get_state: returns true after STATUS"); - ASSERT(out.rpm == 4321, "get_state: RPM correct"); - ASSERT(out.current_x10 == 88, "get_state: current_x10 correct"); -} - -static void test_is_alive_no_frame(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - ASSERT(!vesc_can_is_alive(56u, 0u), "is_alive: false with no frame"); -} - -static void test_is_alive_within_timeout(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_tick_ms = 5000u; - uint8_t buf[8]; - make_status(buf, 100, 0, 0); - vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); - - /* Check alive 500 ms later (within 1000 ms timeout) */ - ASSERT(vesc_can_is_alive(56u, 5500u), "is_alive: true within timeout"); -} - -static void test_is_alive_after_timeout(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_tick_ms = 1000u; - uint8_t buf[8]; - make_status(buf, 100, 0, 0); - vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); - - /* Check alive 1001 ms later — exceeds VESC_ALIVE_TIMEOUT_MS (1000 ms) */ - ASSERT(!vesc_can_is_alive(56u, 2001u), "is_alive: false after timeout"); -} - -static void test_is_alive_at_exact_timeout_boundary(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_tick_ms = 1000u; - uint8_t buf[8]; - make_status(buf, 100, 0, 0); - vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); - - /* At exactly VESC_ALIVE_TIMEOUT_MS: delta = 1000, condition is < 1000 → false */ - ASSERT(!vesc_can_is_alive(56u, 2000u), "is_alive: false at exact timeout boundary"); -} - -static void test_send_tlm_rate_limited(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_tlm_count = 0; - - /* First call at t=0 should fire immediately (pre-wound s_tlm_tick) */ - vesc_can_send_tlm(0u); - ASSERT(g_tlm_count == 1, "send_tlm: fires on first call"); - - /* Second call immediately after: should NOT fire (within 1s window) */ - vesc_can_send_tlm(500u); - ASSERT(g_tlm_count == 1, "send_tlm: rate-limited within 1 s"); - - /* After 1000 ms: should fire again */ - vesc_can_send_tlm(1000u); - ASSERT(g_tlm_count == 2, "send_tlm: fires after 1 s"); -} - -static void test_send_tlm_payload_content(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_tick_ms = 100u; - - /* Inject STATUS into left VESC */ - uint8_t buf[8]; - make_status(buf, 5678, 123, 400); - vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); - - /* Inject STATUS into right VESC */ - make_status(buf, -1234, -50, -200); - vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 68u, buf, 8u); - - /* Inject STATUS_4 into left (for temps) */ - uint8_t buf4[8] = {0x00, 0xC8, 0x01, 0x2C, 0x00, 0x64, 0, 0}; - /* T_fet=200, T_mot=300, I_in=100 */ - vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS_4 << 8u) | 56u, buf4, 6u); - - /* Inject STATUS_5 into left (for voltage) */ - uint8_t buf5[8] = {0, 0, 0, 0, 0x01, 0x00, 0, 0}; - /* V_in×10 = 256 (25.6 V) */ - vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS_5 << 8u) | 56u, buf5, 6u); - - vesc_can_send_tlm(0u); - - ASSERT(g_tlm_count == 1, "send_tlm: TLM sent"); - ASSERT(g_last_tlm.left_rpm == 5678, "send_tlm: left_rpm"); - ASSERT(g_last_tlm.right_rpm == -1234, "send_tlm: right_rpm"); - ASSERT(g_last_tlm.left_current_x10 == 123, "send_tlm: left_current_x10"); - ASSERT(g_last_tlm.right_current_x10 == -50, "send_tlm: right_current_x10"); - ASSERT(g_last_tlm.left_temp_x10 == 200, "send_tlm: left_temp_x10"); - ASSERT(g_last_tlm.right_temp_x10 == 0, "send_tlm: right_temp_x10 (no STATUS_4)"); - ASSERT(g_last_tlm.voltage_x10 == 256, "send_tlm: voltage_x10"); -} - -static void test_send_tlm_alive_flags(void) -{ - reset_stubs(); - vesc_can_init(56u, 68u); - g_tick_ms = 1000u; - - /* Only send STATUS for left */ - uint8_t buf[8]; - make_status(buf, 100, 0, 0); - vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); - - /* TLM at t=1100 (100 ms after last frame — within 1000 ms timeout) */ - vesc_can_send_tlm(0u); /* consume pre-wind */ - g_tlm_count = 0; - vesc_can_send_tlm(1100u); /* but only 100ms have passed — still rate-limited */ - - /* Force TLM at t=1001 to bypass rate limit */ - s_tlm_tick = (uint32_t)(-2000u); /* force next call to send */ - vesc_can_send_tlm(1100u); - - ASSERT(g_last_tlm.left_alive == 1u, "send_tlm: left_alive = 1"); - ASSERT(g_last_tlm.right_alive == 0u, "send_tlm: right_alive = 0 (no STATUS)"); -} - -/* ---- main ---- */ - -int main(void) -{ - test_init_stores_ids(); - test_init_zeroes_state(); - test_init_registers_ext_callback(); - test_send_rpm_ext_id_left(); - test_send_rpm_ext_id_right(); - test_send_rpm_payload_positive(); - test_send_rpm_payload_negative(); - test_send_rpm_zero(); - test_on_frame_status_rpm(); - test_on_frame_status_current(); - test_on_frame_status_duty(); - test_on_frame_status_updates_timestamp(); - test_on_frame_status_right_node(); - test_on_frame_status4_temps(); - test_on_frame_status5_voltage(); - test_on_frame_unknown_pkt_type_ignored(); - test_on_frame_unknown_vesc_id_ignored(); - test_on_frame_short_status_ignored(); - test_get_state_unknown_id_returns_false(); - test_get_state_no_frame_returns_false(); - test_get_state_after_status_returns_true(); - test_is_alive_no_frame(); - test_is_alive_within_timeout(); - test_is_alive_after_timeout(); - test_is_alive_at_exact_timeout_boundary(); - test_send_tlm_rate_limited(); - test_send_tlm_payload_content(); - test_send_tlm_alive_flags(); - - printf("\n%d passed, %d failed\n", g_pass, g_fail); - return g_fail ? 1 : 0; -} diff --git a/test/test_ota.py b/test/test_ota.py deleted file mode 100644 index 6da0d57..0000000 --- a/test/test_ota.py +++ /dev/null @@ -1,260 +0,0 @@ -""" -test_ota.py — OTA firmware update utilities (Issue #124) - -Tests: - - CRC-32/ISO-HDLC (crc32_file / binascii.crc32) - - CRC-32/MPEG-2 (stm32_crc32 — legacy CRC utility, archived to legacy/stm32/) - - CRC-16/XMODEM (_crc16_xmodem — JLink frame integrity) - - DFU_ENTER frame (JLINK_CMD_DFU_ENTER = 0x06, no payload) - - Safety constants (BKP index, flash region, magic value) - -NOTE: flash_firmware.py has been archived to legacy/stm32/scripts/flash_firmware.py. -""" - -import binascii -import os -import struct -import sys -import tempfile - -import pytest - -# Add legacy scripts directory to path so we can import flash_firmware.py -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'legacy', 'stm32', 'scripts')) -from flash_firmware import ( - crc32_file, - stm32_crc32, - _crc16_xmodem, - _build_jlink_frame, - FLASH_BASE, - FLASH_SIZE, - DFU_VID, - DFU_PID, -) - - -# ── CRC-32/ISO-HDLC (crc32_file) ────────────────────────────────────────── - -class TestCrc32File: - def test_known_empty(self, tmp_path): - """binascii.crc32 of empty file = 0x00000000.""" - p = tmp_path / "empty.bin" - p.write_bytes(b'') - assert crc32_file(str(p)) == 0x00000000 - - def test_known_sequence(self, tmp_path): - """CRC-32/ISO-HDLC of b'123456789' = 0xCBF43926 (well-known vector).""" - p = tmp_path / "seq.bin" - p.write_bytes(b'123456789') - assert crc32_file(str(p)) == 0xCBF43926 - - def test_deterministic(self, tmp_path): - """Same file produces same result on repeated calls.""" - p = tmp_path / "data.bin" - p.write_bytes(b'\xDE\xAD\xBE\xEF' * 256) - assert crc32_file(str(p)) == crc32_file(str(p)) - - def test_single_bit_flip(self, tmp_path): - """Flipping one bit changes the CRC.""" - p0 = tmp_path / "d0.bin" - p1 = tmp_path / "d1.bin" - p0.write_bytes(b'\x00' * 64) - p1.write_bytes(b'\x01' + b'\x00' * 63) - assert crc32_file(str(p0)) != crc32_file(str(p1)) - - def test_result_is_uint32(self, tmp_path): - """Result fits in 32 bits.""" - p = tmp_path / "rnd.bin" - p.write_bytes(bytes(range(256))) - result = crc32_file(str(p)) - assert 0 <= result <= 0xFFFFFFFF - - -# ── CRC-32/MPEG-2 (stm32_crc32) ─────────────────────────────────────────── - -class TestStm32Crc32: - def test_result_is_uint32(self): - assert 0 <= stm32_crc32(b'\x00\x00\x00\x00') <= 0xFFFFFFFF - - def test_deterministic(self): - data = b'\xDE\xAD\xBE\xEF' * 256 - assert stm32_crc32(data) == stm32_crc32(data) - - def test_avalanche(self): - d0 = b'\x00' * 256 - d1 = b'\x01' + b'\x00' * 255 - assert stm32_crc32(d0) != stm32_crc32(d1) - - def test_differs_from_iso_hdlc(self): - """MPEG-2 and ISO-HDLC produce different results for non-trivial input.""" - data = b'\x01\x02\x03\x04' * 64 - iso = binascii.crc32(data) & 0xFFFFFFFF - mpeg = stm32_crc32(data) - assert iso != mpeg, "CRC algorithms should differ" - - def test_pads_to_4bytes(self): - """Odd-length input padded to 4-byte boundary with 0xFF.""" - assert stm32_crc32(b'\xAA\xBB\xCC') == stm32_crc32(b'\xAA\xBB\xCC\xFF') - - def test_full_flash_consistent(self): - """All-0xFF 512 KB (erased flash) produces a consistent result.""" - data = b'\xFF' * FLASH_SIZE - r1 = stm32_crc32(data) - r2 = stm32_crc32(data) - assert r1 == r2 - assert 0 <= r1 <= 0xFFFFFFFF - - def test_512kb_multiple_of_4(self): - """Flash size is a multiple of 4 (no padding needed for full image).""" - assert FLASH_SIZE % 4 == 0 - - def test_different_data_different_crc(self): - """Two distinct 4-byte words produce different CRC.""" - a = stm32_crc32(b'\x00\x00\x00\x00') - b = stm32_crc32(b'\xFF\xFF\xFF\xFF') - assert a != b - - def test_word_endian_sensitivity(self): - """Byte ordering within a 32-bit word affects the result.""" - le_word = struct.pack('I', 0xDEADBEEF) # DE AD BE EF in memory - assert stm32_crc32(le_word) != stm32_crc32(be_word) - - -# ── CRC-16/XMODEM (_crc16_xmodem) ───────────────────────────────────────── - -class TestCrc16Xmodem: - def test_empty(self): - """CRC16 of empty input = 0x0000.""" - assert _crc16_xmodem(b'') == 0x0000 - - def test_known_vector(self): - """CRC-16/XMODEM of b'123456789' = 0x31C3 (well-known vector).""" - assert _crc16_xmodem(b'123456789') == 0x31C3 - - def test_single_heartbeat_byte(self): - """CRC of HEARTBEAT command byte (0x01) is deterministic and non-zero.""" - r = _crc16_xmodem(bytes([0x01])) - assert isinstance(r, int) - assert 0 <= r <= 0xFFFF - assert r != 0 - - def test_dfu_cmd_byte(self): - """CRC of DFU_ENTER command byte (0x06) is within 16-bit range.""" - r = _crc16_xmodem(bytes([0x06])) - assert 0 <= r <= 0xFFFF - - def test_deterministic(self): - data = b'\xCA\xFE\xBA\xBE' - assert _crc16_xmodem(data) == _crc16_xmodem(data) - - def test_avalanche(self): - assert _crc16_xmodem(b'\x00') != _crc16_xmodem(b'\x01') - - def test_two_byte_differs_from_one_byte(self): - """CRC of two-byte input differs from single-byte input.""" - assert _crc16_xmodem(bytes([0x06, 0x00])) != _crc16_xmodem(bytes([0x06])) - - -# ── DFU_ENTER frame structure ────────────────────────────────────────────── - -JLINK_CMD_DFU_ENTER = 0x06 -JLINK_CMD_HEARTBEAT = 0x01 -JLINK_CMD_ESTOP = 0x07 - -class TestDfuEnterFrame: - def test_cmd_id(self): - """JLINK_CMD_DFU_ENTER is 0x06 (between PID_SET=0x05 and ESTOP=0x07).""" - assert JLINK_CMD_DFU_ENTER == 0x06 - - def test_cmd_id_between_pid_and_estop(self): - assert JLINK_CMD_DFU_ENTER > 0x05 # > PID_SET - assert JLINK_CMD_DFU_ENTER < JLINK_CMD_ESTOP - - def test_frame_length(self): - """DFU_ENTER frame = 6 bytes: STX LEN CMD CRC_hi CRC_lo ETX.""" - frame = _build_jlink_frame(JLINK_CMD_DFU_ENTER) - assert len(frame) == 6 - - def test_frame_stx(self): - frame = _build_jlink_frame(JLINK_CMD_DFU_ENTER) - assert frame[0] == 0x02 - - def test_frame_etx(self): - frame = _build_jlink_frame(JLINK_CMD_DFU_ENTER) - assert frame[-1] == 0x03 - - def test_frame_len_byte(self): - """LEN byte = 1 (CMD only, no payload).""" - frame = _build_jlink_frame(JLINK_CMD_DFU_ENTER) - assert frame[1] == 1 - - def test_frame_cmd_byte(self): - frame = _build_jlink_frame(JLINK_CMD_DFU_ENTER) - assert frame[2] == JLINK_CMD_DFU_ENTER - - def test_frame_crc_valid(self): - """Embedded CRC validates against CMD byte.""" - frame = _build_jlink_frame(JLINK_CMD_DFU_ENTER) - cmd_byte = frame[2] - crc_hi = frame[3] - crc_lo = frame[4] - expected = _crc16_xmodem(bytes([cmd_byte])) - assert (crc_hi << 8 | crc_lo) == expected - - def test_frame_differs_from_heartbeat(self): - """DFU_ENTER frame is distinct from HEARTBEAT frame.""" - assert (_build_jlink_frame(JLINK_CMD_DFU_ENTER) != - _build_jlink_frame(JLINK_CMD_HEARTBEAT)) - - def test_frame_differs_from_estop(self): - assert (_build_jlink_frame(JLINK_CMD_DFU_ENTER) != - _build_jlink_frame(JLINK_CMD_ESTOP)) - - def test_no_payload(self): - """DFU_ENTER frame has no payload bytes between CMD and CRC.""" - frame = _build_jlink_frame(JLINK_CMD_DFU_ENTER) - # Layout: STX LEN CMD [no payload here] CRC_hi CRC_lo ETX - assert len(frame) == 6 # STX(1)+LEN(1)+CMD(1)+CRC(2)+ETX(1) - - -# ── Safety / constants ───────────────────────────────────────────────────── - -class TestOtaConstants: - def test_dfu_magic(self): - """DFU magic = 0xDEADBEEF (Betaflight-proven pattern).""" - OTA_DFU_MAGIC = 0xDEADBEEF - assert OTA_DFU_MAGIC == 0xDEADBEEF - - def test_dfu_magic_fits_uint32(self): - assert 0 <= 0xDEADBEEF <= 0xFFFFFFFF - - def test_bkp_idx_avoids_bno055(self): - """BKP15 does not overlap BNO055 range BKP0–BKP6 (PR #150).""" - OTA_DFU_BKP_IDX = 15 - BNO055_BKP_RANGE = range(0, 7) - assert OTA_DFU_BKP_IDX not in BNO055_BKP_RANGE - - def test_bkp_idx_valid_esp32s3(self): - """ESP32-S3 has 32 backup registers (BKP0R–BKP31R).""" OTA_DFU_BKP_IDX = 15 - assert 0 <= OTA_DFU_BKP_IDX <= 31 - - def test_flash_base(self): - assert FLASH_BASE == 0x08000000 - - def test_flash_size_512k(self): - assert FLASH_SIZE == 512 * 1024 - - def test_flash_size_word_aligned(self): - assert FLASH_SIZE % 4 == 0 - - def test_dfu_vid_stmicro(self): - """Default VID = 0x0483 (STMicroelectronics).""" - assert DFU_VID == 0x0483 - - def test_dfu_pid_dfu_mode(self): - """Default PID = 0xDF11 (ESP32-S3 DFU mode).""" assert DFU_PID == 0xDF11 - - def test_bkp_idx_not_zero(self): - """BKP15 ≠ 0 — the old request_bootloader() used BKP0R.""" - assert 15 != 0 -- 2.47.2