diff --git a/AUTONOMOUS_ARMING.md b/AUTONOMOUS_ARMING.md index d8b1c42..e0a3311 100644 --- a/AUTONOMOUS_ARMING.md +++ b/AUTONOMOUS_ARMING.md @@ -7,7 +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) -- Sent via USB CDC to the STM32 firmware +- Sent via USB CDC 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 @@ -42,7 +42,7 @@ The robot can now be armed and operated autonomously from the Jetson without req ## Command Protocol -### From Jetson to STM32 (USB CDC) +### From Jetson to ESP32-S3 (USB CDC) ``` A — Request arm (triggers safety hold, then motors enable) D — Request disarm (immediate motor stop) @@ -52,7 +52,7 @@ H — Heartbeat (refresh timeout timer, every 500ms) C, — Drive command: speed, steer (also refreshes heartbeat) ``` -### From STM32 to Jetson (USB CDC) +### From ESP32-S3 to Jetson (USB CDC) 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 e390876..240404c 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -1,12 +1,12 @@ # SaltyLab Firmware — Agent Playbook ## Project -Self-balancing two-wheeled robot: STM32F722 flight controller, hoverboard hub motors, Jetson Nano for AI/SLAM. +Self-balancing two-wheeled robot: Orin Nano Super, 2x VESC (IDs 68 left / 56 right), ESP32-S3 BALANCE, ESP32-S3 IO. ## Team | Agent | Role | Focus | |-------|------|-------| -| **sl-firmware** | Embedded Firmware Lead | STM32 HAL, USB CDC debugging, SPI/UART, PlatformIO, DFU bootloader | +| **sl-firmware** | Embedded Firmware Lead | ESP32-S3/ESP-IDF, USB CDC debugging, SPI/UART, Arduino/ESP-IDF, esptool.py | | **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems | | **sl-perception** | Perception / SLAM Engineer | Jetson Nano, RealSense D435i, RPLIDAR, ROS2, Nav2 | diff --git a/TEAM.md b/TEAM.md index daafce0..24a5f4a 100644 --- a/TEAM.md +++ b/TEAM.md @@ -1,7 +1,7 @@ # SaltyLab — Ideal Team ## Project -Self-balancing two-wheeled robot using a drone flight controller (STM32F722), hoverboard hub motors, and eventually a Jetson Nano for AI/SLAM. +Self-balancing two-wheeled robot using a drone flight controller (ESP32-S3), hoverboard hub motors, and eventually a Jetson Nano for AI/SLAM. ## Current Status - **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand @@ -14,10 +14,10 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho ### 1. Embedded Firmware Engineer (Lead) **Must-have:** -- Deep STM32 HAL experience (F7 series specifically) +- Deep ESP32-S3 HAL experience (F7 series specifically) - USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts) -- SPI + UART + USB coexistence on STM32 -- PlatformIO or bare-metal STM32 toolchain +- SPI + UART + USB coexistence on ESP32-S3 +- PlatformIO or bare-metal ESP32-S3 toolchain - DFU bootloader implementation **Nice-to-have:** @@ -25,7 +25,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho - PID control loop tuning for balance robots - FOC motor control (hoverboard ESC protocol) -**Why:** The immediate blocker is a USB peripheral conflict. Need someone who's debugged STM32 USB issues before — this is not a software logic bug, it's a hardware peripheral interaction issue. +**Why:** The immediate blocker is a USB peripheral conflict. Need someone who's debugged ESP32-S3 USB issues before — this is not a software logic bug, it's a hardware peripheral interaction issue. ### 2. Control Systems / Robotics Engineer **Must-have:** @@ -61,7 +61,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho ## Hardware Reference | Component | Details | |-----------|---------| -| FC | MAMBA F722S (STM32F722RET6, MPU6000) | +| FC | ESP32-S3 BALANCE (ESP32-S3, ICM-42688-P) | | Motors | 2x 8" pneumatic hoverboard hub motors | | ESC | Hoverboard ESC (EFeru FOC firmware) | | Battery | 36V pack | diff --git a/USB_CDC_BUG.md b/USB_CDC_BUG.md index 9a810b2..9bd8c79 100644 --- a/USB_CDC_BUG.md +++ b/USB_CDC_BUG.md @@ -127,7 +127,7 @@ loop — USB would never enumerate cleanly. | LED2 | PC15 | GPIO | | Buzzer | PB2 | GPIO/TIM4_CH3 | -MCU: STM32F722RET6 (MAMBA F722S FC, Betaflight target DIAT-MAMBAF722_2022B) +MCU: ESP32-S3 (ESP32-S3 BALANCE FC) --- diff --git a/chassis/ASSEMBLY.md b/chassis/ASSEMBLY.md index e7f10bc..3473efe 100644 --- a/chassis/ASSEMBLY.md +++ b/chassis/ASSEMBLY.md @@ -56,7 +56,7 @@ 3. Fasten 4× M4×12 SHCS. Torque 2.5 N·m. 4. Insert battery pack; route Velcro straps through slots and cinch. -### 7 FC mount (MAMBA F722S) +### 7 FC mount (ESP32-S3 BALANCE) 1. Place silicone anti-vibration grommets onto nylon M3 standoffs. 2. Lower FC onto standoffs; secure with M3×6 BHCS. Snug only — do not over-torque. 3. Orient USB-C port toward front of robot for cable access. diff --git a/chassis/BOM.md b/chassis/BOM.md index 0a92d8f..35480a3 100644 --- a/chassis/BOM.md +++ b/chassis/BOM.md @@ -41,7 +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 | -| 6 | FC standoff M3×6mm nylon | 4 | Nylon | — | MAMBA F722S vibration isolation | +| 6 | BALANCE board 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 @@ -90,7 +90,7 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th | # | Part | Qty | Spec | Notes | |---|------|-----|------|-------| -| 13 | STM32 MAMBA F722S FC | 1 | 36×36mm PCB, 30.5×30.5mm M3 mount | Oriented USB-C port toward front | +| 13 | 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 Nano B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern | diff --git a/chassis/ip54_BOM.md b/chassis/ip54_BOM.md index 96b7103..c24f4a8 100644 --- a/chassis/ip54_BOM.md +++ b/chassis/ip54_BOM.md @@ -104,7 +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 | -| FC (MAMBA F722S) | 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 | | ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C | | D435i | Passive; housing vent gap on rear cap | 45 °C surface | — | diff --git a/docs/AGENTS.md b/docs/AGENTS.md index f3ac992..5043599 100644 --- a/docs/AGENTS.md +++ b/docs/AGENTS.md @@ -5,17 +5,18 @@ You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read ## Project Overview A hoverboard-based balancing robot with two compute layers: -1. **FC (Flight Controller)** — MAMBA F722S (STM32F722RET6 + 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 Nano** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to FC via UART. Not safety-critical — FC operates independently. +1. **ESP32-S3 BALANCE** — runs the PID balance loop. Safety-critical, operates independently of the Orin. +2. **ESP32-S3 IO** — handles I/O: motor commands to VESCs, sensor polling, CAN/UART comms. +3. **Orin Nano Super** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to ESP32-S3 BALANCE via UART. Not safety-critical. ``` -Jetson (speed+steer via UART1) ←→ ELRS RC (UART3, kill switch) +Orin Nano Super (speed+steer via UART) ←→ ELRS RC (kill switch) │ ▼ - MAMBA F722S (MPU6000 IMU, PID balance) + ESP32-S3 BALANCE (IMU, PID balance loop) │ - ▼ UART2 - Hoverboard ESC (FOC) → 2× 8" hub motors + ▼ CAN / UART + ESP32-S3 IO → VESC 68 (left) + VESC 56 (right) ``` ## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT @@ -26,7 +27,7 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an 2. **Tilt cutoff at ±25°** — motors to zero, require manual re-arm. No retry, no recovery. 3. **Hardware watchdog (50ms)** — if firmware hangs, motors cut. 4. **RC kill switch** — dedicated ELRS channel, checked every loop iteration. Always overrides. -5. **Jetson UART timeout (200ms)** — if Jetson disconnects, motors cut. +5. **Orin UART timeout (200ms)** — if Orin disconnects, motors cut. 6. **Speed hard cap** — firmware limit, start at 10%. Increase only after proven stable. 7. **Never test untethered** until PID is stable for 5+ minutes on a tether. @@ -35,31 +36,16 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an ## Repository Layout ``` -firmware/ # STM32 HAL firmware (PlatformIO) -├── src/ -│ ├── main.c # Entry point, clock config, main loop -│ ├── icm42688.c # ICM-42688-P SPI driver (backup IMU — currently broken) -│ ├── bmp280.c # Barometer driver (disabled) -│ └── status.c # LED + buzzer status patterns -├── include/ -│ ├── config.h # Pin definitions, constants -│ ├── icm42688.h -│ ├── mpu6000.h # MPU6000 driver header (primary IMU) -│ ├── hoverboard.h # Hoverboard ESC UART protocol -│ ├── crsf.h # ELRS CRSF protocol -│ ├── bmp280.h -│ └── status.h -├── lib/USB_CDC/ # USB CDC stack (serial over USB) -│ ├── src/ # CDC implementation, USB descriptors, PCD config -│ └── include/ -└── platformio.ini # Build config +esp32/ # ESP32-S3 firmware (ESP-IDF) +├── balance/ # ESP32-S3 BALANCE — PID loop, IMU, safety +└── io/ # ESP32-S3 IO — VESC CAN, sensors, comms cad/ # OpenSCAD parametric parts (16 files) ├── dimensions.scad # ALL measurements live here — single source of truth ├── assembly.scad # Full robot assembly visualization ├── motor_mount_plate.scad ├── battery_shelf.scad -├── fc_mount.scad # Vibration-isolated FC mount +├── esp32_balance_mount.scad # Vibration-isolated ESP32-S3 BALANCE mount ├── jetson_shelf.scad ├── esc_mount.scad ├── sensor_tower_top.scad @@ -82,55 +68,55 @@ PLATFORM.md # Hardware platform reference ## Hardware Quick Reference -### MAMBA F722S Flight Controller +### ESP32-S3 BALANCE | Spec | Value | |------|-------| -| MCU | STM32F722RET6 (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 | ICM-42688-P (on same SPI1, CS unknown — currently non-functional) | -| Betaflight Target | DIAT-MAMBAF722_2022B | -| USB | OTG FS (PA11/PA12), enumerates as /dev/cu.usbmodemSALTY0011 | -| VID/PID | 0x0483/0x5740 | -| LEDs | PC15 (LED1), PC14 (LED2), active low | -| Buzzer | PB2 (inverted push-pull) | -| Battery ADC | PC1=VBAT, PC3=CURR (ADC3) | -| DFU | Hold yellow BOOT button + plug USB (or send 'R' over CDC) | +| MCU | ESP32-S3 (dual-core Xtensa LX7, 240MHz, 512KB SRAM, 8MB flash) | +| Primary IMU | MPU6000 (SPI) | +| Role | PID balance loop, tilt cutoff, arming | +| Comms to Orin | UART (velocity commands in, telemetry out) | +| Flash | `idf.py -p /dev/ttyUSB0 flash` | -### UART Assignments +### ESP32-S3 IO -| UART | Pins | Connected To | Baud | -|------|------|-------------|------| -| USART1 | PA9/PA10 | Jetson Nano | 115200 | -| USART2 | PA2/PA3 | Hoverboard ESC | 115200 | -| USART3 | PB10/PB11 | ELRS Receiver | 420000 (CRSF) | -| UART4 | — | Spare | — | -| UART5 | — | Spare | — | +| Spec | Value | +|------|-------| +| MCU | ESP32-S3 | +| Role | VESC CAN driver, sensor polling, peripheral I/O | +| VESC IDs | 68 (left), 56 (right) | +| Motor bus | CAN 1Mbit/s | +| Flash | `idf.py -p /dev/ttyUSB1 flash` | + +### UART Assignments (ESP32-S3 BALANCE) + +| UART | Connected To | Baud | +|------|-------------|------| +| UART0 | Orin Nano Super | 115200 | +| UART1 | ESP32-S3 IO | 115200 | +| UART2 | ELRS Receiver | 420000 (CRSF) | ### Motor/ESC - 2× 8" pneumatic hub motors (36V, hoverboard type) -- Hoverboard ESC with FOC firmware -- UART protocol: `{0xABCD, int16 speed, int16 steer, uint16 checksum}` at 115200 -- Speed range: -1000 to +1000 +- 2× VESC motor controllers (CAN IDs 68, 56) +- VESC CAN protocol: standard SET_DUTY / SET_CURRENT / SET_RPM +- Speed range: -1.0 to +1.0 (duty cycle) ### Physical Dimensions (from `cad/dimensions.scad`) | Part | Key Measurement | |------|----------------| -| FC mounting holes | 25.5mm spacing (NOT standard 30.5mm!) | -| FC board size | ~36mm square | +| ESP32-S3 BALANCE board | ~55×28mm (DevKit form factor) | +| ESP32-S3 IO board | ~55×28mm (DevKit form factor) | | Hub motor body | Ø200mm (~8") | | Motor axle | Ø12mm, 45mm long | -| Jetson Nano | 100×80×29mm, M2.5 holes at 86×58mm | +| Orin Nano Super | 100×79mm, M2.5 holes at 86×58mm | | RealSense D435i | 90×25×25mm, 1/4-20 tripod mount | | RPLIDAR A1 | Ø70×41mm, 4× M2.5 on Ø67mm circle | | Kill switch hole | Ø22mm panel mount | | Battery pack | ~180×80×40mm | -| Hoverboard ESC | ~80×50×15mm | +| VESC (each) | ~70×50×15mm | | 2020 extrusion | 20mm square, M5 center bore | | Frame width | ~350mm (axle to axle) | | Frame height | ~500-550mm total | @@ -147,7 +133,7 @@ PLATFORM.md # Hardware platform reference | sensor_tower_top | ASA | 80% | | lidar_standoff (Ø80×80mm) | ASA | 40% | | realsense_bracket | PETG | 60% | -| fc_mount (vibration isolated) | TPU+PETG | — | +| esp32_balance_mount (vibration isolated) | TPU+PETG | — | | bumper front + rear (350×50×30mm) | TPU | 30% | | handle | PETG | 80% | | kill_switch_mount | PETG | 80% | @@ -159,99 +145,75 @@ 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. -2. **DCache breaks SPI on STM32F7** — 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 CDC needs ReceivePacket() primed in CDC_Init** — without it, the OUT endpoint never starts listening. No data reception. +1. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first. +2. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0. Always use explicit error codes. +3. **USB CDC needs RX primed in init** — without it, the OUT endpoint never starts listening. +4. **Watchdog must be fed every loop iteration** — if balance loop stalls, motors must cut within 50ms. +5. **Never change PID and speed limit in the same test** — one variable at a time. -### DFU Reboot (Betaflight Method) - -The firmware supports reboot-to-DFU via USB command: -1. Send `R` byte over USB CDC -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 -5. If magic found: clears it, remaps system memory, jumps to STM32 bootloader at `0x1FF00000` -6. Board appears as DFU device, ready for `dfu-util` flash - -### Build & Flash +### Build & Flash (ESP32-S3) ```bash -cd firmware/ -python3 -m platformio run # Build -dfu-util -a 0 -s 0x08000000:leave -D .pio/build/f722/firmware.bin # Flash +# Balance board +cd esp32/balance/ +idf.py build && idf.py -p /dev/ttyUSB0 flash monitor + +# IO board +cd esp32/io/ +idf.py build && idf.py -p /dev/ttyUSB1 flash monitor ``` -Dev machine: mbpm4 (seb@192.168.87.40), PlatformIO project at `~/Projects/saltylab-firmware/` - -### Clock Configuration - -``` -HSE 8MHz → PLL (M=8, N=432, P=2, Q=9) → SYSCLK 216MHz -PLLSAI (N=384, P=8) → CLK48 48MHz (USB) -APB1 = HCLK/4 = 54MHz -APB2 = HCLK/2 = 108MHz -Fallback: HSI 16MHz if HSE fails (PLL M=16) -``` +Dev machine: mbpm4 (seb@192.168.87.40) ## Current Status & Known Issues ### Working -- USB CDC serial streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`) -- Clock config with HSE + HSI fallback -- Reboot-to-DFU via USB 'R' command -- LED status patterns (status.c) +- IMU streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`) +- VESC CAN communication (IDs 68, 56) +- LED status patterns - Web UI with WebSerial + Three.js 3D visualization -### Broken / In Progress -- **ICM-42688-P SPI reads return all zeros** — was the original IMU target, but SPI communication completely non-functional despite correct pin config. May be dead silicon. Switched to MPU6000 as primary. -- **MPU6000 driver** — header exists but implementation needs completion -- **PID balance loop** — not yet implemented -- **Hoverboard ESC UART** — protocol defined, driver not written -- **ELRS CRSF receiver** — protocol defined, driver not written -- **Barometer (BMP280)** — I2C init hangs, disabled +### In Progress +- PID balance loop tuning +- ELRS CRSF receiver integration +- Orin UART integration ### TODO (Priority Order) -1. Get MPU6000 streaming accel+gyro data +1. Tune PID balance loop on ESP32-S3 BALANCE 2. Implement complementary filter (pitch angle) -3. Write hoverboard ESC UART driver -4. Write PID balance loop with safety checks -5. Wire ELRS receiver, implement CRSF parser -6. Bench test (ESC disconnected, verify PID output) -7. First tethered balance test at 10% speed -8. Jetson UART integration -9. LED subsystem (ESP32-C3) +3. Wire ELRS receiver, implement CRSF parser +4. Bench test (VESCs disconnected, verify PID output) +5. First tethered balance test at 10% speed +6. Orin UART integration +7. LED subsystem (ESP32-S3 IO) ## Communication Protocols -### Jetson → FC (UART1, 50Hz) +### Orin → ESP32-S3 BALANCE (UART0, 50Hz) ```c struct { uint8_t header=0xAA; int16_t speed; int16_t steer; uint8_t mode; uint8_t checksum; }; // mode: 0=idle, 1=balance, 2=follow, 3=RC ``` -### FC → Hoverboard ESC (UART2, loop rate) -```c -struct { uint16_t start=0xABCD; int16_t speed; int16_t steer; uint16_t checksum; }; -// speed/steer: -1000 to +1000 -``` +### ESP32-S3 IO → VESC (CAN, loop rate) +- Standard VESC CAN protocol (SET_DUTY / SET_CURRENT / SET_RPM) +- Node IDs: VESC 68 (left), VESC 56 (right) -### FC → Jetson Telemetry (UART1 TX, 50Hz) +### ESP32-S3 BALANCE → Orin Telemetry (UART0 TX, 50Hz) ``` T:12.3,P:45,L:100,R:-80,S:3\n // T=tilt°, P=PID output, L/R=motor commands, S=state (0-3) ``` -### FC → USB CDC (50Hz JSON) +### ESP32-S3 → USB (50Hz JSON, debug/tuning) ```json -{"ax":123,"ay":-456,"az":16384,"gx":10,"gy":-5,"gz":3,"t":250,"p":0,"bt":0} -// Raw IMU values (int16), t=temp×10, p=pressure, bt=baro temp +{"ax":123,"ay":-456,"az":16384,"gx":10,"gy":-5,"gz":3,"t":250,"p":0} +// Raw IMU values (int16), t=temp×10, p=PID output ``` -## LED Subsystem (ESP32-C3) +## LED Subsystem (ESP32-S3 IO) -ESP32-C3 eavesdrops on FC→Jetson telemetry (listen-only tap on UART1 TX). No extra FC UART needed. +ESP32-S3 IO eavesdrops on BALANCE→Orin telemetry (listen-only). No extra UART needed. | State | Pattern | Color | |-------|---------|-------| @@ -275,7 +237,7 @@ ESP32-C3 eavesdrops on FC→Jetson telemetry (listen-only tap on UART1 TX). No e 1. **Read SALTYLAB.md fully** before making any design decisions 2. **Never remove safety checks** from firmware — add more if needed 3. **All measurements go in `cad/dimensions.scad`** — single source of truth -4. **Test firmware on bench before any motor test** — ESC disconnected, verify outputs on serial +4. **Test firmware on bench before any motor test** — VESCs disconnected, verify outputs on serial 5. **One variable at a time** — don't change PID and speed limit in the same test 6. **Document what you change** — update this file if you add pins, change protocols, or discover hardware quirks -7. **Ask before wiring changes** — wrong connections can fry the FC ($50+ board) +7. **Ask before wiring changes** — wrong connections can fry an ESP32 or VESC diff --git a/docs/FACE_LCD_ANIMATION.md b/docs/FACE_LCD_ANIMATION.md index c100900..ffd8363 100644 --- a/docs/FACE_LCD_ANIMATION.md +++ b/docs/FACE_LCD_ANIMATION.md @@ -1,6 +1,6 @@ # Face LCD Animation System (Issue #507) -Implements expressive face animations on an STM32 LCD display with 5 core emotions and smooth transitions. +Implements expressive face animations on an ESP32-S3 IO LCD display with 5 core emotions and smooth transitions. ## Features @@ -82,7 +82,7 @@ STATUS → Echo current emotion + idle state - Colors: Monochrome (1-bit) or RGB565 ### Microcontroller -- STM32F7xx (Mamba F722S) +- ESP32-S3 IO - Available UART: USART3 (PB10=TX, PB11=RX) - Clock: 216 MHz diff --git a/docs/SALTYLAB.md b/docs/SALTYLAB.md index 49bc664..6cfe105 100644 --- a/docs/SALTYLAB.md +++ b/docs/SALTYLAB.md @@ -32,7 +32,7 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments. |------|--------| | 2x 8" pneumatic hub motors (36 PSI) | ✅ Have | | 1x hoverboard ESC (FOC firmware) | ✅ Have | -| 1x Drone FC (STM32F745 + MPU-6000) | ✅ Have — balance brain | +| 1x ESP32-S3 BALANCE (ICM-42688-P IMU) | ✅ Have — balance brain | | 1x Jetson Nano + Noctua fan | ✅ Have | | 1x RealSense D435i | ✅ Have | | 1x RPLIDAR A1M8 | ✅ Have | @@ -50,13 +50,13 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments. | 1x ELRS receiver (matching) | ✅ Have — mounts on FC UART | ### Drone FC Details — GEPRC GEP-F7 AIO -- **MCU:** STM32F722RET6 (216MHz Cortex-M7, 512KB flash, 256KB RAM) +- **MCU:** ESP32-S3 (Xtensa LX7 dual-core 240MHz, 512KB SRAM, 8MB flash) - **IMU:** TDK ICM-42688-P (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one! - **Flash:** 8MB Winbond W25Q64 (blackbox, unused) - **OSD:** AT7456E (unused) - **4-in-1 ESC:** Built into AIO board (unused — we use hoverboard ESC) - **DFU mode:** Hold yellow BOOT button while plugging USB -- **Firmware:** Custom balance firmware (PlatformIO + STM32 HAL) +- **Firmware:** Custom balance firmware (PlatformIO + ESP-IDF) - **UART pads (confirmed from silkscreen):** - T1/R1 (bottom) → USART1 (PA9/PA10) → Jetson - T2/R2 (right top) → USART2 (PA2/PA3) → Hoverboard ESC @@ -95,7 +95,7 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments. ## Self-Balancing Control — Custom Firmware on Drone FC ### Why a Drone FC? -The F745 board is just a premium STM32 dev board with a high-quality IMU (MPU-6000) already soldered on, proper voltage regulation, and multiple UARTs broken out. We write a lean custom balance firmware (~50 lines of C). +The ESP32-S3 BALANCE board has an ICM-42688-P IMU, proper voltage regulation, and CAN bus interface. We write a lean custom balance firmware using ESP-IDF. ### Architecture ``` @@ -142,7 +142,7 @@ GND ──→ GND 5V ←── 5V ``` -### Custom Firmware (STM32 C) +### Custom Firmware (ESP32-S3 C/C++) ```c // Core balance loop — runs in timer interrupt @ 1-8kHz @@ -280,8 +280,8 @@ GND ──→ Common ground ``` ### Dev Tools -- **Flashing:** STM32CubeProgrammer via USB (DFU mode) or SWD -- **IDE:** PlatformIO + STM32 HAL, or STM32CubeIDE +- **Flashing:** esptool.py via USB (bootloader mode) +- **IDE:** PlatformIO + ESP-IDF, or Arduino IDE with ESP32 core - **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug) ## Physical Design @@ -375,7 +375,7 @@ GND ──→ Common ground - [ ] Install hardware kill switch inline with 36V battery (NC — press to kill) - [ ] Set up ceiling tether point above test area (rated for >15kg) - [ ] Clear test area: 3m radius, no loose items, shoes on -- [ ] Set up PlatformIO project for STM32F745 (STM32 HAL) +- [ ] Set up PlatformIO project for ESP32-S3 (ESP-IDF) - [ ] Write MPU-6000 SPI driver (read gyro+accel, complementary filter) - [ ] Write PID balance loop with ALL safety checks: - ±25° tilt cutoff → disarm, require manual re-arm diff --git a/docs/wiring-diagram.md b/docs/wiring-diagram.md index 824e365..74c7019 100644 --- a/docs/wiring-diagram.md +++ b/docs/wiring-diagram.md @@ -7,7 +7,7 @@ │ ORIN NANO SUPER │ │ (Top Plate — 25W) │ │ │ -│ USB-C ──── STM32 CDC (/dev/stm32-bridge, 921600 baud) │ +│ USB-C ──── ESP32-S3 IO CDC (/dev/esp32-io, 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) │ @@ -25,7 +25,7 @@ │ 921600 baud │ 921600 baud, 3.3V ▼ ▼ ┌─────────────────────────────────────────────────────────────────────┐ -│ MAMBA F722S (FC) │ +│ ESP32-S3 BALANCE / IO (FC) │ │ (Middle Plate — foam mounted) │ │ │ │ USB-C ──── Orin (CDC serial, primary link) │ @@ -72,7 +72,7 @@ |------|----|-----------|-------| | Orin USB-C port | FC USB-C port | USB cable | Data only, FC powered from 5V bus | -- Device: `/dev/ttyACM0` → symlink `/dev/stm32-bridge` +- Device: `/dev/ttyACM0` → symlink `/dev/esp32-io` - Baud: 921600, 8N1 - Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC) @@ -139,7 +139,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct) | 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` | -## FC UART Summary (MAMBA F722S) +## FC UART Summary (ESP32-S3 IO) | UART | Pins | Baud | Assignment | Notes | |------|------|------|------------|-------| @@ -149,7 +149,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct) | 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 CDC | USB-C | 921600 | Jetson primary | `/dev/stm32-bridge` | +| USB CDC | USB-C | 921600 | Jetson primary | `/dev/esp32-io` | ### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header) @@ -209,7 +209,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct) | Device | Interface | Power Draw | |--------|-----------|------------| -| STM32 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) | +| ESP32-S3 IO (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 | @@ -234,7 +234,7 @@ Orin Nano Super delivers up to 25W — USB peripherals are well within budget. └──────┬───────┘ │ UART4 ┌────────────▼────────────┐ - │ MAMBA F722S │ + │ ESP32-S3 BALANCE │ │ │ │ MPU6000 → Balance PID │ │ CRSF → Mode Manager │ diff --git a/flash.sh b/flash.sh index c6ee599..a76cfb4 100755 --- a/flash.sh +++ b/flash.sh @@ -1,7 +1,7 @@ #!/bin/bash # Flash SaltyLab — auto-reboot to DFU if serial port exists PORT=/dev/cu.usbmodemSALTY0011 -FW=.pio/build/f722/firmware.bin +FW=.pio/build/esp32s3/firmware.bin if [ -e "$PORT" ]; then echo "Sending reboot-to-DFU..." diff --git a/include/baro.h b/include/baro.h index be8c01f..03000c3 100644 --- a/include/baro.h +++ b/include/baro.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef BARO_H #define BARO_H diff --git a/include/battery.h b/include/battery.h index a5c969b..68603a2 100644 --- a/include/battery.h +++ b/include/battery.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef BATTERY_H #define BATTERY_H diff --git a/include/battery_adc.h b/include/battery_adc.h index 01e2b1d..eb92dd4 100644 --- a/include/battery_adc.h +++ b/include/battery_adc.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * battery_adc.h — DMA-based battery voltage/current ADC driver (Issue #533) * diff --git a/include/bno055.h b/include/bno055.h index e0cae23..cf7d77e 100644 --- a/include/bno055.h +++ b/include/bno055.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef BNO055_H #define BNO055_H diff --git a/include/buzzer.h b/include/buzzer.h index 82ce46b..5f7c808 100644 --- a/include/buzzer.h +++ b/include/buzzer.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef BUZZER_H #define BUZZER_H diff --git a/include/config.h b/include/config.h index e63cb36..1063d27 100644 --- a/include/config.h +++ b/include/config.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef CONFIG_H #define CONFIG_H diff --git a/include/face_lcd.h b/include/face_lcd.h index d25e276..2c374e5 100644 --- a/include/face_lcd.h +++ b/include/face_lcd.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * face_lcd.h — STM32 LCD Display Driver for Face Animations * diff --git a/include/fan.h b/include/fan.h index ef3e2fb..4c5561b 100644 --- a/include/fan.h +++ b/include/fan.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef FAN_H #define FAN_H diff --git a/include/fault_handler.h b/include/fault_handler.h index 1266524..83e138d 100644 --- a/include/fault_handler.h +++ b/include/fault_handler.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef FAULT_HANDLER_H #define FAULT_HANDLER_H diff --git a/include/i2c1.h b/include/i2c1.h index c4c7859..3670031 100644 --- a/include/i2c1.h +++ b/include/i2c1.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef I2C1_H #define I2C1_H diff --git a/include/jetson_cmd.h b/include/jetson_cmd.h index 16432f2..2250fa8 100644 --- a/include/jetson_cmd.h +++ b/include/jetson_cmd.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef JETSON_CMD_H #define JETSON_CMD_H diff --git a/include/jetson_uart.h b/include/jetson_uart.h index ed63640..df33323 100644 --- a/include/jetson_uart.h +++ b/include/jetson_uart.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef JETSON_UART_H #define JETSON_UART_H diff --git a/include/jlink.h b/include/jlink.h index d99436c..3daf113 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef JLINK_H #define JLINK_H diff --git a/include/orin_can.h b/include/orin_can.h index aea84b0..7f1954e 100644 --- a/include/orin_can.h +++ b/include/orin_can.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef ORIN_CAN_H #define ORIN_CAN_H diff --git a/include/ota.h b/include/ota.h index 2b3d9a5..5f950ff 100644 --- a/include/ota.h +++ b/include/ota.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef OTA_H #define OTA_H diff --git a/include/pid_flash.h b/include/pid_flash.h index db29718..4c28b61 100644 --- a/include/pid_flash.h +++ b/include/pid_flash.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef PID_FLASH_H #define PID_FLASH_H diff --git a/include/power_mgmt.h b/include/power_mgmt.h index df705f1..f9dc2c0 100644 --- a/include/power_mgmt.h +++ b/include/power_mgmt.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef POWER_MGMT_H #define POWER_MGMT_H diff --git a/include/uart_protocol.h b/include/uart_protocol.h index 5f414d0..c94adea 100644 --- a/include/uart_protocol.h +++ b/include/uart_protocol.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef UART_PROTOCOL_H #define UART_PROTOCOL_H diff --git a/include/ultrasonic.h b/include/ultrasonic.h index 2e1c349..5bd1ed2 100644 --- a/include/ultrasonic.h +++ b/include/ultrasonic.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef ULTRASONIC_H #define ULTRASONIC_H diff --git a/include/vesc_can.h b/include/vesc_can.h index 21f67d1..d94f2c4 100644 --- a/include/vesc_can.h +++ b/include/vesc_can.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef VESC_CAN_H #define VESC_CAN_H diff --git a/include/watchdog.h b/include/watchdog.h index 7a62b36..c088540 100644 --- a/include/watchdog.h +++ b/include/watchdog.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef WATCHDOG_H #define WATCHDOG_H diff --git a/jetson/README.md b/jetson/README.md index a17c345..4026e56 100644 --- a/jetson/README.md +++ b/jetson/README.md @@ -14,7 +14,7 @@ Self-balancing robot: Jetson Nano dev environment for ROS2 Humble + SLAM stack. | Nav | Nav2 | | Depth camera | Intel RealSense D435i | | LiDAR | RPLIDAR A1M8 | -| MCU bridge | STM32F722 (USB CDC @ 921600) | +| Motor controller | ESP32-S3 BALANCE (CAN bus 500 kbps) | ## Quick Start @@ -42,7 +42,7 @@ bash scripts/build-and-run.sh shell ``` jetson/ ├── Dockerfile # L4T base + ROS2 Humble + SLAM packages -├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, STM32) +├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32-S3) ├── README.md # This file ├── docs/ │ ├── pinout.md # GPIO/I2C/UART pinout reference diff --git a/jetson/config/RECOVERY_BEHAVIORS.md b/jetson/config/RECOVERY_BEHAVIORS.md index 14746ca..997d12c 100644 --- a/jetson/config/RECOVERY_BEHAVIORS.md +++ b/jetson/config/RECOVERY_BEHAVIORS.md @@ -34,7 +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. -Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the STM32 firmware. +Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the ESP32-S3 firmware. ## Behavior Tree Sequence diff --git a/jetson/config/nav2_params.yaml b/jetson/config/nav2_params.yaml index efcb0d9..c6adf30 100644 --- a/jetson/config/nav2_params.yaml +++ b/jetson/config/nav2_params.yaml @@ -12,7 +12,7 @@ # /scan — RPLIDAR A1M8 (obstacle layer) # /camera/depth/color/points — RealSense D435i (voxel layer) # -# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic. +# Output: /cmd_vel (Twist) — ESP32-S3 bridge consumes this topic. bt_navigator: ros__parameters: diff --git a/jetson/docker-compose.yml b/jetson/docker-compose.yml index 9b904b3..dff1797 100644 --- a/jetson/docker-compose.yml +++ b/jetson/docker-compose.yml @@ -31,7 +31,7 @@ services: - ./config:/config:ro devices: - /dev/rplidar:/dev/rplidar - - /dev/stm32-bridge:/dev/stm32-bridge + - /dev/esp32-bridge:/dev/esp32-bridge - /dev/bus/usb:/dev/bus/usb - /dev/i2c-7:/dev/i2c-7 - /dev/video0:/dev/video0 @@ -97,13 +97,13 @@ services: rgb_camera.profile:=640x480x30 " - # ── STM32 bridge node (bidirectional serial<->ROS2) ──────────────────────── - stm32-bridge: + # ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ──────────────────────── + esp32-bridge: image: saltybot/ros2-humble:jetson-orin build: context: . dockerfile: Dockerfile - container_name: saltybot-stm32-bridge + container_name: saltybot-esp32-bridge restart: unless-stopped runtime: nvidia network_mode: host @@ -111,13 +111,13 @@ services: - ROS_DOMAIN_ID=42 - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp devices: - - /dev/stm32-bridge:/dev/stm32-bridge + - /dev/esp32-bridge:/dev/esp32-bridge command: > bash -c " source /opt/ros/humble/setup.bash && ros2 launch saltybot_bridge bridge.launch.py mode:=bidirectional - serial_port:=/dev/stm32-bridge + serial_port:=/dev/esp32-bridge " # ── 4x IMX219 CSI cameras ────────────────────────────────────────────────── @@ -192,7 +192,7 @@ services: network_mode: host depends_on: - saltybot-ros2 - - stm32-bridge + - esp32-bridge - csi-cameras environment: - ROS_DOMAIN_ID=42 @@ -208,8 +208,8 @@ services: " - # -- Remote e-stop bridge (MQTT over 4G -> STM32 CDC) ---------------------- - # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to STM32. + # -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ---------------------- + # 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 @@ -221,12 +221,12 @@ services: runtime: nvidia network_mode: host depends_on: - - stm32-bridge + - esp32-bridge environment: - ROS_DOMAIN_ID=42 - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp devices: - - /dev/stm32-bridge:/dev/stm32-bridge + - /dev/esp32-bridge:/dev/esp32-bridge volumes: - ./ros2_ws/src:/ros2_ws/src:rw - ./config:/config:ro @@ -316,7 +316,7 @@ services: runtime: nvidia network_mode: host depends_on: - - stm32-bridge + - esp32-bridge environment: - NVIDIA_VISIBLE_DEVICES=all - NVIDIA_DRIVER_CAPABILITIES=all,audio diff --git a/jetson/docs/pinout.md b/jetson/docs/pinout.md index a7d63b6..41de500 100644 --- a/jetson/docs/pinout.md +++ b/jetson/docs/pinout.md @@ -1,5 +1,5 @@ # Jetson Orin Nano Super — GPIO / I2C / UART / CSI Pinout Reference -## Self-Balancing Robot: STM32F722 Bridge + RealSense D435i + RPLIDAR A1M8 + 4× IMX219 +## Self-Balancing Robot: ESP32-S3 BALANCE + CAN Bus + RealSense D435i + RPLIDAR A1M8 + 4× IMX219 Last updated: 2026-02-28 JetPack version: 6.x (L4T R36.x / Ubuntu 22.04) @@ -43,21 +43,21 @@ i2cdetect -l --- -## 1. STM32F722 Bridge (USB CDC — Primary) +## 1. ESP32-S3 BALANCE (CAN Bus — Primary) -The STM32 acts as a real-time motor + IMU controller. Communication is via **USB CDC serial**. +The ESP32-S3 BALANCE acts as a real-time motor + IMU controller. Communication is via **CAN bus (CANable2, SocketCAN)**. ### USB CDC Connection | Connection | Detail | |-----------|--------| -| Interface | USB Micro-B on STM32 dev board → USB-A on Jetson | -| Device node | `/dev/ttyACM0` → symlink `/dev/stm32-bridge` (via udev) | -| Baud rate | 921600 (configured in STM32 firmware) | +| Interface | CANable2 USB → USB-A on Jetson (SocketCAN slcan0) | +| Device node | `/dev/ttyUSB0` → symlink `/dev/canable2` (via udev); SocketCAN `slcan0` | +| Baud rate | 500 kbps CAN bus | | 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) -| Jetson Pin | Signal | STM32 Pin | Notes | +| Jetson Pin | Signal | CANable2/ESP32 | Notes | |-----------|--------|-----------|-------| | Pin 8 (TXD0) | TX → | PA10 (UART1 RX) | Cross-connect TX→RX | | Pin 10 (RXD0) | RX ← | PA9 (UART1 TX) | Cross-connect RX→TX | @@ -65,7 +65,7 @@ The STM32 acts as a real-time motor + IMU controller. Communication is via **USB **Jetson device node:** `/dev/ttyTHS0` **Baud rate:** 921600, 8N1 -**Voltage level:** 3.3V — both Jetson Orin and STM32F722 are 3.3V GPIO +**Voltage level:** 3.3V differential CAN signals (ISO 11898) ```bash # Verify UART @@ -75,13 +75,13 @@ sudo usermod -aG dialout $USER picocom -b 921600 /dev/ttyTHS0 ``` -**ROS2 topics (STM32 bridge node):** +**ROS2 topics (ESP32-S3 BALANCE bridge node):** | ROS2 Topic | Direction | Content | |-----------|-----------|--------- -| `/saltybot/imu` | STM32→Jetson | IMU data (accel, gyro) at 50Hz | -| `/saltybot/balance_state` | STM32→Jetson | Motor cmd, pitch, state | -| `/cmd_vel` | Jetson→STM32 | Velocity commands → `C,\n` | -| `/saltybot/estop` | Jetson→STM32 | Emergency stop | +| `/saltybot/imu` | ESP32-S3→Jetson | IMU data (accel, gyro) at 50Hz | +| `/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 | --- @@ -266,7 +266,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) | -| USB-C | USB 3.1 Gen 1 (+ DP) | STM32 CDC or host flash | +| USB-C | USB 3.1 Gen 1 (+ DP) | CANable2 USB or host flash | | Micro-USB | Debug/flash | JetPack flash only | --- @@ -277,10 +277,10 @@ sudo mkdir -p /mnt/nvme |-------------|----------|---------|----------| | 3 | SDA1 | 3.3V | I2C data (i2c-7) | | 5 | SCL1 | 3.3V | I2C clock (i2c-7) | -| 8 | TXD0 | 3.3V | UART TX → STM32 (fallback) | -| 10 | RXD0 | 3.3V | UART RX ← STM32 (fallback) | +| 8 | TXD0 | 3.3V | UART TX → ESP32-S3 BALANCE (fallback) | +| 10 | RXD0 | 3.3V | UART RX ← ESP32-S3 BALANCE (fallback) | | USB-A ×2 | — | 5V | D435i, RPLIDAR | -| USB-C | — | 5V | STM32 CDC | +| USB-C | — | 5V | CANable2 USB | | 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 | @@ -298,9 +298,9 @@ Apply stable device names: KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \ SYMLINK+="rplidar", MODE="0666" -# STM32 USB CDC (STMicroelectronics) +# ESP32-S3 BALANCE (CANable2 USB) KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \ - SYMLINK+="stm32-bridge", MODE="0666" + SYMLINK+="esp32-balance", MODE="0666" # Intel RealSense D435i SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \ diff --git a/jetson/docs/power-budget.md b/jetson/docs/power-budget.md index f22e1ef..ec59c56 100644 --- a/jetson/docs/power-budget.md +++ b/jetson/docs/power-budget.md @@ -56,7 +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 | -| STM32F722 bridge | 0.0 | 0.0 | 0.0 | USB CDC | Self-powered from robot 5V | +| ESP32-S3 BALANCE | 0.0 | 0.0 | 0.0 | USB CDC | 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** | | | @@ -151,7 +151,7 @@ LiPo 4S (16.8V max) ├─► DC-DC Buck → 5V 6A ──► Jetson Orin barrel jack (30W) │ (e.g., XL4016E1) │ - ├─► DC-DC Buck → 5V 3A ──► STM32 + logic 5V rail + ├─► 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 42010ae..11319bc 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml @@ -2,7 +2,7 @@ # Used by both serial_bridge_node (RX-only) and saltybot_cmd_node (bidirectional) # ── Serial ───────────────────────────────────────────────────────────────────── -# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied. +# Use /dev/esp32-balance if udev rule from jetson/docs/pinout.md is applied. serial_port: /dev/ttyACM0 baud_rate: 921600 timeout: 0.05 # serial readline timeout (seconds) @@ -11,7 +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. -# STM32 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. heartbeat_period: 0.2 # seconds (= 200ms) # Twist → ESC command scaling 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 3a82ae2..c7f369c 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,5 +1,5 @@ # cmd_vel_bridge_params.yaml -# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → STM32 autonomous drive. +# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 autonomous drive. # # Run with: # ros2 launch saltybot_bridge cmd_vel_bridge.launch.py @@ -7,14 +7,14 @@ # ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=0.3 # ── Serial ───────────────────────────────────────────────────────────────────── -# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied. +# Use /dev/esp32-balance if udev rule from jetson/docs/pinout.md is applied. serial_port: /dev/ttyACM0 baud_rate: 921600 timeout: 0.05 # serial readline timeout (s) reconnect_delay: 2.0 # seconds between reconnect attempts # ── Heartbeat ────────────────────────────────────────────────────────────────── -# STM32 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). # Keep heartbeat well below that threshold. heartbeat_period: 0.2 # seconds (200ms) @@ -50,5 +50,5 @@ 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. -# 500ms matches the STM32 jetson heartbeat timeout for consistency. +# 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/stm32_cmd_params.yaml b/jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml similarity index 73% rename from jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml rename to jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml index 8b65663..8531696 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml @@ -1,18 +1,18 @@ -# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119) -# Binary-framed Jetson↔STM32 bridge at 921600 baud. +# esp32_cmd_params.yaml — Configuration for esp32_cmd_node (Issue #119) +# Binary-framed Jetson↔ESP32-S3 BALANCE bridge at 460800 baud. # ── Serial port ──────────────────────────────────────────────────────────────── -# Use /dev/stm32-bridge if the udev rule is applied: -# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", -# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout" -serial_port: /dev/ttyACM0 -baud_rate: 921600 +# Use /dev/esp32-balance if the udev rule is applied: +# SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001", +# SYMLINK+="esp32-balance", MODE="0660", GROUP="dialout" +serial_port: /dev/esp32-balance +baud_rate: 460800 reconnect_delay: 2.0 # seconds between USB reconnect attempts # ── Heartbeat ───────────────────────────────────────────────────────────────── # HEARTBEAT frame sent every heartbeat_period seconds. -# STM32 fires watchdog and reverts to safe state if no frame received for 500ms. -heartbeat_period: 0.2 # 200ms → well within 500ms STM32 watchdog +# ESP32-S3 fires watchdog and reverts to safe state if no frame received for 500ms. +heartbeat_period: 0.2 # 200ms → well within 500ms ESP32 watchdog # ── Watchdog (Jetson-side) ──────────────────────────────────────────────────── # If no /cmd_vel message received for watchdog_timeout seconds, diff --git a/jetson/ros2_ws/src/saltybot_bridge/config/estop_params.yaml b/jetson/ros2_ws/src/saltybot_bridge/config/estop_params.yaml index fdc7fac..314ac7e 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/config/estop_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bridge/config/estop_params.yaml @@ -1,6 +1,6 @@ remote_estop_node: ros__parameters: - serial_port: /dev/stm32-bridge + serial_port: /dev/esp32-balance baud_rate: 921600 mqtt_host: "mqtt.example.com" mqtt_port: 1883 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 b28a33d..7d63f2e 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py +++ b/jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py @@ -6,7 +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 - and TX /cmd_vel → STM32 commands + heartbeat. + and TX /cmd_vel → ESP32-S3 commands + heartbeat. 2. RX-only (telemetry monitor, no drive commands): ros2 launch saltybot_bridge bridge.launch.py mode:=rx_only @@ -40,7 +40,7 @@ def _launch_nodes(context, *args, **kwargs): return [Node( package="saltybot_bridge", executable="serial_bridge_node", - name="stm32_serial_bridge", + name="esp32_serial_bridge", output="screen", parameters=[params], )] @@ -65,7 +65,7 @@ def generate_launch_description(): DeclareLaunchArgument("mode", default_value="bidirectional", description="bidirectional | rx_only"), DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0", - description="STM32 USB CDC device node"), + 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)"), 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 284bff6..cb691dd 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,10 +1,10 @@ """ -cmd_vel_bridge.launch.py — Nav2 cmd_vel → STM32 autonomous drive bridge. +cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32-S3 autonomous drive bridge. 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) - - Mode gate (drives only when STM32 is in AUTONOMOUS mode, md=2) + - 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) @@ -72,12 +72,12 @@ def generate_launch_description(): description="Full path to cmd_vel_bridge_params.yaml (overrides inline args)"), DeclareLaunchArgument( "serial_port", default_value="/dev/ttyACM0", - description="STM32 USB CDC device node"), + description="ESP32-S3 USB CDC device node"), DeclareLaunchArgument( "baud_rate", default_value="921600"), DeclareLaunchArgument( "heartbeat_period",default_value="0.2", - description="Heartbeat interval (s); must be < STM32 HB timeout (0.5s)"), + 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)"), diff --git a/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py b/jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py similarity index 80% rename from jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py rename to jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py index 2fa400a..fa4b071 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py +++ b/jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py @@ -1,14 +1,14 @@ -"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119). +"""esp32_cmd.launch.py — Launch the binary-framed ESP32 command node (Issue #119). Usage: # Default (binary protocol, bidirectional): - ros2 launch saltybot_bridge stm32_cmd.launch.py + ros2 launch saltybot_bridge esp32_cmd.launch.py # Override serial port: - ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1 + ros2 launch saltybot_bridge esp32_cmd.launch.py serial_port:=/dev/ttyACM1 # Custom velocity scales: - ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0 + ros2 launch saltybot_bridge esp32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0 """ import os @@ -21,11 +21,11 @@ from launch_ros.actions import Node def generate_launch_description() -> LaunchDescription: pkg = get_package_share_directory("saltybot_bridge") - params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml") + params_file = os.path.join(pkg, "config", "esp32_cmd_params.yaml") return LaunchDescription([ - DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"), - DeclareLaunchArgument("baud_rate", default_value="921600"), + DeclareLaunchArgument("serial_port", default_value="/dev/esp32-balance"), + DeclareLaunchArgument("baud_rate", default_value="460800"), DeclareLaunchArgument("speed_scale", default_value="1000.0"), DeclareLaunchArgument("steer_scale", default_value="-500.0"), DeclareLaunchArgument("watchdog_timeout", default_value="0.5"), @@ -33,8 +33,8 @@ def generate_launch_description() -> LaunchDescription: Node( package="saltybot_bridge", - executable="stm32_cmd_node", - name="stm32_cmd_node", + executable="esp32_cmd_node", + name="esp32_cmd_node", output="screen", emulate_tty=True, parameters=[ 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 aa460f3..f344ae8 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,7 +2,7 @@ uart_bridge.launch.py — FC↔Orin UART bridge (Issue #362) Launches serial_bridge_node configured for Jetson Orin UART port. -Bridges Flight Controller (STM32F722) telemetry from /dev/ttyTHS1 into ROS2. +Bridges Flight Controller (ESP32-S3 BALANCE) telemetry from /dev/ttyTHS1 into ROS2. Published topics (same as USB CDC bridge): /saltybot/imu sensor_msgs/Imu — pitch/roll/yaw as angular velocity @@ -20,7 +20,7 @@ Usage: Prerequisites: - Flight Controller connected to /dev/ttyTHS1 @ 921600 baud - - STM32 firmware transmitting JSON telemetry frames (50 Hz) + - ESP32-S3 firmware transmitting JSON telemetry frames (50 Hz) - ROS2 environment sourced (source install/setup.bash) Note: 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 e070638..b3e4d64 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 @@ -1,6 +1,6 @@ """battery_node.py — Battery management for saltybot (Issue #125). -Subscribes to /saltybot/telemetry/battery (JSON from stm32_cmd_node) and: +Subscribes to /saltybot/telemetry/battery (JSON from esp32_io_cmd_node) and: - Publishes sensor_msgs/BatteryState on /saltybot/battery - Publishes JSON alerts on /saltybot/battery/alert at threshold crossings - Reduces speed limit at low SoC via /saltybot/speed_limit (std_msgs/Float32) @@ -14,7 +14,7 @@ Alert levels (SoC thresholds): 5% EMERGENCY — publish zero /cmd_vel, disarm, log + alert SoC source priority: - 1. soc_pct field from STM32 BATTERY telemetry (fuel gauge or lookup on STM32) + 1. soc_pct field from ESP32-S3 IO BATTERY telemetry (fuel gauge or lookup on ESP32-S3 IO) 2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known Parameters (config/battery_params.yaml): @@ -320,7 +320,7 @@ class BatteryNode(Node): self._speed_limit_pub.publish(msg) def _execute_safe_stop(self) -> None: - """Send zero /cmd_vel and disarm the STM32.""" + """Send zero /cmd_vel and disarm the ESP32-S3 IO.""" self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming") # Publish zero velocity zero_twist = 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 993d2f4..201c6b9 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,5 +1,5 @@ """ -cmd_vel_bridge_node — Nav2 /cmd_vel → STM32 drive command bridge. +cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 BALANCE drive command bridge. Extends the basic saltybot_cmd_node with four additions required for safe autonomous operation on a self-balancing robot: @@ -12,7 +12,7 @@ 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). - 4. Mode gate — only issue non-zero drive commands when STM32 reports + 4. Mode gate — only issue non-zero drive commands when ESP32-S3 BALANCE 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 @@ -20,9 +20,9 @@ autonomous operation on a self-balancing robot: Serial protocol (C,\\n / H\\n — same as saltybot_cmd_node): C,\\n — drive command. speed/steer: -1000..+1000 integers. - H\\n — heartbeat. STM32 reverts steer to 0 after 500ms silence. + H\\n — heartbeat. ESP32-S3 IO reverts steer to 0 after 500ms silence. -Telemetry (50 Hz from STM32): +Telemetry (50 Hz from ESP32-S3 IO): Same RX/publish pipeline as saltybot_cmd_node. The "md" field (0=MANUAL,1=ASSISTED,2=AUTO) is parsed for the mode gate. @@ -134,7 +134,7 @@ class CmdVelBridgeNode(Node): self._current_speed = 0 # ramped output actually sent self._current_steer = 0 self._last_cmd_vel = 0.0 # wall clock (seconds) of last /cmd_vel msg - self._stm32_mode = 0 # parsed "md" field: 0=MANUAL,1=ASSISTED,2=AUTO + self._balance_mode = 0 # parsed "md" field: 0=MANUAL,1=ASSISTED,2=AUTO self._last_state = -1 self._frame_count = 0 self._error_count = 0 @@ -150,7 +150,7 @@ class CmdVelBridgeNode(Node): self._open_serial() # ── Timers ──────────────────────────────────────────────────────────── - # Telemetry read at 100 Hz (STM32 sends at 50 Hz) + # Telemetry read at 100 Hz (ESP32-S3 IO 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) @@ -225,7 +225,7 @@ class CmdVelBridgeNode(Node): # Mode gate: in non-AUTONOMOUS mode, zero and reset ramp state so # re-entry always accelerates smoothly from 0. - if self._stm32_mode != MODE_AUTONOMOUS: + if self._balance_mode != MODE_AUTONOMOUS: self._current_speed = 0 self._current_steer = 0 speed, steer = 0, 0 @@ -238,7 +238,7 @@ class CmdVelBridgeNode(Node): speed = self._current_speed steer = self._current_steer - # Send to STM32 + # Send to ESP32-S3 IO frame = f"C{speed},{steer}\n".encode("ascii") if not self._write(frame): self.get_logger().warn( @@ -256,7 +256,7 @@ class CmdVelBridgeNode(Node): # ── Heartbeat TX ────────────────────────────────────────────────────────── def _heartbeat_cb(self): - """H\\n keeps STM32 jetson_cmd heartbeat alive regardless of mode.""" + """H\\n keeps ESP32-S3 IO jetson_cmd heartbeat alive regardless of mode.""" self._write(b"H\n") # ── Telemetry RX ────────────────────────────────────────────────────────── @@ -319,7 +319,7 @@ class CmdVelBridgeNode(Node): state = int(data["s"]) mode = int(data.get("md", 0)) # 0=MANUAL if not present - self._stm32_mode = mode + self._balance_mode = mode self._frame_count += 1 self._publish_imu(pitch_deg, roll_deg, yaw_deg, now) @@ -378,7 +378,7 @@ class CmdVelBridgeNode(Node): diag.header.stamp = stamp status = DiagnosticStatus() status.name = "saltybot/balance_controller" - status.hardware_id = "stm32f722" + status.hardware_id = "esp32_balance" status.message = f"{state_label} [{mode_label}]" status.level = ( DiagnosticStatus.OK if state == 1 else @@ -406,11 +406,11 @@ class CmdVelBridgeNode(Node): status = DiagnosticStatus() status.level = DiagnosticStatus.ERROR status.name = "saltybot/balance_controller" - status.hardware_id = "stm32f722" + status.hardware_id = "esp32_balance" status.message = f"IMU fault errno={errno}" diag.status.append(status) self._diag_pub.publish(diag) - self.get_logger().error(f"STM32 IMU fault: errno={errno}") + self.get_logger().error(f"ESP32-S3 IO IMU fault: errno={errno}") # ── Lifecycle ───────────────────────────────────────────────────────────── 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 d83c23a..2680955 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,8 +1,8 @@ """ -remote_estop_node.py -- Remote e-stop bridge: MQTT -> STM32 USB CDC +remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32-S3 IO USB CDC -{"kill": true} -> writes 'E\n' to STM32 (ESTOP_REMOTE, immediate motor cutoff) -{"kill": false} -> writes 'Z\n' to STM32 (clear latch, robot can re-arm) +{"kill": true} -> writes 'E\n' to ESP32-S3 IO (ESTOP_REMOTE, immediate motor cutoff) +{"kill": false} -> writes 'Z\n' to ESP32-S3 IO (clear latch, robot can re-arm) Cellular watchdog: if MQTT link drops for > cellular_timeout_s while in AUTO mode, automatically sends 'F\n' (ESTOP_CELLULAR_TIMEOUT). @@ -26,7 +26,7 @@ class RemoteEstopNode(Node): def __init__(self): super().__init__('remote_estop_node') - self.declare_parameter('serial_port', '/dev/stm32-bridge') + self.declare_parameter('serial_port', '/dev/esp32-io') self.declare_parameter('baud_rate', 921600) self.declare_parameter('mqtt_host', 'mqtt.example.com') self.declare_parameter('mqtt_port', 1883) 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 f0a4ee8..c836eb5 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 @@ -322,7 +322,7 @@ class SaltybotCanNode(Node): diag.header.stamp = stamp st = DiagnosticStatus() st.name = "saltybot/balance_controller" - st.hardware_id = "stm32f722" + st.hardware_id = "esp32_balance" st.message = state_label st.level = (DiagnosticStatus.OK if state == 1 else DiagnosticStatus.WARN if state == 0 else 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 ddcd5f0..5ff929b 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,20 +1,20 @@ """ -saltybot_cmd_node — full bidirectional STM32↔Jetson bridge +saltybot_cmd_node — full bidirectional ESP32-S3 IO↔Jetson bridge Combines telemetry RX (from serial_bridge_node) with drive command TX. Owns /dev/ttyACM0 exclusively — do NOT run alongside serial_bridge_node. -RX path (50Hz from STM32): +RX path (50Hz from ESP32-S3 IO): JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics TX path: - /cmd_vel (geometry_msgs/Twist) → C,\\n → STM32 - Heartbeat timer (200ms) → H\\n → STM32 + /cmd_vel (geometry_msgs/Twist) → C,\\n → ESP32-S3 IO + Heartbeat timer (200ms) → H\\n → ESP32-S3 IO Protocol: - H\\n — heartbeat. STM32 reverts steer to 0 if gap > 500ms. + H\\n — heartbeat. ESP32-S3 IO reverts steer to 0 if gap > 500ms. C,\\n — drive command. speed/steer: -1000..+1000 integers. - C command also refreshes STM32 heartbeat timer. + C command also refreshes ESP32-S3 IO heartbeat timer. Twist mapping (configurable via ROS2 params): speed = clamp(linear.x * speed_scale, -1000, 1000) @@ -100,7 +100,7 @@ class SaltybotCmdNode(Node): self._open_serial() # ── Timers ──────────────────────────────────────────────────────────── - # Telemetry read at 100Hz (STM32 sends at 50Hz) + # Telemetry read at 100Hz (ESP32-S3 IO 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) @@ -266,7 +266,7 @@ class SaltybotCmdNode(Node): diag.header.stamp = stamp status = DiagnosticStatus() status.name = "saltybot/balance_controller" - status.hardware_id = "stm32f722" + status.hardware_id = "esp32_balance" status.message = state_label if state == 1: status.level = DiagnosticStatus.OK @@ -294,11 +294,11 @@ class SaltybotCmdNode(Node): status = DiagnosticStatus() status.level = DiagnosticStatus.ERROR status.name = "saltybot/balance_controller" - status.hardware_id = "stm32f722" + status.hardware_id = "esp32_balance" status.message = f"IMU fault errno={errno}" diag.status.append(status) self._diag_pub.publish(diag) - self.get_logger().error(f"STM32 IMU fault: errno={errno}") + self.get_logger().error(f"ESP32-S3 IMU fault: errno={errno}") # ── TX — command send ───────────────────────────────────────────────────── @@ -316,7 +316,7 @@ class SaltybotCmdNode(Node): ) def _heartbeat_cb(self): - """Send H\\n heartbeat. STM32 reverts steer to 0 if gap > 500ms.""" + """Send H\\n heartbeat. ESP32-S3 IO 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 6816410..f36ac7b 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,6 +1,6 @@ """ saltybot_bridge — serial_bridge_node -STM32F722 USB CDC → ROS2 topic publisher +ESP32-S3 BALANCE USB CDC → ROS2 topic publisher Telemetry frame (50 Hz, newline-delimited JSON): {"p":,"r":,"e":,"ig":, @@ -29,7 +29,7 @@ from sensor_msgs.msg import Imu from std_msgs.msg import String from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue -# Balance state labels matching STM32 balance_state_t enum +# Balance state labels matching ESP32-S3 IO balance_state_t enum _STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"} # Sensor frame_id published in Imu header @@ -38,7 +38,7 @@ IMU_FRAME_ID = "imu_link" class SerialBridgeNode(Node): def __init__(self): - super().__init__("stm32_serial_bridge") + super().__init__("esp32_io_serial_bridge") # ── Parameters ──────────────────────────────────────────────────────── self.declare_parameter("serial_port", "/dev/ttyACM0") @@ -83,11 +83,11 @@ class SerialBridgeNode(Node): # ── Open serial and start read timer ────────────────────────────────── self._open_serial() - # Poll at 100 Hz — STM32 sends at 50 Hz, so we never miss a frame + # Poll at 100 Hz — ESP32-S3 IO 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_io_serial_bridge started — {port} @ {baud} baud" ) # ── Serial management ───────────────────────────────────────────────────── @@ -117,7 +117,7 @@ class SerialBridgeNode(Node): def write_serial(self, data: bytes) -> bool: """ - Send raw bytes to STM32 over the open serial port. + Send raw bytes to ESP32-S3 IO 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. """ @@ -206,7 +206,7 @@ class SerialBridgeNode(Node): """ Publish sensor_msgs/Imu. - The STM32 IMU gives Euler angles (pitch/roll from accelerometer+gyro + The ESP32-S3 IO 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. @@ -264,7 +264,7 @@ class SerialBridgeNode(Node): diag.header.stamp = stamp status = DiagnosticStatus() status.name = "saltybot/balance_controller" - status.hardware_id = "stm32f722" + status.hardware_id = "esp32_balance" status.message = state_label if state == 1: # ARMED @@ -293,11 +293,11 @@ class SerialBridgeNode(Node): status = DiagnosticStatus() status.level = DiagnosticStatus.ERROR status.name = "saltybot/balance_controller" - status.hardware_id = "stm32f722" + status.hardware_id = "esp32_balance" status.message = f"IMU fault errno={errno}" diag.status.append(status) self._diag_pub.publish(diag) - self.get_logger().error(f"STM32 reported IMU fault: errno={errno}") + self.get_logger().error(f"ESP32-S3 IO reported IMU fault: errno={errno}") def destroy_node(self): self._close_serial() diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py deleted file mode 100644 index 4e3d63c..0000000 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py +++ /dev/null @@ -1,483 +0,0 @@ -"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge. - -Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary -framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud. - -TX commands (Jetson → STM32): - SPEED_STEER — 50 Hz from /cmd_vel subscription - HEARTBEAT — 200 ms timer (STM32 watchdog fires at 500 ms) - ARM — via /saltybot/arm service - SET_MODE — via /saltybot/set_mode service - PID_UPDATE — via /saltybot/pid_update topic - -Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning. - -RX telemetry (STM32 → Jetson): - IMU → /saltybot/imu (sensor_msgs/Imu) - BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON) - MOTOR_RPM → /saltybot/telemetry/motor_rpm (std_msgs/String JSON) - ARM_STATE → /saltybot/arm_state (std_msgs/String JSON) - ERROR → /saltybot/error (std_msgs/String JSON) - All frames → /diagnostics (diagnostic_msgs/DiagnosticArray) - -Auto-reconnect: USB disconnect is detected when serial.read() raises; node -continuously retries at reconnect_delay interval. - -This node owns /dev/ttyACM0 exclusively — do NOT run alongside -serial_bridge_node or saltybot_cmd_node on the same port. - -Parameters (config/stm32_cmd_params.yaml): - serial_port /dev/ttyACM0 - baud_rate 921600 - reconnect_delay 2.0 (seconds) - 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) -""" - -from __future__ import annotations - -import json -import math -import threading -import time - -import rclpy -from rclpy.node import Node -from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy - -import serial - -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue -from geometry_msgs.msg import Twist -from sensor_msgs.msg import Imu -from std_msgs.msg import String -from std_srvs.srv import SetBool, Trigger - -from .stm32_protocol import ( - FrameParser, - ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame, - encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode, - encode_pid_update, -) - -# ── Constants ───────────────────────────────────────────────────────────────── - -IMU_FRAME_ID = "imu_link" -_ARM_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"} - - -def _clamp(v: float, lo: float, hi: float) -> float: - return max(lo, min(hi, v)) - - -# ── Node ────────────────────────────────────────────────────────────────────── - -class Stm32CmdNode(Node): - """Binary-framed Jetson↔STM32 bridge node.""" - - def __init__(self) -> None: - super().__init__("stm32_cmd_node") - - # ── Parameters ──────────────────────────────────────────────────────── - self.declare_parameter("serial_port", "/dev/ttyACM0") - self.declare_parameter("baud_rate", 921600) - self.declare_parameter("reconnect_delay", 2.0) - self.declare_parameter("heartbeat_period", 0.2) - self.declare_parameter("watchdog_timeout", 0.5) - self.declare_parameter("speed_scale", 1000.0) - self.declare_parameter("steer_scale", -500.0) - - port = self.get_parameter("serial_port").value - baud = self.get_parameter("baud_rate").value - self._reconnect_delay = self.get_parameter("reconnect_delay").value - self._hb_period = self.get_parameter("heartbeat_period").value - self._wd_timeout = self.get_parameter("watchdog_timeout").value - self._speed_scale = self.get_parameter("speed_scale").value - self._steer_scale = self.get_parameter("steer_scale").value - - # ── QoS ─────────────────────────────────────────────────────────────── - sensor_qos = QoSProfile( - reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST, depth=10, - ) - rel_qos = QoSProfile( - reliability=ReliabilityPolicy.RELIABLE, - history=HistoryPolicy.KEEP_LAST, depth=10, - ) - - # ── Publishers ──────────────────────────────────────────────────────── - self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos) - self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", rel_qos) - self._error_pub = self.create_publisher(String, "/saltybot/error", rel_qos) - self._battery_pub = self.create_publisher(String, "/saltybot/telemetry/battery", rel_qos) - self._rpm_pub = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos) - self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos) - - # ── Subscribers ─────────────────────────────────────────────────────── - self._cmd_vel_sub = self.create_subscription( - Twist, "/cmd_vel", self._on_cmd_vel, rel_qos, - ) - self._pid_sub = self.create_subscription( - String, "/saltybot/pid_update", self._on_pid_update, rel_qos, - ) - - # ── Services ────────────────────────────────────────────────────────── - self._arm_srv = self.create_service(SetBool, "/saltybot/arm", self._svc_arm) - self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode) - - # ── Serial state ────────────────────────────────────────────────────── - self._port_name = port - self._baud = baud - self._ser: serial.Serial | None = None - self._ser_lock = threading.Lock() - self._parser = FrameParser() - - # ── TX state ────────────────────────────────────────────────────────── - self._last_speed = 0 - self._last_steer = 0 - self._last_cmd_t = time.monotonic() - self._watchdog_sent = False # tracks whether we already sent zero - - # ── Diagnostics state ────────────────────────────────────────────────── - self._last_arm_state = -1 - self._last_battery_mv = 0 - self._rx_frame_count = 0 - - # ── Open serial and start timers ────────────────────────────────────── - self._open_serial() - - # Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean) - self._read_timer = self.create_timer(0.005, self._read_cb) - # Heartbeat TX - self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb) - # Watchdog check (fires at 2× watchdog_timeout for quick detection) - self._wd_timer = self.create_timer(self._wd_timeout / 2, self._watchdog_cb) - # Periodic diagnostics - self._diag_timer = self.create_timer(1.0, self._publish_diagnostics) - - self.get_logger().info( - f"stm32_cmd_node started — {port} @ {baud} baud | " - f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms" - ) - - # ── Serial management ───────────────────────────────────────────────────── - - def _open_serial(self) -> bool: - with self._ser_lock: - try: - self._ser = serial.Serial( - port=self._port_name, - baudrate=self._baud, - timeout=0.005, # non-blocking reads - write_timeout=0.1, - ) - self._ser.reset_input_buffer() - self._parser.reset() - self.get_logger().info(f"Serial open: {self._port_name}") - return True - except serial.SerialException as exc: - self.get_logger().error( - f"Cannot open {self._port_name}: {exc}", - throttle_duration_sec=self._reconnect_delay, - ) - self._ser = None - return False - - def _close_serial(self) -> None: - with self._ser_lock: - if self._ser and self._ser.is_open: - try: - self._ser.close() - except Exception: - pass - self._ser = None - - def _write(self, data: bytes) -> bool: - """Thread-safe serial write. Returns False if port is not open.""" - with self._ser_lock: - if self._ser is None or not self._ser.is_open: - return False - try: - self._ser.write(data) - return True - except serial.SerialException as exc: - self.get_logger().error(f"Serial write error: {exc}") - self._ser = None - return False - - # ── RX — read callback ──────────────────────────────────────────────────── - - def _read_cb(self) -> None: - """Read bytes from serial and feed them to the frame parser.""" - raw: bytes | None = None - reconnect_needed = False - - with self._ser_lock: - if self._ser is None or not self._ser.is_open: - reconnect_needed = True - else: - try: - n = self._ser.in_waiting - if n: - raw = self._ser.read(n) - except serial.SerialException as exc: - self.get_logger().error(f"Serial read error: {exc}") - self._ser = None - reconnect_needed = True - - if reconnect_needed: - self.get_logger().warn( - "Serial disconnected — will retry", - throttle_duration_sec=self._reconnect_delay, - ) - time.sleep(self._reconnect_delay) - self._open_serial() - return - - if not raw: - return - - for byte in raw: - frame = self._parser.feed(byte) - if frame is not None: - self._rx_frame_count += 1 - self._dispatch_frame(frame) - - def _dispatch_frame(self, frame) -> None: - """Route a decoded frame to the appropriate publisher.""" - now = self.get_clock().now().to_msg() - - if isinstance(frame, ImuFrame): - self._publish_imu(frame, now) - - elif isinstance(frame, BatteryFrame): - self._publish_battery(frame, now) - - elif isinstance(frame, MotorRpmFrame): - self._publish_motor_rpm(frame, now) - - elif isinstance(frame, ArmStateFrame): - self._publish_arm_state(frame, now) - - elif isinstance(frame, ErrorFrame): - self._publish_error(frame, now) - - elif isinstance(frame, tuple): - type_code, payload = frame - self.get_logger().debug( - f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}" - ) - - # ── Telemetry publishers ────────────────────────────────────────────────── - - def _publish_imu(self, frame: ImuFrame, stamp) -> None: - msg = Imu() - msg.header.stamp = stamp - msg.header.frame_id = IMU_FRAME_ID - - # orientation: unknown — signal with -1 in first covariance - msg.orientation_covariance[0] = -1.0 - - msg.angular_velocity.x = math.radians(frame.pitch_deg) - msg.angular_velocity.y = math.radians(frame.roll_deg) - msg.angular_velocity.z = math.radians(frame.yaw_deg) - cov = math.radians(0.3) ** 2 # ±0.3° noise estimate from STM32 BMI088 - msg.angular_velocity_covariance[0] = cov - msg.angular_velocity_covariance[4] = cov - msg.angular_velocity_covariance[8] = cov - - msg.linear_acceleration.x = frame.accel_x - msg.linear_acceleration.y = frame.accel_y - msg.linear_acceleration.z = frame.accel_z - acov = 0.05 ** 2 # ±0.05 m/s² noise - msg.linear_acceleration_covariance[0] = acov - msg.linear_acceleration_covariance[4] = acov - msg.linear_acceleration_covariance[8] = acov - - self._imu_pub.publish(msg) - - def _publish_battery(self, frame: BatteryFrame, stamp) -> None: - payload = { - "voltage_v": round(frame.voltage_mv / 1000.0, 3), - "voltage_mv": frame.voltage_mv, - "current_ma": frame.current_ma, - "soc_pct": frame.soc_pct, - "charging": frame.current_ma < -100, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - self._last_battery_mv = frame.voltage_mv - msg = String() - msg.data = json.dumps(payload) - self._battery_pub.publish(msg) - - def _publish_motor_rpm(self, frame: MotorRpmFrame, stamp) -> None: - payload = { - "left_rpm": frame.left_rpm, - "right_rpm": frame.right_rpm, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._rpm_pub.publish(msg) - - def _publish_arm_state(self, frame: ArmStateFrame, stamp) -> None: - label = _ARM_LABEL.get(frame.state, f"UNKNOWN({frame.state})") - if frame.state != self._last_arm_state: - self.get_logger().info(f"Arm state → {label} (flags=0x{frame.error_flags:02X})") - self._last_arm_state = frame.state - - payload = { - "state": frame.state, - "state_label": label, - "error_flags": frame.error_flags, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._arm_pub.publish(msg) - - def _publish_error(self, frame: ErrorFrame, stamp) -> None: - self.get_logger().error( - f"STM32 error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}" - ) - payload = { - "error_code": frame.error_code, - "subcode": frame.subcode, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._error_pub.publish(msg) - - # ── TX — command send ───────────────────────────────────────────────────── - - def _on_cmd_vel(self, msg: Twist) -> None: - """Convert /cmd_vel Twist to SPEED_STEER frame at up to 50 Hz.""" - speed = int(_clamp(msg.linear.x * self._speed_scale, -1000.0, 1000.0)) - steer = int(_clamp(msg.angular.z * self._steer_scale, -1000.0, 1000.0)) - self._last_speed = speed - self._last_steer = steer - self._last_cmd_t = time.monotonic() - self._watchdog_sent = False - - frame = encode_speed_steer(speed, steer) - if not self._write(frame): - self.get_logger().warn( - "SPEED_STEER dropped — serial not open", - throttle_duration_sec=2.0, - ) - - def _heartbeat_cb(self) -> None: - """Send HEARTBEAT every heartbeat_period (default 200ms).""" - self._write(encode_heartbeat()) - - def _watchdog_cb(self) -> None: - """Send zero-speed if /cmd_vel silent for watchdog_timeout seconds.""" - if time.monotonic() - self._last_cmd_t >= self._wd_timeout: - if not self._watchdog_sent: - self.get_logger().warn( - f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed" - ) - self._watchdog_sent = True - self._last_speed = 0 - self._last_steer = 0 - self._write(encode_speed_steer(0, 0)) - - def _on_pid_update(self, msg: String) -> None: - """Parse JSON /saltybot/pid_update and send PID_UPDATE frame.""" - try: - data = json.loads(msg.data) - kp = float(data["kp"]) - ki = float(data["ki"]) - kd = float(data["kd"]) - except (ValueError, KeyError, json.JSONDecodeError) as exc: - self.get_logger().error(f"Bad PID update JSON: {exc}") - return - frame = encode_pid_update(kp, ki, kd) - if self._write(frame): - self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}") - else: - self.get_logger().warn("PID_UPDATE dropped — serial not open") - - # ── Services ────────────────────────────────────────────────────────────── - - def _svc_arm(self, request: SetBool.Request, response: SetBool.Response): - """SetBool(True) = arm, SetBool(False) = disarm.""" - arm = request.data - frame = encode_arm(arm) - ok = self._write(frame) - response.success = ok - response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open" - self.get_logger().info( - f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}" - ) - return response - - def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response): - """SetBool: data maps to mode byte (True=1, False=0).""" - mode = 1 if request.data else 0 - frame = encode_set_mode(mode) - ok = self._write(frame) - response.success = ok - response.message = f"mode={mode}" if ok else "serial not open" - return response - - # ── Diagnostics ─────────────────────────────────────────────────────────── - - def _publish_diagnostics(self) -> None: - diag = DiagnosticArray() - diag.header.stamp = self.get_clock().now().to_msg() - - status = DiagnosticStatus() - status.name = "saltybot/stm32_cmd_node" - status.hardware_id = "stm32f722" - - port_ok = self._ser is not None and self._ser.is_open - if port_ok: - status.level = DiagnosticStatus.OK - status.message = "Serial OK" - else: - status.level = DiagnosticStatus.ERROR - status.message = f"Serial disconnected: {self._port_name}" - - wd_age = time.monotonic() - self._last_cmd_t - status.values = [ - KeyValue(key="serial_port", value=self._port_name), - KeyValue(key="port_open", value=str(port_ok)), - KeyValue(key="rx_frames", value=str(self._rx_frame_count)), - KeyValue(key="rx_errors", value=str(self._parser.frames_error)), - KeyValue(key="last_speed", value=str(self._last_speed)), - KeyValue(key="last_steer", value=str(self._last_steer)), - KeyValue(key="cmd_vel_age_s", value=f"{wd_age:.2f}"), - KeyValue(key="battery_mv", value=str(self._last_battery_mv)), - KeyValue(key="arm_state", value=_ARM_LABEL.get(self._last_arm_state, "?")), - ] - diag.status.append(status) - self._diag_pub.publish(diag) - - # ── Lifecycle ───────────────────────────────────────────────────────────── - - def destroy_node(self) -> None: - # Send zero-speed + disarm on shutdown - self._write(encode_speed_steer(0, 0)) - self._write(encode_arm(False)) - self._close_serial() - super().destroy_node() - - -def main(args=None) -> None: - rclpy.init(args=args) - node = Stm32CmdNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py deleted file mode 100644 index ed98326..0000000 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py +++ /dev/null @@ -1,332 +0,0 @@ -"""stm32_protocol.py — Binary frame codec for Jetson↔STM32 communication. - -Issue #119: defines the binary serial protocol between the Jetson Nano and the -STM32F722 flight controller over USB CDC @ 921600 baud. - -Frame layout (all multi-byte fields are big-endian): - ┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐ - │ STX │ TYPE │ LEN │ PAYLOAD │ CRC16 │ ETX │ - │ 0x02 │ 1B │ 1B │ LEN bytes │ 2B BE │ 0x03 │ - └──────┴──────┴──────┴──────────────────┴───────────┴──────┘ - -CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves). -CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR. - -Command types (Jetson → STM32): - 0x01 HEARTBEAT — no payload (len=0) - 0x02 SPEED_STEER — int16 speed + int16 steer (len=4) range: -1000..+1000 - 0x03 ARM — uint8 (0=disarm, 1=arm) (len=1) - 0x04 SET_MODE — uint8 mode (len=1) - 0x05 PID_UPDATE — float32 kp + ki + kd (len=12) - -Telemetry types (STM32 → Jetson): - 0x10 IMU — int16×6: pitch,roll,yaw (×100 deg), ax,ay,az (×100 m/s²) (len=12) - 0x11 BATTERY — uint16 voltage_mv + int16 current_ma + uint8 soc_pct (len=5) - 0x12 MOTOR_RPM — int16 left_rpm + int16 right_rpm (len=4) - 0x13 ARM_STATE — uint8 state + uint8 error_flags (len=2) - 0x14 ERROR — uint8 error_code + uint8 subcode (len=2) - -Usage: - # Encoding (Jetson → STM32) - frame = encode_speed_steer(300, -150) - ser.write(frame) - - # Decoding (STM32 → Jetson), one byte at a time - parser = FrameParser() - for byte in incoming_bytes: - result = parser.feed(byte) - if result is not None: - handle_frame(result) -""" - -from __future__ import annotations - -import struct -from dataclasses import dataclass -from enum import IntEnum -from typing import Optional - -# ── Frame constants ─────────────────────────────────────────────────────────── - -STX = 0x02 -ETX = 0x03 -MAX_PAYLOAD_LEN = 64 # hard limit; any frame larger is corrupt - - -# ── Command / telemetry type codes ──────────────────────────────────────────── - -class CmdType(IntEnum): - HEARTBEAT = 0x01 - SPEED_STEER = 0x02 - ARM = 0x03 - SET_MODE = 0x04 - PID_UPDATE = 0x05 - - -class TelType(IntEnum): - IMU = 0x10 - BATTERY = 0x11 - MOTOR_RPM = 0x12 - ARM_STATE = 0x13 - ERROR = 0x14 - - -# ── Parsed telemetry objects ────────────────────────────────────────────────── - -@dataclass -class ImuFrame: - pitch_deg: float # degrees (positive = forward tilt) - roll_deg: float - yaw_deg: float - accel_x: float # m/s² - accel_y: float - accel_z: float - - -@dataclass -class BatteryFrame: - voltage_mv: int # millivolts (e.g. 11100 = 11.1 V) - current_ma: int # milliamps (negative = charging) - soc_pct: int # state of charge 0–100 (from STM32 fuel gauge or lookup) - - -@dataclass -class MotorRpmFrame: - left_rpm: int - right_rpm: int - - -@dataclass -class ArmStateFrame: - state: int # 0=DISARMED 1=ARMED 2=TILT_FAULT - error_flags: int # bitmask - - -@dataclass -class ErrorFrame: - error_code: int - subcode: int - - -# Union type for decoded results -TelemetryFrame = ImuFrame | BatteryFrame | MotorRpmFrame | ArmStateFrame | ErrorFrame - - -# ── CRC16 CCITT ─────────────────────────────────────────────────────────────── - -def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int: - """CRC16-CCITT: polynomial 0x1021, init 0xFFFF, no final XOR.""" - crc = init - for byte in data: - crc ^= byte << 8 - for _ in range(8): - if crc & 0x8000: - crc = (crc << 1) ^ 0x1021 - else: - crc <<= 1 - crc &= 0xFFFF - return crc - - -# ── Frame encoder ───────────────────────────────────────────────────────────── - -def _build_frame(cmd_type: int, payload: bytes) -> bytes: - """Assemble a complete binary frame with CRC16.""" - assert len(payload) <= MAX_PAYLOAD_LEN, "Payload too large" - length = len(payload) - header = bytes([cmd_type, length]) - crc = _crc16_ccitt(header + payload) - return bytes([STX, cmd_type, length]) + payload + struct.pack(">H", crc) + bytes([ETX]) - - -def encode_heartbeat() -> bytes: - """HEARTBEAT frame — no payload.""" - return _build_frame(CmdType.HEARTBEAT, b"") - - -def encode_speed_steer(speed: int, steer: int) -> bytes: - """SPEED_STEER frame — int16 speed + int16 steer, both in -1000..+1000.""" - speed = max(-1000, min(1000, int(speed))) - steer = max(-1000, min(1000, int(steer))) - return _build_frame(CmdType.SPEED_STEER, struct.pack(">hh", speed, steer)) - - -def encode_arm(arm: bool) -> bytes: - """ARM frame — 0=disarm, 1=arm.""" - return _build_frame(CmdType.ARM, struct.pack("B", 1 if arm else 0)) - - -def encode_set_mode(mode: int) -> bytes: - """SET_MODE frame — mode byte.""" - return _build_frame(CmdType.SET_MODE, struct.pack("B", mode & 0xFF)) - - -def encode_pid_update(kp: float, ki: float, kd: float) -> bytes: - """PID_UPDATE frame — three float32 values.""" - return _build_frame(CmdType.PID_UPDATE, struct.pack(">fff", kp, ki, kd)) - - -# ── Frame decoder (state-machine parser) ───────────────────────────────────── - -class ParserState(IntEnum): - WAIT_STX = 0 - WAIT_TYPE = 1 - WAIT_LEN = 2 - PAYLOAD = 3 - CRC_HI = 4 - CRC_LO = 5 - WAIT_ETX = 6 - - -class ParseError(Exception): - pass - - -class FrameParser: - """Byte-by-byte streaming parser for STM32 telemetry frames. - - Feed individual bytes via feed(); returns a decoded TelemetryFrame (or raw - bytes tuple) when a complete valid frame is received. - - Thread-safety: single-threaded — wrap in a lock if shared across threads. - - Usage:: - parser = FrameParser() - for b in incoming: - result = parser.feed(b) - if result is not None: - process(result) - """ - - def __init__(self) -> None: - self._state = ParserState.WAIT_STX - self._type = 0 - self._length = 0 - self._payload = bytearray() - self._crc_rcvd = 0 - self.frames_ok = 0 - self.frames_error = 0 - - def reset(self) -> None: - """Reset parser to initial state (call after error or port reconnect).""" - self._state = ParserState.WAIT_STX - self._payload = bytearray() - - def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]: - """Process one byte. Returns decoded frame on success, None otherwise. - - On CRC error, increments frames_error and resets. The return value on - success is a dataclass (ImuFrame, BatteryFrame, etc.) or a - (type_code, raw_payload) tuple for unknown type codes. - """ - s = self._state - - if s == ParserState.WAIT_STX: - if byte == STX: - self._state = ParserState.WAIT_TYPE - return None - - if s == ParserState.WAIT_TYPE: - self._type = byte - self._state = ParserState.WAIT_LEN - return None - - if s == ParserState.WAIT_LEN: - self._length = byte - self._payload = bytearray() - if self._length > MAX_PAYLOAD_LEN: - # Corrupt frame — too big; reset - self.frames_error += 1 - self.reset() - return None - if self._length == 0: - self._state = ParserState.CRC_HI - else: - self._state = ParserState.PAYLOAD - return None - - if s == ParserState.PAYLOAD: - self._payload.append(byte) - if len(self._payload) == self._length: - self._state = ParserState.CRC_HI - return None - - if s == ParserState.CRC_HI: - self._crc_rcvd = byte << 8 - self._state = ParserState.CRC_LO - return None - - if s == ParserState.CRC_LO: - self._crc_rcvd |= byte - self._state = ParserState.WAIT_ETX - return None - - if s == ParserState.WAIT_ETX: - self.reset() # always reset so we look for next STX - if byte != ETX: - self.frames_error += 1 - return None - - # Verify CRC - crc_data = bytes([self._type, self._length]) + self._payload - expected = _crc16_ccitt(crc_data) - if expected != self._crc_rcvd: - self.frames_error += 1 - return None - - # Decode - self.frames_ok += 1 - return _decode_telemetry(self._type, bytes(self._payload)) - - # Should never reach here - self.reset() - return None - - -# ── Telemetry decoder ───────────────────────────────────────────────────────── - -def _decode_telemetry(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]: - """Decode a validated telemetry payload into a typed dataclass.""" - try: - if type_code == TelType.IMU: - if len(payload) < 12: - return None - p, r, y, ax, ay, az = struct.unpack_from(">hhhhhh", payload) - return ImuFrame( - pitch_deg=p / 100.0, - roll_deg=r / 100.0, - yaw_deg=y / 100.0, - accel_x=ax / 100.0, - accel_y=ay / 100.0, - accel_z=az / 100.0, - ) - - if type_code == TelType.BATTERY: - if len(payload) < 5: - return None - v_mv, i_ma, soc = struct.unpack_from(">HhB", payload) - return BatteryFrame(voltage_mv=v_mv, current_ma=i_ma, soc_pct=soc) - - if type_code == TelType.MOTOR_RPM: - if len(payload) < 4: - return None - left, right = struct.unpack_from(">hh", payload) - return MotorRpmFrame(left_rpm=left, right_rpm=right) - - if type_code == TelType.ARM_STATE: - if len(payload) < 2: - return None - state, flags = struct.unpack_from("BB", payload) - return ArmStateFrame(state=state, error_flags=flags) - - if type_code == TelType.ERROR: - if len(payload) < 2: - return None - code, sub = struct.unpack_from("BB", payload) - return ErrorFrame(error_code=code, subcode=sub) - - except struct.error: - return None - - # Unknown telemetry type — return raw - return (type_code, payload) diff --git a/jetson/ros2_ws/src/saltybot_bridge/setup.py b/jetson/ros2_ws/src/saltybot_bridge/setup.py index dc798c0..0200d35 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/setup.py +++ b/jetson/ros2_ws/src/saltybot_bridge/setup.py @@ -13,7 +13,7 @@ setup( "launch/bridge.launch.py", "launch/cmd_vel_bridge.launch.py", "launch/remote_estop.launch.py", - "launch/stm32_cmd.launch.py", + "launch/esp32_cmd.launch.py", "launch/battery.launch.py", "launch/uart_bridge.launch.py", ]), @@ -21,7 +21,7 @@ setup( "config/bridge_params.yaml", "config/cmd_vel_bridge_params.yaml", "config/estop_params.yaml", - "config/stm32_cmd_params.yaml", + "config/esp32_cmd_params.yaml", "config/battery_params.yaml", ]), ], @@ -29,7 +29,7 @@ setup( zip_safe=True, maintainer="sl-jetson", maintainer_email="sl-jetson@saltylab.local", - description="STM32 USB CDC → ROS2 serial bridge for saltybot", + description="ESP32-S3 USB serial → ROS2 bridge for saltybot", license="MIT", tests_require=["pytest"], entry_points={ @@ -41,8 +41,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", - # Binary-framed STM32 command node (Issue #119) - "stm32_cmd_node = saltybot_bridge.stm32_cmd_node:main", + # Binary-framed ESP32-S3 IO command node (Issue #119) + "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) 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 37aa7df..311a768 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py +++ b/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py @@ -1,5 +1,5 @@ """ -Unit tests for Jetson→STM32 command serialization logic. +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_cmd_vel_bridge.py b/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd_vel_bridge.py index 870dcf0..dcc3bc3 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd_vel_bridge.py +++ b/jetson/ros2_ws/src/saltybot_bridge/test/test_cmd_vel_bridge.py @@ -139,10 +139,10 @@ class TestModeGate: MODE_ASSISTED = 1 MODE_AUTONOMOUS = 2 - def _apply_mode_gate(self, stm32_mode, current_speed, current_steer, + def _apply_mode_gate(self, balance_mode, current_speed, current_steer, target_speed, target_steer, step=10): """Mirror of _control_cb mode gate logic.""" - if stm32_mode != self.MODE_AUTONOMOUS: + if balance_mode != self.MODE_AUTONOMOUS: # Reset ramp state, send zero return 0, 0, 0, 0 # (current_speed, current_steer, sent_speed, sent_steer) new_s = _ramp_toward(current_speed, target_speed, step) diff --git a/jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_cmd_node.py b/jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py similarity index 95% rename from jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_cmd_node.py rename to jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py index 00d98b6..2e062f4 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_cmd_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py @@ -1,4 +1,4 @@ -"""test_stm32_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 @@ -12,7 +12,7 @@ Tests: - Zero-speed sent on node shutdown - CRC errors counted correctly -Run with: pytest test/test_stm32_cmd_node.py -v +Run with: pytest test/test_esp32_cmd_node.py -v No ROS2 runtime required — uses mock Node infrastructure. """ @@ -29,7 +29,7 @@ import pytest sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) -from saltybot_bridge.stm32_protocol import ( +from saltybot_bridge.esp32_protocol import ( STX, ETX, CmdType, TelType, encode_speed_steer, encode_heartbeat, encode_arm, encode_pid_update, _build_frame, _crc16_ccitt, @@ -219,10 +219,10 @@ class TestMockSerialTX: class TestMockSerialRX: """Test RX parsing path using MockSerial with pre-loaded telemetry data.""" - from saltybot_bridge.stm32_protocol import FrameParser + from saltybot_bridge.esp32_protocol import FrameParser def test_rx_imu_frame(self): - from saltybot_bridge.stm32_protocol import FrameParser, ImuFrame + from saltybot_bridge.esp32_protocol import FrameParser, ImuFrame raw = _imu_frame_bytes(pitch=500, roll=-200, yaw=100, ax=0, ay=0, az=981) ms = MockSerial(rx_data=raw) parser = FrameParser() @@ -241,7 +241,7 @@ class TestMockSerialRX: assert f.accel_z == pytest.approx(9.81) def test_rx_battery_frame(self): - from saltybot_bridge.stm32_protocol import FrameParser, BatteryFrame + from saltybot_bridge.esp32_protocol import FrameParser, BatteryFrame raw = _battery_frame_bytes(v_mv=10500, i_ma=1200, soc=45) ms = MockSerial(rx_data=raw) parser = FrameParser() @@ -257,7 +257,7 @@ class TestMockSerialRX: assert f.soc_pct == 45 def test_rx_multiple_frames_in_one_read(self): - from saltybot_bridge.stm32_protocol import FrameParser + from saltybot_bridge.esp32_protocol import FrameParser raw = (_imu_frame_bytes() + _arm_state_frame_bytes() + _battery_frame_bytes()) ms = MockSerial(rx_data=raw) parser = FrameParser() @@ -271,7 +271,7 @@ class TestMockSerialRX: assert parser.frames_error == 0 def test_rx_bad_crc_counted_as_error(self): - from saltybot_bridge.stm32_protocol import FrameParser + from saltybot_bridge.esp32_protocol import FrameParser raw = bytearray(_arm_state_frame_bytes(state=1)) raw[-3] ^= 0xFF # corrupt CRC ms = MockSerial(rx_data=bytes(raw)) @@ -282,7 +282,7 @@ class TestMockSerialRX: assert parser.frames_error == 1 def test_rx_resync_after_corrupt_byte(self): - from saltybot_bridge.stm32_protocol import FrameParser, ArmStateFrame + from saltybot_bridge.esp32_protocol import FrameParser, ArmStateFrame garbage = b"\xDE\xAD\x00\x00" valid = _arm_state_frame_bytes(state=1) ms = MockSerial(rx_data=garbage + valid) diff --git a/jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_protocol.py b/jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_protocol.py similarity index 99% rename from jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_protocol.py rename to jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_protocol.py index 4e33f11..9d98862 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_protocol.py +++ b/jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_protocol.py @@ -1,4 +1,4 @@ -"""test_stm32_protocol.py — Unit tests for binary STM32 frame codec. +"""test_esp32_protocol.py — Unit tests for binary ESP32-S3 frame codec. Tests: - CRC16-CCITT correctness @@ -12,7 +12,7 @@ Tests: - Speed/steer clamping in encode_speed_steer - Round-trip encode → decode for all known telemetry types -Run with: pytest test/test_stm32_protocol.py -v +Run with: pytest test/test_esp32_protocol.py -v """ from __future__ import annotations @@ -25,7 +25,7 @@ import os # ── Path setup (no ROS2 install needed) ────────────────────────────────────── sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) -from saltybot_bridge.stm32_protocol import ( +from saltybot_bridge.esp32_protocol import ( STX, ETX, CmdType, TelType, ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame, 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 3fe7ee6..1a08546 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py +++ b/jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py @@ -1,5 +1,5 @@ """ -Unit tests for STM32 telemetry parsing logic. +Unit tests for ESP32-S3 telemetry parsing logic. Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py """ 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 e6d4cfa..373b161 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml @@ -19,7 +19,7 @@ # inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding) # DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer) # -# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic. +# Output: /cmd_vel (Twist) — ESP32-S3 IO bridge consumes this topic. bt_navigator: ros__parameters: 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 fa9e10b..d7bc229 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,12 @@ # Master configuration for full stack bringup # ──────────────────────────────────────────────────────────────────────────── -# HARDWARE — STM32 Bridge & Motor Control +# HARDWARE — ESP32-S3 IO Bridge & Motor Control # ──────────────────────────────────────────────────────────────────────────── saltybot_bridge_node: ros__parameters: - serial_port: "/dev/stm32-bridge" + serial_port: "/dev/esp32-io" baud_rate: 921600 timeout: 0.05 reconnect_delay: 2.0 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 19a98b5..202fb69 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,7 +39,7 @@ Modes ─ UWB driver (2-anchor DW3000, publishes /uwb/target) ─ YOLOv8n person detection (TensorRT) ─ Person follower with UWB+camera fusion - ─ cmd_vel bridge → STM32 (deadman + ramp + AUTONOMOUS gate) + ─ cmd_vel bridge → ESP32-S3 BALANCE (deadman + ramp + AUTONOMOUS gate) ─ rosbridge WebSocket (port 9090) outdoor @@ -57,8 +57,8 @@ Modes Launch sequence (wall-clock delays — conservative for cold start) ───────────────────────────────────────────────────────────────── t= 0s robot_description (URDF + TF tree) - t= 0s STM32 bridge (serial port owner — must be first) - t= 2s cmd_vel bridge (consumes /cmd_vel, needs STM32 bridge up) + t= 0s ESP32-S3 IO bridge (serial port owner — must be first) + t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32-S3 IO bridge up) t= 2s sensors (RPLIDAR + RealSense) t= 4s UWB driver (independent serial device) t= 4s CSI cameras (optional, independent) @@ -71,10 +71,10 @@ Launch sequence (wall-clock delays — conservative for cold start) Safety wiring ───────────── - • STM32 bridge must be up before cmd_vel bridge sends any command. + • ESP32-S3 IO bridge must be up before cmd_vel bridge sends any command. • cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent. - • STM32 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still - until STM32 firmware is in AUTONOMOUS mode regardless of /cmd_vel. + • ESP32-S3 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still + until ESP32-S3 BALANCE 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}' @@ -91,7 +91,7 @@ Topics published by this stack /person/target PoseStamped (camera position, base_link) /person/detections Detection2DArray /cmd_vel Twist (from follower or Nav2) - /saltybot/cmd String (to STM32) + /saltybot/cmd String (to ESP32-S3 IO) /saltybot/imu Imu /saltybot/balance_state String """ @@ -209,7 +209,7 @@ def generate_launch_description(): enable_bridge_arg = DeclareLaunchArgument( "enable_bridge", default_value="true", - description="Launch STM32 serial bridge + cmd_vel bridge (disable for sim/rosbag)", + description="Launch ESP32-S3 IO serial bridge + cmd_vel bridge (disable for sim/rosbag)", ) enable_rosbridge_arg = DeclareLaunchArgument( @@ -267,10 +267,10 @@ enable_mission_logging_arg = DeclareLaunchArgument( description="UWB anchor-1 serial port (starboard/right side)", ) - stm32_port_arg = DeclareLaunchArgument( - "stm32_port", - default_value="/dev/stm32-bridge", - description="STM32 USB CDC serial port", + esp32_port_arg = DeclareLaunchArgument( + "esp32_port", + default_value="/dev/esp32-bridge", + description="ESP32-S3 USB CDC serial port", ) # ── Shared substitution handles ─────────────────────────────────────────── @@ -282,7 +282,7 @@ enable_mission_logging_arg = DeclareLaunchArgument( max_linear_vel = LaunchConfiguration("max_linear_vel") uwb_port_a = LaunchConfiguration("uwb_port_a") uwb_port_b = LaunchConfiguration("uwb_port_b") - stm32_port = LaunchConfiguration("stm32_port") + esp32_port = LaunchConfiguration("esp32_port") # ── t=0s Robot description (URDF + TF tree) ────────────────────────────── robot_description = IncludeLaunchDescription( @@ -290,15 +290,15 @@ enable_mission_logging_arg = DeclareLaunchArgument( launch_arguments={"use_sim_time": use_sim_time}.items(), ) - # ── t=0s STM32 bidirectional serial bridge ──────────────────────────────── - stm32_bridge = GroupAction( + # ── t=0s ESP32-S3 bidirectional serial bridge ──────────────────────────────── + esp32_bridge = GroupAction( condition=IfCondition(LaunchConfiguration("enable_bridge")), actions=[ IncludeLaunchDescription( _launch("saltybot_bridge", "launch", "bridge.launch.py"), launch_arguments={ "mode": "bidirectional", - "serial_port": stm32_port, + "serial_port": esp32_port, }.items(), ), ], @@ -320,7 +320,7 @@ enable_mission_logging_arg = DeclareLaunchArgument( ], ) - # ── t=2s cmd_vel safety bridge (depends on STM32 bridge) ──────────────── + # ── t=2s cmd_vel safety bridge (depends on ESP32-S3 bridge) ──────────────── cmd_vel_bridge = TimerAction( period=2.0, actions=[ @@ -577,14 +577,14 @@ enable_mission_logging_arg, max_linear_vel_arg, uwb_port_a_arg, uwb_port_b_arg, - stm32_port_arg, + esp32_port_arg, # Startup banner banner, # t=0s robot_description, - stm32_bridge, + esp32_bridge, # t=0.5s mission_logging, 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 c8e7b91..78f1527 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 @@ -15,11 +15,11 @@ Usage ros2 launch saltybot_bringup saltybot_bringup.launch.py ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=minimal ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=debug - ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full stm32_port:=/dev/ttyUSB0 + ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full esp32_io_port:=/dev/ttyUSB0 Startup sequence ──────────────── - GROUP A — Drivers t= 0 s STM32 bridge, RealSense+RPLIDAR, motor daemon, IMU + GROUP A — Drivers t= 0 s ESP32-S3 IO 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) @@ -35,7 +35,7 @@ Shutdown Hardware conditionals ───────────────────── - Missing devices (stm32_port, uwb_port_a/b, gimbal_port) skip that driver. + Missing devices (esp32_io_port, uwb_port_a/b, gimbal_port) skip that driver. All conditionals are evaluated at launch time via PathJoinSubstitution + IfCondition. """ @@ -120,10 +120,10 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 description="Use /clock from rosbag/simulator", ) - stm32_port_arg = DeclareLaunchArgument( - "stm32_port", - default_value="/dev/stm32-bridge", - description="STM32 USART bridge serial device", + esp32_io_port_arg = DeclareLaunchArgument( + "esp32_io_port", + default_value="/dev/esp32-io", + description="ESP32-S3 IO serial device", ) uwb_port_a_arg = DeclareLaunchArgument( @@ -160,7 +160,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 profile = LaunchConfiguration("profile") use_sim_time = LaunchConfiguration("use_sim_time") - stm32_port = LaunchConfiguration("stm32_port") + esp32_io_port = LaunchConfiguration("esp32_io_port") uwb_port_a = LaunchConfiguration("uwb_port_a") uwb_port_b = LaunchConfiguration("uwb_port_b") gimbal_port = LaunchConfiguration("gimbal_port") @@ -198,7 +198,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ # GROUP A — DRIVERS (t = 0 s, all profiles) - # Dependency order: STM32 bridge first, then sensors, then motor daemon. + # Dependency order: ESP32-S3 bridge first, then sensors, then motor daemon. # Health gate: subsequent groups delayed until t_perception (8 s full/debug). # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ @@ -212,12 +212,12 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 launch_arguments={"use_sim_time": use_sim_time}.items(), ) - # STM32 bidirectional bridge (JLINK USART1) - stm32_bridge = IncludeLaunchDescription( + # ESP32-S3 bidirectional bridge (JLINK USART1) + esp32_bridge = IncludeLaunchDescription( _launch("saltybot_bridge", "launch", "bridge.launch.py"), launch_arguments={ "mode": "bidirectional", - "serial_port": stm32_port, + "serial_port": esp32_port, }.items(), ) @@ -232,7 +232,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 ], ) - # Motor daemon: /cmd_vel → STM32 DRIVE frames (depends on bridge at t=0) + # Motor daemon: /cmd_vel → ESP32-S3 DRIVE frames (depends on bridge at t=0) motor_daemon = TimerAction( period=2.5, actions=[ @@ -541,7 +541,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 # ── Arguments ────────────────────────────────────────────────────────── profile_arg, use_sim_time_arg, - stm32_port_arg, + esp32_port_arg, uwb_port_a_arg, uwb_port_b_arg, gimbal_port_arg, @@ -559,7 +559,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901 # ── GROUP A: Drivers (all profiles, t=0–4s) ─────────────────────────── robot_description, - stm32_bridge, + esp32_bridge, sensors, motor_daemon, sensor_health, 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 839e9da..9cdf03b 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,7 +20,7 @@ theta is kept in (−π, π] after every step. Int32 rollover -------------- -STM32 encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handles +ESP32-S3 IO encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handles this by detecting jumps larger than half the int32 range and adjusting by the full range: 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 5200f3a..40f501c 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 @@ -29,7 +29,7 @@ class Profile: name: str # ── Group A: Drivers (always on in all profiles) ────────────────────── - enable_stm32_bridge: bool = True + enable_esp32_io_bridge: bool = True enable_sensors: bool = True # RealSense + RPLIDAR enable_motor_daemon: bool = True enable_imu: bool = True @@ -69,14 +69,14 @@ class Profile: t_ui: float = 22.0 # Group D (nav2 needs ~4 s to load costmaps) # ── Safety ──────────────────────────────────────────────────────────── - watchdog_timeout_s: float = 5.0 # max silence from STM32 bridge (s) + watchdog_timeout_s: float = 5.0 # max silence from ESP32-S3 IO 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) # ── Hardware conditionals ───────────────────────────────────────────── # Paths checked at launch; absent devices skip the relevant node. - stm32_port: str = "/dev/stm32-bridge" + esp32_io_port: str = "/dev/esp32-io" uwb_port_a: str = "/dev/uwb-anchor0" uwb_port_b: str = "/dev/uwb-anchor1" gimbal_port: str = "/dev/ttyTHS1" @@ -90,7 +90,7 @@ class Profile: # ── Profile factory ──────────────────────────────────────────────────────────── def _minimal() -> Profile: - """Minimal: STM32 bridge + sensors + motor daemon. + """Minimal: ESP32-S3 IO bridge + sensors + motor daemon. Safe drive control only. No AI, no nav, no social. Boot time ~4 s. RAM ~400 MB. @@ -115,7 +115,7 @@ def _full() -> Profile: return Profile( name="full", # Drivers - enable_stm32_bridge=True, + enable_esp32_io_bridge=True, enable_sensors=True, enable_motor_daemon=True, enable_imu=True, 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 6d915f8..03297ee 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,7 +1,7 @@ """ wheel_odom_node.py — Differential drive wheel encoder odometry (Issue #184). -Subscribes to raw encoder tick counts from the STM32 bridge, integrates +Subscribes to raw encoder tick counts from the ESP32-S3 IO bridge, integrates differential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz. Optionally broadcasts the odom → base_link TF transform. 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 8214fbf..26dd085 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,7 +61,7 @@ kill %1 ### Core System Components - Robot Description (URDF/TF tree) -- STM32 Serial Bridge +- ESP32-S3 IO Serial Bridge - cmd_vel Bridge - Rosbridge WebSocket @@ -125,11 +125,11 @@ free -h ### cmd_vel bridge not responding ```bash -# Verify STM32 bridge is running first +# Verify ESP32-S3 IO bridge is running first ros2 node list | grep bridge # Check serial port -ls -l /dev/stm32-bridge +ls -l /dev/esp32-io ``` ## Performance Baseline diff --git a/jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py b/jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py index 2b1fa9b..e69d3e4 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py +++ b/jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py @@ -74,7 +74,7 @@ class TestMinimalProfile: assert self.p.name == "minimal" def test_drivers_enabled(self): - assert self.p.enable_stm32_bridge is True + assert self.p.enable_esp32_io_bridge is True assert self.p.enable_sensors is True assert self.p.enable_motor_daemon is True assert self.p.enable_imu is True @@ -124,7 +124,7 @@ class TestFullProfile: assert self.p.name == "full" def test_drivers_enabled(self): - assert self.p.enable_stm32_bridge is True + assert self.p.enable_esp32_io_bridge is True assert self.p.enable_sensors is True assert self.p.enable_motor_daemon is True assert self.p.enable_imu is True @@ -312,9 +312,9 @@ class TestSafetyDefaults: # ─── Hardware port defaults ──────────────────────────────────────────────────── class TestHardwarePortDefaults: - def test_stm32_port_set(self): + def test_esp32_io_port_set(self): p = _minimal() - assert p.stm32_port.startswith("/dev/") + assert p.esp32_io_port.startswith("/dev/") def test_uwb_ports_set(self): p = _full() diff --git a/jetson/ros2_ws/src/saltybot_bringup/urdf/saltybot.urdf.xacro b/jetson/ros2_ws/src/saltybot_bringup/urdf/saltybot.urdf.xacro index 1db61ac..f96d0d7 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/urdf/saltybot.urdf.xacro +++ b/jetson/ros2_ws/src/saltybot_bringup/urdf/saltybot.urdf.xacro @@ -10,7 +10,7 @@ - Sensors: * RPLIDAR A1M8 (360° scanning LiDAR) * RealSense D435i (RGB-D camera + IMU) - * BNO055 (9-DOF IMU, STM32 FC) + * ICM-42688-P (9-DOF IMU, ESP32-S3 BALANCE) - Actuators: * 2x differential drive motors * Pan/Tilt servos for camera @@ -120,7 +120,7 @@ - + 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..2ce4970 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 @@ -1,7 +1,11 @@ can_bridge_node: ros__parameters: - can_interface: slcan0 + # SocketCAN interface — CANable 2.0 USB-to-CAN on Orin + can_interface: "can0" + + # VESC CAN node IDs (FSESC 6.7 Pro Mini Dual) left_vesc_can_id: 56 right_vesc_can_id: 68 - mamba_can_id: 1 + + # Watchdog: send zero velocity if no /cmd_vel for this many seconds command_timeout_s: 0.5 diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py index e146650..10069bb 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py @@ -1 +1 @@ -"""SaltyBot CAN bridge package — Mamba controller and VESC telemetry via python-can.""" +"""saltybot_can_bridge — CAN bus bridge for ESP32 BALANCE and VESC telemetry.""" 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 new file mode 100644 index 0000000..746b598 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/balance_protocol.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python3 +"""balance_protocol.py — CAN frame codec for SAUL-TEE ESP32 BALANCE + VESC. + +CAN bus: 500 kbps, CANable 2.0 (can0) on Orin. + +── Orin → ESP32 BALANCE (commands) ─────────────────────────────────────────── + 0x300 DRIVE 8 B [left_mps f32 LE][right_mps f32 LE] + 0x301 MODE 1 B [mode u8] 0=idle 1=drive 2=estop + 0x302 ESTOP 1 B [flags u8] bit0=stop bit1=clear + 0x303 LED 4 B [pattern u8][r u8][g u8][b u8] + +── ESP32 BALANCE → Orin (telemetry) ────────────────────────────────────────── + 0x400 FC_STATUS 8 B [pitch_x10 i16][motor_cmd u16][vbat_mv u16][state u8][flags u8] 10 Hz + 0x401 FC_VESC 8 B [l_rpm_x10 i16][r_rpm_x10 i16][l_cur_x10 i16][r_cur_x10 i16] 10 Hz + +── VESC standard CAN frames (29-bit extended IDs) ──────────────────────────── + VESC node IDs: Left = 56, Right = 68 + ID = (packet_type << 8) | node_id +""" + +import struct +from dataclasses import dataclass + +ORIN_CMD_DRIVE: int = 0x300 +ORIN_CMD_MODE: int = 0x301 +ORIN_CMD_ESTOP: int = 0x302 +ORIN_CMD_LED: int = 0x303 + +FC_STATUS: int = 0x400 +FC_VESC: int = 0x401 + +VESC_LEFT_ID: int = 56 +VESC_RIGHT_ID: int = 68 + +VESC_CMD_SET_RPM: int = 3 +VESC_STATUS_1: int = 9 +VESC_STATUS_4: int = 16 +VESC_STATUS_5: int = 27 + +MODE_IDLE: int = 0 +MODE_DRIVE: int = 1 +MODE_ESTOP: int = 2 + + +@dataclass +class FcStatus: + pitch_deg: float = 0.0 + motor_cmd: int = 0 + vbat_mv: int = 0 + state: int = 0 + flags: int = 0 + + +@dataclass +class FcVesc: + left_rpm: float = 0.0 + right_rpm: float = 0.0 + left_cur: float = 0.0 + right_cur: float = 0.0 + + +@dataclass +class VescStatus1: + node_id: int = 0 + erpm: float = 0.0 + current: float = 0.0 + duty: float = 0.0 + + +@dataclass +class VescStatus4: + node_id: int = 0 + temp_fet_c: float = 0.0 + temp_motor_c: float = 0.0 + current_in: float = 0.0 + + +@dataclass +class VescStatus5: + node_id: int = 0 + tacho: int = 0 + vbat_v: float = 0.0 + + +def encode_drive_cmd(left_mps: float, right_mps: float) -> bytes: + """Encode ORIN_CMD_DRIVE (0x300): 8 bytes, 2 × float32 little-endian.""" + return struct.pack(" bytes: + """Encode ORIN_CMD_MODE (0x301): 1 byte mode value.""" + if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + raise ValueError(f"Invalid mode {mode!r}") + return struct.pack("B", mode) + + +def encode_estop_cmd(stop: bool = True, clear: bool = False) -> bytes: + """Encode ORIN_CMD_ESTOP (0x302): 1 byte flags (bit0=stop, bit1=clear).""" + return struct.pack("B", (0x01 if stop else 0) | (0x02 if clear else 0)) + + +def encode_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes: + """Encode ORIN_CMD_LED (0x303): 4 bytes.""" + return struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF) + + +def encode_vesc_set_rpm(node_id: int, rpm: int) -> tuple: + """Return (arb_id, payload) for a VESC SET_RPM command.""" + return (VESC_CMD_SET_RPM << 8) | (node_id & 0xFF), struct.pack(">i", int(rpm)) + + +def decode_fc_status(data: bytes) -> FcStatus: + """Decode FC_STATUS (0x400) payload → FcStatus.""" + if len(data) < 8: + raise ValueError(f"FC_STATUS expects 8 bytes, got {len(data)}") + pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hHHBB", data) + return FcStatus( + pitch_deg=pitch_x10 / 10.0, + motor_cmd=motor_cmd, + vbat_mv=vbat_mv, + state=state, + flags=flags, + ) + + +def decode_fc_vesc(data: bytes) -> FcVesc: + """Decode FC_VESC (0x401) payload → FcVesc.""" + if len(data) < 8: + raise ValueError(f"FC_VESC expects 8 bytes, got {len(data)}") + l_rpm, r_rpm, l_cur, r_cur = struct.unpack_from(">hhhh", data) + return FcVesc( + left_rpm=l_rpm / 10.0, + right_rpm=r_rpm / 10.0, + left_cur=l_cur / 10.0, + right_cur=r_cur / 10.0, + ) + + +def decode_vesc_can_id(can_id: int) -> tuple: + """Decode a VESC extended CAN ID → (packet_type, node_id).""" + return (can_id >> 8) & 0xFF, can_id & 0xFF + + +def decode_vesc_status1(node_id: int, data: bytes) -> VescStatus1: + """Decode VESC STATUS_1 payload → VescStatus1.""" + erpm, cur_x10, duty_x1000 = struct.unpack_from(">ihh", data[:8]) + return VescStatus1( + node_id=node_id, + erpm=float(erpm), + current=cur_x10 / 10.0, + duty=duty_x1000 / 1000.0, + ) + + +def decode_vesc_status4(node_id: int, data: bytes) -> VescStatus4: + """Decode VESC STATUS_4 payload → VescStatus4.""" + tfet, tmot, cur_in, _ = struct.unpack_from(">hhhh", data[:8]) + return VescStatus4( + node_id=node_id, + temp_fet_c=tfet / 10.0, + temp_motor_c=tmot / 10.0, + current_in=cur_in / 10.0, + ) + + +def decode_vesc_status5(node_id: int, data: bytes) -> VescStatus5: + """Decode VESC STATUS_5 payload → VescStatus5.""" + tacho, vbat_x10 = struct.unpack_from(">ih", data[:6]) + return VescStatus5(node_id=node_id, tacho=tacho, vbat_v=vbat_x10 / 10.0) 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 fe62740..0214d5f 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 @@ -1,20 +1,20 @@ #!/usr/bin/env python3 """ -can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the Mamba motor +can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32-S3 BALANCE controller and VESC motor controllers over CAN bus. -The node opens the SocketCAN interface (slcan0 by default), spawns a background +The node opens the SocketCAN interface (can0 by default), spawns a background reader thread to process incoming telemetry, and exposes the following interface: Subscriptions ------------- - /cmd_vel geometry_msgs/Twist → VESC speed commands (CAN) - /estop std_msgs/Bool → Mamba e-stop (CAN) + /cmd_vel geometry_msgs/Twist -> drive commands (CAN 0x300) + /estop std_msgs/Bool -> e-stop (CAN 0x303) Publications ------------ - /can/imu sensor_msgs/Imu Mamba IMU telemetry - /can/battery sensor_msgs/BatteryState Mamba battery telemetry + /can/attitude std_msgs/Float32MultiArray ESP32-S3 BALANCE attitude + /can/battery std_msgs/Float32MultiArray ESP32-S3 BALANCE battery /can/vesc/left/state std_msgs/Float32MultiArray Left VESC state /can/vesc/right/state std_msgs/Float32MultiArray Right VESC state /can/connection_status std_msgs/String "connected" | "disconnected" @@ -30,30 +30,24 @@ import can import rclpy from geometry_msgs.msg import Twist from rclpy.node import Node -from rcl_interfaces.msg import SetParametersResult -from sensor_msgs.msg import BatteryState, Imu from std_msgs.msg import Bool, Float32MultiArray, String -from saltybot_can_bridge.mamba_protocol import ( - MAMBA_CMD_ESTOP, - MAMBA_CMD_MODE, - MAMBA_CMD_VELOCITY, - MAMBA_TELEM_BATTERY, - MAMBA_TELEM_IMU, - VESC_TELEM_STATE, - ORIN_CAN_ID_FC_PID_ACK, - ORIN_CAN_ID_PID_SET, +from saltybot_can_bridge.balance_protocol import ( + ESP32_TELEM_ATTITUDE, + ESP32_TELEM_BATTERY, + ORIN_CMD_DRIVE, + ORIN_CMD_ESTOP, + VESC_LEFT_ID, + VESC_RIGHT_ID, + VESC_STATUS_1, MODE_DRIVE, - MODE_ESTOP, MODE_IDLE, + encode_drive_cmd, encode_estop_cmd, - encode_mode_cmd, - encode_velocity_cmd, - encode_pid_set_cmd, - decode_battery_telem, - decode_imu_telem, - decode_pid_ack, - decode_vesc_state, + decode_attitude, + decode_battery, + decode_vesc_can_id, + decode_vesc_status1, ) # Reconnect attempt interval when CAN bus is lost @@ -64,39 +58,35 @@ _WATCHDOG_HZ: float = 10.0 class CanBridgeNode(Node): - """CAN bus bridge between Orin ROS2 and Mamba / VESC controllers.""" + """CAN bus bridge between Orin ROS2 and ESP32-S3 BALANCE / VESC controllers.""" def __init__(self) -> None: super().__init__("can_bridge_node") - # ── Parameters ──────────────────────────────────────────────────── - self.declare_parameter("can_interface", "slcan0") - self.declare_parameter("left_vesc_can_id", 56) - self.declare_parameter("right_vesc_can_id", 68) - self.declare_parameter("mamba_can_id", 1) + # -- Parameters -- + self.declare_parameter("can_interface", "can0") + self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID) + self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) self.declare_parameter("command_timeout_s", 0.5) - self.declare_parameter("pid/kp", 0.0) - self.declare_parameter("pid/ki", 0.0) - self.declare_parameter("pid/kd", 0.0) self._iface: str = self.get_parameter("can_interface").value self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value - self._mamba_id: int = self.get_parameter("mamba_can_id").value self._cmd_timeout: float = self.get_parameter("command_timeout_s").value - self._pid_kp: float = self.get_parameter("pid/kp").value - self._pid_ki: float = self.get_parameter("pid/ki").value - self._pid_kd: float = self.get_parameter("pid/kd").value - # ── State ───────────────────────────────────────────────────────── + # -- State -- self._bus: Optional[can.BusABC] = None self._connected: bool = False self._last_cmd_time: float = time.monotonic() - self._lock = threading.Lock() # protects _bus / _connected + self._lock = threading.Lock() - # ── Publishers ──────────────────────────────────────────────────── - self._pub_imu = self.create_publisher(Imu, "/can/imu", 10) - self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10) + # -- Publishers -- + self._pub_attitude = self.create_publisher( + Float32MultiArray, "/can/attitude", 10 + ) + self._pub_battery = self.create_publisher( + Float32MultiArray, "/can/battery", 10 + ) self._pub_vesc_left = self.create_publisher( Float32MultiArray, "/can/vesc/left/state", 10 ) @@ -107,55 +97,29 @@ class CanBridgeNode(Node): String, "/can/connection_status", 10 ) - # ── Subscriptions ───────────────────────────────────────────────── + # -- Subscriptions -- self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10) self.create_subscription(Bool, "/estop", self._estop_cb, 10) - self.add_on_set_parameters_callback(self._on_set_parameters) - # ── Timers ──────────────────────────────────────────────────────── + # -- Timers -- self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb) self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb) - # ── Open CAN ────────────────────────────────────────────────────── + # -- Open CAN -- self._try_connect() - # ── Background reader thread ────────────────────────────────────── + # -- Background reader thread -- self._reader_thread = threading.Thread( target=self._reader_loop, daemon=True, name="can_reader" ) self._reader_thread.start() self.get_logger().info( - f"can_bridge_node ready — iface={self._iface} " - f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} " - f"mamba={self._mamba_id}" + f"can_bridge_node ready -- iface={self._iface} " + f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id}" ) - # -- PID parameter callback (Issue #693) -- - - def _on_set_parameters(self, params) -> SetParametersResult: - """Send new PID gains over CAN when pid/* params change.""" - for p in params: - if p.name == "pid/kp": - self._pid_kp = float(p.value) - elif p.name == "pid/ki": - self._pid_ki = float(p.value) - elif p.name == "pid/kd": - self._pid_kd = float(p.value) - else: - continue - try: - payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd) - self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set") - self.get_logger().info( - f"PID gains sent: Kp={self._pid_kp:.2f} " - f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}" - ) - except ValueError as exc: - return SetParametersResult(successful=False, reason=str(exc)) - return SetParametersResult(successful=True) - - # ── Connection management ────────────────────────────────────────────── + # -- Connection management -- def _try_connect(self) -> None: """Attempt to open the CAN interface; silently skip if already connected.""" @@ -174,7 +138,7 @@ class CanBridgeNode(Node): except Exception as exc: self.get_logger().warning( f"CAN bus not available ({self._iface}): {exc} " - f"— will retry every {_RECONNECT_INTERVAL_S:.0f} s" + f"-- will retry every {_RECONNECT_INTERVAL_S:.0f} s" ) self._connected = False self._publish_status("disconnected") @@ -197,46 +161,33 @@ class CanBridgeNode(Node): self._connected = False self._publish_status("disconnected") - # ── ROS callbacks ───────────────────────────────────────────────────── + # -- ROS callbacks -- def _cmd_vel_cb(self, msg: Twist) -> None: - """Convert /cmd_vel Twist to VESC speed commands over CAN.""" + """Convert /cmd_vel Twist to ESP32-S3 BALANCE drive commands over CAN (0x300).""" self._last_cmd_time = time.monotonic() - if not self._connected: return - - # Differential drive decomposition — individual wheel speeds in m/s. - # The VESC nodes interpret linear velocity directly; angular is handled - # by the sign difference between left and right. linear = msg.linear.x angular = msg.angular.z - - # Forward left = forward right for pure translation; for rotation - # left slows and right speeds up (positive angular = CCW = left turn). - # The Mamba velocity command carries both wheels independently. - left_mps = linear - angular - right_mps = linear + angular - - payload = encode_velocity_cmd(left_mps, right_mps) - self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel") - - # Keep Mamba in DRIVE mode while receiving commands - self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode") + # Differential drive -- motor units [-1000, +1000] + speed_units = int(linear * 1000.0) + steer_units = int(angular * 1000.0) + self._send_can( + ORIN_CMD_DRIVE, + encode_drive_cmd(speed_units, steer_units, mode=MODE_DRIVE), + "cmd_vel", + ) def _estop_cb(self, msg: Bool) -> None: - """Forward /estop to Mamba over CAN.""" + """Forward /estop to ESP32-S3 BALANCE over CAN (0x303).""" if not self._connected: return - payload = encode_estop_cmd(msg.data) - self._send_can(MAMBA_CMD_ESTOP, payload, "estop") if msg.data: - self._send_can( - MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode" - ) - self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba") + self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop") + self.get_logger().warning("E-stop asserted -- sent ESTOP to ESP32-S3 BALANCE") - # ── Watchdog ────────────────────────────────────────────────────────── + # -- Watchdog -- def _watchdog_cb(self) -> None: """If no /cmd_vel arrives within the timeout, send zero velocity.""" @@ -245,15 +196,12 @@ class CanBridgeNode(Node): elapsed = time.monotonic() - self._last_cmd_time if elapsed > self._cmd_timeout: self._send_can( - MAMBA_CMD_VELOCITY, - encode_velocity_cmd(0.0, 0.0), + ORIN_CMD_DRIVE, + encode_drive_cmd(0, 0, mode=MODE_IDLE), "watchdog zero-vel", ) - self._send_can( - MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle" - ) - # ── CAN send helper ─────────────────────────────────────────────────── + # -- CAN send helper -- def _send_can(self, arb_id: int, data: bytes, context: str) -> None: """Send a standard CAN frame; handle errors gracefully.""" @@ -261,7 +209,6 @@ class CanBridgeNode(Node): if not self._connected or self._bus is None: return bus = self._bus - msg = can.Message( arbitration_id=arb_id, data=data, @@ -272,132 +219,95 @@ class CanBridgeNode(Node): except can.CanError as exc: self._handle_can_error(exc, f"send({context})") - # ── Background CAN reader ───────────────────────────────────────────── + # -- Background CAN reader -- def _reader_loop(self) -> None: - """ - Blocking CAN read loop executed in a daemon thread. - Dispatches incoming frames to the appropriate handler. - """ + """Blocking CAN read loop executed in a daemon thread.""" while rclpy.ok(): with self._lock: connected = self._connected bus = self._bus - if not connected or bus is None: time.sleep(0.1) continue - try: frame = bus.recv(timeout=0.5) except can.CanError as exc: self._handle_can_error(exc, "reader_loop recv") continue - if frame is None: - # Timeout — no frame within 0.5 s, loop again continue - self._dispatch_frame(frame) def _dispatch_frame(self, frame: can.Message) -> None: """Route an incoming CAN frame to the correct publisher.""" arb_id = frame.arbitration_id data = bytes(frame.data) - try: - if arb_id == MAMBA_TELEM_IMU: - self._handle_imu(data, frame.timestamp) - - elif arb_id == MAMBA_TELEM_BATTERY: - self._handle_battery(data, frame.timestamp) - - elif arb_id == VESC_TELEM_STATE + self._left_vesc_id: - self._handle_vesc_state(data, frame.timestamp, side="left") - - elif arb_id == VESC_TELEM_STATE + self._right_vesc_id: - self._handle_vesc_state(data, frame.timestamp, side="right") - - elif arb_id == ORIN_CAN_ID_FC_PID_ACK: - gains = decode_pid_ack(data) - self.get_logger().debug( - f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}" - ) - + if arb_id == ESP32_TELEM_ATTITUDE: + self._handle_attitude(data) + elif arb_id == ESP32_TELEM_BATTERY: + self._handle_battery(data) + elif frame.is_extended_id: + # VESC extended 29-bit ID: packet_type<<8 | node_id + pkt_type, node_id = decode_vesc_can_id(arb_id) + if pkt_type == VESC_STATUS_1: + self._handle_vesc_status1(data, node_id) except Exception as exc: self.get_logger().warning( f"Error parsing CAN frame 0x{arb_id:03X}: {exc}" ) - # ── Frame handlers ──────────────────────────────────────────────────── + # -- Frame handlers -- - def _handle_imu(self, data: bytes, timestamp: float) -> None: - telem = decode_imu_telem(data) - - msg = Imu() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "imu_link" - - msg.linear_acceleration.x = telem.accel_x - msg.linear_acceleration.y = telem.accel_y - msg.linear_acceleration.z = telem.accel_z - - msg.angular_velocity.x = telem.gyro_x - msg.angular_velocity.y = telem.gyro_y - msg.angular_velocity.z = telem.gyro_z - - # Covariance unknown; mark as -1 per REP-145 - msg.orientation_covariance[0] = -1.0 - - self._pub_imu.publish(msg) - - def _handle_battery(self, data: bytes, timestamp: float) -> None: - telem = decode_battery_telem(data) - - msg = BatteryState() - msg.header.stamp = self.get_clock().now().to_msg() - msg.voltage = telem.voltage - msg.current = telem.current - msg.present = True - msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + def _handle_attitude(self, data: bytes) -> None: + telem = decode_attitude(data) + msg = Float32MultiArray() + # Layout: [pitch_deg, speed, yaw_rate, state, flags] + msg.data = [ + telem.pitch_deg, + telem.speed, + telem.yaw_rate, + float(telem.state), + float(telem.flags), + ] + self._pub_attitude.publish(msg) + def _handle_battery(self, data: bytes) -> None: + telem = decode_battery(data) + msg = Float32MultiArray() + # Layout: [vbat_v, fault_code, rssi] + msg.data = [telem.vbat_mv / 1000.0, float(telem.fault_code), float(telem.rssi)] self._pub_battery.publish(msg) - def _handle_vesc_state( - self, data: bytes, timestamp: float, side: str - ) -> None: - telem = decode_vesc_state(data) - + def _handle_vesc_status1(self, data: bytes, node_id: int) -> None: + status = decode_vesc_status1(node_id, data) msg = Float32MultiArray() - # Layout: [erpm, duty, voltage, current] - msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current] - - if side == "left": + # Layout: [erpm, current, duty] + msg.data = [status.erpm, status.current, status.duty] + if node_id == self._left_vesc_id: self._pub_vesc_left.publish(msg) - else: + elif node_id == self._right_vesc_id: self._pub_vesc_right.publish(msg) - # ── Status helper ───────────────────────────────────────────────────── + # -- Status helper -- def _publish_status(self, status: str) -> None: msg = String() msg.data = status self._pub_status.publish(msg) - # ── Shutdown ────────────────────────────────────────────────────────── + # -- Shutdown -- def destroy_node(self) -> None: """Send zero velocity and shut down the CAN bus cleanly.""" if self._connected and self._bus is not None: try: self._send_can( - MAMBA_CMD_VELOCITY, - encode_velocity_cmd(0.0, 0.0), + ORIN_CMD_DRIVE, + encode_drive_cmd(0, 0, mode=MODE_IDLE), "shutdown", ) - self._send_can( - MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown" - ) except Exception: pass try: diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py deleted file mode 100644 index 804ae29..0000000 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py +++ /dev/null @@ -1,224 +0,0 @@ -#!/usr/bin/env python3 -""" -mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller -and VESC telemetry. - -CAN message layout ------------------- -Command frames (Orin → Mamba / 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 - -Telemetry frames (Mamba → 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) - -VESC telemetry frame (VESC → Orin): - VESC_TELEM_STATE 0x300 16 bytes erpm (f32) | duty (f32) | voltage (f32) | current (f32) - -All multi-byte fields are big-endian. - -Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674 -""" - -import struct -from dataclasses import dataclass -from typing import Tuple - -# --------------------------------------------------------------------------- -# CAN message IDs -# --------------------------------------------------------------------------- - -MAMBA_CMD_VELOCITY: int = 0x100 -MAMBA_CMD_MODE: int = 0x101 -MAMBA_CMD_ESTOP: int = 0x102 - -MAMBA_TELEM_IMU: int = 0x200 -MAMBA_TELEM_BATTERY: int = 0x201 - -VESC_TELEM_STATE: int = 0x300 -ORIN_CAN_ID_PID_SET: int = 0x305 -ORIN_CAN_ID_FC_PID_ACK: int = 0x405 - -# --------------------------------------------------------------------------- -# Mode constants -# --------------------------------------------------------------------------- - -MODE_IDLE: int = 0 -MODE_DRIVE: int = 1 -MODE_ESTOP: int = 2 - -# --------------------------------------------------------------------------- -# Data classes for decoded telemetry -# --------------------------------------------------------------------------- - - -@dataclass -class ImuTelemetry: - """Decoded IMU telemetry from Mamba (MAMBA_TELEM_IMU).""" - - accel_x: float = 0.0 # m/s² - accel_y: float = 0.0 - accel_z: float = 0.0 - gyro_x: float = 0.0 # rad/s - gyro_y: float = 0.0 - gyro_z: float = 0.0 - - -@dataclass -class BatteryTelemetry: - """Decoded battery telemetry from Mamba (MAMBA_TELEM_BATTERY).""" - - voltage: float = 0.0 # V - current: float = 0.0 # A - - -@dataclass -class VescStateTelemetry: - """Decoded VESC state telemetry (VESC_TELEM_STATE).""" - - erpm: float = 0.0 # electrical RPM - duty: float = 0.0 # duty cycle [-1.0, 1.0] - voltage: float = 0.0 # bus voltage, V - current: float = 0.0 # phase current, A - - -@dataclass -class PidGains: - """Balance PID gains (Issue #693).""" - kp: float = 0.0 - ki: float = 0.0 - kd: float = 0.0 - - -# --------------------------------------------------------------------------- -# Encode helpers -# --------------------------------------------------------------------------- - -_FMT_VEL = ">ff" # 2 × float32, big-endian -_FMT_MODE = ">B" # 1 × uint8 -_FMT_ESTOP = ">B" # 1 × uint8 -_FMT_IMU = ">ffffff" # 6 × float32 -_FMT_BAT = ">ff" # 2 × float32 -_FMT_VESC = ">ffff" # 4 × float32 - - -def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes: - """ - Encode a MAMBA_CMD_VELOCITY payload. - - Parameters - ---------- - left_mps: target left wheel speed in m/s (positive = forward) - right_mps: target right wheel speed in m/s (positive = forward) - - Returns - ------- - 8-byte big-endian payload suitable for a CAN frame. - """ - return struct.pack(_FMT_VEL, float(left_mps), float(right_mps)) - - -def encode_mode_cmd(mode: int) -> bytes: - """ - Encode a MAMBA_CMD_MODE payload. - - Parameters - ---------- - mode: one of MODE_IDLE (0), MODE_DRIVE (1), MODE_ESTOP (2) - - Returns - ------- - 1-byte payload. - """ - if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): - raise ValueError(f"Invalid mode {mode!r}; expected 0, 1, or 2") - return struct.pack(_FMT_MODE, mode) - - -def encode_estop_cmd(stop: bool = True) -> bytes: - """ - Encode a MAMBA_CMD_ESTOP payload. - - Parameters - ---------- - stop: True to assert e-stop, False to clear. - - Returns - ------- - 1-byte payload (0x01 = stop, 0x00 = clear). - """ - return struct.pack(_FMT_ESTOP, 0x01 if stop else 0x00) - - -def encode_pid_set_cmd(kp: float, ki: float, kd: float) -> bytes: - """Encode ORIN_CAN_ID_PID_SET (6 bytes, uint16 BE x3). Issue #693.""" - if kp < 0.0 or ki < 0.0 or kd < 0.0: - raise ValueError("PID gains must be non-negative") - return struct.pack(_FMT_PID, round(min(kp,_PID_KP_MAX)*100), round(min(ki,_PID_KI_MAX)*100), round(min(kd,_PID_KD_MAX)*100)) - - -# --------------------------------------------------------------------------- -# Decode helpers -# --------------------------------------------------------------------------- - -def decode_imu_telem(data: bytes) -> ImuTelemetry: - """ - Decode a MAMBA_TELEM_IMU payload. - - Parameters - ---------- - data: exactly 24 bytes (6 × float32, big-endian). - - Returns - ------- - ImuTelemetry dataclass instance. - - Raises - ------ - struct.error if data is the wrong length. - """ - ax, ay, az, gx, gy, gz = struct.unpack(_FMT_IMU, data) - return ImuTelemetry( - accel_x=ax, accel_y=ay, accel_z=az, - gyro_x=gx, gyro_y=gy, gyro_z=gz, - ) - - -def decode_battery_telem(data: bytes) -> BatteryTelemetry: - """ - Decode a MAMBA_TELEM_BATTERY payload. - - Parameters - ---------- - data: exactly 8 bytes (2 × float32, big-endian). - - Returns - ------- - BatteryTelemetry dataclass instance. - """ - voltage, current = struct.unpack(_FMT_BAT, data) - return BatteryTelemetry(voltage=voltage, current=current) - - -def decode_vesc_state(data: bytes) -> VescStateTelemetry: - """ - Decode a VESC_TELEM_STATE payload. - - Parameters - ---------- - data: exactly 16 bytes (4 × float32, big-endian). - - Returns - ------- - VescStateTelemetry dataclass instance. - """ - erpm, duty, voltage, current = struct.unpack(_FMT_VESC, data) - return VescStateTelemetry(erpm=erpm, duty=duty, voltage=voltage, current=current) - - -def decode_pid_ack(data: bytes) -> PidGains: - """Decode ORIN_CAN_ID_FC_PID_ACK (6 bytes). Issue #693.""" - kp_x100, ki_x100, kd_x100 = struct.unpack(_FMT_PID, data) - return PidGains(kp=kp_x100/100.0, ki=ki_x100/100.0, kd=kd_x100/100.0) diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/setup.py b/jetson/ros2_ws/src/saltybot_can_bridge/setup.py index 88161c9..d5fb4c1 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/setup.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/setup.py @@ -1,21 +1,22 @@ -from setuptools import setup +from setuptools import find_packages, setup package_name = "saltybot_can_bridge" setup( name=package_name, - version="0.1.0", - packages=[package_name], + version="0.0.1", + packages=find_packages(exclude=["test"]), data_files=[ - ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), - (f"share/{package_name}", ["package.xml"]), - (f"share/{package_name}/config", ["config/can_bridge_params.yaml"]), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/config", ["config/can_bridge_params.yaml"]), + ("share/" + package_name + "/launch", ["launch/can_bridge.launch.py"]), ], - install_requires=["setuptools", "python-can"], + install_requires=["setuptools"], zip_safe=True, - maintainer="sl-controls", - maintainer_email="sl-controls@saltylab.local", - description="CAN bus bridge for Mamba controller and VESC telemetry", + maintainer="seb", + maintainer_email="seb@example.com", + description="CAN bus bridge for ESP32 BALANCE and VESC telemetry", license="MIT", tests_require=["pytest"], entry_points={ 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 2b8f9bf..efb3908 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 @@ -1,8 +1,8 @@ #!/usr/bin/env python3 """ -Unit tests for saltybot_can_bridge.mamba_protocol. +Unit tests for saltybot_can_bridge.balance_protocol. -No ROS2 or CAN hardware required — tests exercise encode/decode round-trips +No ROS2 or CAN hardware required -- tests exercise encode/decode round-trips and boundary conditions entirely in Python. Run with: pytest test/test_can_bridge.py -v @@ -11,243 +11,165 @@ Run with: pytest test/test_can_bridge.py -v import struct import unittest -from saltybot_can_bridge.mamba_protocol import ( - MAMBA_CMD_ESTOP, - MAMBA_CMD_MODE, - MAMBA_CMD_VELOCITY, - MAMBA_TELEM_BATTERY, - MAMBA_TELEM_IMU, - VESC_TELEM_STATE, - MODE_DRIVE, - MODE_ESTOP, +from saltybot_can_bridge.balance_protocol import ( + ORIN_CMD_DRIVE, + ORIN_CMD_ARM, + ORIN_CMD_PID, + ORIN_CMD_ESTOP, + ESP32_TELEM_ATTITUDE, + ESP32_TELEM_BATTERY, + VESC_LEFT_ID, + VESC_RIGHT_ID, + VESC_STATUS_1, MODE_IDLE, + MODE_DRIVE, + MODE_AUTONOMOUS, + AttitudeTelemetry, BatteryTelemetry, - ImuTelemetry, - VescStateTelemetry, - decode_battery_telem, - decode_imu_telem, - decode_vesc_state, + VescStatus1, + decode_attitude, + decode_battery, + decode_vesc_status1, + encode_drive_cmd, + encode_arm_cmd, encode_estop_cmd, - encode_mode_cmd, - encode_velocity_cmd, ) class TestMessageIDs(unittest.TestCase): - """Verify the CAN message ID constants are correct.""" + """Verify CAN message ID constants match the spec.""" - def test_command_ids(self): - self.assertEqual(MAMBA_CMD_VELOCITY, 0x100) - self.assertEqual(MAMBA_CMD_MODE, 0x101) - self.assertEqual(MAMBA_CMD_ESTOP, 0x102) + def test_orin_cmd_ids(self): + self.assertEqual(ORIN_CMD_DRIVE, 0x300) + self.assertEqual(ORIN_CMD_ARM, 0x301) + self.assertEqual(ORIN_CMD_PID, 0x302) + self.assertEqual(ORIN_CMD_ESTOP, 0x303) - def test_telemetry_ids(self): - self.assertEqual(MAMBA_TELEM_IMU, 0x200) - self.assertEqual(MAMBA_TELEM_BATTERY, 0x201) - self.assertEqual(VESC_TELEM_STATE, 0x300) + def test_esp32_telem_ids(self): + self.assertEqual(ESP32_TELEM_ATTITUDE, 0x400) + self.assertEqual(ESP32_TELEM_BATTERY, 0x401) + + def test_vesc_ids(self): + self.assertEqual(VESC_LEFT_ID, 56) + self.assertEqual(VESC_RIGHT_ID, 68) + self.assertEqual(VESC_STATUS_1, 9) -class TestVelocityEncode(unittest.TestCase): - """Tests for encode_velocity_cmd.""" +class TestDriveEncode(unittest.TestCase): + """Tests for encode_drive_cmd.""" def test_zero_velocity(self): - payload = encode_velocity_cmd(0.0, 0.0) + payload = encode_drive_cmd(0, 0) self.assertEqual(len(payload), 8) - left, right = struct.unpack(">ff", payload) - self.assertAlmostEqual(left, 0.0, places=5) - self.assertAlmostEqual(right, 0.0, places=5) + speed, steer, mode, flags, pad = struct.unpack(">hhBBH", payload) + self.assertEqual(speed, 0) + self.assertEqual(steer, 0) + self.assertEqual(mode, MODE_DRIVE) - def test_forward_velocity(self): - payload = encode_velocity_cmd(1.5, 1.5) - left, right = struct.unpack(">ff", payload) - self.assertAlmostEqual(left, 1.5, places=5) - self.assertAlmostEqual(right, 1.5, places=5) + def test_forward(self): + payload = encode_drive_cmd(500, 0) + speed, steer, mode, flags, pad = struct.unpack(">hhBBH", payload) + self.assertEqual(speed, 500) + self.assertEqual(steer, 0) - def test_differential_velocity(self): - payload = encode_velocity_cmd(-0.5, 0.5) - left, right = struct.unpack(">ff", payload) - self.assertAlmostEqual(left, -0.5, places=5) - self.assertAlmostEqual(right, 0.5, places=5) + def test_clamping_high(self): + payload = encode_drive_cmd(9999, 9999) + speed, steer, mode, flags, pad = struct.unpack(">hhBBH", payload) + self.assertEqual(speed, 1000) + self.assertEqual(steer, 1000) - def test_large_velocity(self): - # No clamping at the protocol layer — values are sent as-is - payload = encode_velocity_cmd(10.0, -10.0) - left, right = struct.unpack(">ff", payload) - self.assertAlmostEqual(left, 10.0, places=3) - self.assertAlmostEqual(right, -10.0, places=3) + def test_clamping_low(self): + payload = encode_drive_cmd(-9999, -9999) + speed, steer, mode, flags, pad = struct.unpack(">hhBBH", payload) + self.assertEqual(speed, -1000) + self.assertEqual(steer, -1000) - def test_payload_is_big_endian(self): - # Sanity check: first 4 bytes encode left speed - payload = encode_velocity_cmd(1.0, 0.0) - (left,) = struct.unpack(">f", payload[:4]) - self.assertAlmostEqual(left, 1.0, places=5) + def test_mode_idle(self): + payload = encode_drive_cmd(0, 0, mode=MODE_IDLE) + speed, steer, mode_byte, flags, pad = struct.unpack(">hhBBH", payload) + self.assertEqual(mode_byte, MODE_IDLE) -class TestModeEncode(unittest.TestCase): - """Tests for encode_mode_cmd.""" +class TestArmEncode(unittest.TestCase): + """Tests for encode_arm_cmd.""" - def test_idle_mode(self): - payload = encode_mode_cmd(MODE_IDLE) - self.assertEqual(payload, b"\x00") + def test_arm(self): + payload = encode_arm_cmd(True) + self.assertEqual(len(payload), 1) + self.assertEqual(payload[0], 0x01) - def test_drive_mode(self): - payload = encode_mode_cmd(MODE_DRIVE) - self.assertEqual(payload, b"\x01") - - def test_estop_mode(self): - payload = encode_mode_cmd(MODE_ESTOP) - self.assertEqual(payload, b"\x02") - - def test_invalid_mode_raises(self): - with self.assertRaises(ValueError): - encode_mode_cmd(99) - - def test_invalid_mode_negative_raises(self): - with self.assertRaises(ValueError): - encode_mode_cmd(-1) + def test_disarm(self): + payload = encode_arm_cmd(False) + self.assertEqual(len(payload), 1) + self.assertEqual(payload[0], 0x00) class TestEstopEncode(unittest.TestCase): """Tests for encode_estop_cmd.""" - def test_estop_assert(self): - self.assertEqual(encode_estop_cmd(True), b"\x01") - - def test_estop_clear(self): - self.assertEqual(encode_estop_cmd(False), b"\x00") - - def test_estop_default_is_stop(self): - self.assertEqual(encode_estop_cmd(), b"\x01") + def test_estop_magic_byte(self): + payload = encode_estop_cmd() + self.assertEqual(len(payload), 1) + self.assertEqual(payload[0], 0xE5) -class TestImuDecodeRoundTrip(unittest.TestCase): - """Round-trip tests for IMU telemetry.""" +class TestAttitudeDecode(unittest.TestCase): + """Tests for decode_attitude (0x400).""" - def _encode_imu(self, ax, ay, az, gx, gy, gz) -> bytes: - return struct.pack(">ffffff", ax, ay, az, gx, gy, gz) + def test_roundtrip(self): + import struct as _s + # Encode a known attitude: pitch=12.5, speed=1.5, yaw_rate=-0.5, state=1, flags=0 + fmt = ">eeeBB" + raw = _s.pack(fmt, 12.5, 1.5, -0.5, 1, 0) + b"\x00" # pad to 8 bytes + att = decode_attitude(raw) + self.assertAlmostEqual(att.pitch_deg, 12.5, places=1) + self.assertAlmostEqual(att.speed, 1.5, places=1) + self.assertEqual(att.state, 1) - def test_zero_imu(self): - data = self._encode_imu(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - telem = decode_imu_telem(data) - self.assertIsInstance(telem, ImuTelemetry) - self.assertAlmostEqual(telem.accel_x, 0.0, places=5) - self.assertAlmostEqual(telem.gyro_z, 0.0, places=5) - - def test_nominal_imu(self): - data = self._encode_imu(0.1, -0.2, 9.81, 0.01, -0.02, 0.03) - telem = decode_imu_telem(data) - self.assertAlmostEqual(telem.accel_x, 0.1, places=4) - self.assertAlmostEqual(telem.accel_y, -0.2, places=4) - self.assertAlmostEqual(telem.accel_z, 9.81, places=3) - self.assertAlmostEqual(telem.gyro_x, 0.01, places=5) - self.assertAlmostEqual(telem.gyro_y, -0.02, places=5) - self.assertAlmostEqual(telem.gyro_z, 0.03, places=5) - - def test_imu_bad_length_raises(self): - with self.assertRaises(struct.error): - decode_imu_telem(b"\x00" * 10) # too short + def test_too_short_raises(self): + with self.assertRaises(ValueError): + decode_attitude(b"\x00" * 3) -class TestBatteryDecodeRoundTrip(unittest.TestCase): - """Round-trip tests for battery telemetry.""" +class TestBatteryDecode(unittest.TestCase): + """Tests for decode_battery (0x401).""" - def _encode_bat(self, voltage, current) -> bytes: - return struct.pack(">ff", voltage, current) + def test_roundtrip(self): + import struct as _s + raw = _s.pack(">HBb", 24000, 0, -65) + bat = decode_battery(raw) + self.assertEqual(bat.vbat_mv, 24000) + self.assertEqual(bat.fault_code, 0) + self.assertEqual(bat.rssi, -65) - def test_nominal_battery(self): - data = self._encode_bat(24.6, 3.2) - telem = decode_battery_telem(data) - self.assertIsInstance(telem, BatteryTelemetry) - self.assertAlmostEqual(telem.voltage, 24.6, places=3) - self.assertAlmostEqual(telem.current, 3.2, places=4) - - def test_zero_battery(self): - data = self._encode_bat(0.0, 0.0) - telem = decode_battery_telem(data) - self.assertAlmostEqual(telem.voltage, 0.0, places=5) - self.assertAlmostEqual(telem.current, 0.0, places=5) - - def test_max_voltage(self): - # 6S LiPo max ~25.2 V; test with a high value - data = self._encode_bat(25.2, 0.0) - telem = decode_battery_telem(data) - self.assertAlmostEqual(telem.voltage, 25.2, places=3) - - def test_battery_bad_length_raises(self): - with self.assertRaises(struct.error): - decode_battery_telem(b"\x00" * 4) # too short + def test_too_short_raises(self): + with self.assertRaises(ValueError): + decode_battery(b"\x00" * 2) -class TestVescStateDecodeRoundTrip(unittest.TestCase): - """Round-trip tests for VESC state telemetry.""" +class TestVescStatus1Decode(unittest.TestCase): + """Tests for decode_vesc_status1.""" - def _encode_vesc(self, erpm, duty, voltage, current) -> bytes: - return struct.pack(">ffff", erpm, duty, voltage, current) - - def test_nominal_vesc(self): - data = self._encode_vesc(3000.0, 0.25, 24.0, 5.5) - telem = decode_vesc_state(data) - self.assertIsInstance(telem, VescStateTelemetry) - self.assertAlmostEqual(telem.erpm, 3000.0, places=2) - self.assertAlmostEqual(telem.duty, 0.25, places=5) - self.assertAlmostEqual(telem.voltage, 24.0, places=4) - self.assertAlmostEqual(telem.current, 5.5, places=4) - - def test_zero_vesc(self): - data = self._encode_vesc(0.0, 0.0, 0.0, 0.0) - telem = decode_vesc_state(data) - self.assertAlmostEqual(telem.erpm, 0.0) - self.assertAlmostEqual(telem.duty, 0.0) - - def test_reverse_erpm(self): - data = self._encode_vesc(-1500.0, -0.15, 23.0, 2.0) - telem = decode_vesc_state(data) - self.assertAlmostEqual(telem.erpm, -1500.0, places=2) - self.assertAlmostEqual(telem.duty, -0.15, places=5) - - def test_vesc_bad_length_raises(self): - with self.assertRaises(struct.error): - decode_vesc_state(b"\x00" * 8) # too short (need 16) + def test_roundtrip(self): + import struct as _s + erpm = 12000 + cur_x10 = 35 # 3.5 A + duty_x1000 = 450 # 0.45 + raw = _s.pack(">ihh", erpm, cur_x10, duty_x1000) + st = decode_vesc_status1(VESC_LEFT_ID, raw) + self.assertEqual(st.node_id, VESC_LEFT_ID) + self.assertAlmostEqual(st.erpm, 12000.0, places=1) + self.assertAlmostEqual(st.current, 3.5, places=3) + self.assertAlmostEqual(st.duty, 0.45, places=3) -class TestEncodeDecodeIdentity(unittest.TestCase): - """Cross-module identity tests: encode then decode must be identity.""" +class TestModeConstants(unittest.TestCase): + """Tests for mode byte constants.""" - def test_velocity_identity(self): - """encode_velocity_cmd raw bytes must decode to the same floats.""" - left, right = 0.75, -0.3 - payload = encode_velocity_cmd(left, right) - decoded_l, decoded_r = struct.unpack(">ff", payload) - self.assertAlmostEqual(decoded_l, left, places=5) - self.assertAlmostEqual(decoded_r, right, places=5) - - def test_imu_identity(self): - accel = (0.5, -0.5, 9.8) - gyro = (0.1, -0.1, 0.2) - raw = struct.pack(">ffffff", *accel, *gyro) - telem = decode_imu_telem(raw) - self.assertAlmostEqual(telem.accel_x, accel[0], places=4) - self.assertAlmostEqual(telem.accel_y, accel[1], places=4) - self.assertAlmostEqual(telem.accel_z, accel[2], places=3) - self.assertAlmostEqual(telem.gyro_x, gyro[0], places=5) - self.assertAlmostEqual(telem.gyro_y, gyro[1], places=5) - self.assertAlmostEqual(telem.gyro_z, gyro[2], places=5) - - def test_battery_identity(self): - voltage, current = 22.4, 8.1 - raw = struct.pack(">ff", voltage, current) - telem = decode_battery_telem(raw) - self.assertAlmostEqual(telem.voltage, voltage, places=3) - self.assertAlmostEqual(telem.current, current, places=4) - - def test_vesc_identity(self): - erpm, duty, voltage, current = 5000.0, 0.5, 24.5, 10.0 - raw = struct.pack(">ffff", erpm, duty, voltage, current) - telem = decode_vesc_state(raw) - self.assertAlmostEqual(telem.erpm, erpm, places=2) - self.assertAlmostEqual(telem.duty, duty, places=5) - self.assertAlmostEqual(telem.voltage, voltage, places=3) - self.assertAlmostEqual(telem.current, current, places=4) + def test_mode_values(self): + self.assertEqual(MODE_IDLE, 0) + self.assertEqual(MODE_DRIVE, 1) + self.assertEqual(MODE_AUTONOMOUS, 2) if __name__ == "__main__": 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 41bdd6c..fcf052d 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 @@ -1,69 +1,56 @@ #!/usr/bin/env python3 """ protocol_defs.py — CAN message ID constants and frame builders/parsers for the -Orin↔Mamba↔VESC integration test suite. +Orin <-> ESP32 BALANCE <-> VESC integration test suite. All IDs and payload formats are derived from: - include/orin_can.h — Orin↔FC (Mamba) protocol - include/vesc_can.h — VESC CAN protocol - saltybot_can_bridge/mamba_protocol.py — existing bridge constants + saltybot_can_bridge/balance_protocol.py — canonical codec CAN IDs used in tests --------------------- -Orin → FC (Mamba) 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_MODE 0x302 uint8 mode byte - ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR) +Orin → ESP32 BALANCE commands (standard 11-bit): + ORIN_CMD_DRIVE 0x300 left_mps (f32 LE) | right_mps (f32 LE) + ORIN_CMD_MODE 0x301 uint8 mode (0=idle, 1=drive, 2=estop) + ORIN_CMD_ESTOP 0x302 uint8 flags (bit0=stop, bit1=clear) + ORIN_CMD_LED 0x303 uint8 pattern | r | g | b -FC (Mamba) → 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 +ESP32 BALANCE → Orin telemetry (standard 11-bit): + FC_STATUS 0x400 8 bytes (pitch_x10 i16 | motor_cmd u16 | vbat_mv u16 | state u8 | flags u8) + FC_VESC 0x401 8 bytes (l_rpm_x10 i16 | r_rpm_x10 i16 | l_cur_x10 i16 | r_cur_x10 i16) -Mamba ↔ VESC internal commands (matching mamba_protocol.py): - 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 - -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) +VESC STATUS (extended 29-bit): + arb_id = (VESC_STATUS_1 << 8) | vesc_node_id = (9 << 8) | node_id + Payload: int32 ERPM (BE), int16 current×10 (BE), int16 duty×1000 (BE) """ import struct from typing import Tuple # --------------------------------------------------------------------------- -# Orin → FC (Mamba) command IDs (from orin_can.h) +# Orin → ESP32 BALANCE command IDs # --------------------------------------------------------------------------- -ORIN_CMD_HEARTBEAT: int = 0x300 -ORIN_CMD_DRIVE: int = 0x301 -ORIN_CMD_MODE: int = 0x302 -ORIN_CMD_ESTOP: int = 0x303 +ORIN_CMD_DRIVE: int = 0x300 +ORIN_CMD_MODE: int = 0x301 +ORIN_CMD_ESTOP: int = 0x302 +ORIN_CMD_LED: int = 0x303 # --------------------------------------------------------------------------- -# FC (Mamba) → Orin telemetry IDs (from orin_can.h) +# ESP32 BALANCE → Orin telemetry IDs # --------------------------------------------------------------------------- FC_STATUS: int = 0x400 FC_VESC: int = 0x401 -FC_IMU: int = 0x402 -FC_BARO: int = 0x403 # --------------------------------------------------------------------------- -# Mamba → VESC internal command IDs (from mamba_protocol.py) +# VESC constants # --------------------------------------------------------------------------- -MAMBA_CMD_VELOCITY: int = 0x100 -MAMBA_CMD_MODE: int = 0x101 -MAMBA_CMD_ESTOP: int = 0x102 +VESC_STATUS_1: int = 9 # STATUS_1 packet type (upper byte of arb_id) +VESC_CMD_SET_RPM: int = 3 -MAMBA_TELEM_IMU: int = 0x200 -MAMBA_TELEM_BATTERY: int = 0x201 -VESC_TELEM_STATE: int = 0x300 +VESC_CAN_ID_LEFT: int = 56 +VESC_CAN_ID_RIGHT: int = 68 # --------------------------------------------------------------------------- # Mode constants @@ -73,242 +60,111 @@ MODE_IDLE: int = 0 MODE_DRIVE: int = 1 MODE_ESTOP: int = 2 -# --------------------------------------------------------------------------- -# VESC protocol constants (from vesc_can.h) -# --------------------------------------------------------------------------- -VESC_PKT_STATUS: int = 9 # STATUS packet type (upper byte of arb_id) -VESC_PKT_SET_RPM: int = 3 # SET_RPM packet type - -VESC_CAN_ID_LEFT: int = 56 -VESC_CAN_ID_RIGHT: int = 68 - - -def VESC_STATUS_ID(vesc_node_id: int) -> int: - """ - Return the 29-bit extended arbitration ID for a VESC STATUS frame. - - Formula (from vesc_can.h): arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id - = (9 << 8) | vesc_node_id - """ - return (VESC_PKT_STATUS << 8) | vesc_node_id - - -def VESC_SET_RPM_ID(vesc_node_id: int) -> int: - """ - Return the 29-bit extended arbitration ID for a VESC SET_RPM command. - - Formula: arb_id = (VESC_PKT_SET_RPM << 8) | vesc_node_id - = (3 << 8) | vesc_node_id - """ - return (VESC_PKT_SET_RPM << 8) | vesc_node_id +def VESC_STATUS1_ID(vesc_node_id: int) -> int: + """Return the 29-bit extended arbitration ID for a VESC STATUS_1 frame.""" + return (VESC_STATUS_1 << 8) | vesc_node_id # --------------------------------------------------------------------------- -# Frame builders — Orin → FC +# Frame builders — Orin → ESP32 BALANCE # --------------------------------------------------------------------------- -def build_heartbeat(seq: int = 0) -> bytes: - """Build a HEARTBEAT payload: uint32 sequence counter (big-endian, 4 bytes).""" - return struct.pack(">I", seq & 0xFFFFFFFF) - - -def build_drive_cmd(speed: int, steer: int) -> bytes: - """ - Build an ORIN_CMD_DRIVE payload. - - Parameters - ---------- - speed: int, −1000..+1000 (mapped directly to int16) - steer: int, −1000..+1000 - """ - return struct.pack(">hh", int(speed), int(steer)) +def build_drive_cmd(left_mps: float, right_mps: float) -> bytes: + """Build ORIN_CMD_DRIVE payload (8 bytes, 2 × float32 little-endian).""" + return struct.pack(" bytes: - """Build an ORIN_CMD_MODE payload (1 byte).""" - return struct.pack(">B", mode & 0xFF) + """Build ORIN_CMD_MODE payload (1 byte).""" + return struct.pack("B", mode & 0xFF) -def build_estop_cmd(action: int = 1) -> bytes: - """Build an ORIN_CMD_ESTOP payload. action=1 → ESTOP, 0 → CLEAR.""" - return struct.pack(">B", action & 0xFF) +def build_estop_cmd(stop: bool = True, clear: bool = False) -> bytes: + """Build ORIN_CMD_ESTOP payload (1 byte flags).""" + return struct.pack("B", (0x01 if stop else 0) | (0x02 if clear else 0)) # --------------------------------------------------------------------------- -# Frame builders — Mamba velocity commands (mamba_protocol.py encoding) -# --------------------------------------------------------------------------- - -def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes: - """ - Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). - - Matches encode_velocity_cmd() in mamba_protocol.py. - """ - return struct.pack(">ff", float(left_mps), float(right_mps)) - - -# --------------------------------------------------------------------------- -# Frame builders — FC → Orin telemetry +# Frame builders — ESP32 BALANCE → Orin telemetry # --------------------------------------------------------------------------- def build_fc_status( pitch_x10: int = 0, motor_cmd: int = 0, vbat_mv: int = 24000, - balance_state: int = 1, + state: int = 1, flags: int = 0, ) -> bytes: - """ - Build an FC_STATUS (0x400) payload. - - Layout (orin_can_fc_status_t, 8 bytes, big-endian): - int16 pitch_x10 - int16 motor_cmd - uint16 vbat_mv - uint8 balance_state - uint8 flags [bit0=estop_active, bit1=armed] - """ + """Build FC_STATUS (0x400) payload (8 bytes, big-endian).""" return struct.pack( - ">hhHBB", + ">hHHBB", int(pitch_x10), - int(motor_cmd), + int(motor_cmd) & 0xFFFF, int(vbat_mv) & 0xFFFF, - int(balance_state) & 0xFF, + int(state) & 0xFF, int(flags) & 0xFF, ) def build_fc_vesc( - left_rpm_x10: int = 0, - right_rpm_x10: int = 0, - left_current_x10: int = 0, - right_current_x10: int = 0, + l_rpm_x10: int = 0, + r_rpm_x10: int = 0, + l_cur_x10: int = 0, + r_cur_x10: int = 0, ) -> bytes: - """ - Build an FC_VESC (0x401) payload. - - Layout (orin_can_fc_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). - """ + """Build FC_VESC (0x401) payload (8 bytes, big-endian).""" return struct.pack( ">hhhh", - int(left_rpm_x10), - int(right_rpm_x10), - int(left_current_x10), - int(right_current_x10), + int(l_rpm_x10), + int(r_rpm_x10), + int(l_cur_x10), + int(r_cur_x10), ) -def build_vesc_status( - rpm: int = 0, +def build_vesc_status1( + erpm: int = 0, current_x10: int = 0, duty_x1000: int = 0, ) -> bytes: - """ - Build a VESC STATUS (packet type 9) payload. - - Layout (from vesc_can.h / VESC FW 6.x, big-endian): - int32 rpm - int16 current × 10 - int16 duty × 1000 - Total: 8 bytes. - """ - return struct.pack( - ">ihh", - int(rpm), - int(current_x10), - int(duty_x1000), - ) + """Build VESC STATUS_1 payload (8 bytes, big-endian).""" + return struct.pack(">ihh", int(erpm), int(current_x10), int(duty_x1000)) # --------------------------------------------------------------------------- # Frame parsers # --------------------------------------------------------------------------- -def parse_fc_status(data: bytes): - """ - Parse an FC_STATUS (0x400) payload. +def parse_drive_cmd(data: bytes) -> Tuple[float, float]: + """Parse ORIN_CMD_DRIVE payload → (left_mps, right_mps).""" + if len(data) < 8: + raise ValueError(f"ORIN_CMD_DRIVE needs 8 bytes, got {len(data)}") + return struct.unpack("hhHBB", data[:8] - ) + pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack(">hHHBB", data[:8]) return { "pitch_x10": pitch_x10, + "pitch_deg": pitch_x10 / 10.0, "motor_cmd": motor_cmd, "vbat_mv": vbat_mv, - "balance_state": balance_state, + "state": state, "flags": flags, - "estop_active": bool(flags & 0x01), - "armed": bool(flags & 0x02), } -def parse_fc_vesc(data: bytes): - """ - Parse an FC_VESC (0x401) payload. - - Returns - ------- - dict with keys: left_rpm_x10, right_rpm_x10, left_current_x10, - right_current_x10, left_rpm (float), right_rpm (float) - """ +def parse_vesc_status1(data: bytes): + """Parse VESC STATUS_1 payload.""" if len(data) < 8: - raise ValueError(f"FC_VESC needs 8 bytes, got {len(data)}") - left_rpm_x10, right_rpm_x10, left_cur_x10, right_cur_x10 = struct.unpack( - ">hhhh", data[:8] - ) + raise ValueError(f"VESC STATUS_1 needs 8 bytes, got {len(data)}") + erpm, cur_x10, duty_x1000 = struct.unpack(">ihh", data[:8]) return { - "left_rpm_x10": left_rpm_x10, - "right_rpm_x10": right_rpm_x10, - "left_current_x10": left_cur_x10, - "right_current_x10": right_cur_x10, - "left_rpm": left_rpm_x10 * 10.0, - "right_rpm": right_rpm_x10 * 10.0, - } - - -def parse_vesc_status(data: bytes): - """ - Parse a VESC STATUS (packet type 9) payload. - - Returns - ------- - dict with keys: rpm, current_x10, duty_x1000, current (float), duty (float) - """ - if len(data) < 8: - raise ValueError(f"VESC STATUS needs 8 bytes, got {len(data)}") - rpm, current_x10, duty_x1000 = struct.unpack(">ihh", data[:8]) - return { - "rpm": rpm, - "current_x10": current_x10, - "duty_x1000": duty_x1000, - "current": current_x10 / 10.0, + "erpm": erpm, + "current": cur_x10 / 10.0, "duty": duty_x1000 / 1000.0, } - - -def parse_velocity_cmd(data: bytes) -> Tuple[float, float]: - """ - Parse a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). - - Returns - ------- - (left_mps, right_mps) - """ - if len(data) < 8: - raise ValueError(f"MAMBA_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/setup.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py index eff1956..c499830 100644 --- a/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py @@ -1,20 +1,20 @@ -from setuptools import setup +from setuptools import find_packages, setup package_name = "saltybot_can_e2e_test" setup( name=package_name, - version="0.1.0", - packages=[package_name], + version="0.0.1", + packages=find_packages(exclude=["test"]), data_files=[ - ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), - (f"share/{package_name}", ["package.xml"]), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], install_requires=["setuptools"], zip_safe=True, - maintainer="sl-jetson", - maintainer_email="sl-jetson@saltylab.local", - description="End-to-end CAN integration tests for Orin↔Mamba↔VESC full loop", + maintainer="seb", + maintainer_email="seb@example.com", + description="Orin <-> ESP32 BALANCE <-> VESC end-to-end CAN integration tests", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py index db8c471..1967cee 100644 --- a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py @@ -1,93 +1,27 @@ -#!/usr/bin/env python3 """ -conftest.py — pytest fixtures for the saltybot_can_e2e_test suite. - -No ROS2 node infrastructure is started; all tests run purely in Python. +conftest.py — shared pytest fixtures for saltybot CAN end-to-end tests. """ -import sys -import os - -# Ensure the package root is on sys.path so relative imports work when running -# pytest directly from the saltybot_can_e2e_test/ directory. -_pkg_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) -if _pkg_root not in sys.path: - sys.path.insert(0, _pkg_root) - -# Also add the saltybot_can_bridge package so we can import mamba_protocol. -_bridge_pkg = os.path.join( - os.path.dirname(_pkg_root), "saltybot_can_bridge" -) -if _bridge_pkg not in sys.path: - sys.path.insert(0, _bridge_pkg) - import pytest -from saltybot_can_e2e_test.can_mock import MockCANBus -from saltybot_can_e2e_test.protocol_defs import ( - VESC_CAN_ID_LEFT, - VESC_CAN_ID_RIGHT, +from saltybot_can_bridge.balance_protocol import ( + encode_drive_cmd, + encode_mode_cmd, + encode_estop_cmd, + decode_fc_status, + decode_fc_vesc, + decode_vesc_status1, ) -# --------------------------------------------------------------------------- -# Core fixtures -# --------------------------------------------------------------------------- - - -@pytest.fixture(scope="function") -def mock_can_bus(): - """ - Provide a fresh MockCANBus instance per test function. - The bus is automatically shut down after each test. - """ - bus = MockCANBus(loopback=False) - yield bus - bus.shutdown() - - -@pytest.fixture(scope="function") -def loopback_can_bus(): - """ - MockCANBus in loopback mode — sent frames are also queued for recv. - Useful for testing round-trip behaviour without a second node. - """ - bus = MockCANBus(loopback=True) - yield bus - bus.shutdown() - - -@pytest.fixture(scope="function") +@pytest.fixture def bridge_components(): - """ - Return the mamba_protocol encode/decode callables and a fresh mock bus. - - Yields a dict with keys: - bus — MockCANBus instance - encode_vel — encode_velocity_cmd(left, right) → bytes - encode_mode — encode_mode_cmd(mode) → bytes - encode_estop — encode_estop_cmd(stop) → bytes - decode_vesc — decode_vesc_state(data) → VescStateTelemetry - """ - from saltybot_can_bridge.mamba_protocol import ( - encode_velocity_cmd, - encode_mode_cmd, - encode_estop_cmd, - decode_vesc_state, - decode_battery_telem, - decode_imu_telem, - ) - - bus = MockCANBus(loopback=False) + """Return a dict of encode/decode helpers from balance_protocol.""" yield { - "bus": bus, - "encode_vel": encode_velocity_cmd, + "encode_drive": encode_drive_cmd, "encode_mode": encode_mode_cmd, "encode_estop": encode_estop_cmd, - "decode_vesc": decode_vesc_state, - "decode_battery": decode_battery_telem, - "decode_imu": decode_imu_telem, - "left_vesc_id": VESC_CAN_ID_LEFT, - "right_vesc_id": VESC_CAN_ID_RIGHT, + "decode_fc_status": decode_fc_status, + "decode_fc_vesc": decode_fc_vesc, + "decode_vesc": decode_vesc_status1, } - bus.shutdown() 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 dfbee87..65aa561 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 @@ -1,193 +1,47 @@ -#!/usr/bin/env python3 """ -test_drive_command.py — Integration tests for the drive command path. - -Tests verify: - DRIVE cmd → Mamba receives velocity command frame → mock VESC status response - → FC_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 +test_drive_command.py — E2E tests for ORIN_CMD_DRIVE (0x300) encoding. """ import struct import pytest from saltybot_can_e2e_test.protocol_defs import ( - MAMBA_CMD_VELOCITY, - MAMBA_CMD_MODE, - FC_VESC, - MODE_DRIVE, + ORIN_CMD_DRIVE, + build_drive_cmd, + parse_drive_cmd, MODE_IDLE, - VESC_CAN_ID_LEFT, - VESC_CAN_ID_RIGHT, - VESC_STATUS_ID, - build_velocity_cmd, - build_fc_vesc, - build_vesc_status, - parse_velocity_cmd, - parse_fc_vesc, -) -from saltybot_can_bridge.mamba_protocol import ( - encode_velocity_cmd, - encode_mode_cmd, + MODE_DRIVE, ) -# --------------------------------------------------------------------------- -# Helper -# --------------------------------------------------------------------------- +class TestDriveCommandEncoding: -def _send_drive(bus, left_mps: float, right_mps: float) -> None: - """Simulate the bridge encoding and sending a velocity command.""" - from saltybot_can_e2e_test.can_mock import MockCANBus + def test_frame_id(self): + assert ORIN_CMD_DRIVE == 0x300 - payload = encode_velocity_cmd(left_mps, right_mps) - # Create a minimal message object compatible with our mock - class _Msg: - def __init__(self, arb_id, data): - self.arbitration_id = arb_id - 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))) - - -# --------------------------------------------------------------------------- -# Tests -# --------------------------------------------------------------------------- - -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 Mamba receives - a MAMBA_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) - assert len(vel_frames) == 1, "Expected exactly one velocity command frame" - - left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) - assert abs(left - 1.0) < 1e-4, f"Left speed {left} != 1.0" - assert abs(right - 1.0) < 1e-4, f"Right speed {right} != 1.0" - - def test_drive_forward_mode_drive_sent(self, mock_can_bus): - """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) - 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 Mamba computes RPM from m/s and broadcasts FC_VESC.) - This test checks the FC_VESC frame format and parser. - """ - # Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics) - fc_payload = build_fc_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) - - 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 parsed["left_rpm_x10"] == 300 - assert parsed["right_rpm_x10"] == 300 - assert abs(parsed["left_rpm"] - 3000.0) < 0.1 - - -class TestDriveTurn: - def test_drive_turn_differential_rpm(self, mock_can_bus): - """ - DRIVE cmd (0.5, −0.5) → verify differential RPM in velocity command. - """ - _send_drive(mock_can_bus, 0.5, -0.5) - - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) - assert len(vel_frames) == 1 - - left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) - assert abs(left - 0.5) < 1e-4, f"Left speed {left} != 0.5" - assert abs(right - (-0.5)) < 1e-4, f"Right speed {right} != -0.5" - # Signs must be opposite for a zero-radius spin - 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( - left_rpm_x10=150, right_rpm_x10=-150, - left_current_x10=30, right_current_x10=30, - ) - mock_can_bus.inject(FC_VESC, fc_payload) - frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_vesc(bytes(frame.data)) - assert parsed["left_rpm_x10"] > 0 - assert parsed["right_rpm_x10"] < 0 - - -class TestDriveZero: - def test_drive_zero_stops_motors(self, mock_can_bus): - """ - After a non-zero drive cmd, sending zero velocity must result in - RPM=0 being commanded to both VESCs. - """ - _send_drive(mock_can_bus, 1.0, 1.0) - mock_can_bus.reset() # clear prior frames - - _send_drive(mock_can_bus, 0.0, 0.0) - - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_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" - assert abs(right) < 1e-5, "Right motor not stopped" - - -class TestDriveCmdTimeout: - def test_drive_cmd_timeout_sends_zero(self, mock_can_bus): - """ - Simulate the watchdog behaviour: if no DRIVE cmd arrives for >500 ms, - 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. - zero_payload = encode_velocity_cmd(0.0, 0.0) - - class _Msg: - def __init__(self, arb_id, data): - self.arbitration_id = arb_id - 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))) - - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) - assert len(vel_frames) == 1 - left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + def test_zero_drive(self): + payload = build_drive_cmd(0.0, 0.0) + left, right = parse_drive_cmd(payload) assert abs(left) < 1e-5 assert abs(right) < 1e-5 - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) - assert len(mode_frames) == 1 - assert bytes(mode_frames[0].data) == bytes([MODE_IDLE]) + def test_forward(self): + payload = build_drive_cmd(1.5, 1.5) + left, right = parse_drive_cmd(payload) + assert abs(left - 1.5) < 1e-4 + assert abs(right - 1.5) < 1e-4 + def test_differential(self): + payload = build_drive_cmd(-0.5, 0.5) + left, right = parse_drive_cmd(payload) + assert abs(left - (-0.5)) < 1e-4 + assert abs(right - 0.5) < 1e-4 -@pytest.mark.parametrize("left_mps,right_mps", [ - (0.5, 0.5), - (1.0, 0.0), - (0.0, -1.0), - (-0.5, -0.5), -]) -def test_drive_cmd_payload_roundtrip(mock_can_bus, left_mps, right_mps): - """Parametrized: encode then decode must recover original velocities.""" - payload = encode_velocity_cmd(left_mps, right_mps) - l, r = parse_velocity_cmd(payload) - assert abs(l - left_mps) < 1e-4 - assert abs(r - right_mps) < 1e-4 + def test_payload_is_little_endian(self): + # First 4 bytes encode left speed in LE float32 + payload = build_drive_cmd(1.0, 0.0) + (left,) = struct.unpack(" None: - """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))) - - 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))) - - def send_drive(self, left_mps: float, right_mps: float) -> None: - """Send velocity command only if ESTOP is not active.""" - if self._estop_active: - # 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))) - - @property - def estop_active(self) -> bool: - return self._estop_active - - -# --------------------------------------------------------------------------- -# Tests -# --------------------------------------------------------------------------- - -class TestEstopHaltsMotors: - def test_estop_command_halts_motors(self, mock_can_bus): - """ - After injecting ESTOP, zero velocity must be commanded immediately. - """ - sm = EstopStateMachine(mock_can_bus) - sm.assert_estop() - - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_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" - assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP" - - def test_estop_mode_frame_sent(self, mock_can_bus): - """ESTOP mode byte must be broadcast on CAN.""" - sm = EstopStateMachine(mock_can_bus) - sm.assert_estop() - - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_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.""" - sm = EstopStateMachine(mock_can_bus) - sm.assert_estop() - - estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) - assert len(estop_frames) >= 1 - assert bytes(estop_frames[-1].data) == b"\x01", \ - f"ESTOP payload {estop_frames[-1].data!r} != 0x01" - - -class TestEstopPersists: - def test_estop_persists_after_drive_cmd(self, mock_can_bus): - """ - After ESTOP, injecting a DRIVE command must NOT forward velocity to the bus. - """ - sm = EstopStateMachine(mock_can_bus) - sm.assert_estop() - mock_can_bus.reset() # start fresh after initial ESTOP frames - - sm.send_drive(1.0, 1.0) # should be suppressed - - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) - assert len(vel_frames) == 0, \ - "Velocity command was forwarded while ESTOP is active" - - def test_estop_mode_unchanged_after_drive_attempt(self, mock_can_bus): - """ - After ESTOP, attempting DRIVE must not change the mode to DRIVE. - """ - sm = EstopStateMachine(mock_can_bus) - sm.assert_estop() - mock_can_bus.reset() - - 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) - assert all( - bytes(f.data) != bytes([MODE_DRIVE]) for f in mode_frames - ), "MODE=DRIVE was set despite active ESTOP" + def test_stop_bit_clear_not_set(self): + payload = build_estop_cmd(stop=True, clear=False) + assert payload[0] & 0x02 == 0x00 class TestEstopClear: - def test_estop_clear_restores_drive(self, mock_can_bus): - """After ESTOP_CLEAR, drive commands must be accepted again.""" - sm = EstopStateMachine(mock_can_bus) - sm.assert_estop() - sm.clear_estop() - mock_can_bus.reset() - sm.send_drive(0.8, 0.8) + def test_clear_bit_set(self): + payload = build_estop_cmd(stop=False, clear=True) + assert payload[0] & 0x02 == 0x02 - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_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.""" - sm = EstopStateMachine(mock_can_bus) - sm.assert_estop() - sm.clear_estop() - - estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) - assert len(estop_frames) >= 2 - # Last ESTOP frame should be the clear - assert bytes(estop_frames[-1].data) == b"\x00", \ - f"ESTOP clear payload {estop_frames[-1].data!r} != 0x00" - - def test_estop_clear_mode_returns_to_idle(self, mock_can_bus): - """After clearing ESTOP, the mode frame must be MODE_IDLE.""" - sm = EstopStateMachine(mock_can_bus) - sm.assert_estop() - sm.clear_estop() - - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_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" + def test_stop_not_set_on_clear(self): + payload = build_estop_cmd(stop=False, clear=True) + assert payload[0] & 0x01 == 0x00 -class TestFirmwareSideEstop: - def test_fc_status_estop_flag_detected(self, mock_can_bus): - """ - Simulate firmware sending estop via FC_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( - 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) +class TestAttitudeEstopFlag: - 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 parsed["estop_active"] is True, \ - "estop_active flag not set in FC_STATUS" - assert parsed["balance_state"] == 2 + def test_estop_active_flag_in_fc_status(self): + # FC_STATUS flags bit0 = estop active + raw = build_fc_status(flags=0x01) + status = parse_fc_status(raw) + assert status["flags"] & 0x01 == 0x01 - 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) - frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_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) - frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_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( - pitch_x10=150, - motor_cmd=-200, - vbat_mv=23800, - balance_state=1, - flags=0x03, - ) - mock_can_bus.inject(FC_STATUS, payload) - frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_status(bytes(frame.data)) - assert parsed["pitch_x10"] == 150 - assert parsed["motor_cmd"] == -200 - assert parsed["vbat_mv"] == 23800 - assert parsed["balance_state"] == 1 - assert parsed["estop_active"] is True - assert parsed["armed"] is True + def test_no_estop_flag(self): + raw = build_fc_status(flags=0x00) + status = parse_fc_status(raw) + assert status["flags"] & 0x01 == 0x00 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 a7e9f9a..b10a68a 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,315 +1,78 @@ -#!/usr/bin/env python3 """ -test_fc_vesc_broadcast.py — FC_VESC broadcast and VESC STATUS integration tests. +test_fc_vesc_broadcast.py — E2E tests for ESP32 BALANCE telemetry parsing. 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) - - current_x10 scaling matches protocol spec - -No ROS2 or real CAN hardware required. -Run with: python -m pytest test/test_fc_vesc_broadcast.py -v +- VESC STATUS_1 extended frames (arb_id = (9 << 8) | node_id) +- FC_STATUS (0x400) attitude/battery frame +- FC_VESC (0x401) VESC RPM/current broadcast frame """ import struct -import time -import threading import pytest -from saltybot_can_e2e_test.can_mock import MockCANBus from saltybot_can_e2e_test.protocol_defs import ( + FC_STATUS, FC_VESC, VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT, - VESC_STATUS_ID, - VESC_SET_RPM_ID, - VESC_TELEM_STATE, - build_vesc_status, + VESC_STATUS1_ID, + build_fc_status, build_fc_vesc, - parse_fc_vesc, - parse_vesc_status, -) -from saltybot_can_bridge.mamba_protocol import ( - VESC_TELEM_STATE as BRIDGE_VESC_TELEM_STATE, - decode_vesc_state, + build_vesc_status1, + parse_fc_status, + parse_vesc_status1, ) -# --------------------------------------------------------------------------- -# Helpers -# --------------------------------------------------------------------------- +class TestVescStatusParsing: -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 + def test_status1_id_left(self): + arb_id = VESC_STATUS1_ID(VESC_CAN_ID_LEFT) + assert arb_id == (9 << 8) | 56 - This represents the Mamba → Orin telemetry path. - """ + def test_status1_id_right(self): + arb_id = VESC_STATUS1_ID(VESC_CAN_ID_RIGHT) + assert arb_id == (9 << 8) | 68 - def __init__(self, bus: MockCANBus): - self._bus = bus - self._left_rpm_x10 = 0 - self._right_rpm_x10 = 0 - self._left_current_x10 = 0 - self._right_current_x10 = 0 - self._left_seen = False - self._right_seen = False + def test_parse_vesc_status1_nominal(self): + raw = build_vesc_status1(erpm=3000, current_x10=55, duty_x1000=250) + result = parse_vesc_status1(raw) + assert abs(result["erpm"] - 3000) < 1 + assert abs(result["current"] - 5.5) < 0.01 + assert abs(result["duty"] - 0.25) < 0.001 - 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. - """ - node_id = arb_id & 0xFF - parsed = parse_vesc_status(data) - rpm_x10 = parsed["rpm"] // 10 # convert full RPM to RPM/10 - - if node_id == VESC_CAN_ID_LEFT: - self._left_rpm_x10 = rpm_x10 - self._left_current_x10 = parsed["current_x10"] - self._left_seen = True - elif node_id == VESC_CAN_ID_RIGHT: - self._right_rpm_x10 = rpm_x10 - self._right_current_x10 = parsed["current_x10"] - self._right_seen = True - - # Broadcast FC_VESC whenever we receive any update - self._broadcast_fc_vesc() - - def _broadcast_fc_vesc(self) -> None: - payload = build_fc_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) + def test_parse_vesc_status1_reverse(self): + raw = build_vesc_status1(erpm=-1500, current_x10=-20, duty_x1000=-150) + result = parse_vesc_status1(raw) + assert result["erpm"] == -1500 + assert abs(result["duty"] - (-0.15)) < 0.001 -def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int, - current_x10: int = 50, duty_x1000: int = 250) -> None: - """Inject a VESC STATUS extended frame for the given node ID.""" - arb_id = VESC_STATUS_ID(vesc_id) - payload = build_vesc_status(rpm=rpm, current_x10=current_x10, duty_x1000=duty_x1000) - bus.inject(arb_id, payload, is_extended_id=True) +class TestFcStatusTelemetry: + + def test_frame_id(self): + assert FC_STATUS == 0x400 + + def test_nominal(self): + raw = build_fc_status(pitch_x10=125, motor_cmd=300, vbat_mv=24000, state=1, flags=0) + status = parse_fc_status(raw) + assert abs(status["pitch_deg"] - 12.5) < 0.01 + assert status["vbat_mv"] == 24000 + assert status["state"] == 1 + + def test_zero(self): + raw = build_fc_status(pitch_x10=0, motor_cmd=0, vbat_mv=0, state=0, flags=0) + status = parse_fc_status(raw) + assert status["pitch_x10"] == 0 -# --------------------------------------------------------------------------- -# Tests -# --------------------------------------------------------------------------- +class TestFcVescTelemetry: -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 - the correct left RPM (rpm / 10). - """ - agg = VescStatusAggregator(mock_can_bus) + def test_frame_id(self): + assert FC_VESC == 0x401 - # Left VESC: 3000 RPM → rpm_x10 = 300 - arb_id = VESC_STATUS_ID(VESC_CAN_ID_LEFT) - payload = build_vesc_status(rpm=3000, current_x10=55) - 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 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.""" - agg = VescStatusAggregator(mock_can_bus) - - arb_id = VESC_STATUS_ID(VESC_CAN_ID_RIGHT) - payload = build_vesc_status(rpm=2000, current_x10=40) - agg.process_vesc_status(arb_id, payload) - - frame = mock_can_bus.recv(timeout=0.1) - assert frame is not None - parsed = parse_fc_vesc(bytes(frame.data)) - assert parsed["right_rpm_x10"] == 200 - - def test_left_vesc_id_matches_constant(self): - """VESC_STATUS_ID(56) must equal (9 << 8) | 56 = 0x938.""" - assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == (9 << 8) | 56 - assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == 0x938 - - def test_right_vesc_id_matches_constant(self): - """VESC_STATUS_ID(68) must equal (9 << 8) | 68 = 0x944.""" - assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == (9 << 8) | 68 - assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == 0x944 - - -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. - """ - agg = VescStatusAggregator(mock_can_bus) - - # Left: 3000 RPM - agg.process_vesc_status( - VESC_STATUS_ID(VESC_CAN_ID_LEFT), - build_vesc_status(rpm=3000, current_x10=50), - ) - # Right: -1500 RPM (reverse) - agg.process_vesc_status( - VESC_STATUS_ID(VESC_CAN_ID_RIGHT), - build_vesc_status(rpm=-1500, current_x10=30), - ) - - # Drain two FC_VESC frames (one per update), check the latest - frames = [] - while True: - f = mock_can_bus.recv(timeout=0.05) - if f is None: - break - frames.append(f) - - assert len(frames) >= 2, "Expected at least 2 FC_VESC frames" - # Last frame must have both sides - last = parse_fc_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.""" - agg = VescStatusAggregator(mock_can_bus) - agg.process_vesc_status( - VESC_STATUS_ID(VESC_CAN_ID_LEFT), - build_vesc_status(rpm=1000, current_x10=55), - ) - agg.process_vesc_status( - VESC_STATUS_ID(VESC_CAN_ID_RIGHT), - build_vesc_status(rpm=1000, current_x10=42), - ) - frames = [] - while True: - f = mock_can_bus.recv(timeout=0.05) - if f is None: - break - frames.append(f) - last = parse_fc_vesc(bytes(frames[-1].data)) - assert last["left_current_x10"] == 55 - assert last["right_current_x10"] == 42 - - -class TestVescBroadcastRate: - def test_fc_vesc_broadcast_at_10hz(self, mock_can_bus): - """ - Simulate FC_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 - - timestamps = [] - stop_event = threading.Event() - - def broadcaster(): - while not stop_event.is_set(): - t = time.monotonic() - mock_can_bus.inject( - FC_VESC, - build_fc_vesc(100, -100, 30, 30), - timestamp=t, - ) - timestamps.append(t) - time.sleep(_interval) - - t = threading.Thread(target=broadcaster, daemon=True) - t.start() - time.sleep(0.15) # collect ~1.5 broadcasts - stop_event.set() - t.join(timeout=0.2) - - assert len(timestamps) >= 1, "No FC_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" - - 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) - assert len(payload) == 8 - - -class TestVescCurrentScaling: - def test_current_x10_scaling(self, mock_can_bus): - """ - Verify current_x10 scaling: 5.5 A → current_x10=55. - build_vesc_status stores current_x10 directly; parse_vesc_status - returns current = current_x10 / 10. - """ - payload = build_vesc_status(rpm=1000, current_x10=55, duty_x1000=250) - parsed = parse_vesc_status(payload) - assert parsed["current_x10"] == 55 - assert abs(parsed["current"] - 5.5) < 0.01 - - def test_current_negative_scaling(self, mock_can_bus): - """Negative current (regen) must scale correctly.""" - payload = build_vesc_status(rpm=-500, current_x10=-30) - parsed = parse_vesc_status(payload) - assert parsed["current_x10"] == -30 - 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( - left_rpm_x10=200, - right_rpm_x10=200, - left_current_x10=55, - right_current_x10=42, - ) - mock_can_bus.inject(FC_VESC, payload) - frame = mock_can_bus.recv(timeout=0.1) - parsed = parse_fc_vesc(bytes(frame.data)) - assert parsed["left_current_x10"] == 55 - assert parsed["right_current_x10"] == 42 - - @pytest.mark.parametrize("vesc_id", [VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT]) - def test_vesc_status_id_both_nodes(self, vesc_id, mock_can_bus): - """ - VESC_STATUS_ID(vesc_id) must produce the correct 29-bit extended arb_id - for both the left (56) and right (68) node IDs. - """ - expected = (9 << 8) | vesc_id - assert VESC_STATUS_ID(vesc_id) == expected - - @pytest.mark.parametrize("vesc_id,rpm,expected_rpm_x10", [ - (VESC_CAN_ID_LEFT, 3000, 300), - (VESC_CAN_ID_LEFT, -1500, -150), - (VESC_CAN_ID_RIGHT, 2000, 200), - (VESC_CAN_ID_RIGHT, 0, 0), - ]) - def test_rpm_x10_conversion_parametrized( - self, mock_can_bus, vesc_id, rpm, expected_rpm_x10 - ): - """Parametrized: verify rpm → rpm_x10 conversion for both VESCs.""" - agg = VescStatusAggregator(mock_can_bus) - agg.process_vesc_status( - VESC_STATUS_ID(vesc_id), - build_vesc_status(rpm=rpm), - ) - frame = mock_can_bus.recv(timeout=0.05) - assert frame is not None - parsed = parse_fc_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}" - else: - assert parsed["right_rpm_x10"] == expected_rpm_x10, \ - f"right_rpm_x10={parsed['right_rpm_x10']} expected {expected_rpm_x10}" + def test_nominal(self): + raw = build_fc_vesc(l_rpm_x10=300, r_rpm_x10=-300, l_cur_x10=55, r_cur_x10=-55) + l_rpm, r_rpm, l_cur, r_cur = struct.unpack(">hhhh", raw) + assert l_rpm == 300 + assert r_rpm == -300 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 adf7db2..8b100a7 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 @@ -1,238 +1,51 @@ -#!/usr/bin/env python3 """ -test_heartbeat_timeout.py — Tests for heartbeat loss and recovery. +test_heartbeat_timeout.py — E2E tests for watchdog / heartbeat timeout behavior. -Covers: - - Heartbeat loss triggers e-stop escalation (timeout logic) - - Heartbeat recovery restores previous mode - - Heartbeat interval is sent at ~100 ms - -No ROS2 or real CAN hardware required. -Run with: python -m pytest test/test_heartbeat_timeout.py -v +When no drive command is received within the timeout period, the bridge should +send zero-velocity ORIN_CMD_DRIVE with MODE_IDLE. """ -import time import struct -import threading import pytest -from saltybot_can_e2e_test.can_mock import MockCANBus from saltybot_can_e2e_test.protocol_defs import ( - ORIN_CMD_HEARTBEAT, - ORIN_CMD_ESTOP, + ORIN_CMD_DRIVE, ORIN_CMD_MODE, - MAMBA_CMD_VELOCITY, - MAMBA_CMD_MODE, - MAMBA_CMD_ESTOP, + ORIN_CMD_ESTOP, MODE_IDLE, - MODE_DRIVE, - MODE_ESTOP, - build_heartbeat, - build_estop_cmd, + build_drive_cmd, build_mode_cmd, - build_velocity_cmd, - parse_velocity_cmd, -) -from saltybot_can_bridge.mamba_protocol import ( - encode_velocity_cmd, - encode_mode_cmd, - encode_estop_cmd, + parse_drive_cmd, ) -# Heartbeat timeout from orin_can.h: 500 ms -ORIN_HB_TIMEOUT_MS = 500 -ORIN_HB_TIMEOUT_S = ORIN_HB_TIMEOUT_MS / 1000.0 -# Expected heartbeat interval -HB_INTERVAL_MS = 100 -HB_INTERVAL_S = HB_INTERVAL_MS / 1000.0 +class TestWatchdogZeroVelocity: + + def test_watchdog_sends_zero_drive(self): + """Watchdog must send zero-velocity drive command.""" + payload = build_drive_cmd(0.0, 0.0) + left, right = parse_drive_cmd(payload) + assert abs(left) < 1e-5 + assert abs(right) < 1e-5 + assert len(payload) == 8 + + def test_watchdog_drive_id(self): + assert ORIN_CMD_DRIVE == 0x300 + + def test_watchdog_mode_idle(self): + payload = build_mode_cmd(MODE_IDLE) + assert payload == b"\x00" + + def test_watchdog_mode_id(self): + assert ORIN_CMD_MODE == 0x301 -# --------------------------------------------------------------------------- -# Helpers -# --------------------------------------------------------------------------- +class TestEstopOnTimeout: -class HeartbeatSimulator: - """ - Simulate periodic heartbeat injection into a MockCANBus. + def test_estop_frame_id(self): + assert ORIN_CMD_ESTOP == 0x302 - Call start() to begin sending heartbeats every interval_s. - Call stop() to cease — after ORIN_HB_TIMEOUT_S the firmware would declare - Orin offline. - """ - - def __init__(self, bus: MockCANBus, interval_s: float = HB_INTERVAL_S): - self._bus = bus - self._interval_s = interval_s - self._seq = 0 - self._running = False - self._thread: threading.Thread | None = None - - def start(self): - self._running = True - self._thread = threading.Thread(target=self._run, daemon=True) - self._thread.start() - - def stop(self): - self._running = False - - def _run(self): - while self._running: - self._bus.inject( - ORIN_CMD_HEARTBEAT, - build_heartbeat(self._seq), - is_extended_id=False, - ) - self._seq += 1 - time.sleep(self._interval_s) - - -def _simulate_estop_on_timeout(bus: MockCANBus) -> None: - """ - Simulate the firmware-side logic: when heartbeat timeout expires, - the FC sends an e-stop command by setting estop mode on the Mamba bus. - We model this as the bridge sending zero velocity + ESTOP mode. - """ - - class _Msg: - def __init__(self, arb_id, data): - self.arbitration_id = arb_id - 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))) - - -# --------------------------------------------------------------------------- -# Tests -# --------------------------------------------------------------------------- - -class TestHeartbeatLoss: - def test_heartbeat_loss_triggers_estop(self, mock_can_bus): - """ - After heartbeat ceases, the bridge must command zero velocity and - set ESTOP mode. We simulate this directly using _simulate_estop_on_timeout. - """ - # First confirm the bus is clean - assert len(mock_can_bus.get_sent_frames()) == 0 - - # 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) - 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) - 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) - 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) - assert len(vel_frames) >= 1 - for f in vel_frames: - l, r = parse_velocity_cmd(bytes(f.data)) - assert abs(l) < 1e-5 - assert abs(r) < 1e-5 - - -class TestHeartbeatRecovery: - def test_heartbeat_recovery_restores_drive_mode(self, mock_can_bus): - """ - After heartbeat loss + recovery, drive commands must be accepted again. - We simulate: ESTOP → clear estop → send drive → verify velocity frame. - """ - - class _Msg: - def __init__(self, arb_id, data): - self.arbitration_id = arb_id - self.data = bytearray(data) - self.is_extended_id = False - - # Phase 1: timeout → estop - _simulate_estop_on_timeout(mock_can_bus) - 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))) - - estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_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) - 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) - assert len(vel_frames) >= 1 - l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) - assert abs(l - 0.5) < 1e-4 - - def test_heartbeat_sequence_increments(self, mock_can_bus): - """Heartbeat payloads must have incrementing sequence numbers.""" - payloads = [] - for seq in range(5): - mock_can_bus.inject( - ORIN_CMD_HEARTBEAT, - build_heartbeat(seq), - is_extended_id=False, - ) - - for i in range(5): - frame = mock_can_bus.recv(timeout=0.05) - assert frame is not None - (seq_val,) = struct.unpack(">I", bytes(frame.data)) - assert seq_val == i, f"Expected seq {i}, got {seq_val}" - - -class TestHeartbeatInterval: - def test_heartbeat_interval_approx_100ms(self, mock_can_bus): - """ - Start HeartbeatSimulator, collect timestamps over ~300 ms, and verify - the average interval is within 20% of 100 ms. - """ - sim = HeartbeatSimulator(mock_can_bus, interval_s=0.1) - sim.start() - time.sleep(0.35) - sim.stop() - - timestamps = [] - while True: - frame = mock_can_bus.recv(timeout=0.01) - if frame is None: - break - if frame.arbitration_id == ORIN_CMD_HEARTBEAT: - timestamps.append(frame.timestamp) - - assert len(timestamps) >= 2, "Not enough heartbeat frames captured" - - intervals = [ - timestamps[i + 1] - timestamps[i] - for i in range(len(timestamps) - 1) - ] - avg_interval = sum(intervals) / len(intervals) - # Allow ±30 ms tolerance (OS scheduling jitter in CI) - assert 0.07 <= avg_interval <= 0.13, \ - f"Average heartbeat interval {avg_interval*1000:.1f} ms not ~100 ms" - - def test_heartbeat_payload_is_4_bytes(self, mock_can_bus): - """Heartbeat payload must be exactly 4 bytes (uint32 sequence).""" - for seq in (0, 1, 0xFFFFFFFF): - payload = build_heartbeat(seq) - assert len(payload) == 4, \ - f"Heartbeat payload length {len(payload)} != 4" + def test_estop_stop_flag(self): + from saltybot_can_e2e_test.protocol_defs import build_estop_cmd + payload = build_estop_cmd(stop=True, clear=False) + assert payload[0] & 0x01 == 0x01 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 d87ab29..59b4a0a 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 @@ -1,236 +1,63 @@ -#!/usr/bin/env python3 """ -test_mode_switching.py — Mode transition integration tests. - -Covers: - - idle → drive: drive commands become accepted - - drive → estop: immediate motor stop - - MODE frame byte values match protocol constants - - Unknown mode byte is ignored (no state change) - -No ROS2 or real CAN hardware required. -Run with: python -m pytest test/test_mode_switching.py -v +test_mode_switching.py — E2E tests for ORIN_CMD_MODE (0x301) encoding. """ -import struct 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, + ORIN_CMD_MODE, MODE_IDLE, MODE_DRIVE, MODE_ESTOP, build_mode_cmd, - build_velocity_cmd, - parse_velocity_cmd, ) -from saltybot_can_bridge.mamba_protocol import ( - encode_velocity_cmd, - encode_mode_cmd, - encode_estop_cmd, -) - - -# --------------------------------------------------------------------------- -# Helpers -# --------------------------------------------------------------------------- - -class _Msg: - def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False): - self.arbitration_id = arb_id - self.data = bytearray(data) - self.is_extended_id = is_extended_id - - -class ModeStateMachine: - """ - Minimal state machine tracking mode transitions and gating commands. - """ - - def __init__(self, bus: MockCANBus): - self._bus = bus - self._mode = MODE_IDLE - - def set_mode(self, mode: int) -> bool: - """ - Transition to mode. Returns True if accepted, False if invalid. - Invalid mode values (not 0, 1, 2) are ignored. - """ - if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): - return False # silently ignore - - prev_mode = self._mode - self._mode = mode - self._bus.send(_Msg(MAMBA_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))) - - return True - - def send_drive(self, left_mps: float, right_mps: float) -> bool: - """ - Send a velocity command. Returns True if forwarded, False if blocked. - """ - if self._mode != MODE_DRIVE: - return False - self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) - return True - - @property - def mode(self) -> int: - return self._mode - - -# --------------------------------------------------------------------------- -# Tests -# --------------------------------------------------------------------------- - -class TestIdleToDrive: - def test_idle_to_drive_mode_frame(self, mock_can_bus): - """Transitioning to DRIVE must emit a MODE=drive frame.""" - sm = ModeStateMachine(mock_can_bus) - sm.set_mode(MODE_DRIVE) - - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) - assert len(mode_frames) == 1 - assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE]) - - def test_idle_blocks_drive_commands(self, mock_can_bus): - """In IDLE mode, drive commands must be suppressed.""" - sm = ModeStateMachine(mock_can_bus) - # Attempt drive without entering DRIVE mode - 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) - assert len(vel_frames) == 0 - - def test_drive_mode_allows_commands(self, mock_can_bus): - """After entering DRIVE mode, velocity commands must be forwarded.""" - sm = ModeStateMachine(mock_can_bus) - sm.set_mode(MODE_DRIVE) - mock_can_bus.reset() - - 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) - assert len(vel_frames) == 1 - l, r = parse_velocity_cmd(bytes(vel_frames[0].data)) - assert abs(l - 0.5) < 1e-4 - assert abs(r - 0.5) < 1e-4 - - -class TestDriveToEstop: - def test_drive_to_estop_stops_motors(self, mock_can_bus): - """Transitioning DRIVE → ESTOP must immediately send zero velocity.""" - sm = ModeStateMachine(mock_can_bus) - sm.set_mode(MODE_DRIVE) - sm.send_drive(1.0, 1.0) - mock_can_bus.reset() - - sm.set_mode(MODE_ESTOP) - - vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_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" - assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP" - - def test_drive_to_estop_mode_frame(self, mock_can_bus): - """DRIVE → ESTOP must broadcast MODE=estop.""" - sm = ModeStateMachine(mock_can_bus) - sm.set_mode(MODE_DRIVE) - sm.set_mode(MODE_ESTOP) - - mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_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): - """After DRIVE → ESTOP, drive commands must be blocked.""" - sm = ModeStateMachine(mock_can_bus) - sm.set_mode(MODE_DRIVE) - sm.set_mode(MODE_ESTOP) - mock_can_bus.reset() - - 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) - assert len(vel_frames) == 0 class TestModeCommandEncoding: - def test_mode_idle_byte(self, mock_can_bus): - """MODE_IDLE must encode as 0x00.""" - assert encode_mode_cmd(MODE_IDLE) == b"\x00" - def test_mode_drive_byte(self, mock_can_bus): - """MODE_DRIVE must encode as 0x01.""" - assert encode_mode_cmd(MODE_DRIVE) == b"\x01" + def test_frame_id(self): + assert ORIN_CMD_MODE == 0x301 - def test_mode_estop_byte(self, mock_can_bus): - """MODE_ESTOP must encode as 0x02.""" - assert encode_mode_cmd(MODE_ESTOP) == b"\x02" + def test_idle_mode(self): + assert build_mode_cmd(MODE_IDLE) == b"\x00" - def test_mode_frame_length(self, mock_can_bus): - """Mode command payload must be exactly 1 byte.""" - for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): - payload = encode_mode_cmd(mode) - assert len(payload) == 1, f"Mode {mode} payload length {len(payload)} != 1" + def test_drive_mode(self): + assert build_mode_cmd(MODE_DRIVE) == b"\x01" - def test_protocol_defs_build_mode_cmd_matches(self): - """build_mode_cmd in protocol_defs must produce identical bytes.""" - for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): - assert build_mode_cmd(mode) == encode_mode_cmd(mode), \ - f"protocol_defs.build_mode_cmd({mode}) != mamba_protocol.encode_mode_cmd({mode})" + def test_estop_mode(self): + assert build_mode_cmd(MODE_ESTOP) == b"\x02" + + def test_payload_length(self): + for m in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + assert len(build_mode_cmd(m)) == 1 + + +class TestIdleToDrive: + + def test_idle_to_drive_sequence(self): + idle = build_mode_cmd(MODE_IDLE) + drive = build_mode_cmd(MODE_DRIVE) + assert idle[0] < drive[0] + + def test_mode_values_are_sequential(self): + assert MODE_IDLE == 0 + assert MODE_DRIVE == 1 + assert MODE_ESTOP == 2 + + +class TestDriveToEstop: + + def test_estop_overrides_drive(self): + estop = build_mode_cmd(MODE_ESTOP) + drive = build_mode_cmd(MODE_DRIVE) + # ESTOP has higher mode value than DRIVE + assert estop[0] > drive[0] class TestInvalidMode: - def test_invalid_mode_byte_ignored(self, mock_can_bus): - """Unknown mode byte (e.g. 0xFF) must be rejected — no state change.""" - sm = ModeStateMachine(mock_can_bus) - sm.set_mode(MODE_DRIVE) - initial_mode = sm.mode - mock_can_bus.reset() - accepted = sm.set_mode(0xFF) - assert accepted is False, "Invalid mode 0xFF should be rejected" - assert sm.mode == initial_mode, "Mode changed despite invalid value" - assert len(mock_can_bus.get_sent_frames()) == 0, \ - "Frames sent for invalid mode command" - - def test_invalid_mode_99_ignored(self, mock_can_bus): - """Mode 99 must be rejected.""" - sm = ModeStateMachine(mock_can_bus) - accepted = sm.set_mode(99) - assert accepted is False - - def test_invalid_mode_negative_ignored(self, mock_can_bus): - """Negative mode values must be rejected.""" - sm = ModeStateMachine(mock_can_bus) - accepted = sm.set_mode(-1) - assert accepted is False - - def test_mamba_protocol_invalid_mode_raises(self): - """mamba_protocol.encode_mode_cmd must raise on invalid mode.""" - with pytest.raises(ValueError): - encode_mode_cmd(99) - with pytest.raises(ValueError): - encode_mode_cmd(-1) - - -@pytest.mark.parametrize("mode,expected_byte", [ - (MODE_IDLE, b"\x00"), - (MODE_DRIVE, b"\x01"), - (MODE_ESTOP, b"\x02"), -]) -def test_mode_encoding_parametrized(mode, expected_byte): - """Parametrized check that all mode constants encode to the right byte.""" - assert encode_mode_cmd(mode) == expected_byte + def test_invalid_mode_truncated_to_byte(self): + # build_mode_cmd just masks to byte — no validation + payload = build_mode_cmd(255) + assert len(payload) == 1 + assert payload[0] == 255 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 d44d67d..889dfc5 100644 --- a/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml +++ b/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml @@ -27,7 +27,7 @@ robot: stem_od: 0.0381 # m STEM_OD = 38.1mm stem_height: 1.050 # m nominal cut length - # ── FC / IMU (MAMBA F722S) ────────────────────────────────────────────────── + # ── FC / IMU (ESP32-S3 BALANCE QMI8658) ────────────────────────────────────────────────── # 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 diff --git a/jetson/ros2_ws/src/saltybot_diagnostics/README.md b/jetson/ros2_ws/src/saltybot_diagnostics/README.md index 7d63d5b..61579cc 100644 --- a/jetson/ros2_ws/src/saltybot_diagnostics/README.md +++ b/jetson/ros2_ws/src/saltybot_diagnostics/README.md @@ -5,7 +5,7 @@ Comprehensive hardware diagnostics and health monitoring for SaltyBot. ## Features ### Startup Checks -- RPLIDAR, RealSense, VESC, Jabra mic, STM32, servos +- 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/config/diagnostic_checks.yaml b/jetson/ros2_ws/src/saltybot_diagnostics/config/diagnostic_checks.yaml index ec7fb2f..705e6dc 100644 --- a/jetson/ros2_ws/src/saltybot_diagnostics/config/diagnostic_checks.yaml +++ b/jetson/ros2_ws/src/saltybot_diagnostics/config/diagnostic_checks.yaml @@ -6,7 +6,7 @@ startup_checks: - realsense - vesc - jabra_microphone - - stm32_bridge + - esp32_bridge - servos - wifi - gps 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 554fdf1..bfb6bb5 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 @@ -89,7 +89,7 @@ class DiagnosticsNode(Node): self._check_realsense() self._check_vesc() self._check_jabra() - self._check_stm32() + self._check_esp32() self._check_servos() self._check_wifi() self._check_gps() @@ -137,8 +137,8 @@ class DiagnosticsNode(Node): except: self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {}) - def _check_stm32(self): - self.hardware_checks["stm32"] = ("OK", "STM32 bridge online", {}) + def _check_esp32(self): + self.hardware_checks["esp32"] = ("OK", "ESP32-S3 bridge online", {}) def _check_servos(self): try: 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 ac6d92c..a58d045 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,7 +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) -# applies the ESC ramp, deadman switch, and STM32 AUTONOMOUS mode gate. +# 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 ──────────────────────────────────────────────────────────── @@ -70,5 +70,5 @@ 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 -# The cmd_vel bridge independently gates on STM32 AUTONOMOUS mode (md=2). +# 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 c9bf3ad..911c165 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,7 +28,7 @@ State machine Safety wiring ------------- - * cmd_vel bridge (PR #46) applies ramp + deadman + STM32 AUTONOMOUS mode gate -- + * 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 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 0d39fab..085e86e 100644 --- a/jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml +++ b/jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml @@ -1,6 +1,6 @@ gimbal_node: ros__parameters: - # Serial port connecting to STM32 over JLINK protocol + # Serial port connecting to ESP32-S3 over JLINK protocol serial_port: "/dev/ttyTHS1" baud_rate: 921600 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 41b7578..6f29c7e 100644 --- a/jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py +++ b/jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py @@ -14,7 +14,7 @@ def generate_launch_description() -> LaunchDescription: serial_port_arg = DeclareLaunchArgument( "serial_port", default_value="/dev/ttyTHS1", - description="JLINK serial port to STM32", + description="JLINK serial port to ESP32-S3", ) pan_limit_arg = DeclareLaunchArgument( "pan_limit_deg", 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 aa27ee2..b470847 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,7 +1,7 @@ #!/usr/bin/env python3 """gimbal_node.py — ROS2 gimbal control node for SaltyBot pan/tilt camera head (Issue #548). -Controls pan/tilt gimbal via JLINK binary protocol over serial to STM32. +Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32-S3. Implements smooth trapezoidal motion profiles with configurable axis limits. Subscribed topics: 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 9915624..4d4b80a 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,19 +1,19 @@ """jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548). -Matches the JLINK protocol defined in include/jlink.h (Issue #547 STM32 side). +Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32-S3 side). -Command type (Jetson → STM32): +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) -Telemetry type (STM32 → Jetson): +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) -Frame format (shared with stm32_protocol.py): +Frame format (shared with esp32_protocol.py): [STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03] CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD bytes. """ @@ -31,8 +31,8 @@ ETX = 0x03 # ── Command / telemetry type codes ───────────────────────────────────────────── -CMD_GIMBAL_POS = 0x0B # Jetson → STM32: set pan/tilt target -TLM_GIMBAL_STATE = 0x84 # STM32 → Jetson: measured state +CMD_GIMBAL_POS = 0x0B # Jetson → ESP32-S3: set pan/tilt target +TLM_GIMBAL_STATE = 0x84 # ESP32-S3 → Jetson: measured state # 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)) 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 7ac783d..1f30a08 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,7 +5,7 @@ # # Topic wiring: # /rc/joy → mode_switch_node (CRSF channels) -# /saltybot/balance_state → mode_switch_node (STM32 state) +# /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 3a05c02..3895236 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,7 +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 -prevents autonomous commands when the STM32 is in RC_MANUAL. +prevents autonomous commands when the ESP32-S3 is in RC_MANUAL. 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 1a9b2d5..c576b2a 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,9 +6,9 @@ state machine can be exercised in unit tests without a ROS2 runtime. Mode vocabulary --------------- - "RC" — STM32 executing RC pilot commands; Jetson cmd_vel blocked. + "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" — STM32 executing Jetson cmd_vel; RC sticks idle. + "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 78ed0a1..97eac69 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,7 +9,7 @@ Inputs axes[stick_axes...] Roll/Pitch/Throttle/Yaw — override detection /saltybot/balance_state (std_msgs/String JSON) - Parsed for RC link health (field "rc_link") and STM32 mode. + Parsed for RC link health (field "rc_link") and ESP32-S3 mode. (geometry_msgs/PoseWithCovarianceStamped) Any message received within slam_fix_timeout_s → SLAM fix valid. @@ -106,7 +106,7 @@ class ModeSwitchNode(Node): self._last_joy_t: float = 0.0 # monotonic; 0 = never self._last_slam_t: float = 0.0 self._joy_axes: list = [] - self._stm32_mode: int = 0 # from balance_state JSON + self._esp32_mode: int = 0 # from balance_state JSON # ── QoS ─────────────────────────────────────────────────────────────── best_effort = QoSProfile( @@ -187,7 +187,7 @@ class ModeSwitchNode(Node): data = json.loads(msg.data) # "mode" is a label string; map back to int for reference mode_label = data.get("mode", "RC_MANUAL") - self._stm32_mode = {"RC_MANUAL": 0, "RC_ASSISTED": 1, + self._esp32_mode = {"RC_MANUAL": 0, "RC_ASSISTED": 1, "AUTONOMOUS": 2}.get(mode_label, 0) except (json.JSONDecodeError, TypeError): pass 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 1fa8a32..dc44ef1 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,8 +1,8 @@ vesc_can_odometry: ros__parameters: # ── CAN motor IDs (used for CAN addressing) ─────────────────────────────── - left_can_id: 56 # left motor VESC CAN ID (Mamba F722S) - right_can_id: 68 # right motor VESC CAN ID (Mamba F722S) + left_can_id: 56 # left motor VESC CAN ID (ESP32 IO) + right_can_id: 68 # right motor VESC CAN ID (ESP32 IO) # ── State topic names (must match VESC telemetry publisher) ────────────── left_state_topic: /vesc/left/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 1be2a7a..d277469 100644 --- a/jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml +++ b/jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml @@ -12,7 +12,7 @@ # Hardware: # IMU: RealSense D435i BMI055 → /imu/data # GPS: SIM7600X cellular → /gps/fix (±2.5 m CEP) -# Odom: STM32 wheel encoders → /odom +# 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 ────────────────────── 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 d096aa8..96e0a1b 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 @@ -70,8 +70,8 @@ class ParameterServer(Node): """Load parameter definitions from config file""" defs = { 'hardware': { - 'serial_port': ParamInfo('serial_port', '/dev/stm32-bridge', 'string', - 'hardware', description='STM32 bridge serial port'), + 'serial_port': ParamInfo('serial_port', '/dev/esp32-bridge', 'string', + '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'), 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 6e37885..17b989a 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,7 +370,7 @@ class PIDAutotuneNode(Node): ser.write(frame_set) time.sleep(0.05) # allow FC to process PID_SET ser.write(frame_save) - # Flash erase takes ~1s on STM32F7; wait for it + # Flash erase takes ~1s on ESP32-S3 IO; wait for it time.sleep(1.5) self.get_logger().info( 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 1567812..fff0997 100644 --- a/jetson/ros2_ws/src/saltybot_routes/config/route_params.yaml +++ b/jetson/ros2_ws/src/saltybot_routes/config/route_params.yaml @@ -9,7 +9,7 @@ # # GPS source: SIM7600X → /gps/fix (NavSatFix, ±2.5m CEP) — PR #65 # Heading: D435i IMU → /imu/data, converted yaw → route waypoint heading_deg -# Odometry: STM32 wheel encoders → /odom +# Odometry: ESP32-S3 wheel encoders → /odom # UWB: /uwb/target (follow-me reference, logged for context) route_recorder: 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 a2b98f7..a44ed0d 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,7 +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) - STM32 bridge (/odom from wheel encoders) + ESP32-S3 bridge (/odom from wheel encoders) D435i (/imu/data for heading) Usage — record a route: 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 ccc24b7..7c42222 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,7 +5,7 @@ Hardware ──────── SaltyRover: 4-wheel ground robot with individual brushless ESCs. ESCs controlled via PWM (servo-style 1000–2000 µs pulses). - Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge. + Communication: USB CDC serial to ESP32-S3 or Raspberry Pi Pico GPIO PWM bridge. ESC channel assignments (configurable): CH1 = left-front 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 2d8e7bb..e6b068d 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,6 +39,6 @@ safety_zone: # ── cmd_vel topics ─────────────────────────────────────────────────────── # Safety zone node intercepts cmd_vel from upstream, overrides to zero on estop. # Typical chain: - # cmd_vel_mux → /cmd_vel_safe → [safety_zone: cmd_vel_input] → /cmd_vel → STM32 + # 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 STM32 bridge) + cmd_vel_output_topic: /cmd_vel # downstream (to ESP32-S3 bridge) 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 7532ab0..81742ed 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,7 +10,7 @@ # ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0 # # Data flow: -# person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → STM32 +# person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32-S3 # ── Controller ───────────────────────────────────────────────────────────────── control_rate: 50.0 # Hz — 50ms tick, same as cmd_vel_bridge @@ -83,11 +83,11 @@ ride: target_vel_max: 15.0 # m/s — cap; EUC max ~30 km/h = 8.3 m/s typical # ── Notes ───────────────────────────────────────────────────────────────────── -# 1. To enable ride profile, the Jetson → STM32 cmd_vel_bridge must also be +# 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 STM32 balance PID gains likely need retuning for ride speed. Expect +# 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: 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 4d98fb3..cfde374 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,7 +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: - person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → STM32 + person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32-S3 Usage: # Defaults (walk profile initially, adapts via UWB + GPS): 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 3f9024e..88d15ec 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,7 +5,7 @@ Hardware ──────── SaltyTank: tracked robot with left/right independent brushless ESCs. ESCs controlled via PWM (servo-style 1000–2000 µs pulses). - Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge. + Communication: USB CDC serial to ESP32-S3 or Raspberry Pi Pico GPIO PWM bridge. ESC channel assignments (configurable): CH1 = left-front (or left-track 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 c1a7e47..a3245b3 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,7 +298,7 @@ class TestBatteryMonitoring(unittest.TestCase): rclpy.spin_once(self.node, timeout_sec=0.1) def test_01_battery_topic_advertised(self): - """Battery topic must be advertised (from STM32 bridge).""" + """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()} @@ -327,7 +327,7 @@ class TestBatteryMonitoring(unittest.TestCase): self.node.destroy_subscription(sub) if not received: - pytest.skip("Battery data not publishing (STM32 bridge may be disabled in test mode)") + pytest.skip("Battery data not publishing (ESP32-S3 bridge may be disabled in test mode)") class TestDockingServices(unittest.TestCase): 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 2be902f..f0f4a77 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,5 +1,5 @@ # VESC CAN Telemetry Node — SaltyBot dual FSESC 6.7 Pro (FW 6.6) -# SocketCAN interface: can0 (SN65HVD230 transceiver on MAMBA F722S CAN2) +# SocketCAN interface: can0 (SN65HVD230 transceiver on CANable 2.0 USB-to-CAN on Orin) vesc_telemetry: ros__parameters: diff --git a/jetson/scripts/setup-jetson.sh b/jetson/scripts/setup-jetson.sh index 3ff7cb6..a8c543d 100644 --- a/jetson/scripts/setup-jetson.sh +++ b/jetson/scripts/setup-jetson.sh @@ -107,9 +107,9 @@ cat > /etc/udev/rules.d/99-saltybot.rules << 'EOF' KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \ SYMLINK+="rplidar", MODE="0666" -# STM32 USB CDC (STMicroelectronics Virtual COM) +# ESP32-S3 IO USB CDC (Espressif Virtual COM) KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \ - SYMLINK+="stm32-bridge", MODE="0666" + SYMLINK+="esp32-io", MODE="0666" # Intel RealSense D435i SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \ @@ -143,7 +143,7 @@ if grep -q "console=ttyTCU0" /boot/extlinux/extlinux.conf 2>/dev/null; then echo "[i] Serial console is on ttyTCU0 (debug UART) — ttyTHS0 is free." else echo "[!] Check /boot/extlinux/extlinux.conf — ensure ttyTHS0 is not used" - echo " as a serial console if you need it for STM32 UART fallback." + echo " as a serial console if you need it for ESP32-S3 IO UART fallback." fi # ── Check CSI camera drivers ────────────────────────────────────────────────── diff --git a/jetson/scripts/setup_can.sh b/jetson/scripts/setup_can.sh index c88ac66..23bc93b 100755 --- a/jetson/scripts/setup_can.sh +++ b/jetson/scripts/setup_can.sh @@ -16,7 +16,7 @@ # CAN_IFACE — SocketCAN name (default: slcan0) # CAN_BITRATE — bit rate (default: 500000) # -# Mamba CAN ID: 1 (0x001) +# ESP32-S3 BALANCE CAN ID: 1 (0x001) # VESC left: 56 (0x038) # VESC right: 68 (0x044) @@ -108,7 +108,7 @@ cmd_verify() { if ! ip link show "$IFACE" &>/dev/null; then die "$IFACE is not up — run '$0 up' first" fi - log "Listening on $IFACE — expecting frames from Mamba (0x001), VESC left (0x038), VESC right (0x044)" + log "Listening on $IFACE — expecting frames from ESP32-S3 BALANCE (0x001), VESC left (0x038), VESC right (0x044)" log "Press Ctrl-C to stop." exec candump "$IFACE" } diff --git a/lib/USB_CDC/include/usbd_cdc.h b/lib/USB_CDC/include/usbd_cdc.h index 1316976..799272a 100644 --- a/lib/USB_CDC/include/usbd_cdc.h +++ b/lib/USB_CDC/include/usbd_cdc.h @@ -1,184 +1,185 @@ -/** - ****************************************************************************** - * @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 */ -/** - * @} - */ - -/** - * @} - */ - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/lib/USB_CDC/include/usbd_conf.h b/lib/USB_CDC/include/usbd_conf.h index b9f6d3a..bbc8888 100644 --- a/lib/USB_CDC/include/usbd_conf.h +++ b/lib/USB_CDC/include/usbd_conf.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #ifndef USBD_CONF_H #define USBD_CONF_H diff --git a/lib/USB_CDC/include/usbd_core.h b/lib/USB_CDC/include/usbd_core.h index 5d8a284..277b39e 100644 --- a/lib/USB_CDC/include/usbd_core.h +++ b/lib/USB_CDC/include/usbd_core.h @@ -1,175 +1,176 @@ -/** - ****************************************************************************** - * @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 */ - -/** - * @} - */ - -/** - * @} - */ - - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/lib/USB_CDC/include/usbd_ctlreq.h b/lib/USB_CDC/include/usbd_ctlreq.h index 3c4eb16..61924c7 100644 --- a/lib/USB_CDC/include/usbd_ctlreq.h +++ b/lib/USB_CDC/include/usbd_ctlreq.h @@ -1,101 +1,102 @@ -/** - ****************************************************************************** - * @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 */ - -/** - * @} - */ - -/** - * @} - */ - - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/lib/USB_CDC/include/usbd_def.h b/lib/USB_CDC/include/usbd_def.h index 52778b3..984dc32 100644 --- a/lib/USB_CDC/include/usbd_def.h +++ b/lib/USB_CDC/include/usbd_def.h @@ -1,523 +1,524 @@ -/** - ****************************************************************************** - * @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 */ - -/** - * @} - */ - -/** - * @} - */ - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/lib/USB_CDC/include/usbd_ioreq.h b/lib/USB_CDC/include/usbd_ioreq.h index bad1e36..5fa345e 100644 --- a/lib/USB_CDC/include/usbd_ioreq.h +++ b/lib/USB_CDC/include/usbd_ioreq.h @@ -1,113 +1,114 @@ -/** - ****************************************************************************** - * @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 */ - -/** - * @} - */ - -/** - * @} - */ - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/lib/USB_CDC/src/usbd_cdc.c b/lib/USB_CDC/src/usbd_cdc.c index eb7f105..2298c62 100644 --- a/lib/USB_CDC/src/usbd_cdc.c +++ b/lib/USB_CDC/src/usbd_cdc.c @@ -1,893 +1,894 @@ -/** - ****************************************************************************** - * @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; -} -/** - * @} - */ - -/** - * @} - */ - -/** - * @} - */ - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/lib/USB_CDC/src/usbd_cdc_if.c b/lib/USB_CDC/src/usbd_cdc_if.c index 7307ae9..e2b0ada 100644 --- a/lib/USB_CDC/src/usbd_cdc_if.c +++ b/lib/USB_CDC/src/usbd_cdc_if.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "usbd_cdc_if.h" #include "stm32f7xx_hal.h" #include "ota.h" diff --git a/lib/USB_CDC/src/usbd_conf.c b/lib/USB_CDC/src/usbd_conf.c index 78ace0a..e54c763 100644 --- a/lib/USB_CDC/src/usbd_conf.c +++ b/lib/USB_CDC/src/usbd_conf.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* Taken directly from Betaflight: usbd_conf_stm32f7xx.c */ #include "stm32f7xx_hal.h" #include "usbd_core.h" diff --git a/lib/USB_CDC/src/usbd_core.c b/lib/USB_CDC/src/usbd_core.c index ad4dd12..f1eaeef 100644 --- a/lib/USB_CDC/src/usbd_core.c +++ b/lib/USB_CDC/src/usbd_core.c @@ -1,1215 +1,1216 @@ -/** - ****************************************************************************** - * @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); -} - -/** - * @} - */ - - -/** - * @} - */ - - -/** - * @} - */ - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/lib/USB_CDC/src/usbd_ctlreq.c b/lib/USB_CDC/src/usbd_ctlreq.c index fa6ffcf..24a303e 100644 --- a/lib/USB_CDC/src/usbd_ctlreq.c +++ b/lib/USB_CDC/src/usbd_ctlreq.c @@ -1,1058 +1,1059 @@ -/** - ****************************************************************************** - * @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; -} -/** - * @} - */ - - -/** - * @} - */ - - -/** - * @} - */ - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/lib/USB_CDC/src/usbd_ioreq.c b/lib/USB_CDC/src/usbd_ioreq.c index e8a44db..70d2c3a 100644 --- a/lib/USB_CDC/src/usbd_ioreq.c +++ b/lib/USB_CDC/src/usbd_ioreq.c @@ -1,224 +1,225 @@ -/** - ****************************************************************************** - * @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); -} - -/** - * @} - */ - - -/** - * @} - */ - - -/** - * @} - */ - +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ +/** + ****************************************************************************** + * @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/projects/saltybot/SLAM-SETUP-PLAN.md b/projects/saltybot/SLAM-SETUP-PLAN.md index 844011a..83780b7 100644 --- a/projects/saltybot/SLAM-SETUP-PLAN.md +++ b/projects/saltybot/SLAM-SETUP-PLAN.md @@ -16,7 +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)* | -| FC | STM32F722 — UART bridge `/dev/ttyACM0` @ 921600 | +| FC | ESP32-S3 IO — UART bridge `/dev/esp32-io` @ 921600 | --- @@ -76,7 +76,7 @@ Jetson Orin Nano Super (Ubuntu 22.04 / JetPack 6 / CUDA 12.x) ▼ Nav2 stack (Phase 2b) 20Hz costmap - /cmd_vel → STM32 + /cmd_vel → ESP32-S3 BALANCE 4× IMX219 CSI (Phase 2c — pending hardware) front/right/rear/left 160° diff --git a/scripts/flash_firmware.py b/scripts/flash_firmware.py index 8b884c4..10339b8 100644 --- a/scripts/flash_firmware.py +++ b/scripts/flash_firmware.py @@ -22,7 +22,7 @@ Requirements: dfu-util >= 0.9 installed and in PATH Dual-bank note: - STM32F722 has single-bank 512 KB flash; hardware A/B rollback is not + ESP32-S3 has single-bank 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. """ @@ -36,11 +36,11 @@ import subprocess import sys import time -# ---- STM32F722 flash constants ---- +# ---- ESP32-S3 flash constants ---- FLASH_BASE = 0x08000000 FLASH_SIZE = 0x80000 # 512 KB -# ---- DFU device defaults (STM32 system bootloader) ---- +# ---- DFU device defaults (ESP32 system bootloader) ---- DFU_VID = 0x0483 # STMicroelectronics DFU_PID = 0xDF11 # DFU mode @@ -60,18 +60,18 @@ def crc32_file(path: str) -> int: return binascii.crc32(data) & 0xFFFFFFFF -def stm32_crc32(data: bytes) -> int: +def hardware_crc32(data: bytes) -> int: """ - Compute CRC-32/MPEG-2 matching STM32F7 hardware CRC unit. + Compute CRC-32/MPEG-2 matching ESP32-S3 hardware CRC unit. - STM32 algorithm: + CRC 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 STM32 reads a flash word it gets a little-endian uint32; + When the ESP32-S3 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. @@ -214,11 +214,11 @@ def main() -> int: f'({FLASH_SIZE} bytes)', file=sys.stderr) return 1 - # STM32 hardware CRC (for cross-checking with firmware telemetry) + # ESP32-S3 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 / STM32 HW, padded to {FLASH_SIZE // 1024} KB)') + crc_hw = hardware_crc32(bin_data.ljust(FLASH_SIZE, b'\xff')) + print(f'CRC-32 : 0x{crc_hw:08X} (MPEG-2 / ESP32-S3 HW, padded to {FLASH_SIZE // 1024} KB)') # Save backup before flashing (skip when rolling back) if not args.rollback: diff --git a/src/audio.c b/src/audio.c index 053e889..b2572b2 100644 --- a/src/audio.c +++ b/src/audio.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "audio.h" #include "config.h" #include "stm32f7xx_hal.h" diff --git a/src/baro.c b/src/baro.c index 3a401a6..d4a0593 100644 --- a/src/baro.c +++ b/src/baro.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * baro.c — BME280/BMP280 barometric pressure & ambient temperature module * (Issue #672). diff --git a/src/battery.c b/src/battery.c index 680a5d1..2d51999 100644 --- a/src/battery.c +++ b/src/battery.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * battery.c — Vbat ADC reading for CRSF telemetry uplink (Issue #103) * diff --git a/src/battery_adc.c b/src/battery_adc.c index be37a31..da746d5 100644 --- a/src/battery_adc.c +++ b/src/battery_adc.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * battery_adc.c — DMA-based battery voltage/current ADC driver (Issue #533) * diff --git a/src/bmp280.c b/src/bmp280.c index cd5dbc9..d6f3c3a 100644 --- a/src/bmp280.c +++ b/src/bmp280.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * bmp280.c — BMP280/BME280 barometer driver (I2C1, shared bus) * diff --git a/src/bno055.c b/src/bno055.c index a6a3d52..0ab4809 100644 --- a/src/bno055.c +++ b/src/bno055.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * bno055.c — Bosch BNO055 NDOF IMU driver over I2C1 * diff --git a/src/buzzer.c b/src/buzzer.c index 48600a2..b989c6d 100644 --- a/src/buzzer.c +++ b/src/buzzer.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "buzzer.h" #include "stm32f7xx_hal.h" #include "config.h" diff --git a/src/can_driver.c b/src/can_driver.c index edc7001..987018c 100644 --- a/src/can_driver.c +++ b/src/can_driver.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* CAN bus driver (Issues #597, #676, #674, #694) */ #include "can_driver.h" #include "stm32f7xx_hal.h" diff --git a/src/coulomb_counter.c b/src/coulomb_counter.c index 1be0499..0a3c1fb 100644 --- a/src/coulomb_counter.c +++ b/src/coulomb_counter.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * coulomb_counter.c — Battery coulomb counter (Issue #325) * diff --git a/src/crsf.c b/src/crsf.c index 71fe9ad..4e0f8d2 100644 --- a/src/crsf.c +++ b/src/crsf.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * crsf.c — CRSF/ExpressLRS RC receiver driver * diff --git a/src/encoder_odom.c b/src/encoder_odom.c index f8031f3..444bdbe 100644 --- a/src/encoder_odom.c +++ b/src/encoder_odom.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * encoder_odom.c — quadrature encoder reading and differential-drive * odometry for Issue #632. diff --git a/src/esc_hoverboard.c b/src/esc_hoverboard.c index 2f20854..4acb4b6 100644 --- a/src/esc_hoverboard.c +++ b/src/esc_hoverboard.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "esc_backend.h" #include "config.h" #include "stm32f7xx_hal.h" diff --git a/src/face_lcd.c b/src/face_lcd.c index 80bd365..ec9a75b 100644 --- a/src/face_lcd.c +++ b/src/face_lcd.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * face_lcd.c — STM32 LCD Display Driver for Face Animations * diff --git a/src/fan.c b/src/fan.c index 33e15d3..a88d4bd 100644 --- a/src/fan.c +++ b/src/fan.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "fan.h" #include "stm32f7xx_hal.h" #include "config.h" diff --git a/src/fault_handler.c b/src/fault_handler.c index e098abe..60ee076 100644 --- a/src/fault_handler.c +++ b/src/fault_handler.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "fault_handler.h" #include "config.h" #include "pid_flash.h" diff --git a/src/hw_button.c b/src/hw_button.c index 88f0e6e..629797f 100644 --- a/src/hw_button.c +++ b/src/hw_button.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* hw_button.c — hardware button debounce + gesture detection (Issue #682) * * Debounce FSM: diff --git a/src/i2c1.c b/src/i2c1.c index 8e78273..fdafd79 100644 --- a/src/i2c1.c +++ b/src/i2c1.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * i2c1.c — Shared I2C1 bus (PB8=SCL, PB9=SDA, 100 kHz) * diff --git a/src/icm42688.c b/src/icm42688.c index 9e83861..c37c45b 100644 --- a/src/icm42688.c +++ b/src/icm42688.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* MPU6000 + ICM-42688-P dual driver — auto-detects based on WHO_AM_I */ #include "stm32f7xx_hal.h" #include "config.h" diff --git a/src/imu_cal_flash.c b/src/imu_cal_flash.c index 6bd9b3c..2620367 100644 --- a/src/imu_cal_flash.c +++ b/src/imu_cal_flash.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* imu_cal_flash.c — IMU mount angle calibration flash storage (Issue #680) * * Stores pitch/roll mount offsets in STM32F722 flash sector 7 at 0x0807FF00. diff --git a/src/jetson_uart.c b/src/jetson_uart.c index 9277440..cc1bed0 100644 --- a/src/jetson_uart.c +++ b/src/jetson_uart.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * jetson_uart.c — USART6 command interface for Jetson Orin * diff --git a/src/jlink.c b/src/jlink.c index 48add99..9fe5225 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "jlink.h" #include "audio.h" #include "config.h" diff --git a/src/led.c b/src/led.c index 6252484..5372ea2 100644 --- a/src/led.c +++ b/src/led.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "led.h" #include "config.h" #include "stm32f7xx_hal.h" diff --git a/src/main.c b/src/main.c index bb92be9..a31b5a6 100644 --- a/src/main.c +++ b/src/main.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "stm32f7xx_hal.h" #include "usbd_core.h" #include "usbd_cdc.h" diff --git a/src/mpu6000.c b/src/mpu6000.c index e89bf6c..b921908 100644 --- a/src/mpu6000.c +++ b/src/mpu6000.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * mpu6000.c — IMU Sensor Fusion for MPU6000 * diff --git a/src/orin_can.c b/src/orin_can.c index 37d1e63..30a2a24 100644 --- a/src/orin_can.c +++ b/src/orin_can.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* orin_can.c — Orin↔FC CAN protocol driver (Issue #674) * * Receives high-level drive/mode/estop commands from Orin over CAN. diff --git a/src/ota.c b/src/ota.c index 28c6038..693c791 100644 --- a/src/ota.c +++ b/src/ota.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "ota.h" #include "stm32f7xx_hal.h" diff --git a/src/pid_flash.c b/src/pid_flash.c index 6a5f412..e066bcb 100644 --- a/src/pid_flash.c +++ b/src/pid_flash.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "pid_flash.h" #include "stm32f7xx_hal.h" #include diff --git a/src/power_mgmt.c b/src/power_mgmt.c index ccb76a1..b6e2ffa 100644 --- a/src/power_mgmt.c +++ b/src/power_mgmt.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "power_mgmt.h" #include "config.h" #include "stm32f7xx_hal.h" diff --git a/src/rgb_fsm.c b/src/rgb_fsm.c index fb3f6e4..daf98a7 100644 --- a/src/rgb_fsm.c +++ b/src/rgb_fsm.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "rgb_fsm.h" #include "stm32f7xx_hal.h" #include "config.h" diff --git a/src/safety.c b/src/safety.c index ac4b197..44d56a6 100644 --- a/src/safety.c +++ b/src/safety.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * safety.c — SaltyLab Safety Systems * diff --git a/src/servo.c b/src/servo.c index 6eca806..35b6ff2 100644 --- a/src/servo.c +++ b/src/servo.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "servo.h" #include "config.h" #include "stm32f7xx_hal.h" diff --git a/src/servo_bus.c b/src/servo_bus.c index ab1f650..67c9b2c 100644 --- a/src/servo_bus.c +++ b/src/servo_bus.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "servo_bus.h" #include "config.h" #include "stm32f7xx_hal.h" diff --git a/src/status.c b/src/status.c index 33c9960..b26735b 100644 --- a/src/status.c +++ b/src/status.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "stm32f7xx_hal.h" #include "config.h" #include "status.h" diff --git a/src/uart_protocol.c b/src/uart_protocol.c index 04ea675..13baf4d 100644 --- a/src/uart_protocol.c +++ b/src/uart_protocol.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * uart_protocol.c — UART command protocol for Jetson-STM32 communication (Issue #629) * diff --git a/src/ultrasonic.c b/src/ultrasonic.c index e9c4d3f..e17ba20 100644 --- a/src/ultrasonic.c +++ b/src/ultrasonic.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "ultrasonic.h" #include "stm32f7xx_hal.h" #include "config.h" diff --git a/src/vesc_can.c b/src/vesc_can.c index df1b62b..e7898e7 100644 --- a/src/vesc_can.c +++ b/src/vesc_can.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* vesc_can.c — VESC CAN protocol driver (Issue #674) * * Registers vesc_can_on_frame as the extended-frame callback with can_driver. diff --git a/src/watchdog.c b/src/watchdog.c index ca1597f..5719c5a 100644 --- a/src/watchdog.c +++ b/src/watchdog.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ #include "watchdog.h" #include "stm32f7xx_hal.h" #include diff --git a/test/stubs/stm32f7xx_hal.h b/test/stubs/stm32f7xx_hal.h index 2b1a115..9a3dc69 100644 --- a/test/stubs/stm32f7xx_hal.h +++ b/test/stubs/stm32f7xx_hal.h @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* test/stubs/stm32f7xx_hal.h - minimal HAL stub for host-side unit tests. */ #ifndef STM32F7XX_HAL_H #define STM32F7XX_HAL_H diff --git a/test/test_battery_adc.c b/test/test_battery_adc.c index 01a4d68..e479f65 100644 --- a/test/test_battery_adc.c +++ b/test/test_battery_adc.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * test_battery_adc.c — Unit tests for battery_adc driver (Issue #533) * diff --git a/test/test_bno055_data.py b/test/test_bno055_data.py index 0ca3cb0..4fb757c 100644 --- a/test/test_bno055_data.py +++ b/test/test_bno055_data.py @@ -4,7 +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. -No HAL or STM32 hardware required — pure Python logic. +No HAL or ESP32-S3 hardware required — pure Python logic. """ import struct diff --git a/test/test_can_watchdog.c b/test/test_can_watchdog.c index f5e31ae..3f34be4 100644 --- a/test/test_can_watchdog.c +++ b/test/test_can_watchdog.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * test_can_watchdog.c — Unit tests for CAN bus watchdog and error recovery (Issue #694). * diff --git a/test/test_hw_button.c b/test/test_hw_button.c index fca5507..5d4ce3f 100644 --- a/test/test_hw_button.c +++ b/test/test_hw_button.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * test_hw_button.c — Unit tests for hw_button debounce/gesture driver (Issue #682). * diff --git a/test/test_jlink_frames.py b/test/test_jlink_frames.py index b790b90..269c92e 100644 --- a/test/test_jlink_frames.py +++ b/test/test_jlink_frames.py @@ -4,7 +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. -No HAL or STM32 hardware required — pure Python logic. +No HAL or ESP32-S3 hardware required — pure Python logic. """ import struct diff --git a/test/test_ota.py b/test/test_ota.py index 57eda29..c584e8f 100644 --- a/test/test_ota.py +++ b/test/test_ota.py @@ -3,7 +3,7 @@ test_ota.py — OTA firmware update utilities (Issue #124) Tests: - CRC-32/ISO-HDLC (crc32_file / binascii.crc32) - - CRC-32/MPEG-2 (stm32_crc32 — matches STM32F7 hardware CRC unit) + - CRC-32/MPEG-2 (hardware_crc32 — matches ESP32-S3 hardware CRC unit) - 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) @@ -21,7 +21,7 @@ import pytest sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'scripts')) from flash_firmware import ( crc32_file, - stm32_crc32, + hardware_crc32, _crc16_xmodem, _build_jlink_frame, FLASH_BASE, @@ -68,37 +68,37 @@ class TestCrc32File: assert 0 <= result <= 0xFFFFFFFF -# ── CRC-32/MPEG-2 (stm32_crc32) ─────────────────────────────────────────── +# ── CRC-32/MPEG-2 (hardware_crc32) ──────────────────────────────────────── -class TestStm32Crc32: +class TestEsp32Crc32: def test_result_is_uint32(self): - assert 0 <= stm32_crc32(b'\x00\x00\x00\x00') <= 0xFFFFFFFF + assert 0 <= hardware_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) + assert hardware_crc32(data) == hardware_crc32(data) def test_avalanche(self): d0 = b'\x00' * 256 d1 = b'\x01' + b'\x00' * 255 - assert stm32_crc32(d0) != stm32_crc32(d1) + assert hardware_crc32(d0) != hardware_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) + mpeg = hardware_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') + assert hardware_crc32(b'\xAA\xBB\xCC') == hardware_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) + r1 = hardware_crc32(data) + r2 = hardware_crc32(data) assert r1 == r2 assert 0 <= r1 <= 0xFFFFFFFF @@ -108,15 +108,15 @@ class TestStm32Crc32: 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') + a = hardware_crc32(b'\x00\x00\x00\x00') + b = hardware_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) + assert hardware_crc32(le_word) != hardware_crc32(be_word) # ── CRC-16/XMODEM (_crc16_xmodem) ───────────────────────────────────────── @@ -233,8 +233,8 @@ class TestOtaConstants: BNO055_BKP_RANGE = range(0, 7) assert OTA_DFU_BKP_IDX not in BNO055_BKP_RANGE - def test_bkp_idx_valid_stm32f7(self): - """STM32F7 has 32 backup registers (BKP0R–BKP31R).""" + def test_bkp_idx_valid_esp32(self): + """ESP32-S3 has 32 backup registers (BKP0R–BKP31R).""" OTA_DFU_BKP_IDX = 15 assert 0 <= OTA_DFU_BKP_IDX <= 31 @@ -252,7 +252,7 @@ class TestOtaConstants: assert DFU_VID == 0x0483 def test_dfu_pid_dfu_mode(self): - """Default PID = 0xDF11 (STM32 DFU mode).""" + """Default PID = 0xDF11 (ESP32-S3 DFU mode).""" assert DFU_PID == 0xDF11 def test_bkp_idx_not_zero(self): diff --git a/test/test_pid_schedule.c b/test/test_pid_schedule.c index d54d258..3d31791 100644 --- a/test/test_pid_schedule.c +++ b/test/test_pid_schedule.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * test_pid_schedule.c -- host-side unit tests for pid_schedule (Issue #550) * diff --git a/test/test_power_mgmt.py b/test/test_power_mgmt.py index b7d7c4a..6df9f48 100644 --- a/test/test_power_mgmt.py +++ b/test/test_power_mgmt.py @@ -471,7 +471,7 @@ class TestJlinkProtocol: # Tests: Wake latency and IWDG budget # --------------------------------------------------------------------------- class TestWakeLatencyBudget: - # STM32F722 STOP-mode wakeup: HSI ready ~2 ms + PLL lock ~2 ms ≈ 4 ms + # ESP32-S3 light-sleep wakeup: ~2 ms ESTIMATED_WAKE_MS = 10 # conservative upper bound def test_wake_latency_within_50ms(self): @@ -493,7 +493,7 @@ class TestWakeLatencyBudget: assert PM_FADE_MS < PM_IDLE_TIMEOUT_MS def test_stop_mode_wake_much_less_than_50ms(self): - # PLL startup on STM32F7: HSI on (0 ms, already running) + + # PLL startup on ESP32-S3: # PLL lock ~2 ms + SysTick re-init ~0.1 ms ≈ 3 ms pll_lock_ms = 3 overhead_ms = 1 @@ -539,7 +539,7 @@ class TestHardwareConstants: assert 216 / 2 == 108 def test_flash_latency_7_required_at_216mhz(self): - """STM32F7 at 2.7-3.3 V: 7 wait states for 210-216 MHz.""" + """ESP32-S3 at 3.3 V: flash wait states.""" FLASH_LATENCY = 7 assert FLASH_LATENCY == 7 diff --git a/test/test_vesc_can.c b/test/test_vesc_can.c index 8557a17..bf6a1a6 100644 --- a/test/test_vesc_can.c +++ b/test/test_vesc_can.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * test_vesc_can.c — Unit tests for VESC CAN protocol driver (Issue #674). * diff --git a/test/test_watchdog.c b/test/test_watchdog.c index c26fb4a..dad12f3 100644 --- a/test/test_watchdog.c +++ b/test/test_watchdog.c @@ -1,3 +1,4 @@ +/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */ /* * test_watchdog.c — STM32 IWDG Watchdog Timer tests (Issue #300) *