seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

chore: remove all Mamba/STM32/BlackPill legacy hardware references #719

Closed
sl-jetson wants to merge 2 commits from sl-firmware/cleanup-legacy-hw into main
pull from: sl-firmware/cleanup-legacy-hw
merge into: seb:main
Conversation 0 Commits 2 Files Changed 215 +15 -24425
136 changed files with 15 additions and 24425 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit c958cf4474 - Show all commits

4
TEAM.md
View File

@ -6,7 +6,7 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove
## Current Status ## Current Status
- **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand - **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand
- **Firmware:** Balance PID + hoverboard ESC protocol written, but blocked by USB Serial (CH343) bug - **Firmware:** Balance PID + hoverboard ESC protocol written, but blocked by USB Serial (CH343) bug
- **Blocker:** USB Serial (CH343) TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB on ESP32-S3 — see `legacy/stm32/USB_CDC_BUG.md` for historical context - **Blocker:** USB Serial (CH343) TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB on ESP32-S3
--- ---
## Roles Needed ## Roles Needed
@ -70,4 +70,4 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove
## Repo ## Repo
- Gitea: https://gitea.vayrette.com/seb/saltylab-firmware - Gitea: https://gitea.vayrette.com/seb/saltylab-firmware
- Design doc: `projects/saltybot/SALTYLAB.md` - Design doc: `projects/saltybot/SALTYLAB.md`
- Bug doc: `legacy/stm32/USB_CDC_BUG.md` (archived — STM32 era) - Design doc: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`

2
docs/AGENTS.md
View File

@ -18,7 +18,7 @@ Jetson (speed+steer via UART1) ←→ ELRS RC (UART3, kill switch)
Hoverboard ESC (FOC) → 2× 8" hub motors``` Hoverboard ESC (FOC) → 2× 8" hub motors```
Frame: `[0xAA][LEN][TYPE][PAYLOAD][CRC8]` Frame: `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
Legacy `src/` STM32 HAL code is **archived — do not extend.** Active firmware: `esp32/balance_fw/` (ESP32-S3 BALANCE) and `esp32/io_fw/` (ESP32-S3 IO).
## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT ## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT

27
docs/wiring-diagram.md
View File

@ -1,13 +1,11 @@
# SaltyLab / SAUL-TEE Wiring Reference # SaltyLab / SAUL-TEE Wiring Reference
> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32 retired.
> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN.
> **Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md) > **Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md)
> Historical STM32/Mamba wiring below is **obsolete** — retained for reference only. > New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN.
--- ---
## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md) ## System Overview
``` ```
┌─────────────────────────────────────────────────────────────────────┐ ┌─────────────────────────────────────────────────────────────────────┐
@ -65,7 +63,7 @@
## Wire-by-Wire Connections ## Wire-by-Wire Connections
### 1. Orin ↔ FC (Primary: USB Serial (CH343)) ### 1. Orin ↔ ESP32-S3 BALANCE (Primary: USB Serial via CH343)
| From | To | Wire | Notes | | From | To | Wire | Notes |
|------|----|------|-------| |------|----|------|-------|
| Orin USB-A | CANable2 USB | USB cable | SocketCAN slcan0 @ 500 kbps | | Orin USB-A | CANable2 USB | USB cable | SocketCAN slcan0 @ 500 kbps |
@ -139,17 +137,14 @@ BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor)
| CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` | | CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` |
## FC UART Summary (ESP32-S3 BALANCE) ## ESP32-S3 BALANCE — UART Summary
| UART | Pins | Baud | Assignment | Notes | | UART | GPIO Pins | Baud | Assignment | Notes |
|------|------|------|------------|-------| |------|-----------|------|------------|-------|
| USART1 | PB6=TX, PB7=RX | — | SmartAudio/VTX | Unused in SaltyLab | | UART0 (CRSF primary) | IO44=RX, IO43=TX | 400000 | TBS Crossfire RC | via ESP32-S3 IO board |
| USART2 | PA2=TX, PA3=RX | 26400 | Hoverboard ESC | Binary motor commands | | UART1 (inter-board) | IO17=TX, IO18=RX | 460800 | ESP32-S3 IO ↔ BALANCE | binary `[0xAA][LEN][TYPE]` |
| USART3 | PB10=TX, PB11=RX | — | Available | Was SBUS default | | CAN (SN65HVD230) | IO43=TX, IO44=RX | 500 kbps | VESCs + Orin CANable2 | ISO 11898 |
| UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control | | USB Serial (CH343) | USB-C | 460800 | Orin primary | `/dev/balance-esp` |
| UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional |
| USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link |
| USB Serial (CH343) | USB-C | 921600 | Jetson primary | `/dev/esp32-bridge` |
### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header) ### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
@ -208,7 +203,7 @@ BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor)
| Device | Interface | Power Draw | | Device | Interface | Power Draw |
|--------|-----------|------------| |--------|-----------|------------|
| ESP32-S3 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) || RealSense D435i | USB-A | ~1.5W (3.5W peak) | | ESP32-S3 BALANCE (CH343) | USB-C | ~0.5W (data only, BALANCE on 5V bus) || RealSense D435i | USB-A | ~1.5W (3.5W peak) |
| RPLIDAR A1M8 | USB-A | ~2.6W (motor on) | | RPLIDAR A1M8 | USB-A | ~2.6W (motor on) |
| SIM7600A | USB | ~1W idle, 3W TX peak | | SIM7600A | USB | ~1W idle, 3W TX peak |
| Leap Motion | USB-A | ~0.5W | | Leap Motion | USB-A | ~0.5W |

2
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py
View File

@ -84,7 +84,7 @@ class SerialBridgeNode(Node):
# Poll at 100 Hz — ESP32-S3 sends at 50 Hz, so we never miss a frame self._timer = self.create_timer(0.01, self._read_cb) # Poll at 100 Hz — ESP32-S3 sends at 50 Hz, so we never miss a frame self._timer = self.create_timer(0.01, self._read_cb)
self.get_logger().info( self.get_logger().info(
f"stm32_serial_bridge started — {port} @ {baud} baud" f"esp32_serial_bridge started — {port} @ {baud} baud"
) )
# ── Serial management ───────────────────────────────────────────────────── # ── Serial management ─────────────────────────────────────────────────────

4
legacy/stm32/README.md
View File

@ -1,4 +0,0 @@
# Legacy STM32 Firmware (Archived 2026-04-04)
This directory contains the archived STM32F7 (Mamba F722S) firmware.
Hardware retired 2026-04-04. Replaced by ESP32-S3 BALANCE + ESP32-S3 IO.
See docs/SAUL-TEE-SYSTEM-REFERENCE.md for current architecture.

149
legacy/stm32/USB_CDC_BUG.md
View File

@ -1,149 +0,0 @@
# USB CDC TX Bug — Investigation & Resolution
**Issue #524** | Investigated 2026-03-06 | **RESOLVED** (PR #10)
---
## Problem
Balance firmware produced no USB CDC output. Minimal "hello" test firmware worked fine.
- USB enumerated correctly in both cases (port appeared as `/dev/cu.usbmodemSALTY0011`)
- DFU reboot via RTC backup register worked (Betaflight-proven pattern)
- Balance firmware: port opened, no data ever arrived
---
## Root Causes Found (Two Independent Bugs)
### Bug 1 (Primary): DCache Coherency — USB Buffers Were Cached
**The Cortex-M7 has a split Harvard cache (ICache + DCache). The USB OTG FS
peripheral's internal DMA engine reads directly from physical SRAM. The CPU
writes through the DCache. If the cache line was not flushed before the USB
FIFO loader fired, the peripheral read stale/zero bytes from SRAM.**
This is the classic Cortex-M7 DMA coherency trap. The test firmware worked
because it ran before DCache was enabled or because the tiny buffer happened to
be flushed by the time the FIFO loaded. The balance firmware with DCache enabled
throughout never flushed the TX buffer, so USB TX always transferred zeros or
nothing.
**Fix applied** (`lib/USB_CDC/src/usbd_conf.c`, `lib/USB_CDC/src/usbd_cdc_if.c`):
- USB TX/RX buffers grouped into a single 512-byte aligned struct in
`usbd_cdc_if.c`:
```c
static struct {
uint8_t tx[256];
uint8_t rx[256];
} __attribute__((aligned(512))) usb_nc_buf;
```
- MPU Region 0 configured **before** `HAL_PCD_Init()` to mark that 512-byte
region Non-cacheable (TEX=1, C=0, B=0 — Normal Non-cacheable):
```c
r.TypeExtField = MPU_TEX_LEVEL1;
r.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
r.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
```
- `SCB_EnableDCache()` left enabled in `main.c` — DCache stays on globally for
performance; only the USB buffers are excluded via MPU.
- `CDC_Transmit()` always copies caller data into `UserTxBuffer` before calling
`USBD_CDC_TransmitPacket()`, so the USB hardware always reads from the
non-cacheable region regardless of where the caller's buffer lives.
### Bug 2 (Secondary): IWDG Started Before Long Peripheral Inits
`mpu6000_init()` + `mpu6000_calibrate()` block for ~510ms (gyro bias
integration). If IWDG had been started with a 50ms timeout before these calls,
the watchdog would have fired during calibration and reset the MCU in a hard
loop — USB would never enumerate cleanly.
**Fix applied** (`src/main.c`, `src/safety.c`):
- `safety_init()` (which calls `watchdog_init(2000)`) is deferred to **after**
all peripheral inits, after IMU calibration, after USB enumeration delay:
```c
/* USB CDC, status, IMU, hoverboard, balance, motors, CRSF, audio,
* buzzer, LEDs, power, servo, ultrasonic, mode manager, battery,
* I2C sensors — ALL init first */
safety_init(); /* IWDG starts HERE — 2s timeout */
```
- IWDG timeout extended to 2000ms (from 50ms) to accommodate worst-case main
loop delays (BNO055 I2C reads at ~3ms each, audio/buzzer blocking patterns).
---
## Investigation: What Was Ruled Out
### DMA Channel Conflicts
- USB OTG FS does **not** use DMA (`hpcd.Init.dma_enable = 0`); it uses the
internal FIFO with CPU-driven transfers. No DMA channel conflict possible.
- SPI1 (IMU/MPU6000): DMA2 Stream 0/3
- USART2 (hoverboard ESC): DMA1 Stream 5/6
- UART4 (CRSF/ELRS): DMA1 Stream 2/4
- No overlapping DMA streams between any peripheral.
### USB Interrupt Priority Starvation
- `OTG_FS_IRQn` configured at NVIC priority 6 (`HAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0)`).
- No other ISR in the codebase uses a priority ≤6 that could starve USB.
- SysTick runs at default priority 15 (lowest). Not a factor.
### GPIO Pin Conflicts
- USB OTG FS: PA11 (DM), PA12 (DP) — AF10
- SPI1 (IMU): PA4 (NSS), PA5 (SCK), PA6 (MISO), PA7 (MOSI) — no overlap
- USART2 (hoverboard): PA2 (TX), PA3 (RX) — no overlap
- LEDs: PC14, PC15 — no overlap
- Buzzer: PB2 — no overlap
- No GPIO conflicts with USB OTG FS pins.
### Clock Tree
- USB requires a 48 MHz clock. `SystemClock_Config()` routes 48 MHz from PLLSAI
(`RCC_CLK48SOURCE_PLLSAIP`, PLLSAIN=384, PLLSAIP=DIV8 → 384/8=48 MHz). ✓
- PLLSAI is independent of PLL1 (system clock) and PLLSAI.PLLSAIQ (I2S).
No clock tree contention.
### TxState Stuck-Busy
- `CDC_Init()` resets `hcdc->TxState = 0` on every host (re)connect. ✓
- `CDC_Transmit()` includes a busy-count recovery (force-clears TxState after
100 consecutive BUSY returns). ✓
- Not a contributing factor once the DCache issue is fixed.
---
## Hardware Reference
| Signal | Pin | Peripheral |
|--------|-----|------------|
| USB D- | PA11 | OTG_FS AF10 |
| USB D+ | PA12 | OTG_FS AF10 |
| IMU SCK | PA5 | SPI1 |
| IMU MISO | PA6 | SPI1 |
| IMU MOSI | PA7 | SPI1 |
| IMU CS | PA4 | GPIO |
| ESC TX | PA2 | USART2 |
| ESC RX | PA3 | USART2 |
| LED1 | PC14 | GPIO |
| LED2 | PC15 | GPIO |
| Buzzer | PB2 | GPIO/TIM4_CH3 |
MCU: ESP32RET6 (ESP32 BALANCE FC, Betaflight target DIAT-MAMBAF722_2022B)
---
## Files Changed (PR #10)
- `lib/USB_CDC/src/usbd_cdc_if.c` — 512-byte aligned non-cacheable buffer struct, `CDC_Transmit` copy-to-fixed-buffer
- `lib/USB_CDC/src/usbd_conf.c` — `USB_NC_MPU_Config()` MPU region before `HAL_PCD_Init()`
- `src/main.c` — `safety_init()` deferred after all peripheral init; DCache stays enabled with comment
- `src/safety.c` / `src/watchdog.c` — IWDG timeout 2000ms; `watchdog_was_reset_by_watchdog()` for reset detection logging
---
## Lessons Learned
1. **Cortex-M7 + DMA + DCache = always configure MPU non-cacheable regions for DMA buffers.** The cache is not write-through to SRAM; the DMA engine sees physical SRAM, not the cache. The MPU is the correct fix (not `SCB_CleanDCache_by_Addr` before every TX, which is fragile).
2. **IWDG must start after all slow blocking inits.** IMU calibration can take 500ms+. The IWDG cannot be paused once started. Defer `safety_init()` until the main loop is ready to kick the watchdog every cycle.
3. **USB enumeration success does not prove data flow.** The host handshake and port appearance can succeed even when TX buffers are incoherent. Test with actual data transfer, not just enumeration.

106
legacy/stm32/include/audio.h
View File

@ -1,106 +0,0 @@
#ifndef AUDIO_H
#define AUDIO_H
#include <stdint.h>
#include <stdbool.h>
/*
* audio.h — I2S audio output driver (Issue #143)
*
* Hardware: SPI3 repurposed as I2S3 master TX (blackbox flash not used
* on balance bot). Supports MAX98357A (I2S class-D amp) and PCM5102A
* (I2S DAC + external amp) — both use standard Philips I2S.
*
* Pin assignment (SPI3 / I2S3, defined in config.h):
* PC10 I2S3_CK (BCLK) AF6
* PA15 I2S3_WS (LRCLK) AF6
* PB5 I2S3_SD (DIN) AF6
* PC5 AUDIO_MUTE (GPIO) active-high = enabled; low = muted/shutdown
*
* PLLI2S: N=192, R=2 → 96 MHz I2S clock → 22058 Hz (< 0.04% from 22050)
* DMA1 Stream7 Channel0 (SPI3_TX), circular, double-buffer ping-pong.
*
* Mixer priority (highest to lowest):
* 1. PCM audio chunks from Jetson (via JLINK_CMD_AUDIO, written to FIFO)
* 2. Notification tones (queued by audio_play_tone)
* 3. Silence
*
* Volume applies to all sources via integer sample scaling (0–100).
*/
/* Maximum int16_t samples per JLINK_CMD_AUDIO frame (252-byte payload / 2) */
#define AUDIO_CHUNK_MAX_SAMPLES 126u
/* Pre-defined notification tones */
typedef enum {
AUDIO_TONE_BEEP_SHORT = 0, /* 880 Hz, 100 ms — acknowledge / UI feedback */
AUDIO_TONE_BEEP_LONG = 1, /* 880 Hz, 500 ms — generic warning */
AUDIO_TONE_STARTUP = 2, /* C5→E5→G5 arpeggio (3 × 120 ms) */
AUDIO_TONE_ARM = 3, /* 880 Hz→1047 Hz two-beep ascending */
AUDIO_TONE_DISARM = 4, /* 880 Hz→659 Hz two-beep descending */
AUDIO_TONE_FAULT = 5, /* 200 Hz buzz, 500 ms — tilt/safety fault */
AUDIO_TONE_COUNT
} AudioTone;
/*
* audio_init()
*
* Configure PLLI2S, GPIO, DMA1 Stream7, and SPI3/I2S3.
* Pre-fills DMA buffer with silence, starts circular DMA TX, then
* unmutes the amp. Call once before safety_init().
*/
void audio_init(void);
/*
* audio_mute(mute)
*
* Drive AUDIO_MUTE_PIN: false = hardware-muted (SD/XSMT low),
* true = active (amp enabled). Does NOT stop DMA; allows instant
* un-mute without DMA restart clicks.
*/
void audio_mute(bool active);
/*
* audio_set_volume(vol)
*
* Software volume 0–100. Applied in ISR fill path via integer scaling.
* 0 = silence, 100 = full scale (±16384 for square wave, passthrough for PCM).
*/
void audio_set_volume(uint8_t vol);
/*
* audio_play_tone(tone)
*
* Queue a pre-defined notification tone. The tone plays after any tones
* already in the queue. Returns false if the tone queue is full (depth 4).
* Tones are pre-empted by incoming PCM audio from the Jetson.
*/
bool audio_play_tone(AudioTone tone);
/*
* audio_write_pcm(samples, n)
*
* Write mono 16-bit 22050 Hz PCM samples into the Jetson PCM FIFO.
* Called from jlink_process() dispatch on JLINK_CMD_AUDIO (main-loop context).
* Returns the number of samples actually accepted (0 if FIFO is full).
*/
uint16_t audio_write_pcm(const int16_t *samples, uint16_t n);
/*
* audio_tick(now_ms)
*
* Advance the tone sequencer state machine. Must be called every 1 ms
* from the main loop. Manages step transitions and gap timing; updates
* the volatile active-tone parameters read by the ISR fill path.
*/
void audio_tick(uint32_t now_ms);
/*
* audio_is_playing()
*
* Returns true if the DMA is running (always true after audio_init()
* unless the amp is hardware-muted or the I2S peripheral has an error).
*/
bool audio_is_playing(void);
#endif /* AUDIO_H */

53
legacy/stm32/include/balance.h
View File

@ -1,53 +0,0 @@
#ifndef BALANCE_H
#define BALANCE_H
#include <stdint.h>
#include "mpu6000.h"
#include "slope_estimator.h"
/*
* SaltyLab Balance Controller
*
* Consumes fused IMUData (pitch + pitch_rate from mpu6000 complementary filter)
* PID controller → motor speed command
* Safety: tilt cutoff, arming, watchdog
*/
typedef enum {
BALANCE_DISARMED = 0, /* Motors off, waiting for arm command */
BALANCE_ARMED = 1, /* Active balancing */
BALANCE_TILT_FAULT = 2, /* Tilt exceeded limit, motors killed */
BALANCE_PARKED = 3, /* PID frozen, motors off — quick re-arm via button (Issue #682) */
} balance_state_t;
typedef struct {
/* State */
balance_state_t state;
float pitch_deg; /* Current pitch angle (degrees) */
float pitch_rate; /* Gyro pitch rate (deg/s) */
/* PID internals */
float integral;
float prev_error;
int16_t motor_cmd; /* Output to ESC: -1000..+1000 */
/* Tuning */
float kp, ki, kd;
float setpoint; /* Target pitch angle (degrees) — tune for COG offset */
/* Safety */
float max_tilt; /* Cutoff angle (degrees) */
int16_t max_speed; /* Speed limit */
/* Slope compensation (Issue #600) */
slope_estimator_t slope;
} balance_t;
void balance_init(balance_t *b);
void balance_update(balance_t *b, const IMUData *imu, float dt);
void balance_arm(balance_t *b);
void balance_disarm(balance_t *b);
void balance_park(balance_t *b); /* ARMED -> PARKED: freeze PID, zero motors (Issue #682) */
void balance_unpark(balance_t *b); /* PARKED -> ARMED if pitch < 20 deg (Issue #682) */
#endif

66
legacy/stm32/include/baro.h
View File

@ -1,66 +0,0 @@
#ifndef BARO_H
#define BARO_H
#include <stdint.h>
#include <stdbool.h>
/*
* baro — BME280/BMP280 barometric pressure & ambient temperature module
* (Issue #672).
*
* Reads pressure and temperature from the BME280 at BARO_READ_HZ (1 Hz),
* computes pressure altitude using the ISA barometric formula, and publishes
* JLINK_TLM_BARO (0x8D) telemetry to the Orin at BARO_TLM_HZ (1 Hz).
*
* Runs entirely on the Mamba F722S — no Orin dependency.
* Altitude is exposed via baro_get_alt_cm() for use by slope compensation
* in the balance PID (Issue #672 requirement).
*
* Usage:
* 1. Call i2c1_init() then bmp280_init() and pass the chip_id result.
* 2. Call baro_tick(now_ms) every ms from the main loop.
* 3. Call baro_get_alt_cm() to read the latest altitude.
*/
/* ---- Configuration ---- */
#define BARO_READ_HZ 1u /* sensor poll rate (Hz) */
#define BARO_TLM_HZ 1u /* JLink telemetry rate (Hz) */
/* ---- Data ---- */
typedef struct {
int32_t pressure_pa; /* barometric pressure (Pa) */
int16_t temp_x10; /* ambient temperature (°C × 10; e.g. 235 = 23.5 °C) */
int32_t alt_cm; /* pressure altitude above ISA sea level (cm) */
int16_t humidity_pct_x10; /* %RH × 10 (BME280 only); -1 if BMP280/absent */
bool valid; /* true once at least one reading has been obtained */
} baro_data_t;
/* ---- API ---- */
/*
* baro_init(chip_id) — register chip type from bmp280_init() result.
* chip_id : 0x58 = BMP280, 0x60 = BME280, 0 = absent/not found.
* Call after i2c1_init() and bmp280_init(); no-op if chip_id == 0.
*/
void baro_init(int chip_id);
/*
* baro_tick(now_ms) — rate-limited sensor read + JLink telemetry publish.
* Call every ms from the main loop. No-op if chip absent.
* Reads at BARO_READ_HZ; sends JLINK_TLM_BARO at BARO_TLM_HZ.
*/
void baro_tick(uint32_t now_ms);
/*
* baro_get(out) — copy latest baro data into *out.
* Returns true on success; false if no valid reading yet.
*/
bool baro_get(baro_data_t *out);
/*
* baro_get_alt_cm() — latest pressure altitude (cm above ISA sea level).
* Returns 0 if no valid reading. Used by slope compensation in balance PID.
*/
int32_t baro_get_alt_cm(void);
#endif /* BARO_H */

49
legacy/stm32/include/battery.h
View File

@ -1,49 +0,0 @@
#ifndef BATTERY_H
#define BATTERY_H
/*
* battery.h — Vbat ADC reading for CRSF telemetry (Issue #103)
*
* Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722).
* Voltage divider: 10 kΩ / 1 kΩ → 11:1 ratio.
* Resolution: 12-bit (0–4095), Vref = 3.3 V.
*
* Filtered output in millivolts. Reading is averaged over
* BATTERY_SAMPLES conversions (software oversampling) to reduce noise.
*/
#include <stdint.h>
/* Initialise ADC3 for single-channel Vbat reading on PC1. */
void battery_init(void);
/*
* battery_read_mv() — blocking single-shot read; returns Vbat in mV.
* Takes ~1 µs (12-bit conversion at 36 MHz APB2 / 8 prescaler = 4.5 MHz ADC clk).
* Returns 0 if ADC not initialised or conversion times out.
*/
uint32_t battery_read_mv(void);
/*
* battery_estimate_pct() — coarse SoC estimate from Vbat (mV).
* Works for 3S LiPo (10.5–12.6 V) and 4S (14.0–16.8 V).
* Detection is automatic based on voltage.
* Returns 0–100, or 255 if voltage is out of range.
*/
uint8_t battery_estimate_pct(uint32_t voltage_mv);
/*
* battery_accumulate_coulombs() — periodically integrate battery current.
* Call every 10-20 ms (50-100 Hz) from main loop to accumulate coulombs.
* Reads motor currents from INA219 sensors.
*/
void battery_accumulate_coulombs(void);
/*
* battery_get_soc_coulomb() — get coulomb-based SoC estimate.
* Returns 0–100 (percent), or 255 if coulomb counter not yet valid.
* Preferred over voltage-based when valid.
*/
uint8_t battery_get_soc_coulomb(void);
#endif /* BATTERY_H */

143
legacy/stm32/include/battery_adc.h
View File

@ -1,143 +0,0 @@
/*
* battery_adc.h — DMA-based battery voltage/current ADC driver (Issue #533)
*
* Hardware:
* ADC3 channel IN11 (PC1) — Vbat through 10kΩ/1kΩ divider (11:1 ratio)
* ADC3 channel IN13 (PC3) — Ibat via shunt amplifier (ADC_IBAT_SCALE=115)
* DMA2 Stream0 Channel2 — ADC3 → memory circular (8-word buffer)
* USART1 (jlink) — telemetry to Jetson via JLINK_TLM_BATTERY (0x82)
*
* HOW IT WORKS:
* 1. ADC3 runs in continuous scan mode, alternating IN11 (Vbat) and IN13 (Ibat)
* at APB2/8 clock (≈ 13.5 MHz ADC clock on STM32F7 @ 216 MHz).
* 480-cycle sampling per channel → ~35 µs per scan pair, ~28 kHz scan rate.
*
* 2. DMA2_Stream0 (circular) fills an 8-word buffer: 4 Vbat samples followed
* by 4 Ibat samples per DMA half-complete cycle. Interleaved layout:
* [vbat0, ibat0, vbat1, ibat1, vbat2, ibat2, vbat3, ibat3]
*
* 3. battery_adc_tick() (call from main loop, 10–100 Hz) averages the 4 Vbat
* and 4 Ibat raw values (4× hardware oversampling), then feeds a 1st-order
* IIR low-pass filter:
* filtered += (raw - filtered) >> BATTERY_ADC_LPF_SHIFT
* With LPF_SHIFT=3 (α = 1/8) and 100 Hz tick rate, cutoff ≈ 4 Hz.
*
* 4. Calibration scales and offsets the filtered output:
* vbat_mv = filtered_raw * (VBAT_AREF_MV * VBAT_SCALE_NUM) / 4096
* + cal.vbat_offset_mv
* ibat_ma = filtered_raw * ADC_IBAT_SCALE_MA_PER_COUNT / 1000
* + cal.ibat_offset_ma
* User calibration adjusts cal.vbat_offset_mv to null out divider tolerance.
*
* 5. battery_adc_publish() sends JLINK_TLM_BATTERY (0x82) to Jetson at 1 Hz.
*
* 6. battery_adc_check_pm() monitors for low voltage. If Vbat drops below
* BATTERY_ADC_LOW_MV for BATTERY_ADC_LOW_HOLD_MS, calls
* power_mgmt_notify_battery(vbat_mv) which requests sleep (Issue #467).
*
* Interrupt safety:
* s_dma_buf is written by DMA hardware; battery_adc_tick() reads it with a
* brief __disable_irq() snapshot to prevent torn reads of the 16-bit words.
* All other state is private to the main-loop call path.
*/
#ifndef BATTERY_ADC_H
#define BATTERY_ADC_H
#include <stdint.h>
#include <stdbool.h>
/* ---- Low-pass filter ---- */
/* IIR shift: α = 1/8 → cutoff ≈ 4 Hz at 100 Hz tick rate */
#define BATTERY_ADC_LPF_SHIFT 3u
/* ---- Low-voltage thresholds (mV) ---- */
/* 3S LiPo: 9.0 V cell floor ×3 = 9900 mV full, 9000 mV absolute minimum */
#define BATTERY_ADC_LOW_MV 10200u /* ≈ 15% SoC — warn / throttle */
#define BATTERY_ADC_CRITICAL_MV 9600u /* ≈ 5% SoC — request sleep (#467) */
#define BATTERY_ADC_LOW_HOLD_MS 5000u /* must stay below this long to act */
/* 4S LiPo equivalents (auto-detected when Vbat ≥ 13 V at boot) */
#define BATTERY_ADC_LOW_MV_4S 13600u
#define BATTERY_ADC_CRITICAL_MV_4S 12800u
/* ---- Telemetry rate ---- */
#define BATTERY_ADC_PUBLISH_HZ 1u /* JLINK_TLM_BATTERY TX rate */
/* ---- Calibration struct ---- */
typedef struct {
int16_t vbat_offset_mv; /* additive offset after scale (mV, ±500 clamp) */
int16_t ibat_offset_ma; /* additive offset for current (mA, ±200 clamp) */
uint16_t vbat_scale_num; /* divider numerator override; 0 = use VBAT_SCALE_NUM */
uint16_t vbat_scale_den; /* divider denominator override; 0 = use 1 */
} battery_adc_cal_t;
/* ---- API ---- */
/*
* battery_adc_init() — configure ADC3 continuous-scan + DMA2_Stream0.
* Must be called after __HAL_RCC_ADC3_CLK_ENABLE / GPIO clock enables.
* Call once during system init, before battery_adc_tick().
*/
void battery_adc_init(void);
/*
* battery_adc_tick(now_ms) — average DMA buffer, apply IIR LPF, update state.
* Call from main loop at 10–100 Hz. Non-blocking (<5 µs).
*/
void battery_adc_tick(uint32_t now_ms);
/*
* battery_adc_get_voltage_mv() — calibrated, LPF-filtered Vbat in mV.
* Returns 0 if ADC not initialised.
*/
uint32_t battery_adc_get_voltage_mv(void);
/*
* battery_adc_get_current_ma() — calibrated, LPF-filtered Ibat in mA.
* Positive = discharging (load current). Returns 0 if not initialised.
*/
int32_t battery_adc_get_current_ma(void);
/*
* battery_adc_get_raw_voltage_mv() — unfiltered last-tick average (mV).
* Useful for calibration; use filtered version for control logic.
*/
uint32_t battery_adc_get_raw_voltage_mv(void);
/*
* battery_adc_calibrate(cal) — store calibration constants.
* Applies immediately to subsequent battery_adc_tick() calls.
* Pass NULL to reset to defaults (0 offset, default scale).
*/
void battery_adc_calibrate(const battery_adc_cal_t *cal);
/*
* battery_adc_get_calibration(out_cal) — read back current calibration.
*/
void battery_adc_get_calibration(battery_adc_cal_t *out_cal);
/*
* battery_adc_publish(now_ms) — send JLINK_TLM_BATTERY (0x82) frame.
* Rate-limited to BATTERY_ADC_PUBLISH_HZ; safe to call every main loop tick.
*/
void battery_adc_publish(uint32_t now_ms);
/*
* battery_adc_check_pm(now_ms) — evaluate low-voltage thresholds.
* Calls power_mgmt_notify_battery() on sustained critical voltage.
* Call from main loop after battery_adc_tick().
*/
void battery_adc_check_pm(uint32_t now_ms);
/*
* battery_adc_is_low() — true if Vbat below BATTERY_ADC_LOW_MV (warn level).
*/
bool battery_adc_is_low(void);
/*
* battery_adc_is_critical() — true if Vbat below BATTERY_ADC_CRITICAL_MV.
*/
bool battery_adc_is_critical(void);
#endif /* BATTERY_ADC_H */

28
legacy/stm32/include/bmp280.h
View File

@ -1,28 +0,0 @@
#ifndef BMP280_H
#define BMP280_H
#include <stdint.h>
/*
* BMP280 / BME280 barometer driver.
*
* Probes I2C1 at 0x76 then 0x77.
* Returns chip_id (0x58=BMP280, 0x60=BME280) on success, negative if not found.
* Requires i2c1_init() to have been called first.
*
* All I2C operations use 100ms timeouts — init will not hang on missing hardware.
*/
int bmp280_init(void);
void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10);
/*
* BME280-only humidity readout. Call AFTER bmp280_read() (uses cached t_fine).
* Returns humidity in %RH × 10 (e.g. 500 = 50.0 %RH).
* Returns -1 if chip is BMP280 (no humidity) or not initialised.
*/
int16_t bmp280_read_humidity(void);
/* Convert pressure (Pa) to altitude above sea level (cm), ISA p0=101325 Pa. */
int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa);
#endif /* BMP280_H */

99
legacy/stm32/include/bno055.h
View File

@ -1,99 +0,0 @@
#ifndef BNO055_H
#define BNO055_H
#include <stdint.h>
#include <stdbool.h>
#include "mpu6000.h" /* IMUData */
/*
* BNO055 NDOF IMU driver over I2C1 (shared bus — PB8=SCL, PB9=SDA).
*
* Issue #135: auto-detected alongside MPU6000. Acts as:
* PRIMARY — when MPU6000 init fails (seamless fallback)
* AUGMENT — when both present; BNO055 provides better NDOF-fused yaw
*
* I2C addresses probed: 0x28 (ADR=0, default) then 0x29 (ADR=1).
* Chip-ID register 0x00 must read 0xA0.
*
* Operating mode: NDOF (0x0C) — 9DOF fusion with magnetometer.
* Falls back to IMUPLUS (0x08, no mag) if mag calibration stalls.
*
* Calibration offsets are saved to/restored from STM32 RTC backup
* registers (BKP0R–BKP6R = 28 bytes), identified by a magic word.
* If valid offsets are present, bno055_is_ready() returns true
* immediately after init. Otherwise, waits for gyro+accel cal ≥ 2.
*
* Temperature compensation is handled internally by the BNO055 silicon
* (it compensates all three sensors continuously). bno055_temperature()
* exposes the onboard thermometer reading for telemetry.
*
* Loop-rate note: BNO055 reads over I2C1 @100kHz take ~3ms, so the
* main balance loop drops from ~1kHz (MPU6000/SPI) to ~250Hz when
* BNO055 is active. 250Hz is sufficient for stable self-balancing.
* PID gain tuning may be required when switching IMU sources.
*/
/* ---- Calibration status nibble masks (CALIB_STAT reg 0x35) ---- */
#define BNO055_CAL_SYS_MASK 0xC0u /* bits [7:6] — overall system */
#define BNO055_CAL_GYR_MASK 0x30u /* bits [5:4] — gyroscope */
#define BNO055_CAL_ACC_MASK 0x0Cu /* bits [3:2] — accelerometer */
#define BNO055_CAL_MAG_MASK 0x03u /* bits [1:0] — magnetometer */
/* Each field: 0=uncalibrated, 3=fully calibrated */
/*
* bno055_init() — probe I2C1 for BNO055, reset, enter NDOF mode,
* restore saved calibration offsets if present.
* Requires i2c1_init() already called.
* Returns 0 on success, -1 if not found.
* Blocks ~750ms (POR + mode-switch settle).
* Call BEFORE safety_init() (IWDG not yet running).
*/
int bno055_init(void);
/*
* bno055_read(data) — fill IMUData from BNO055 NDOF fusion output.
* Uses Euler angles for pitch/roll/yaw and gyro registers for pitch_rate.
* Triggers one I2C burst read (~3ms at 100kHz).
* Call from main loop balance gate (not every loop iteration).
*/
void bno055_read(IMUData *data);
/*
* bno055_is_ready() — true when BNO055 is suitable for balance arming.
* True immediately if offsets were restored from backup RAM.
* Otherwise true once gyro calibration ≥ 2 and accel ≥ 2.
*/
bool bno055_is_ready(void);
/*
* bno055_calib_status() — raw CALIB_STAT byte.
* Use BNO055_CAL_*_MASK to extract individual sensor calibration levels.
* Returned value is updated lazily on each bno055_read() call.
*/
uint8_t bno055_calib_status(void);
/*
* bno055_temperature() — onboard temperature in °C (gyro source).
* Updated once per second (every ~250 calls to bno055_read()).
* Range: -40..+85°C. Use for telemetry reporting only.
*/
int8_t bno055_temperature(void);
/*
* bno055_save_offsets() — write current calibration offsets to
* STM32 RTC backup registers BKP0R–BKP6R (22 bytes + magic).
* Call once after sys+acc+gyr calibration all reach level 3.
* Returns true if successful, false if BNO055 not present.
* Temporarily switches to CONFIGMODE — do NOT call while armed.
*/
bool bno055_save_offsets(void);
/*
* bno055_restore_offsets() — read offsets from RTC backup registers
* and write them to BNO055 hardware (in CONFIGMODE).
* Called automatically by bno055_init().
* Returns true if valid offsets found and applied.
*/
bool bno055_restore_offsets(void);
#endif /* BNO055_H */

146
legacy/stm32/include/buzzer.h
View File

@ -1,146 +0,0 @@
#ifndef BUZZER_H
#define BUZZER_H
#include <stdint.h>
#include <stdbool.h>
/*
* buzzer.h — Piezo buzzer melody driver (Issue #253)
*
* STM32F722 driver for piezo buzzer on PA8 using TIM1 PWM.
* Plays predefined melodies and tones with non-blocking queue.
*
* Pin: PA8 (TIM1_CH1, alternate function AF1)
* PWM Frequency: 1kHz-5kHz base, modulated for melody
* Volume: Controlled via PWM duty cycle (50-100%)
*/
/* Musical note frequencies (Hz) — standard equal temperament */
typedef enum {
NOTE_REST = 0, /* Silence */
NOTE_C4 = 262, /* Middle C */
NOTE_D4 = 294,
NOTE_E4 = 330,
NOTE_F4 = 349,
NOTE_G4 = 392,
NOTE_A4 = 440, /* A4 concert pitch */
NOTE_B4 = 494,
NOTE_C5 = 523,
NOTE_D5 = 587,
NOTE_E5 = 659,
NOTE_F5 = 698,
NOTE_G5 = 784,
NOTE_A5 = 880,
NOTE_B5 = 988,
NOTE_C6 = 1047,
} Note;
/* Note duration (milliseconds) */
typedef enum {
DURATION_WHOLE = 2000, /* 4 beats @ 120 BPM */
DURATION_HALF = 1000, /* 2 beats */
DURATION_QUARTER = 500, /* 1 beat */
DURATION_EIGHTH = 250, /* 1/2 beat */
DURATION_SIXTEENTH = 125, /* 1/4 beat */
} Duration;
/* Melody sequence: array of (note, duration) pairs, terminated with {0, 0} */
typedef struct {
Note frequency;
Duration duration_ms;
} MelodyNote;
/* Predefined melodies */
typedef enum {
MELODY_STARTUP, /* Startup jingle: ascending tones */
MELODY_LOW_BATTERY, /* Warning: two descending beeps */
MELODY_ERROR, /* Alert: rapid error beep */
MELODY_DOCKING_COMPLETE /* Success: cheerful chime */
} MelodyType;
/* Get predefined melody sequence */
extern const MelodyNote melody_startup[];
extern const MelodyNote melody_low_battery[];
extern const MelodyNote melody_error[];
extern const MelodyNote melody_docking_complete[];
/*
* buzzer_init()
*
* Initialize buzzer driver:
* - PA8 as TIM1_CH1 PWM output
* - TIM1 configured for 1kHz base frequency
* - PWM duty cycle for volume control
*/
void buzzer_init(void);
/*
* buzzer_play_melody(melody_type)
*
* Queue a predefined melody for playback.
* Non-blocking: returns immediately, melody plays asynchronously.
* Multiple calls queue melodies in sequence.
*
* Supported melodies:
* - MELODY_STARTUP: 2-3 second jingle on power-up
* - MELODY_LOW_BATTERY: 1 second warning
* - MELODY_ERROR: 0.5 second alert beep
* - MELODY_DOCKING_COMPLETE: 1-1.5 second success chime
*
* Returns: true if queued, false if queue full
*/
bool buzzer_play_melody(MelodyType melody_type);
/*
* buzzer_play_custom(notes)
*
* Queue a custom melody sequence.
* Notes array must be terminated with {NOTE_REST, 0}.
* Useful for error codes or custom notifications.
*
* Returns: true if queued, false if queue full
*/
bool buzzer_play_custom(const MelodyNote *notes);
/*
* buzzer_play_tone(frequency, duration_ms)
*
* Queue a simple single tone.
* Useful for beeps and alerts.
*
* Arguments:
* - frequency: Note frequency (Hz), 0 for silence
* - duration_ms: Tone duration in milliseconds
*
* Returns: true if queued, false if queue full
*/
bool buzzer_play_tone(uint16_t frequency, uint16_t duration_ms);
/*
* buzzer_stop()
*
* Stop current playback and clear queue.
* Buzzer returns to silence immediately.
*/
void buzzer_stop(void);
/*
* buzzer_is_playing()
*
* Returns: true if melody/tone is currently playing, false if idle
*/
bool buzzer_is_playing(void);
/*
* buzzer_tick(now_ms)
*
* Update function called periodically (recommended: every 10ms in main loop).
* Manages melody timing and PWM frequency transitions.
* Must be called regularly for non-blocking operation.
*
* Arguments:
* - now_ms: current time in milliseconds (from HAL_GetTick() or similar)
*/
void buzzer_tick(uint32_t now_ms);
#endif /* BUZZER_H */

54
legacy/stm32/include/can_driver.h
View File

@ -1,54 +0,0 @@
#ifndef CAN_DRIVER_H
#define CAN_DRIVER_H
#include <stdint.h>
#include <stdbool.h>
#define CAN_NUM_MOTORS 2u
#define CAN_NODE_LEFT 0u
#define CAN_NODE_RIGHT 1u
#define CAN_ID_VEL_CMD_BASE 0x100u
#define CAN_ID_ENABLE_CMD_BASE 0x110u
#define CAN_ID_FEEDBACK_BASE 0x200u
#define CAN_FILTER_STDID 0x200u
#define CAN_FILTER_MASK 0x7E0u
#define CAN_PRESCALER 6u
#define CAN_TX_RATE_HZ 100u
#define CAN_NODE_TIMEOUT_MS 100u
#define CAN_WDOG_RESTART_MS 200u
typedef struct { int16_t velocity_rpm; int16_t torque_x100; } can_cmd_t;
typedef struct {
int16_t velocity_rpm; int16_t current_ma; int16_t position_x100;
int8_t temperature_c; uint8_t fault; uint32_t last_rx_ms;
} can_feedback_t;
typedef struct {
uint32_t tx_count; uint32_t rx_count; uint16_t err_count;
uint8_t bus_off; uint8_t _pad;
} can_stats_t;
typedef enum {
CAN_ERR_NOMINAL = 0u, CAN_ERR_WARNING = 1u,
CAN_ERR_ERROR_PASSIVE = 2u, CAN_ERR_BUS_OFF = 3u,
} can_error_state_t;
typedef struct {
uint32_t restart_count; uint32_t busoff_count;
uint16_t errpassive_count; uint16_t errwarn_count;
can_error_state_t error_state; uint8_t tec; uint8_t rec; uint8_t busoff_pending;
uint32_t busoff_ms;
} can_wdog_t;
void can_driver_init(void);
void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd);
void can_driver_send_enable(uint8_t node_id, bool enable);
bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out);
bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms);
void can_driver_get_stats(can_stats_t *out);
void can_driver_process(void);
can_error_state_t can_driver_watchdog_tick(uint32_t now_ms);
void can_driver_get_wdog(can_wdog_t *out);
#ifdef TEST_HOST
void can_driver_inject_esr(uint32_t esr_val);
#endif
typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len);
typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len);
void can_driver_set_ext_cb(can_ext_frame_cb_t cb);
void can_driver_set_std_cb(can_std_frame_cb_t cb);
void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len);
void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len);
#endif /* CAN_DRIVER_H */

308
legacy/stm32/include/config.h
View File

@ -1,308 +0,0 @@
#ifndef CONFIG_H
#define CONFIG_H
// ============================================
// SaltyLab Balance Bot — MAMBA F722S FC
// Pin assignments from Betaflight: DIAT-MAMBAF722_2022B
// ============================================
// --- IMU: MPU6000 (SPI1) ---
// SPI1: PA5=SCK, PA6=MISO, PA7=MOSI
// WHO_AM_I = 0x68
#define MPU_SPI SPI1
#define MPU_CS_PORT GPIOA
#define MPU_CS_PIN GPIO_PIN_4 // GYRO_CS 1
#define MPU_EXTI_PORT GPIOC
#define MPU_EXTI_PIN GPIO_PIN_4 // GYRO_EXTI 1 (data ready IRQ)
#define GYRO_ALIGN CW270 // gyro_1_sensor_align = CW270
// --- Barometer: BMP280 or DPS310 (I2C1) ---
#define BARO_I2C I2C1
#define BARO_SCL_PORT GPIOB
#define BARO_SCL_PIN GPIO_PIN_8 // I2C_SCL 1
#define BARO_SDA_PORT GPIOB
#define BARO_SDA_PIN GPIO_PIN_9 // I2C_SDA 1
// Magnetometer also on I2C1 (external, header only)
// --- LEDs ---
#define LED1_PORT GPIOC
#define LED1_PIN GPIO_PIN_15 // LED 1 (active low)
#define LED2_PORT GPIOC
#define LED2_PIN GPIO_PIN_14 // LED 2 (active low)
// --- Buzzer ---
#define BEEPER_PORT GPIOB
#define BEEPER_PIN GPIO_PIN_2 // BEEPER 1
#define BEEPER_INVERTED 1 // beeper_inversion = ON
// beeper_od = OFF (push-pull)
// --- Battery Monitoring (ADC3) ---
#define ADC_VBAT_PORT GPIOC
#define ADC_VBAT_PIN GPIO_PIN_1 // ADC_BATT 1
#define ADC_CURR_PORT GPIOC
#define ADC_CURR_PIN GPIO_PIN_3 // ADC_CURR 1
#define ADC_IBAT_SCALE 115 // ibata_scale
// --- LED Strip (WS2812 NeoPixel, Issue #193) ---
// TIM3_CH1 PWM on PB4 for 8-LED ring status indicator
#define LED_STRIP_TIM TIM3
#define LED_STRIP_CHANNEL TIM_CHANNEL_1
#define LED_STRIP_PORT GPIOB
#define LED_STRIP_PIN GPIO_PIN_4 // LED_STRIP 1 (TIM3_CH1)
#define LED_STRIP_AF GPIO_AF2_TIM3 // Alternate function
#define LED_STRIP_NUM_LEDS 8u // 8-LED ring
#define LED_STRIP_FREQ_HZ 800000u // 800 kHz PWM for NeoPixel (1.25 µs per bit)
// --- Servo Pan-Tilt (Issue #206) ---
// TIM4_CH1 (PB6) for pan servo, TIM4_CH2 (PB7) for tilt servo
#define SERVO_TIM TIM4
#define SERVO_PAN_PORT GPIOB
#define SERVO_PAN_PIN GPIO_PIN_6 // TIM4_CH1
#define SERVO_PAN_CHANNEL TIM_CHANNEL_1
#define SERVO_TILT_PORT GPIOB
#define SERVO_TILT_PIN GPIO_PIN_7 // TIM4_CH2
#define SERVO_TILT_CHANNEL TIM_CHANNEL_2
#define SERVO_AF GPIO_AF2_TIM4 // Alternate function
#define SERVO_FREQ_HZ 50u // 50 Hz (20ms period, standard servo)
#define SERVO_MIN_US 500u // 500µs = 0°
#define SERVO_MAX_US 2500u // 2500µs = 180°
#define SERVO_CENTER_US 1500u // 1500µs = 90°
// --- OSD: MAX7456 (SPI2) ---
#define OSD_SPI SPI2
#define OSD_CS_PORT GPIOB
#define OSD_CS_PIN GPIO_PIN_12 // OSD_CS 1
// SPI2: PB13=SCK, PB14=MISO, PB15=MOSI
// --- Blackbox Flash: M25P16 (SPI3) ---
#define FLASH_SPI SPI3
#define FLASH_CS_PORT GPIOA
#define FLASH_CS_PIN GPIO_PIN_15 // FLASH_CS 1
// SPI3: PC10=SCK, PC11=MISO, PB5=MOSI
// --- Motor Outputs (PWM/DShot) ---
#define MOTOR1_PORT GPIOC
#define MOTOR1_PIN GPIO_PIN_8 // TIM8_CH3
#define MOTOR2_PORT GPIOC
#define MOTOR2_PIN GPIO_PIN_9 // TIM8_CH4
#define MOTOR3_PORT GPIOA
#define MOTOR3_PIN GPIO_PIN_8 // TIM1_CH1
#define MOTOR4_PORT GPIOA
#define MOTOR4_PIN GPIO_PIN_9 // TIM1_CH2
#define MOTOR5_PORT GPIOB
#define MOTOR5_PIN GPIO_PIN_0 // TIM3_CH3
#define MOTOR6_PORT GPIOB
#define MOTOR6_PIN GPIO_PIN_1 // TIM3_CH4
#define MOTOR7_PORT GPIOA
#define MOTOR7_PIN GPIO_PIN_10 // TIM1_CH3
#define MOTOR8_PORT GPIOB
#define MOTOR8_PIN GPIO_PIN_4 // TIM3_CH1
// --- UARTs ---
// USART1: PB6=TX, PB7=RX (serial 0, SmartAudio/VTX)
#define UART1_TX_PORT GPIOB
#define UART1_TX_PIN GPIO_PIN_6
#define UART1_RX_PORT GPIOB
#define UART1_RX_PIN GPIO_PIN_7
// USART2: PA2=TX, PA3=RX (serial 1)
#define UART2_TX_PORT GPIOA
#define UART2_TX_PIN GPIO_PIN_2
#define UART2_RX_PORT GPIOA
#define UART2_RX_PIN GPIO_PIN_3
// USART3: PB10=TX, PB11=RX (serial 2, SBUS RX default)
#define UART3_TX_PORT GPIOB
#define UART3_TX_PIN GPIO_PIN_10
#define UART3_RX_PORT GPIOB
#define UART3_RX_PIN GPIO_PIN_11
// UART4: PA0=TX, PA1=RX (serial 3)
#define UART4_TX_PORT GPIOA
#define UART4_TX_PIN GPIO_PIN_0
#define UART4_RX_PORT GPIOA
#define UART4_RX_PIN GPIO_PIN_1
// UART5: PC12=TX, PD2=RX (serial 4)
#define UART5_TX_PORT GPIOC
#define UART5_TX_PIN GPIO_PIN_12
#define UART5_RX_PORT GPIOD
#define UART5_RX_PIN GPIO_PIN_2
// USART6: PC6=TX, PC7=RX (serial 5)
#define UART6_TX_PORT GPIOC
#define UART6_TX_PIN GPIO_PIN_6
#define UART6_RX_PORT GPIOC
#define UART6_RX_PIN GPIO_PIN_7
// --- PINIO (switchable outputs, e.g. VTX power) ---
#define PINIO1_PORT GPIOC
#define PINIO1_PIN GPIO_PIN_2 // pinio_config = 129 (USER1)
#define PINIO2_PORT GPIOC
#define PINIO2_PIN GPIO_PIN_0 // pinio_config = 129 (USER2)
// --- JLink: Jetson Serial Binary Protocol (USART1, Issue #120) ---
#define JLINK_BAUD 921600 /* USART1 baud rate */
#define JLINK_HB_TIMEOUT_MS 1000 /* Jetson heartbeat timeout (ms) */
#define JLINK_TLM_HZ 50 /* STATUS telemetry TX rate (Hz) */
// --- Firmware Version ---
#define FW_MAJOR 1
#define FW_MINOR 0
#define FW_PATCH 0
// --- SaltyLab Assignments ---
// Hoverboard ESC: USART2 (PA2=TX, PA3=RX) or USART3
// ELRS Receiver: UART4 (PA0=TX, PA1=RX) — CRSF 420000 baud
// Jetson (JLink binary protocol, Issue #120): USART1 (PB6=TX, PB7=RX) @ 921600
// USART6 (PC6=TX, PC7=RX): legacy Jetson CDC path — reserved for VESC (Issue #383)
// Debug: UART5 (PC12=TX, PD2=RX)
// --- ESC Backend Selection (Issue #388) ---
// Pluggable ESC abstraction layer — supports multiple backends:
// HOVERBOARD: EFeru FOC (USART2 @ 115200) — current default
// VESC: FSESC 4.20 Plus (USART6 @ 921600, balance mode) — future
#define ESC_BACKEND HOVERBOARD /* HOVERBOARD or VESC */
// --- CRSF / ExpressLRS ---
// CH1[0]=steer CH2[1]=throttle CH5[4]=arm CH6[5]=mode
#define CRSF_ARM_THRESHOLD 1750 /* CH5 raw value; > threshold = armed */
#define CRSF_STEER_MAX 400 /* CH1 range: -400..+400 motor counts */
#define CRSF_FAILSAFE_MS 500 /* Disarm after this ms without a frame (Issue #103) */
// --- Battery ADC (ADC3, PC1 = ADC123_IN11) ---
/* Mamba F722: 10kΩ + 1kΩ voltage divider → 11:1 ratio */
#define VBAT_SCALE_NUM 11 /* Numerator of divider ratio */
#define VBAT_AREF_MV 3300 /* ADC reference in mV */
#define VBAT_ADC_BITS 12 /* 12-bit ADC → 4096 counts */
/* Filtered Vbat in mV: (raw * 3300 * 11) / 4096, updated at 10Hz */
// --- CRSF Telemetry TX (uplink: FC → ELRS module → pilot handset) ---
#define CRSF_TELEMETRY_HZ 1 /* Telemetry TX rate (Hz) */
// --- PID Tuning ---
#define PID_KP 35.0f
#define PID_KI 1.0f
#define PID_KD 1.0f
#define PID_INTEGRAL_MAX 500.0f
#define PID_LOOP_HZ 1000
// --- Safety ---
#define MAX_TILT_DEG 25.0f
#define RC_TIMEOUT_MS 500
#define ARMING_HOLD_MS 3000
#define MAX_SPEED_LIMIT 100
#define WATCHDOG_TIMEOUT_MS 50
// --- Motor Driver ---
#define MOTOR_CMD_MAX 1000 /* ESC range: -1000..+1000 */
#define MOTOR_STEER_RAMP_RATE 20 /* counts/ms — steer ramp only */
// --- IMU Calibration ---
#define GYRO_CAL_SAMPLES 1000 /* gyro bias samples (~1s at 1ms/sample) */
// --- RC / Mode Manager ---
/* CRSF channel indices (0-based; CRSF range 172-1811, center 992) */
#define CRSF_CH_STEER 0 /* CH1 — right stick horizontal (steer) */
#define CRSF_CH_SPEED 1 /* CH2 — right stick vertical (throttle) */
#define CRSF_CH_ARM 4 /* CH5 — arm switch (2-pos) */
#define CRSF_CH_MODE 5 /* CH6 — mode switch (3-pos) */
/* Deadband around CRSF center (992) in raw counts (~2% of range) */
#define CRSF_DEADBAND 30
/* CH6 mode thresholds (raw CRSF counts) */
#define CRSF_MODE_LOW_THRESH 600 /* <= → RC_MANUAL */
#define CRSF_MODE_HIGH_THRESH 1200 /* >= → AUTONOMOUS */
/* Max speed bias RC can add to balance PID output (counts, same scale as ESC) */
#define MOTOR_RC_SPEED_MAX 300
/* Full blend transition time: MANUAL→AUTO takes this many ms */
#define MODE_BLEND_MS 500
// --- Power Management (STOP mode, Issue #178) ---
#define PM_IDLE_TIMEOUT_MS 30000u // 30s no activity → PM_SLEEP_PENDING
#define PM_FADE_MS 3000u // LED fade-out duration before STOP entry
#define PM_LED_PERIOD_MS 2000u // sleep-pending triangle-wave period (ms)
// Estimated per-subsystem currents (mA) — used for JLINK_TLM_POWER telemetry
#define PM_CURRENT_BASE_MA 30 // SPI1(IMU)+UART4(CRSF)+USART1(JLink)+core
#define PM_CURRENT_AUDIO_MA 8 // I2S3 + amplifier quiescent
#define PM_CURRENT_OSD_MA 5 // SPI2 OSD (MAX7456)
#define PM_CURRENT_DEBUG_MA 1 // UART5 + USART6
#define PM_CURRENT_STOP_MA 1 // MCU in STOP mode (< 1 mA)
#define PM_TLM_HZ 1 // JLINK_TLM_POWER transmit rate (Hz)
// --- Audio Amplifier (I2S3, Issue #143) ---
// SPI3 repurposed as I2S3; blackbox flash unused on balance bot
#define AUDIO_BCLK_PORT GPIOC
#define AUDIO_BCLK_PIN GPIO_PIN_10 // I2S3_CK (PC10, AF6)
#define AUDIO_LRCK_PORT GPIOA
#define AUDIO_LRCK_PIN GPIO_PIN_15 // I2S3_WS (PA15, AF6)
#define AUDIO_DOUT_PORT GPIOB
#define AUDIO_DOUT_PIN GPIO_PIN_5 // I2S3_SD (PB5, AF6)
#define AUDIO_MUTE_PORT GPIOC
#define AUDIO_MUTE_PIN GPIO_PIN_5 // active-high = amp enabled
// PLLI2S: N=192, R=2 → I2S clock=96 MHz → FS≈22058 Hz (< 0.04% error)
#define AUDIO_SAMPLE_RATE 22050u // nominal sample rate (Hz)
#define AUDIO_BUF_HALF 441u // DMA half-buffer: 20ms at 22050 Hz
#define AUDIO_VOLUME_DEFAULT 80u // default volume 0-100
// --- Gimbal Servo Bus (ST3215, USART3 half-duplex, Issue #547) ---
// Half-duplex single-wire on PB10 (USART3_TX, AF7) at 1 Mbps.
// USART3 is available: not assigned to any active subsystem.
#define SERVO_BUS_UART USART3
#define SERVO_BUS_PORT GPIOB
#define SERVO_BUS_PIN GPIO_PIN_10 // USART3_TX, AF7
#define SERVO_BUS_BAUD 1000000u // 1 Mbps (ST3215 default)
#define GIMBAL_PAN_ID 1u // ST3215 servo ID for pan
#define GIMBAL_TILT_ID 2u // ST3215 servo ID for tilt
#define GIMBAL_TLM_HZ 50u // position feedback rate (Hz)
#define GIMBAL_PAN_LIMIT_DEG 180.0f // pan soft limit (deg each side)
#define GIMBAL_TILT_LIMIT_DEG 90.0f // tilt soft limit (deg each side)
// --- CAN Bus Driver (Issue #597, remapped Issue #676) ---
// CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) — SCL/SDA pads on Mamba F722S MK2
// I2C1 freed: BME280 moved to I2C2 (PB10/PB11); PB8/PB9 repurposed for CAN1
#define CAN_RPM_SCALE 10 // motor_cmd to RPM: 1 cmd count = 10 RPM
#define CAN_TLM_HZ 1u // JLINK_TLM_CAN_STATS transmit rate (Hz)
// --- LVC: Low Voltage Cutoff (Issue #613) ---
// 3-stage undervoltage protection; voltages in mV
#define LVC_WARNING_MV 21000u // 21.0 V -- buzzer alert, full power
#define LVC_CRITICAL_MV 19800u // 19.8 V -- 50% motor power reduction
#define LVC_CUTOFF_MV 18600u // 18.6 V -- motors disabled, latch until reboot
#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter
#define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz)
// --- UART Command Protocol (Issue #629) ---
// Jetson-STM32 binary command protocol on UART5 (PC12/PD2)
// NOTE: Spec requested USART1 @ 115200; USART1 is occupied by JLink @ 921600.
#define UART_PROT_BAUD 115200u // baud rate for UART5 Jetson protocol
#define UART_PROT_HB_TIMEOUT_MS 500u // heartbeat timeout: Jetson considered lost after 500 ms
// --- Encoder Odometry (Issue #632) ---
// Left encoder: TIM2 (32-bit), CH1=PA15 (AF1), CH2=PB3 (AF1)
// Right encoder: TIM3 (16-bit), CH1=PC6 (AF2), CH2=PC7 (AF2)
// Encoder mode 3: count on both A and B edges (x4 resolution)
#define ENC_LEFT_TIM TIM2
#define ENC_LEFT_CH1_PORT GPIOA
#define ENC_LEFT_CH1_PIN GPIO_PIN_15 // TIM2_CH1, AF1
#define ENC_LEFT_CH2_PORT GPIOB
#define ENC_LEFT_CH2_PIN GPIO_PIN_3 // TIM2_CH2, AF1
#define ENC_LEFT_AF GPIO_AF1_TIM2
#define ENC_RIGHT_TIM TIM3
#define ENC_RIGHT_CH1_PORT GPIOC
#define ENC_RIGHT_CH1_PIN GPIO_PIN_6 // TIM3_CH1, AF2
#define ENC_RIGHT_CH2_PORT GPIOC
#define ENC_RIGHT_CH2_PIN GPIO_PIN_7 // TIM3_CH2, AF2
#define ENC_RIGHT_AF GPIO_AF2_TIM3
// --- Hardware Button (Issue #682) ---
// Active-low push button on PC2 (internal pull-up)
#define BTN_PORT GPIOC
#define BTN_PIN GPIO_PIN_2
#define BTN_DEBOUNCE_MS 20u // ms debounce window
#define BTN_LONG_MIN_MS 1500u // ms threshold: LONG press
#define BTN_COMMIT_MS 500u // ms quiet after lone SHORT -> PARK event
#define BTN_SEQ_TIMEOUT_MS 3000u // ms: sequence window; expired buffer abandoned
#endif // CONFIG_H

45
legacy/stm32/include/coulomb_counter.h
View File

@ -1,45 +0,0 @@
#ifndef COULOMB_COUNTER_H
#define COULOMB_COUNTER_H
/*
* coulomb_counter.h — Battery coulomb counter for SoC estimation (Issue #325)
*
* Integrates battery current over time to track Ah consumed and remaining.
* Provides accurate SoC independent of load, with fallback to voltage.
*
* Usage:
* 1. Call coulomb_counter_init(capacity_mah) at startup
* 2. Call coulomb_counter_accumulate(current_ma) at 50–100 Hz
* 3. Call coulomb_counter_get_soc_pct() to get current SoC
* 4. Call coulomb_counter_reset() on charge complete
*/
#include <stdint.h>
#include <stdbool.h>
/* Initialize coulomb counter with battery capacity (mAh). */
void coulomb_counter_init(uint16_t capacity_mah);
/*
* Accumulate coulomb from current reading + elapsed time.
* Call this at regular intervals (e.g., 50–100 Hz from telemetry loop).
* current_ma: battery current in milliamps (positive = discharge)
*/
void coulomb_counter_accumulate(int16_t current_ma);
/* Get current SoC as percentage (0–100, 255 = error). */
uint8_t coulomb_counter_get_soc_pct(void);
/* Get consumed mAh (total charge removed from battery). */
uint16_t coulomb_counter_get_consumed_mah(void);
/* Get remaining capacity in mAh. */
uint16_t coulomb_counter_get_remaining_mah(void);
/* Reset accumulated coulombs (e.g., on charge complete). */
void coulomb_counter_reset(void);
/* Check if coulomb counter is active (initialized and has measurements). */
bool coulomb_counter_is_valid(void);
#endif /* COULOMB_COUNTER_H */

69
legacy/stm32/include/crsf.h
View File

@ -1,69 +0,0 @@
#ifndef CRSF_H
#define CRSF_H
#include <stdint.h>
#include <stdbool.h>
/*
* CRSF/ExpressLRS RC receiver state.
*
* Updated from ISR context on every valid frame.
* Read from main loop — values are naturally atomic (8/16-bit on Cortex-M).
* last_rx_ms == 0 means no frame received yet (USB-only mode).
*/
typedef struct {
uint16_t channels[16]; /* Raw CRSF values, 172 (988µs) – 1811 (2012µs) */
uint32_t last_rx_ms; /* HAL_GetTick() at last valid RC frame */
bool armed; /* CH5 arm switch: true when channels[4] > CRSF_ARM_THRESHOLD */
/* Link statistics (from 0x14 frames, optional) */
int8_t rssi_dbm; /* Uplink RSSI in dBm (negative, e.g. -85) */
uint8_t link_quality; /* Uplink link quality 0–100 % */
int8_t snr; /* Uplink SNR in dB */
} CRSFState;
/*
* crsf_init() — configure UART4 (PA0=TX, PA1=RX) at 420000 baud with
* DMA1 circular RX and IDLE interrupt. Call once before safety_init().
*/
void crsf_init(void);
/*
* crsf_parse_byte() — feed one byte into the frame parser.
* Called automatically from DMA/IDLE ISR. Available for unit tests.
*/
void crsf_parse_byte(uint8_t byte);
/*
* crsf_to_range() — map raw CRSF value (172–1811) linearly to [min, max].
* Clamps at boundaries. Midpoint 992 → (min+max)/2.
*/
int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max);
/*
* crsf_send_battery() — transmit CRSF battery-sensor telemetry frame (type 0x08)
* back to the ELRS TX module over UART4 TX. Call at CRSF_TELEMETRY_HZ (1 Hz).
*
* voltage_mv : battery voltage in millivolts (e.g. 12600 for 3S full)
* capacity_mah : remaining battery capacity in mAh (Issue #325, coulomb counter)
* remaining_pct: state-of-charge 0–100 % (255 = unknown)
*
* Frame: [0xC8][12][0x08][v16_hi][v16_lo][c16_hi][c16_lo][cap24×3][rem][CRC]
* voltage unit: 100 mV (12600 mV → 126)
* capacity unit: mAh (3-byte big-endian, max 16.7M mAh)
*/
void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah,
uint8_t remaining_pct);
/*
* crsf_send_flight_mode() — transmit CRSF flight-mode frame (type 0x21)
* for display on the pilot's handset OSD.
*
* armed: true → "ARMED\0"
* false → "DISARM\0"
*/
void crsf_send_flight_mode(bool armed);
extern volatile CRSFState crsf_state;
#endif /* CRSF_H */

151
legacy/stm32/include/encoder_odom.h
View File

@ -1,151 +0,0 @@
#ifndef ENCODER_ODOM_H
#define ENCODER_ODOM_H
#include <stdint.h>
#include <stdbool.h>
/*
* encoder_odom — quadrature encoder reading and differential-drive odometry
* (Issue #632).
*
* HARDWARE:
* Left encoder : TIM2 (32-bit) in encoder mode 3
* CH1 = PA15 (AF1), CH2 = PB3 (AF1)
* Right encoder : TIM3 (16-bit) in encoder mode 3
* CH1 = PC6 (AF2), CH2 = PC7 (AF2)
*
* Both channels count on every edge (×4 resolution).
* TIM2 ARR = 0xFFFFFFFF (32-bit, never overflows in practice).
* TIM3 ARR = 0xFFFF (16-bit, delta decoded via int16_t subtraction).
*
* ODOMETRY MODEL (differential drive):
*
* meters_per_tick = (π × wheel_diam_mm × 1e-3) / ticks_per_rev
*
* d_left = Δticks_left × meters_per_tick
* d_right = Δticks_right × meters_per_tick
*
* d_center = (d_left + d_right) / 2
* dθ = (d_right - d_left) / wheel_base_mm × 1e-3 (radians)
*
* x += d_center × cos(θ)
* y += d_center × sin(θ)
* θ += dθ
*
* For small dt this is the standard Euler-forward integration; suitable for
* the 50 Hz odometry tick rate.
*
* RPM:
* rpm = Δticks × 60.0 / (ticks_per_rev × dt_s)
*
* FLASH CONFIG (ENC_FLASH_ADDR in sector 7):
* Stores ticks_per_rev, wheel_diam_mm, wheel_base_mm validated by magic.
* Falls back to compile-time defaults on magic mismatch.
* Sector 7 is shared with PID flash; saving encoder config must be
* coordinated with pid_flash_save_all() to avoid mutual erasure.
*
* TELEMETRY:
* JLINK_TLM_ODOM (0x8C) published at ENC_TLM_HZ (50 Hz):
* jlink_tlm_odom_t { int16 rpm_left, int16 rpm_right,
* int32 x_mm, int32 y_mm,
* int16 theta_cdeg, int16 speed_mmps }
* 16 bytes, 22-byte frame.
*/
/* ---- Default hardware parameters (override in flash config) ---- */
/* Hoverboard 6.5" wheels with typical geared-motor encoder: */
#define ENC_TICKS_PER_REV_DEFAULT 1320u /* 33 CPR × 40:1 gear = 1320 ticks/rev */
#define ENC_WHEEL_DIAM_MM_DEFAULT 165u /* 6.5" ≈ 165 mm diameter */
#define ENC_WHEEL_BASE_MM_DEFAULT 540u /* ~540 mm axle-to-axle separation */
/* ---- Flash config ---- */
/* Stored in sector 7 immediately before the PID schedule area (0x0807FF40).
* 64-byte block: magic(4) + config(12) + pad(48). */
#define ENC_FLASH_ADDR 0x0807FF00UL
#define ENC_FLASH_MAGIC 0x534C4503UL /* 'SLE\x03' — encoder config v3 */
typedef struct __attribute__((packed)) {
uint32_t magic; /* ENC_FLASH_MAGIC when valid */
uint32_t ticks_per_rev; /* encoder ticks per full wheel revolution */
uint16_t wheel_diam_mm; /* wheel outer diameter (mm) */
uint16_t wheel_base_mm; /* lateral wheel separation centre-to-centre (mm) */
uint8_t _pad[48]; /* reserved — total 64 bytes */
} enc_flash_config_t;
/* ---- Runtime configuration ---- */
typedef struct {
uint32_t ticks_per_rev;
uint16_t wheel_diam_mm;
uint16_t wheel_base_mm;
} enc_config_t;
/* ---- Runtime state ---- */
typedef struct {
/* Encoder counters (last sampled) */
uint32_t cnt_left; /* last TIM2->CNT */
uint16_t cnt_right; /* last TIM3->CNT */
/* Wheel speeds */
int16_t rpm_left; /* left wheel RPM (signed; + = forward) */
int16_t rpm_right; /* right wheel RPM (signed) */
int16_t speed_mmps; /* linear speed of centre point (mm/s) */
/* Pose (relative to last reset) */
float x_mm; /* forward displacement (mm) */
float y_mm; /* lateral displacement (mm, + = left) */
float theta_rad; /* heading (radians, + = CCW from start) */
/* Internal */
float meters_per_tick; /* pre-computed from config */
float wheel_base_m; /* wheel_base_mm / 1000.0 */
uint32_t last_tick_ms; /* HAL_GetTick() at last encoder_odom_tick() */
uint32_t last_tlm_ms; /* HAL_GetTick() at last TLM transmission */
enc_config_t cfg; /* active hardware parameters */
} encoder_odom_t;
/* ---- Configuration ---- */
#define ENC_TLM_HZ 50u /* JLINK_TLM_ODOM transmit rate (Hz) */
/* ---- API ---- */
/*
* encoder_odom_init(eo) — configure TIM2/TIM3 in encoder mode, load flash
* config (falling back to defaults), reset pose.
* Call once during system init.
*/
void encoder_odom_init(encoder_odom_t *eo);
/*
* encoder_odom_tick(eo, now_ms) — sample encoder counters, update RPM and
* integrate odometry. Call from main loop at any rate ≥ 10 Hz (50 Hz ideal).
*/
void encoder_odom_tick(encoder_odom_t *eo, uint32_t now_ms);
/*
* encoder_odom_reset_pose(eo) — zero x/y/theta without resetting counters or
* config. Call whenever odometry reference frame should be re-anchored.
*/
void encoder_odom_reset_pose(encoder_odom_t *eo);
/*
* encoder_odom_save_config(cfg) — write enc_flash_config_t to ENC_FLASH_ADDR.
* WARNING: erases sector 7 — must NOT be called while armed and must be
* coordinated with PID flash saves (both records are in sector 7).
* Returns true on success.
*/
bool encoder_odom_save_config(const enc_config_t *cfg);
/*
* encoder_odom_load_config(cfg) — load config from flash.
* Returns true if flash magic valid; false = defaults applied to *cfg.
*/
bool encoder_odom_load_config(enc_config_t *cfg);
/*
* encoder_odom_send_tlm(eo, now_ms) — transmit JLINK_TLM_ODOM (0x8C) frame.
* Rate-limited to ENC_TLM_HZ; safe to call every tick.
*/
void encoder_odom_send_tlm(const encoder_odom_t *eo, uint32_t now_ms);
#endif /* ENCODER_ODOM_H */

79
legacy/stm32/include/esc_backend.h
View File

@ -1,79 +0,0 @@
#ifndef ESC_BACKEND_H
#define ESC_BACKEND_H
#include <stdint.h>
#include <stdbool.h>
/*
* ESC Backend Abstraction Layer
*
* Provides a pluggable interface for different ESC implementations:
* - Hoverboard (EFeru FOC firmware, UART @ 115200)
* - VESC (via UART @ 921600, with balance mode) — future
*
* Allows motor_driver.c to remain ESC-agnostic. Backend selection
* via ESC_BACKEND compile-time define in config.h.
*
* Issue #388: ESC abstraction layer
* Blocks Issue #383: VESC integration
*/
/* Telemetry snapshot from ESC (polled on-demand) */
typedef struct {
int16_t speed; /* Motor speed (PWM duty or RPM, backend-dependent) */
int16_t steer; /* Steering position (0 = centered) */
uint16_t voltage_mv; /* Battery voltage in millivolts */
int16_t current_ma; /* Motor current in milliamps (signed: discharge/charge) */
int16_t temperature_c; /* ESC temperature in °C */
uint16_t fault; /* Fault code (backend-specific) */
} esc_telemetry_t;
/* Virtual function table for ESC backends */
typedef struct {
/* Initialize ESC hardware and UART (called once at startup) */
void (*init)(void);
/* Send motor command to ESC (called at ~50Hz from motor_driver_update)
* speed: -1000..+1000 (forward/reverse)
* steer: -1000..+1000 (left/right)
*/
void (*send)(int16_t speed, int16_t steer);
/* Emergency stop: send zero and disable output
* (called from safety or mode manager)
*/
void (*estop)(void);
/* Query current ESC state
* Returns latest telemetry snapshot (may be cached/stale on some backends).
* Safe to call from any context (non-blocking).
*/
void (*get_telemetry)(esc_telemetry_t *out);
/* Optional: resume from estop (not all backends use this) */
void (*resume)(void);
} esc_backend_t;
/*
* Register a backend implementation at runtime.
* Typically called during init sequence before motor_driver_init().
*/
void esc_backend_register(const esc_backend_t *backend);
/*
* Get the currently active backend.
* Returns pointer to vtable; nullptr if no backend registered.
*/
const esc_backend_t *esc_backend_get(void);
/*
* High-level convenience wrappers (match motor_driver.c interface).
* These call through the active backend if registered.
*/
void esc_init(void);
void esc_send(int16_t speed, int16_t steer);
void esc_estop(void);
void esc_resume(void);
void esc_get_telemetry(esc_telemetry_t *out);
#endif /* ESC_BACKEND_H */

111
legacy/stm32/include/face_animation.h
View File

@ -1,111 +0,0 @@
/*
* face_animation.h — Face Emotion Renderer for LCD Display
*
* Renders expressive face animations for 5 core emotions:
* - HAPPY: upturned eyes, curved smile
* - SAD: downturned eyes, frown
* - CURIOUS: raised eyebrows, wide eyes, slight tilt
* - ANGRY: downturned brows, narrowed eyes, clenched mouth
* - SLEEPING: closed eyes, relaxed mouth, gentle sway (optional)
*
* HOW IT WORKS:
* - State machine with smooth transitions (easing over N frames)
* - Idle behavior: periodic blinking (duration configurable)
* - Each emotion has parameterized eye/mouth shapes (position, angle, curvature)
* - Transitions interpolate between emotion parameter sets
* - render() draws current state to LCD framebuffer via face_lcd_*() API
* - tick() advances frame counter, handles transitions, triggers blink
*
* ANIMATION SPECS:
* - Frame rate: 30 Hz (via systick)
* - Transition time: 0.5–1.0s (15–30 frames)
* - Blink duration: 100–150 ms (3–5 frames)
* - Blink interval: 4–6 seconds (120–180 frames at 30Hz)
*
* API:
* - face_animation_init() — Initialize state machine
* - face_animation_set_emotion(emotion) — Request state change (with smooth transition)
* - face_animation_tick() — Advance animation by 1 frame (call at 30Hz from systick)
* - face_animation_render() — Draw current face to LCD framebuffer
*/
#ifndef FACE_ANIMATION_H
#define FACE_ANIMATION_H
#include <stdint.h>
#include <stdbool.h>
/* === Emotion Types === */
typedef enum {
FACE_HAPPY = 0,
FACE_SAD = 1,
FACE_CURIOUS = 2,
FACE_ANGRY = 3,
FACE_SLEEPING = 4,
FACE_NEUTRAL = 5, /* Default state */
} face_emotion_t;
/* === Animation Parameters (per emotion) === */
typedef struct {
int16_t eye_x; /* Eye horizontal offset from center (pixels) */
int16_t eye_y; /* Eye vertical offset from center (pixels) */
int16_t eye_open_y; /* Eye open height (pixels) */
int16_t eye_close_y; /* Eye close height (pixels, 0=fully closed) */
int16_t brow_angle; /* Eyebrow angle (-30..+30 degrees, tilt) */
int16_t brow_y_offset; /* Eyebrow vertical offset (pixels) */
int16_t mouth_x; /* Mouth horizontal offset (pixels) */
int16_t mouth_y; /* Mouth vertical offset (pixels) */
int16_t mouth_width; /* Mouth width (pixels) */
int16_t mouth_curve; /* Curvature: >0=smile, <0=frown, 0=neutral */
uint8_t blink_interval_ms; /* Idle blink interval (seconds, in 30Hz ticks) */
} face_params_t;
/* === Public API === */
/**
* Initialize face animation system.
* Sets initial emotion to NEUTRAL, clears blink timer.
*/
void face_animation_init(void);
/**
* Request a state change to a new emotion.
* Triggers smooth transition (easing) over TRANSITION_FRAMES.
*/
void face_animation_set_emotion(face_emotion_t emotion);
/**
* Advance animation by one frame.
* Called by systick ISR at 30 Hz.
* Handles:
* - Transition interpolation
* - Blink timing and rendering
* - Idle animations (sway, subtle movements)
*/
void face_animation_tick(void);
/**
* Render current face state to LCD framebuffer.
* Draws eyes, brows, mouth, and optional idle animations.
* Should be called after face_animation_tick().
*/
void face_animation_render(void);
/**
* Get current emotion (transition-aware).
* Returns the target emotion, or current if transition in progress.
*/
face_emotion_t face_animation_get_emotion(void);
/**
* Trigger a blink immediately (for special events).
* Overrides idle blink timer.
*/
void face_animation_blink_now(void);
/**
* Check if animation is idle (no active transition).
*/
bool face_animation_is_idle(void);
#endif // FACE_ANIMATION_H

116
legacy/stm32/include/face_lcd.h
View File

@ -1,116 +0,0 @@
/*
* face_lcd.h — STM32 LCD Display Driver for Face Animations
*
* Low-level abstraction for driving a small LCD/OLED display via SPI or I2C.
* Supports pixel/line drawing primitives and full framebuffer operations.
*
* HOW IT WORKS:
* - Initializes display (SPI/I2C, resolution, rotation)
* - Provides framebuffer (in RAM or on-device)
* - Exposes primitives: draw_pixel, draw_line, draw_circle, fill_rect
* - Implements vsync-driven 30Hz refresh from systick
* - Non-blocking DMA transfers for rapid display updates
*
* HARDWARE ASSUMPTIONS:
* - SPI2 or I2C (configurable via #define LCD_INTERFACE)
* - Typical sizes: 128×64, 240×135, 320×240
* - Pixel depth: 1-bit (monochrome) or 16-bit (RGB565)
* - Controller: SSD1306, ILI9341, ST7789, etc.
*
* API:
* - face_lcd_init(width, height, bpp) — Initialize display
* - face_lcd_clear() — Clear framebuffer
* - face_lcd_pixel(x, y, color) — Set pixel
* - face_lcd_line(x0, y0, x1, y1, color) — Draw line (Bresenham)
* - face_lcd_circle(cx, cy, r, color) — Draw circle
* - face_lcd_fill_rect(x, y, w, h, color) — Filled rectangle
* - face_lcd_flush() — Push framebuffer to display (async via DMA)
* - face_lcd_is_busy() — Check if transfer in progress
* - face_lcd_tick() — Called by systick ISR for 30Hz vsync
*/
#ifndef FACE_LCD_H
#define FACE_LCD_H
#include <stdint.h>
#include <stdbool.h>
/* === Configuration === */
#define LCD_INTERFACE SPI /* SPI or I2C */
#define LCD_WIDTH 128 /* pixels */
#define LCD_HEIGHT 64 /* pixels */
#define LCD_BPP 1 /* bits per pixel (1=mono, 16=RGB565) */
#define LCD_REFRESH_HZ 30 /* target refresh rate */
#if LCD_BPP == 1
typedef uint8_t lcd_color_t;
#define LCD_BLACK 0x00
#define LCD_WHITE 0x01
#define LCD_FBSIZE (LCD_WIDTH * LCD_HEIGHT / 8) /* 1024 bytes */
#else /* RGB565 */
typedef uint16_t lcd_color_t;
#define LCD_BLACK 0x0000
#define LCD_WHITE 0xFFFF
#define LCD_FBSIZE (LCD_WIDTH * LCD_HEIGHT * 2) /* 16384 bytes */
#endif
/* === Public API === */
/**
* Initialize LCD display and framebuffer.
* Called once at startup.
*/
void face_lcd_init(void);
/**
* Clear entire framebuffer to black.
*/
void face_lcd_clear(void);
/**
* Set a single pixel in the framebuffer.
* (Does NOT push to display immediately.)
*/
void face_lcd_pixel(uint16_t x, uint16_t y, lcd_color_t color);
/**
* Draw a line from (x0,y0) to (x1,y1) using Bresenham algorithm.
*/
void face_lcd_line(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1,
lcd_color_t color);
/**
* Draw a circle with center (cx, cy) and radius r.
*/
void face_lcd_circle(uint16_t cx, uint16_t cy, uint16_t r, lcd_color_t color);
/**
* Fill a rectangle at (x, y) with width w and height h.
*/
void face_lcd_fill_rect(uint16_t x, uint16_t y, uint16_t w, uint16_t h,
lcd_color_t color);
/**
* Push framebuffer to display (async via DMA if available).
* Returns immediately; transfer happens in background.
*/
void face_lcd_flush(void);
/**
* Check if a display transfer is currently in progress.
* Returns true if DMA/SPI is busy, false if idle.
*/
bool face_lcd_is_busy(void);
/**
* Called by systick ISR (~30Hz) to drive vsync and maintain refresh.
* Updates frame counter and triggers flush if a new frame is needed.
*/
void face_lcd_tick(void);
/**
* Get framebuffer address (for direct access if needed).
*/
uint8_t *face_lcd_get_fb(void);
#endif // FACE_LCD_H

76
legacy/stm32/include/face_uart.h
View File

@ -1,76 +0,0 @@
/*
* face_uart.h — UART Command Interface for Face Animations
*
* Receives emotion commands from Jetson Orin via UART (USART3 by default).
* Parses simple text commands and updates face animation state.
*
* PROTOCOL:
* Text-based commands (newline-terminated):
* HAPPY — Set emotion to happy
* SAD — Set emotion to sad
* CURIOUS — Set emotion to curious
* ANGRY — Set emotion to angry
* SLEEP — Set emotion to sleeping
* NEUTRAL — Set emotion to neutral
* BLINK — Trigger immediate blink
* STATUS — Echo current emotion + animation state
*
* Example:
* > HAPPY\n
* < OK: HAPPY\n
*
* INTERFACE:
* - UART3 (PB10=TX, PB11=RX) at 115200 baud
* - RX ISR pushes bytes into ring buffer
* - face_uart_process() checks for complete commands (polling)
* - Case-insensitive command parsing
* - Echoes command results to TX for debugging
*
* API:
* - face_uart_init() — Configure UART3 @ 115200
* - face_uart_process() — Parse and execute commands (call from main loop)
* - face_uart_rx_isr() — Called by UART3 RX interrupt
* - face_uart_send() — Send response string (used internally)
*/
#ifndef FACE_UART_H
#define FACE_UART_H
#include <stdint.h>
#include <stdbool.h>
/* === Configuration === */
#define FACE_UART_INSTANCE USART3 /* USART3 (PB10=TX, PB11=RX) */
#define FACE_UART_BAUD 115200 /* 115200 baud */
#define FACE_UART_RX_BUF_SZ 128 /* RX ring buffer size */
/* === Public API === */
/**
* Initialize UART for face commands.
* Configures USART3 @ 115200, enables RX interrupt.
*/
void face_uart_init(void);
/**
* Process any pending RX data and execute commands.
* Should be called periodically from main loop (or low-priority task).
* Returns immediately if no complete command available.
*/
void face_uart_process(void);
/**
* UART3 RX interrupt handler.
* Called by HAL when a byte is received.
* Pushes byte into ring buffer.
*/
void face_uart_rx_isr(uint8_t byte);
/**
* Send a response string to UART3 TX.
* Used for echoing status/ack messages.
* Non-blocking (pushes to TX queue).
*/
void face_uart_send(const char *str);
#endif // FACE_UART_H

162
legacy/stm32/include/fan.h
View File

@ -1,162 +0,0 @@
#ifndef FAN_H
#define FAN_H
#include <stdint.h>
#include <stdbool.h>
/*
* fan.h — Cooling fan PWM speed controller (Issue #263)
*
* STM32F722 driver for brushless cooling fan on PA9 using TIM1_CH2 PWM.
* Temperature-based speed curve with smooth ramp transitions.
*
* Pin: PA9 (TIM1_CH2, alternate function AF1)
* PWM Frequency: 25 kHz (suitable for brushless DC fan)
* Speed Range: 0-100% duty cycle
*
* Temperature Curve:
* - Below 40°C: Fan off (0%)
* - 40-50°C: Linear ramp from 0% to 30%
* - 50-70°C: Linear ramp from 30% to 100%
* - Above 70°C: Fan at maximum (100%)
*/
/* Fan speed state */
typedef enum {
FAN_OFF, /* Motor disabled (0% duty) */
FAN_LOW, /* Low speed (5-30%) */
FAN_MEDIUM, /* Medium speed (31-60%) */
FAN_HIGH, /* High speed (61-99%) */
FAN_FULL /* Maximum speed (100%) */
} FanState;
/*
* fan_init()
*
* Initialize fan controller:
* - PA9 as TIM1_CH2 PWM output
* - TIM1 configured for 25 kHz frequency
* - PWM duty cycle control (0-100%)
* - Ramp rate limiter for smooth transitions
*/
void fan_init(void);
/*
* fan_set_speed(percentage)
*
* Set fan speed directly (bypasses temperature control).
* Used for manual testing or emergency cooling.
*
* Arguments:
* - percentage: 0-100% duty cycle
*
* Returns: true if set successfully, false if invalid value
*/
bool fan_set_speed(uint8_t percentage);
/*
* fan_get_speed()
*
* Get current fan speed setting.
*
* Returns: Current speed 0-100%
*/
uint8_t fan_get_speed(void);
/*
* fan_set_target_speed(percentage)
*
* Set target speed with smooth ramping.
* Speed transitions over time according to ramp rate.
*
* Arguments:
* - percentage: Target speed 0-100%
*
* Returns: true if set successfully
*/
bool fan_set_target_speed(uint8_t percentage);
/*
* fan_update_temperature(temp_celsius)
*
* Update temperature reading and apply speed curve.
* Calculates target speed based on temperature curve.
* Speed transition is smoothed via ramp limiter.
*
* Temperature Curve:
* - temp < 40°C: 0% (off)
* - 40°C ≤ temp < 50°C: 0% + (temp - 40) * 3% per °C = linear to 30%
* - 50°C ≤ temp < 70°C: 30% + (temp - 50) * 3.5% per °C = linear to 100%
* - temp ≥ 70°C: 100% (full)
*
* Arguments:
* - temp_celsius: Temperature in degrees Celsius (int16_t for negative values)
*/
void fan_update_temperature(int16_t temp_celsius);
/*
* fan_get_temperature()
*
* Get last recorded temperature.
*
* Returns: Temperature in °C (or 0 if not yet set)
*/
int16_t fan_get_temperature(void);
/*
* fan_get_state()
*
* Get current fan operational state.
*
* Returns: FAN_OFF, FAN_LOW, FAN_MEDIUM, FAN_HIGH, or FAN_FULL
*/
FanState fan_get_state(void);
/*
* fan_set_ramp_rate(percentage_per_ms)
*
* Configure speed ramp rate for smooth transitions.
* Default: 5% per 100ms = 0.05% per ms.
* Higher values = faster transitions.
*
* Arguments:
* - percentage_per_ms: Speed change per millisecond (e.g., 1 = 1% per ms)
*
* Typical ranges:
* - 0.01 = very slow (100% change in 10 seconds)
* - 0.05 = slow (100% change in 2 seconds)
* - 0.1 = medium (100% change in 1 second)
* - 1.0 = fast (100% change in 100ms)
*/
void fan_set_ramp_rate(float percentage_per_ms);
/*
* fan_is_ramping()
*
* Check if speed is currently transitioning.
*
* Returns: true if speed is ramping toward target, false if at target
*/
bool fan_is_ramping(void);
/*
* fan_tick(now_ms)
*
* Update function called periodically (recommended: every 10-100ms).
* Processes speed ramp transitions.
* Must be called regularly for smooth ramping operation.
*
* Arguments:
* - now_ms: current time in milliseconds (from HAL_GetTick() or similar)
*/
void fan_tick(uint32_t now_ms);
/*
* fan_disable()
*
* Disable fan immediately (set to 0% duty).
* Useful for shutdown or emergency stop.
*/
void fan_disable(void);
#endif /* FAN_H */

140
legacy/stm32/include/fault_handler.h
View File

@ -1,140 +0,0 @@
#ifndef FAULT_HANDLER_H
#define FAULT_HANDLER_H
#include <stdint.h>
#include <stdbool.h>
/*
* fault_handler.h — STM32F7 fault detection and recovery (Issue #565)
*
* Features:
* - HardFault / BusFault / UsageFault / MemManage vector hooks with full
* Cortex-M7 register dump (R0-R3, LR, PC, xPSR, CFSR, HFSR, MMFAR, BFAR)
* - .noinit SRAM ring: fault frame captured and magic-tagged, survives
* NVIC_SystemReset(); persisted to flash on the subsequent boot
* - MPU Region 0 stack-guard (32 bytes at __stack_end, no-access) → MemManage
* fault detected as FAULT_STACK_OVF
* - Brownout detect via RCC_CSR_BORRSTF on boot → FAULT_BROWNOUT
* - Persistent fault log: last 8 entries × 64 bytes in flash sector 7
* at 0x08060000 (below the PID store at 0x0807FFC0)
* - JLINK_TLM_FAULT_LOG (0x85): 20-byte summary sent via JLink on boot
* and on JLINK_CMD_FAULT_LOG_GET (0x0C) request
* - LED blink codes on LED2 (PC14, active-low) for 10 s after recovery:
* HARDFAULT = 3 fast blinks (100 ms)
* WATCHDOG = 2 slow blinks (300 ms)
* BROWNOUT = 1 long blink (500 ms)
* STACK_OVF = 4 fast blinks (100 ms)
* BUS_FAULT = alternating 3+1
* USAGE_FAULT = 2 fast blinks
* - Auto-recovery: fault → .noinit capture → NVIC_SystemReset()
* On next boot fault_handler_init() re-runs safely: persists, prints, blinks
*
* Flash layout within sector 7 (0x08060000, 128 KB):
* Slot 0-7: 0x08060000 – 0x080601FF (8 × 64 bytes = 512 bytes fault log)
* PID store: 0x0807FFC0 – 0x0807FFFF (64 bytes, managed by pid_flash.c)
*/
/* ---- Fault types ---- */
typedef enum {
FAULT_NONE = 0x00,
FAULT_HARDFAULT = 0x01, /* HardFault escalation */
FAULT_WATCHDOG = 0x02, /* IWDG timeout reset */
FAULT_BROWNOUT = 0x03, /* Brown-out reset (BOR) */
FAULT_STACK_OVF = 0x04, /* MPU stack guard MemManage */
FAULT_BUS_FAULT = 0x05, /* BusFault */
FAULT_USAGE_FAULT = 0x06, /* UsageFault */
FAULT_MEM_FAULT = 0x07, /* MemManageFault (non-stack-guard) */
FAULT_ASSERT = 0x08, /* Software assertion */
} FaultType;
/* ---- Flash fault log constants ---- */
#define FAULT_LOG_MAX_ENTRIES 8u
#define FAULT_LOG_MAGIC 0xFADE5A01u
#define FAULT_LOG_BASE_ADDR 0x08060000UL /* start of flash sector 7 */
#define FAULT_LOG_ENTRY_SIZE 64u /* bytes per entry */
/* ---- Flash fault log entry (64 bytes, packed) ---- */
typedef struct __attribute__((packed)) {
uint32_t magic; /* FAULT_LOG_MAGIC when valid */
uint8_t fault_type; /* FaultType */
uint8_t reset_count; /* lifetime reset counter */
uint16_t _pad0;
uint32_t timestamp_ms; /* HAL_GetTick() at reset (0 if pre-tick) */
uint32_t pc; /* faulting instruction address */
uint32_t lr; /* link register at fault */
uint32_t r0;
uint32_t r1;
uint32_t r2;
uint32_t r3;
uint32_t cfsr; /* SCB->CFSR: combined fault status register */
uint32_t hfsr; /* SCB->HFSR: hard fault status register */
uint32_t mmfar; /* SCB->MMFAR: memory manage fault address */
uint32_t bfar; /* SCB->BFAR: bus fault address */
uint32_t sp; /* stack pointer value at fault */
uint8_t _pad1[4]; /* pad to 64 bytes */
} fault_log_entry_t; /* 64 bytes */
/*
* fault_handler_init() — call early in main(), before safety_init().
* 1. Increments reset counter (.noinit SRAM).
* 2. Checks .noinit SRAM for a pending fault capture; if found: persists to
* flash, prints CDC register dump, starts LED blink code.
* 3. Detects brownout via RCC_CSR_BORRSTF; logs if detected.
* 4. Clears RCC reset flags.
* 5. Installs MPU Region 0 stack guard.
* 6. Enables MemManage, BusFault, UsageFault (SCB->SHCSR).
*/
void fault_handler_init(void);
/*
* fault_mpu_guard_init() — configure MPU Region 0 as a 32-byte no-access
* guard at __stack_end (bottom of main stack). Generates MemManage on
* stack overflow. Called automatically by fault_handler_init().
*/
void fault_mpu_guard_init(void);
/*
* fault_get_last_type() — most recent fault type from flash log, or FAULT_NONE.
*/
FaultType fault_get_last_type(void);
/*
* fault_log_read(idx, out) — read flash slot 0..7.
* Returns false if slot empty or idx out of range.
*/
bool fault_log_read(uint8_t idx, fault_log_entry_t *out);
/*
* fault_log_get_count() — number of valid (occupied) log slots, 0-8.
*/
uint8_t fault_log_get_count(void);
/*
* fault_log_clear() — erase fault log, restore PID if previously saved.
* Erases all of sector 7 (~1 s stall). Do not call while armed.
*/
void fault_log_clear(void);
/*
* fault_assert(file, line) — software fault at runtime; captures return
* address, writes SRAM magic, triggers NVIC_SystemReset().
* Use via FAULT_ASSERT(cond) macro below.
*/
void fault_assert_impl(const char *file, int line);
#define FAULT_ASSERT(cond) \
do { if (!(cond)) fault_assert_impl(__FILE__, __LINE__); } while (0)
/*
* fault_led_tick(now_ms) — drive LED2 blink code from main loop (1 ms).
* Self-disables after 10 s so it doesn't interfere with normal LED state.
*/
void fault_led_tick(uint32_t now_ms);
/* C-level fault dispatch (called from naked asm stubs; not for direct use) */
void fault_hard_c(uint32_t *frame);
void fault_mem_c(uint32_t *frame);
void fault_bus_c(uint32_t *frame);
void fault_usage_c(uint32_t *frame);
#endif /* FAULT_HANDLER_H */

72
legacy/stm32/include/gimbal.h
View File

@ -1,72 +0,0 @@
#ifndef GIMBAL_H
#define GIMBAL_H
#include <stdint.h>
#include <stdbool.h>
/*
* gimbal.h — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
*
* Manages dual ST3215 serial bus servos:
* Pan servo: ID GIMBAL_PAN_ID (config.h, default 1)
* Tilt servo: ID GIMBAL_TILT_ID (config.h, default 2)
*
* Position units: degrees x10 (int16), matching JLink protocol convention.
* e.g. 900 = 90.0°, -450 = -45.0°
*
* Limits:
* Pan: -1800..+1800 (x10 deg) = -180..+180 deg
* Tilt: -900..+900 (x10 deg) = -90..+90 deg
*
* The gimbal_tick() function polls servo feedback at GIMBAL_TLM_HZ (50 Hz).
* Alternates reading pan position on even ticks, tilt on odd ticks — each
* servo polled at 25 Hz to keep bus utilization low.
*/
typedef struct {
/* Command state */
int16_t cmd_pan_x10; /* Commanded pan (deg x10) */
int16_t cmd_tilt_x10; /* Commanded tilt (deg x10) */
uint16_t cmd_speed; /* Servo bus speed (0=max, 1-4095) */
bool torque_enabled; /* True when torques are enabled */
/* Feedback state (updated at ~25 Hz per axis) */
int16_t fb_pan_x10; /* Measured pan (deg x10) */
int16_t fb_tilt_x10; /* Measured tilt (deg x10) */
uint16_t fb_pan_speed; /* Raw speed register, pan servo */
uint16_t fb_tilt_speed; /* Raw speed register, tilt servo */
/* Diagnostics */
uint32_t rx_ok; /* Successful position reads */
uint32_t rx_err; /* Failed position reads */
uint32_t _last_tick_ms; /* Internal: last tick timestamp */
uint8_t _poll_phase; /* Internal: alternates 0=pan 1=tilt */
} gimbal_t;
/*
* gimbal_init(g) — enable torque on both servos, center them.
* servo_bus_init() must be called first.
*/
void gimbal_init(gimbal_t *g);
/*
* gimbal_set_pos(g, pan_x10, tilt_x10, speed) — command a new pan/tilt
* position. pan_x10 and tilt_x10 are degrees×10, clamped to servo limits.
* speed: 0=max servo speed, 1-4095 = scaled.
*/
void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
uint16_t speed);
/*
* gimbal_torque(g, enable) — enable or disable torque on both servos.
*/
void gimbal_torque(gimbal_t *g, bool enable);
/*
* gimbal_tick(g, now_ms) — poll servo feedback at GIMBAL_TLM_HZ.
* Call every 1 ms from the main loop; function self-throttles.
*/
void gimbal_tick(gimbal_t *g, uint32_t now_ms);
#endif /* GIMBAL_H */

29
legacy/stm32/include/hoverboard.h
View File

@ -1,29 +0,0 @@
#ifndef HOVERBOARD_H
#define HOVERBOARD_H
#include <stdint.h>
/*
* Hoverboard ESC UART protocol (EFeru FOC firmware)
* USART2: PA2=TX, PA3=RX @ 115200 baud
*
* Packet: [0xABCD] [steer:i16] [speed:i16] [checksum:u16]
* Checksum = start ^ steer ^ speed
* Speed range: -1000 to +1000
* Must send at >=50Hz or ESC times out (TIMEOUT=20 * DELAY_IN_MAIN_LOOP=5ms = 100ms)
*/
#define HOVERBOARD_START_FRAME 0xABCD
#define HOVERBOARD_BAUD 38400
typedef struct __attribute__((packed)) {
uint16_t start;
int16_t steer;
int16_t speed;
uint16_t checksum;
} hoverboard_cmd_t;
void hoverboard_init(void);
void hoverboard_send(int16_t speed, int16_t steer);
#endif

61
legacy/stm32/include/hw_button.h
View File

@ -1,61 +0,0 @@
#ifndef HW_BUTTON_H
#define HW_BUTTON_H
#include <stdint.h>
#include <stdbool.h>
/*
* hw_button — hardware button debounce + gesture detection (Issue #682).
*
* Debounce FSM:
* IDLE → (raw press detected) → DEBOUNCING
* DEBOUNCING → (still pressed after BTN_DEBOUNCE_MS) → HELD
* HELD → (released) → classify press type, back to IDLE
*
* Press types:
* SHORT held < BTN_LONG_MIN_MS from confirmed start
* LONG held >= BTN_LONG_MIN_MS
*
* Sequence detection:
* [SHORT, SHORT, LONG] -> BTN_EVENT_REARM_COMBO (fires on LONG release)
* [SHORT] + BTN_COMMIT_MS quiet timeout -> BTN_EVENT_PARK
*
* Config constants (can be overridden in config.h):
* BTN_DEBOUNCE_MS 20 ms debounce window
* BTN_LONG_MIN_MS 1500 ms threshold for LONG press
* BTN_COMMIT_MS 500 ms quiet after lone SHORT -> PARK
* BTN_SEQ_TIMEOUT_MS 3000 ms sequence window; expired sequence is abandoned
* BTN_PORT GPIOC
* BTN_PIN GPIO_PIN_2
*/
typedef enum {
BTN_EVENT_NONE = 0,
BTN_EVENT_PARK = 1, /* single short press + quiet */
BTN_EVENT_REARM_COMBO = 2, /* SHORT + SHORT + LONG */
} hw_btn_event_t;
/*
* hw_button_init() — configure GPIO (active-low pull-up), zero FSM state.
* Call once at startup.
*/
void hw_button_init(void);
/*
* hw_button_tick(now_ms) — advance debounce FSM and sequence detector.
* Call every ms from the main loop. Returns BTN_EVENT_NONE unless a
* complete gesture was recognised this tick.
*/
hw_btn_event_t hw_button_tick(uint32_t now_ms);
/*
* hw_button_is_pressed() — true while button is confirmed held (post-debounce).
*/
bool hw_button_is_pressed(void);
#ifdef TEST_HOST
/* Inject a simulated raw pin state for host-side unit tests. */
void hw_button_inject(bool pressed);
#endif
#endif /* HW_BUTTON_H */

17
legacy/stm32/include/i2c1.h
View File

@ -1,17 +0,0 @@
#ifndef I2C1_H
#define I2C1_H
#include "stm32f7xx_hal.h"
/*
* Shared I2C1 bus handle — used by baro (BMP280/DPS310) and mag
* (QMC5883L/HMC5883L/IST8310) drivers.
*
* Call i2c1_init() once in main() before any I2C probes.
* PB8 = SCL, PB9 = SDA (AF4_I2C1, open-drain, 100 kHz).
*/
extern I2C_HandleTypeDef hi2c1;
int i2c1_init(void);
#endif /* I2C1_H */

10
legacy/stm32/include/icm42688.h
View File

@ -1,10 +0,0 @@
#ifndef ICM42688_H
#define ICM42688_H
#include <stdint.h>
typedef struct {
int16_t ax, ay, az, gx, gy, gz, temp_x10;
} icm42688_data_t;
int icm42688_init(void);
void icm42688_read(icm42688_data_t *d);
void icm42688_get_trace(uint8_t *out, int *len);
#endif

51
legacy/stm32/include/imu_cal_flash.h
View File

@ -1,51 +0,0 @@
#ifndef IMU_CAL_FLASH_H
#define IMU_CAL_FLASH_H
#include <stdint.h>
#include <stdbool.h>
/*
* IMU mount angle calibration flash storage (Issue #680).
*
* Sector 7 (128 KB at 0x08060000) layout:
* 0x0807FF00 imu_cal_flash_t (64 bytes) ← this module
* 0x0807FF40 pid_sched_flash_t (128 bytes) ← pid_flash.c
* 0x0807FFC0 pid_flash_t (64 bytes) ← pid_flash.c
*
* Calibration flow:
* 1. Mount robot at its installed angle, power on, let IMU converge (~5s).
* 2. Send 'O' via USB CDC (dev-only path).
* 3. Firmware captures current pitch + roll as mount offsets, saves to flash.
* 4. mpu6000_read() subtracts offsets from output on every subsequent read.
*
* The sector erase preserves existing PID data by reading it first.
*/
#define IMU_CAL_FLASH_ADDR 0x0807FF00UL
#define IMU_CAL_FLASH_MAGIC 0x534C5403UL /* 'SLT\x03' — version 3 */
typedef struct __attribute__((packed)) {
uint32_t magic; /* IMU_CAL_FLASH_MAGIC when valid */
float pitch_offset; /* degrees subtracted from IMU pitch output */
float roll_offset; /* degrees subtracted from IMU roll output */
uint8_t _pad[52]; /* padding to 64 bytes */
} imu_cal_flash_t; /* 64 bytes total */
/*
* imu_cal_flash_load() — read saved mount offsets from flash.
* Returns true and fills *pitch_offset / *roll_offset if magic is valid.
* Returns false if no valid calibration stored (caller keeps 0.0f defaults).
*/
bool imu_cal_flash_load(float *pitch_offset, float *roll_offset);
/*
* imu_cal_flash_save() — erase sector 7 and write all three records atomically:
* imu_cal_flash_t at 0x0807FF00
* pid_sched_flash_t at 0x0807FF40 (preserved from existing flash)
* pid_flash_t at 0x0807FFC0 (preserved from existing flash)
* Must be called while disarmed — sector erase stalls CPU ~1s.
* Returns true on success.
*/
bool imu_cal_flash_save(float pitch_offset, float roll_offset);
#endif /* IMU_CAL_FLASH_H */

117
legacy/stm32/include/ina219.h
View File

@ -1,117 +0,0 @@
#ifndef INA219_H
#define INA219_H
#include <stdint.h>
#include <stdbool.h>
/*
* ina219.h — INA219 power monitor driver (Issue #214)
*
* I2C1 driver for motor current/voltage/power monitoring.
* Supports 2 sensors (left/right motor) on I2C1 (PB8=SCL, PB9=SDA).
*
* INA219 specs:
* - I2C addresses: 0x40–0x4F (configurable via address pins)
* - Bus voltage: 0–26V, 4mV/LSB
* - Shunt voltage: ±327mV, 10µV/LSB
* - Current: derived from shunt voltage (calibration-dependent)
* - Power: (Bus V × Current) / internal gain
*
* Typical usage for motor monitoring:
* - 0.1Ω shunt resistor → ~3.27A max (at ±327mV)
* - Calibration: set max expected current, driver calculates LSB
* - Read functions return actual voltage/current/power values
*/
/* INA219 sensors (2 motors) */
typedef enum {
INA219_LEFT_MOTOR = 0, /* Address 0x40 */
INA219_RIGHT_MOTOR = 1, /* Address 0x41 */
INA219_COUNT
} INA219Sensor;
/* INA219 measurement data */
typedef struct {
uint16_t bus_voltage_mv; /* Bus voltage in mV (0–26000) */
int16_t shunt_voltage_uv; /* Shunt voltage in µV (±327000) */
int16_t current_ma; /* Current in mA (signed) */
uint32_t power_mw; /* Power in mW */
} INA219Data;
/*
* ina219_init()
*
* Initialize I2C1 and both INA219 sensors (left + right motor).
* Performs auto-calibration for typical motor current monitoring.
* Call once at startup after i2c1_init().
*/
void ina219_init(void);
/*
* ina219_calibrate(sensor, max_current_ma, shunt_ohms_milli)
*
* Manually calibrate a sensor for expected max current and shunt resistance.
* Calculates internal calibration register value.
*
* Example:
* ina219_calibrate(INA219_LEFT_MOTOR, 5000, 100); // 5A max, 0.1Ω shunt
*/
void ina219_calibrate(INA219Sensor sensor, uint16_t max_current_ma, uint16_t shunt_ohms_milli);
/*
* ina219_read(sensor, data)
*
* Read all measurements from a sensor (voltage, current, power).
* Blocks until measurements are ready (typically <1ms at default ADC resolution).
*
* Returns: true if read successful, false on I2C error.
*/
bool ina219_read(INA219Sensor sensor, INA219Data *data);
/*
* ina219_read_bus_voltage_mv(sensor, voltage_mv)
*
* Read bus voltage only (faster than full read).
* Returns: true if successful.
*/
bool ina219_read_bus_voltage_mv(INA219Sensor sensor, uint16_t *voltage_mv);
/*
* ina219_read_current_ma(sensor, current_ma)
*
* Read current only (requires prior calibration).
* Returns: true if successful.
*/
bool ina219_read_current_ma(INA219Sensor sensor, int16_t *current_ma);
/*
* ina219_read_power_mw(sensor, power_mw)
*
* Read power consumption only.
* Returns: true if successful.
*/
bool ina219_read_power_mw(INA219Sensor sensor, uint32_t *power_mw);
/*
* ina219_alert_enable(sensor, current_limit_ma)
*
* Enable alert pin when current exceeds limit (overcurrent protection).
* Alert pin: GPIO, active high, open-drain output.
*/
void ina219_alert_enable(INA219Sensor sensor, uint16_t current_limit_ma);
/*
* ina219_alert_disable(sensor)
*
* Disable alert for a sensor.
*/
void ina219_alert_disable(INA219Sensor sensor);
/*
* ina219_reset(sensor)
*
* Perform soft reset on a sensor (clears all registers to default).
*/
void ina219_reset(INA219Sensor sensor);
#endif /* INA219_H */

76
legacy/stm32/include/jetson_cmd.h
View File

@ -1,76 +0,0 @@
#ifndef JETSON_CMD_H
#define JETSON_CMD_H
#include <stdint.h>
#include <stdbool.h>
/*
* Jetson→STM32 command protocol over USB CDC (bidirectional, same /dev/ttyACM0)
*
* Commands (newline-terminated ASCII, sent by Jetson):
* H\n — heartbeat (every 200ms). Must arrive within 500ms or
* jetson_cmd_is_active() returns false → steer reverts to 0.
* C<spd>,<str>\n — drive command: speed -1000..+1000, steer -1000..+1000.
* Also refreshes the heartbeat timer.
*
* Speed→setpoint:
* Speed is converted to a setpoint offset (degrees) before calling balance_update().
* Positive speed → forward tilt → robot moves forward.
* Max offset is ±JETSON_SPEED_MAX_DEG (see below).
*
* Steer:
* Passed directly to motor_driver_update() as steer_cmd.
* Motor driver ramps and clamps with balance headroom (see motor_driver.h).
*
* Integration pattern in main.c (after the cdc_cmd_ready block):
*
* // Process buffered C command (parsed here, not in ISR)
* if (jetson_cmd_ready) { jetson_cmd_ready = 0; jetson_cmd_process(); }
*
* // Apply setpoint offset and steer when active
* float base_sp = bal.setpoint;
* if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset();
* balance_update(&bal, &imu, dt);
* bal.setpoint = base_sp;
*
* // Steer injection in 50Hz ESC block
* int16_t jsteer = jetson_cmd_is_active(now) ? jetson_cmd_steer() : 0;
* motor_driver_update(&motors, bal.motor_cmd, jsteer, now);
*/
/* Heartbeat timeout: if no H or C within this window, commands deactivate */
#define JETSON_HB_TIMEOUT_MS 500
/* Max setpoint offset from Jetson speed command (speed=1000 → +N degrees tilt) */
#define JETSON_SPEED_MAX_DEG 4.0f /* ±4° → enough for ~0.5 m/s */
/*
* jetson_cmd_process()
* Call from main loop (NOT ISR) when jetson_cmd_ready is set.
* Parses jetson_cmd_buf (the C<spd>,<str> frame) with sscanf.
*/
void jetson_cmd_process(void);
/*
* jetson_cmd_is_active(now)
* Returns true if a heartbeat (H or C command) arrived within JETSON_HB_TIMEOUT_MS.
* If false, main loop should fall back to RC or zero steer.
*/
bool jetson_cmd_is_active(uint32_t now_ms);
/* Current steer command after latest C frame, clamped to ±1000 */
int16_t jetson_cmd_steer(void);
/* Setpoint offset (degrees) derived from latest speed command. */
float jetson_cmd_sp_offset(void);
/*
* Externals — declared here, defined in usbd_cdc_if.c alongside the other
* CDC volatile flags (cdc_streaming, cdc_arm_request, etc.).
* Main loop checks jetson_cmd_ready; ISR sets it.
*/
extern volatile uint8_t jetson_cmd_ready; /* set by ISR on C frame */
extern volatile char jetson_cmd_buf[32]; /* C<spd>,<str>\0 from ISR */
extern volatile uint32_t jetson_hb_tick; /* HAL_GetTick() of last H or C */
#endif /* JETSON_CMD_H */

16
legacy/stm32/include/jetson_uart.h
View File

@ -1,16 +0,0 @@
#ifndef JETSON_UART_H
#define JETSON_UART_H
#include <stdint.h>
#include "stm32f7xx_hal.h"
/* Initialize USART6 for Jetson communication (921600 baud) */
void jetson_uart_init(void);
/* Send data back to Jetson (telemetry, status) */
void jetson_uart_send(const uint8_t *data, uint16_t len);
/* Called from HAL_UART_RxCpltCallback — handles byte accumulation */
void jetson_uart_rx_callback(UART_HandleTypeDef *huart);
#endif /* JETSON_UART_H */

441
legacy/stm32/include/jlink.h
View File

@ -1,441 +0,0 @@
#ifndef JLINK_H
#define JLINK_H
#include <stdint.h>
#include <stdbool.h>
#include "pid_flash.h" /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */
/*
* JLink -- Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX).
*
* Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated
* hardware UART at 921600 baud using DMA circular RX and IDLE interrupt.
*
* Frame format (both directions):
* [STX=0x02][LEN][CMD][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
*
* STX : frame start sentinel (0x02)
* LEN : count of CMD + PAYLOAD bytes (1 + payload_len)
* CMD : command/telemetry type byte
* PAYLOAD: 0..N bytes depending on CMD
* CRC16 : CRC16-XModem over CMD+PAYLOAD (poly 0x1021, init 0), big-endian
* ETX : frame end sentinel (0x03)
*
* Jetson to STM32 commands:
* 0x01 HEARTBEAT - no payload; refreshes heartbeat timer
* 0x02 DRIVE - int16 speed (-1000..+1000), int16 steer (-1000..+1000)
* 0x03 ARM - no payload; request arm (same interlock as CDC 'A')
* 0x04 DISARM - no payload; disarm immediately
* 0x05 PID_SET - float kp, float ki, float kd (12 bytes, IEEE-754 LE)
* 0x06 DFU_ENTER - no payload; request OTA DFU reboot (denied while armed)
* 0x07 ESTOP - no payload; engage emergency stop
* 0x08 AUDIO - int16 PCM samples (up to 126 samples)
* 0x09 SLEEP - no payload; request STOP-mode sleep
* 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531)
* 0x0B GIMBAL_POS - int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547)
* 0x0C SCHED_GET - no payload; reply with TLM_SCHED (Issue #550)
* 0x0D SCHED_SET - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550)
* 0x0E SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550)
* 0x0F FAULT_LOG_GET - no payload; reply with TLM_FAULT_LOG (Issue #565)
* 0x10 CAN_STATS_GET - no payload; reply with TLM_CAN_STATS (Issue #597)
*
* STM32 to Jetson telemetry:
* 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
* 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
* 0x82 BATTERY - jlink_tlm_battery_t (10 bytes, Issue #533)
* 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
* 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547)
* 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
* 0x86 MOTOR_CURRENT - jlink_tlm_motor_current_t (8 bytes, Issue #584)
* 0x87 FAULT_LOG - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
* 0x88 SLOPE - jlink_tlm_slope_t (4 bytes), sent at SLOPE_TLM_HZ (Issue #600)
* 0x89 CAN_STATS - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597)
* 0x8A STEERING - jlink_tlm_steering_t (8 bytes), sent at STEER_TLM_HZ (Issue #616)
* 0x8B LVC - jlink_tlm_lvc_t (4 bytes), sent at LVC_TLM_HZ (Issue #613)
* 0x8C ODOM - jlink_tlm_odom_t (16 bytes), sent at ENC_TLM_HZ (Issue #632)
*
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
* RC_ASSISTED modes the Jetson speed offset and steer are injected via
* mode_manager_set_auto_cmd() and blended per the existing blend ramp.
*
* Heartbeat: if no valid frame arrives within JLINK_HB_TIMEOUT_MS (1000ms),
* jlink_is_active() returns false and the main loop clears the auto command.
*/
/* ---- Frame constants ---- */
#define JLINK_STX 0x02u
#define JLINK_ETX 0x03u
/* ---- Command IDs (Jetson to STM32) ---- */
#define JLINK_CMD_HEARTBEAT 0x01u
#define JLINK_CMD_DRIVE 0x02u
#define JLINK_CMD_ARM 0x03u
#define JLINK_CMD_DISARM 0x04u
#define JLINK_CMD_PID_SET 0x05u
#define JLINK_CMD_DFU_ENTER 0x06u
#define JLINK_CMD_ESTOP 0x07u
#define JLINK_CMD_AUDIO 0x08u /* PCM audio chunk: int16 samples, up to 126 */
#define JLINK_CMD_SLEEP 0x09u /* no payload; request STOP-mode sleep */
#define JLINK_CMD_PID_SAVE 0x0Au /* no payload; save Kp/Ki/Kd to flash (Issue #531) */
#define JLINK_CMD_GIMBAL_POS 0x0Bu /* int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) */
#define JLINK_CMD_SCHED_GET 0x0Cu /* no payload; reply TLM_SCHED (Issue #550) */
#define JLINK_CMD_SCHED_SET 0x0Du /* uint8 num_bands + N*16-byte entries (Issue #550) */
#define JLINK_CMD_SCHED_SAVE 0x0Eu /* float kp,ki,kd; save sched+single to flash (Issue #550) */
#define JLINK_CMD_FAULT_LOG_GET 0x0Fu /* no payload; reply TLM_FAULT_LOG (Issue #565) */
#define JLINK_CMD_CAN_STATS_GET 0x10u /* no payload; reply TLM_CAN_STATS (Issue #597) */
/* ---- Telemetry IDs (STM32 to Jetson) ---- */
#define JLINK_TLM_STATUS 0x80u
#define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */
#define JLINK_TLM_BATTERY 0x82u /* jlink_tlm_battery_t (10 bytes, Issue #533) */
#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */
#define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */
#define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */
#define JLINK_TLM_MOTOR_CURRENT 0x86u /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */
#define JLINK_TLM_FAULT_LOG 0x87u /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
#define JLINK_TLM_SLOPE 0x88u /* jlink_tlm_slope_t (4 bytes, Issue #600) */
#define JLINK_TLM_CAN_STATS 0x89u /* jlink_tlm_can_stats_t (16 bytes, Issue #597) */
#define JLINK_TLM_STEERING 0x8Au /* jlink_tlm_steering_t (8 bytes, Issue #616) */
#define JLINK_TLM_LVC 0x8Bu /* jlink_tlm_lvc_t (4 bytes, Issue #613) */
#define JLINK_TLM_ODOM 0x8Cu /* jlink_tlm_odom_t (16 bytes, Issue #632) */
#define JLINK_TLM_BARO 0x8Du /* jlink_tlm_baro_t (12 bytes, Issue #672) */
#define JLINK_TLM_VESC_STATE 0x8Eu /* jlink_tlm_vesc_state_t (22 bytes, Issue #674) */
#define JLINK_TLM_CAN_WDOG 0x8Fu /* jlink_tlm_can_wdog_t (16 bytes, Issue #694) */
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
typedef struct __attribute__((packed)) {
int16_t pitch_x10; /* pitch degrees x10 */
int16_t roll_x10; /* roll degrees x10 */
int16_t yaw_x10; /* yaw degrees x10 (gyro-integrated) */
int16_t motor_cmd; /* ESC output -1000..+1000 */
uint16_t vbat_mv; /* battery millivolts */
int8_t rssi_dbm; /* CRSF RSSI (dBm, negative) */
uint8_t link_quality; /* CRSF LQ 0-100 */
uint8_t balance_state; /* 0=DISARMED, 1=ARMED, 2=TILT_FAULT */
uint8_t rc_armed; /* crsf_state.armed (1=armed) */
uint8_t mode; /* robot_mode_t: 0=RC_MANUAL,1=ASSISTED,2=AUTONOMOUS */
uint8_t estop; /* EstopSource value */
uint8_t soc_pct; /* state-of-charge 0-100, 255=unknown */
uint8_t fw_major;
uint8_t fw_minor;
uint8_t fw_patch;
} jlink_tlm_status_t; /* 20 bytes */
/* ---- Telemetry POWER payload (11 bytes, packed) ---- */
typedef struct __attribute__((packed)) {
uint8_t power_state; /* PowerState: 0=ACTIVE,1=SLEEP_PENDING,2=SLEEPING,3=WAKING */
uint16_t est_total_ma; /* estimated total current draw (mA) */
uint16_t est_audio_ma; /* estimated I2S3+amp current (mA); 0 if gated */
uint16_t est_osd_ma; /* estimated OSD SPI2 current (mA); 0 if gated */
uint32_t idle_ms; /* ms since last cmd_vel activity */
} jlink_tlm_power_t; /* 11 bytes */
/* ---- Telemetry BATTERY payload (10 bytes, packed) Issue #533 ---- */
typedef struct __attribute__((packed)) {
uint16_t vbat_mv; /* DMA-sampled LPF-filtered Vbat (mV) */
int16_t ibat_ma; /* DMA-sampled LPF-filtered Ibat (mA, + = discharge) */
uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */
uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */
int8_t cal_offset; /* vbat_offset_mv / 4 (+-127 -> +-508 mV) */
uint8_t lpf_shift; /* IIR shift factor (alpha = 1/2^lpf_shift) */
uint8_t soc_pct; /* voltage-based SoC 0-100, 255 = unknown */
} jlink_tlm_battery_t; /* 10 bytes */
/* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */
/* Sent after JLINK_CMD_PID_SAVE is processed; confirms gains written to flash. */
typedef struct __attribute__((packed)) {
float kp; /* Kp saved */
float ki; /* Ki saved */
float kd; /* Kd saved */
uint8_t saved_ok; /* 1 = flash write verified, 0 = write failed */
} jlink_tlm_pid_result_t; /* 13 bytes */
/* ---- Telemetry GIMBAL_STATE payload (10 bytes, packed) Issue #547 ---- */
/* Sent at GIMBAL_TLM_HZ (50 Hz); reports measured pan/tilt and speed. */
typedef struct __attribute__((packed)) {
int16_t pan_x10; /* Measured pan angle, deg x10 */
int16_t tilt_x10; /* Measured tilt angle, deg x10 */
uint16_t pan_speed_raw; /* Present speed register, pan servo */
uint16_t tilt_speed_raw; /* Present speed register, tilt servo */
uint8_t torque_en; /* 1 = torque enabled */
uint8_t rx_err_pct; /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */
} jlink_tlm_gimbal_state_t; /* 10 bytes */
/* ---- Telemetry SCHED payload (1 + N*16 bytes, packed) Issue #550 ---- */
/* Sent in response to JLINK_CMD_SCHED_GET; N = num_bands (1..PID_SCHED_MAX_BANDS). */
typedef struct __attribute__((packed)) {
uint8_t num_bands; /* number of valid entries */
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* up to 6 x 16 = 96 bytes */
} jlink_tlm_sched_t; /* 1 + 96 = 97 bytes max */
/* ---- Telemetry MOTOR_CURRENT payload (8 bytes, packed) Issue #584 ---- */
/* Published at MOTOR_CURR_TLM_HZ; reports measured current and protection state. */
typedef struct __attribute__((packed)) {
int32_t current_ma; /* filtered battery/motor current (mA, + = discharge) */
uint8_t limit_pct; /* soft-limit reduction applied: 0=none, 100=full cutoff */
uint8_t state; /* MotorCurrentState: 0=NORMAL,1=SOFT_LIMIT,2=COOLDOWN */
uint8_t fault_count; /* lifetime hard-cutoff trips (saturates at 255) */
uint8_t _pad; /* reserved */
} jlink_tlm_motor_current_t; /* 8 bytes */
/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */
/* Sent at SLOPE_TLM_HZ (1 Hz) by slope_estimator_send_tlm(). */
typedef struct __attribute__((packed)) {
int16_t slope_x100; /* terrain slope estimate (degrees x100; + = nose-up) */
uint8_t active; /* 1 = slope estimation enabled */
uint8_t _pad;
} jlink_tlm_slope_t; /* 4 bytes */
/* ---- Telemetry FAULT_LOG payload (20 bytes, packed) Issue #565 ---- */
/* Sent on boot (if last fault != NONE) and in response to FAULT_LOG_GET. */
typedef struct __attribute__((packed)) {
uint8_t fault_type; /* FaultType of most recent entry */
uint8_t entry_count; /* number of valid entries in flash log (0-8) */
uint8_t reset_count; /* lifetime reset counter */
uint8_t _pad;
uint32_t timestamp_ms; /* HAL_GetTick() at fault */
uint32_t pc; /* faulting PC */
uint32_t lr; /* link register at fault */
uint32_t cfsr; /* SCB->CFSR */
uint32_t hfsr; /* SCB->HFSR */
} jlink_tlm_fault_log_t; /* 20 bytes */
/* ---- Telemetry CAN_STATS payload (16 bytes, packed) Issue #597 ---- */
/* Sent at CAN_TLM_HZ (1 Hz) and in response to CAN_STATS_GET. */
typedef struct __attribute__((packed)) {
uint32_t tx_count; /* total VELOCITY_CMD frames sent */
uint32_t rx_count; /* total valid FEEDBACK frames received */
uint16_t err_count; /* CAN error frame count */
uint8_t bus_off; /* 1 = bus-off state */
uint8_t node_faults; /* bit0 = node 0 fault active, bit1 = node 1 */
int16_t vel0_rpm; /* node 0 current velocity (RPM) */
int16_t vel1_rpm; /* node 1 current velocity (RPM) */
} jlink_tlm_can_stats_t; /* 16 bytes */
/* ---- Telemetry STEERING payload (8 bytes, packed) Issue #616 ---- */
/* Published at STEER_TLM_HZ (10 Hz); reports yaw-rate PID state. */
typedef struct __attribute__((packed)) {
int16_t target_x10; /* target yaw rate, deg/s × 10 (0.1 deg/s resolution) */
int16_t actual_x10; /* measured yaw rate, deg/s × 10 */
int16_t output; /* differential motor output (-STEER_OUTPUT_MAX..+MAX) */
uint8_t enabled; /* 1 = PID active */
uint8_t _pad; /* reserved */
} jlink_tlm_steering_t; /* 8 bytes */
/* ---- Telemetry LVC payload (4 bytes, packed) Issue #613 ---- */
/* Sent at LVC_TLM_HZ (1 Hz); reports battery voltage and LVC protection state. */
typedef struct __attribute__((packed)) {
uint16_t voltage_mv; /* battery voltage (mV) */
uint8_t percent; /* 0-100: fuel gauge within CUTOFF..WARNING; 255=unknown */
uint8_t protection_state; /* LvcState: 0=NORMAL,1=WARNING,2=CRITICAL,3=CUTOFF */
} jlink_tlm_lvc_t; /* 4 bytes */
/* ---- Telemetry ODOM payload (16 bytes, packed) Issue #632 ---- */
/* Sent at ENC_TLM_HZ (50 Hz); wheel RPM, pose, and linear speed. */
typedef struct __attribute__((packed)) {
int16_t rpm_left; /* left wheel RPM (signed; + = forward) */
int16_t rpm_right; /* right wheel RPM (signed) */
int32_t x_mm; /* forward displacement (mm, int32) */
int32_t y_mm; /* lateral displacement (mm, int32) */
int16_t theta_cdeg; /* heading in centidegrees (0.01 deg steps) */
int16_t speed_mmps; /* linear speed of centre point (mm/s) */
} jlink_tlm_odom_t; /* 16 bytes */
/* ---- Telemetry BARO payload (12 bytes, packed) Issue #672 ---- */
/* Sent at BARO_TLM_HZ (1 Hz); reports ambient pressure, temperature, altitude. */
typedef struct __attribute__((packed)) {
int32_t pressure_pa; /* barometric pressure (Pa) */
int16_t temp_x10; /* ambient temperature (°C × 10; e.g. 235 = 23.5 °C) */
int32_t alt_cm; /* pressure altitude above ISA sea level (cm) */
int16_t humidity_pct_x10; /* %RH × 10 (BME280 only); -1 = BMP280/absent */
} jlink_tlm_baro_t; /* 12 bytes */
/* ---- Telemetry CAN_WDOG payload (16 bytes, packed) Issue #694 ---- */
/* Sent at 1 Hz; reports CAN bus-error severity and restart history. */
typedef struct __attribute__((packed)) {
uint32_t restart_count; /* SW bus-off restarts since boot */
uint32_t busoff_count; /* lifetime bus-off entry events */
uint16_t errpassive_count; /* error-passive transitions */
uint16_t errwarn_count; /* error-warning transitions */
uint8_t error_state; /* can_error_state_t: 0=OK,1=WARN,2=EP,3=BOFF */
uint8_t tec; /* transmit error counter (ESR[23:16]) */
uint8_t rec; /* receive error counter (ESR[31:24]) */
uint8_t _pad; /* reserved */
} jlink_tlm_can_wdog_t; /* 16 bytes */
/* ---- Telemetry VESC_STATE payload (22 bytes, packed) Issue #674 ---- */
/* Sent at VESC_TLM_HZ (1 Hz) by vesc_can_send_tlm(). */
typedef struct __attribute__((packed)) {
int32_t left_rpm; /* left VESC actual RPM */
int32_t right_rpm; /* right VESC actual RPM */
int16_t left_current_x10; /* left phase current (A × 10) */
int16_t right_current_x10; /* right phase current (A × 10) */
int16_t left_temp_x10; /* left FET temperature (°C × 10) */
int16_t right_temp_x10; /* right FET temperature (°C × 10) */
int16_t voltage_x10; /* input voltage (V × 10; from STATUS_5) */
uint8_t left_fault; /* left VESC fault code (0 = none) */
uint8_t right_fault; /* right VESC fault code (0 = none) */
uint8_t left_alive; /* 1 = left VESC alive (STATUS within 1 s) */
uint8_t right_alive; /* 1 = right VESC alive (STATUS within 1 s) */
} jlink_tlm_vesc_state_t; /* 22 bytes */
/* ---- Volatile state (read from main loop) ---- */
typedef struct {
/* Drive command - updated on JLINK_CMD_DRIVE */
volatile int16_t speed; /* -1000..+1000 */
volatile int16_t steer; /* -1000..+1000 */
/* Heartbeat timer - updated on any valid frame */
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame; 0=none */
/* One-shot request flags - set by parser, cleared by main loop */
volatile uint8_t arm_req;
volatile uint8_t disarm_req;
volatile uint8_t estop_req;
/* PID update - set by parser, cleared by main loop */
volatile uint8_t pid_updated;
volatile float pid_kp;
volatile float pid_ki;
volatile float pid_kd;
/* DFU reboot request - set by parser, cleared by main loop */
volatile uint8_t dfu_req;
/* Sleep request - set by JLINK_CMD_SLEEP, cleared by main loop */
volatile uint8_t sleep_req;
/* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */
volatile uint8_t pid_save_req;
/* Gimbal position command - set by JLINK_CMD_GIMBAL_POS (Issue #547) */
volatile uint8_t gimbal_updated; /* set by parser, cleared by main loop */
volatile int16_t gimbal_pan_x10; /* pan angle deg x10 */
volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */
volatile uint16_t gimbal_speed; /* servo speed 0-4095 (0=max) */
/* PID schedule commands (Issue #550) - set by parser, cleared by main loop */
volatile uint8_t sched_get_req; /* SCHED_GET: main loop calls jlink_send_sched_telemetry() */
volatile uint8_t sched_save_req; /* SCHED_SAVE: main loop calls pid_schedule_flash_save() */
volatile float sched_save_kp; /* kp for single-PID record in SCHED_SAVE */
volatile float sched_save_ki;
volatile float sched_save_kd;
/* Fault log request (Issue #565) - set by JLINK_CMD_FAULT_LOG_GET, cleared by main loop */
volatile uint8_t fault_log_req;
/* CAN stats request (Issue #597) - set by JLINK_CMD_CAN_STATS_GET, cleared by main loop */
volatile uint8_t can_stats_req;
} JLinkState;
extern volatile JLinkState jlink_state;
/* ---- SCHED_SET receive buffer -- Issue #550 ---- */
/*
* Populated by the parser on JLINK_CMD_SCHED_SET. Main loop reads via
* jlink_get_sched_set() and calls pid_schedule_set_table() before clearing.
*/
typedef struct {
volatile uint8_t ready; /* set by parser, cleared by main loop */
volatile uint8_t num_bands;
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* copied from frame */
} JLinkSchedSetBuf;
/* ---- API ---- */
void jlink_init(void);
bool jlink_is_active(uint32_t now_ms);
void jlink_process(void);
void jlink_send_telemetry(const jlink_tlm_status_t *status);
void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt);
void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
/*
* jlink_send_gimbal_state(state) - transmit JLINK_TLM_GIMBAL_STATE (0x84)
* frame (16 bytes) at GIMBAL_TLM_HZ (50 Hz). Issue #547.
*/
void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state);
/*
* jlink_send_sched_telemetry(tlm) - transmit JLINK_TLM_SCHED (0x85) in
* response to SCHED_GET. tlm->num_bands determines actual frame size.
* Issue #550.
*/
void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm);
/*
* jlink_get_sched_set() - return pointer to the most-recently received
* SCHED_SET payload buffer (static storage in jlink.c). Main loop calls
* pid_schedule_set_table() from this buffer, then clears ready. Issue #550.
*/
JLinkSchedSetBuf *jlink_get_sched_set(void);
/*
* jlink_send_motor_current_tlm(tlm) - transmit JLINK_TLM_MOTOR_CURRENT (0x86)
* frame (14 bytes total) to Jetson. Issue #584.
* Rate-limiting is handled by motor_current_send_tlm(); call from there only.
*/
void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm);
/*
* jlink_send_fault_log(fl) - transmit JLINK_TLM_FAULT_LOG (0x87) frame
* (26 bytes) on boot (if fault log non-empty) and in response to
* FAULT_LOG_GET. Issue #565.
*/
void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl);
/*
* jlink_send_slope_tlm(tlm) - transmit JLINK_TLM_SLOPE (0x88) frame
* (10 bytes) at SLOPE_TLM_HZ (1 Hz). Called from slope_estimator_send_tlm().
* Issue #600.
*/
void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
/*
* jlink_send_can_stats(tlm) - transmit JLINK_TLM_CAN_STATS (0x89) frame
* (22 bytes) at CAN_TLM_HZ (1 Hz) and in response to CAN_STATS_GET.
* Issue #597.
*/
void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm);
/*
* jlink_send_steering_tlm(tlm) - transmit JLINK_TLM_STEERING (0x8A) frame
* (14 bytes total) to Jetson. Issue #616.
* Rate-limiting is handled by steering_pid_send_tlm(); call from there only.
*/
void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm);
/*
* jlink_send_lvc_tlm(tlm) - transmit JLINK_TLM_LVC (0x8B) frame
* (10 bytes total) at LVC_TLM_HZ (1 Hz). Issue #613.
*/
void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm);
/*
* jlink_send_odom_tlm(tlm) - transmit JLINK_TLM_ODOM (0x8C) frame
* (22 bytes total) at ENC_TLM_HZ (50 Hz). Issue #632.
* Rate-limiting handled by encoder_odom_send_tlm(); call from there only.
*/
void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm);
/*
* jlink_send_baro_tlm(tlm) - transmit JLINK_TLM_BARO (0x8D) frame
* (18 bytes total) at BARO_TLM_HZ (1 Hz). Issue #672.
* Rate-limiting handled by baro_tick(); call from there only.
*/
void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm);
/*
* jlink_send_vesc_state_tlm(tlm) - transmit JLINK_TLM_VESC_STATE (0x8E) frame
* (28 bytes total) at VESC_TLM_HZ (1 Hz). Issue #674.
* Rate-limiting handled by vesc_can_send_tlm(); call from there only.
*/
void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm);
/*
* jlink_send_can_wdog_tlm(tlm) - transmit JLINK_TLM_CAN_WDOG (0x8F) frame
* (22 bytes total) at 1 Hz. Issue #694.
*/
void jlink_send_can_wdog_tlm(const jlink_tlm_can_wdog_t *tlm);
#endif /* JLINK_H */

101
legacy/stm32/include/led.h
View File

@ -1,101 +0,0 @@
#ifndef LED_H
#define LED_H
#include <stdint.h>
#include <stdbool.h>
/*
* led.h — WS2812B NeoPixel status indicator driver (Issue #193)
*
* Hardware: TIM3_CH1 PWM on PB4 at 800 kHz (1.25 µs per bit).
* Controls an 8-LED ring with state-based animations:
* - Boot: Blue chase (startup sequence)
* - Armed: Solid green
* - Error: Red blinking (visual alert)
* - Low Battery: Yellow pulsing (warning)
* - Charging: Green breathing (soft indication)
* - E-Stop: Red strobe (immediate action required)
*
* State transitions are non-blocking via a 1 ms timer callback (led_tick).
* Each state defines its own animation envelope: color, timing, and brightness.
*
* WS2812 protocol (NRZ):
* - Bit "0": High 350 ns, Low 800 ns (1.25 µs total)
* - Bit "1": High 700 ns, Low 600 ns (1.25 µs total)
* - Reset: Low > 50 µs
*
* PWM-based implementation via DMA:
* - 10 levels: [350 ns, 400, 450, 500, 550, 600, 650, 700, 750, 800]
* - Bit "0" → High 350-400 ns Bit "1" → High 650-800 ns
* - Each bit requires one PWM cycle; 24 bits/LED × 8 LEDs = 192 cycles
* - DMA rings through buffer, auto-reloads on update events
*/
/* LED state enumeration */
typedef enum {
LED_STATE_BOOT = 0, /* Blue chase (startup) */
LED_STATE_ARMED = 1, /* Solid green */
LED_STATE_ERROR = 2, /* Red blinking */
LED_STATE_LOW_BATT = 3, /* Yellow pulsing */
LED_STATE_CHARGING = 4, /* Green breathing */
LED_STATE_ESTOP = 5, /* Red strobe */
LED_STATE_COUNT
} LEDState;
/* RGB color (8-bit per channel) */
typedef struct {
uint8_t r;
uint8_t g;
uint8_t b;
} RGBColor;
/*
* led_init()
*
* Configure TIM3_CH1 PWM on PB4 at 800 kHz, set up DMA for bit streaming,
* and initialize the LED buffer. Call once at startup, after buzzer_init()
* but before the main loop.
*/
void led_init(void);
/*
* led_set_state(state)
*
* Change the LED display state. The animation runs non-blocking via led_tick().
* Valid states: LED_STATE_BOOT, LED_STATE_ARMED, LED_STATE_ERROR, etc.
*/
void led_set_state(LEDState state);
/*
* led_get_state()
*
* Return the current LED state.
*/
LEDState led_get_state(void);
/*
* led_set_color(r, g, b)
*
* Manually set the LED ring to a solid color. Overrides the current state
* animation until led_set_state() is called again.
*/
void led_set_color(uint8_t r, uint8_t g, uint8_t b);
/*
* led_tick(now_ms)
*
* Advance animation state machine. Must be called every 1 ms from the main loop.
* Handles state-specific animations: chase timing, pulse envelope, strobe phase, etc.
* Updates the DMA buffer with new LED values without blocking.
*/
void led_tick(uint32_t now_ms);
/*
* led_is_animating()
*
* Returns true if the current state is actively animating (e.g., chase, pulse, strobe).
* Returns false for static states (armed, error solid).
*/
bool led_is_animating(void);
#endif /* LED_H */

39
legacy/stm32/include/lvc.h
View File

@ -1,39 +0,0 @@
#ifndef LVC_H
#define LVC_H
#include <stdint.h>
#include <stdbool.h>
/*
* lvc.h -- Low Voltage Cutoff (LVC) protection (Issue #613)
*
* 3-stage battery voltage protection using battery_read_mv():
*
* LVC_WARNING (21.0 V) -- periodic buzzer alert; full power maintained
* LVC_CRITICAL (19.8 V) -- faster buzzer; motor commands scaled to 50%
* LVC_CUTOFF (18.6 V) -- error buzzer; motors disabled; latched until reboot
*
* Recovery uses LVC_HYSTERESIS_MV to prevent threshold chatter.
* CUTOFF is one-way: once latched, only a power-cycle clears it.
*
* Integration:
* lvc_init() -- call once during system init
* lvc_tick(now_ms, vbat_mv) -- call each main loop tick (1 kHz)
* lvc_get_power_scale() -- returns 0/50/100; apply to motor speed
* lvc_is_cutoff() -- true when motors must be disabled
*/
typedef enum {
LVC_NORMAL = 0, /* Vbat >= WARNING threshold */
LVC_WARNING = 1, /* Vbat < 21.0 V -- alert only */
LVC_CRITICAL = 2, /* Vbat < 19.8 V -- 50% power */
LVC_CUTOFF = 3, /* Vbat < 18.6 V -- motors off */
} LvcState;
void lvc_init(void);
void lvc_tick(uint32_t now_ms, uint32_t vbat_mv);
LvcState lvc_get_state(void);
uint8_t lvc_get_power_scale(void); /* 100 = full, 50 = critical, 0 = cutoff */
bool lvc_is_cutoff(void);
#endif /* LVC_H */

26
legacy/stm32/include/mag.h
View File

@ -1,26 +0,0 @@
#ifndef MAG_H
#define MAG_H
#include <stdint.h>
typedef enum {
MAG_NONE = 0,
MAG_QMC5883L, /* I2C 0x0D — most common on external compass modules */
MAG_HMC5883L, /* I2C 0x1E — legacy Honeywell part */
MAG_IST8310, /* I2C 0x0E — iSentek part, common on CUAV boards */
} mag_type_t;
/*
* Auto-detect magnetometer on I2C1.
* Returns detected type (MAG_NONE if nothing found).
* Requires i2c1_init() before calling.
*/
mag_type_t mag_init(void);
/*
* Read compass heading (degrees × 10, 0–3599).
* Returns -1 if data not ready or no sensor.
*/
int16_t mag_read_heading(void);
#endif /* MAG_H */

74
legacy/stm32/include/mode_manager.h
View File

@ -1,74 +0,0 @@
#ifndef MODE_MANAGER_H
#define MODE_MANAGER_H
#include <stdint.h>
#include <stdbool.h>
/*
* SaltyLab Mode Manager
*
* Resolves three operating modes selected by RC CH6 (3-pos switch):
*
* RC_MANUAL — RC steer (CH4) and speed bias (CH3) applied directly.
* Balance PID remains active for stability.
* RC_ASSISTED — RC inputs blended 50/50 with Jetson autonomous commands.
* AUTONOMOUS — Jetson commands only; RC CH5 arm switch still kills motors.
*
* Transitions between modes are smoothed over MODE_BLEND_MS (~500ms) to
* prevent jerky handoffs. A single `blend` scalar (0=pure RC, 1=pure auto)
* drives all interpolation; adjacent-mode steps take ~250ms each.
*
* RC safety rule: if RC is alive and CH5 is disarmed, the main loop MUST
* disarm regardless of mode. mode_manager only blends commands — kill
* authority lives in the main loop.
*
* Autonomous commands are set by the Jetson serial bridge via
* mode_manager_set_auto_cmd(). They default to zero (no motion).
*/
typedef enum {
MODE_RC_MANUAL = 0,
MODE_RC_ASSISTED = 1,
MODE_AUTONOMOUS = 2,
} robot_mode_t;
typedef struct {
robot_mode_t target; /* Mode requested by CH6 (or fallback) */
float blend; /* 0.0=pure RC .. 1.0=pure auto, smoothly ramped */
bool rc_alive; /* Cached RC liveness (set in update) */
int16_t auto_steer; /* Jetson steer cmd (-1000..+1000) */
int16_t auto_speed_bias;/* Jetson speed bias (-MOTOR_RC_SPEED_MAX..+) */
} mode_manager_t;
/* Initialise — call once before the main loop */
void mode_manager_init(mode_manager_t *m);
/*
* Call every main-loop tick (1ms) to:
* - read CH6, update target mode
* - cache RC liveness
* - advance blend ramp toward target blend value
*/
void mode_manager_update(mode_manager_t *m, uint32_t now);
/* Set autonomous commands from the Jetson serial bridge */
void mode_manager_set_auto_cmd(mode_manager_t *m,
int16_t steer,
int16_t speed_bias);
/*
* Blended steer command to pass to motor_driver_update().
* Returns 0 when RC is not alive and no autonomous steer set.
*/
int16_t mode_manager_get_steer(const mode_manager_t *m);
/*
* Blended speed bias to add to bal.motor_cmd before motor_driver_update().
* Returns 0 when RC is not alive and no autonomous speed set.
*/
int16_t mode_manager_get_speed_bias(const mode_manager_t *m);
/* Quantised current mode (based on blend position, not target) */
robot_mode_t mode_manager_active(const mode_manager_t *m);
#endif

121
legacy/stm32/include/motor_current.h
View File

@ -1,121 +0,0 @@
#ifndef MOTOR_CURRENT_H
#define MOTOR_CURRENT_H
#include <stdint.h>
#include <stdbool.h>
/*
* motor_current — ADC-based motor current monitoring and overload protection
* for Issue #584.
*
* Hardware:
* ADC3 IN13 (PC3, ADC_CURR_PIN) is already sampled by battery_adc.c via
* DMA2_Stream0 circular. This module reads battery_adc_get_current_ma()
* each tick rather than running a second ADC, since total discharge current
* on this single-motor balance bot equals motor current plus ~30 mA overhead.
*
* Behaviour:
* MC_NORMAL : current_ma < MOTOR_CURR_SOFT_MA — full output
* MC_SOFT_LIMIT : current_ma in [SOFT_MA, HARD_MA) — linear PWM reduction
* MC_COOLDOWN : hard cutoff latched after HARD_MA sustained for
* MOTOR_CURR_OVERLOAD_MS (2 s) — zero output for
* MOTOR_CURR_COOLDOWN_MS (10 s), then MC_NORMAL
*
* Soft limit formula (MC_SOFT_LIMIT):
* scale = (HARD_MA - current_ma) / (HARD_MA - SOFT_MA) [0..1]
* limited_cmd = (int16_t)(cmd * scale)
*
* Fault event:
* On each hard-cutoff trip, s_fault_count is incremented (saturates at 255)
* and motor_current_fault_pending() returns true for one main-loop tick so
* the caller can append a fault log entry.
*
* Main-loop integration (pseudo-code):
*
* void main_loop_tick(uint32_t now_ms) {
* battery_adc_tick(now_ms);
* motor_current_tick(now_ms);
*
* if (motor_current_fault_pending())
* fault_log_append(FAULT_MOTOR_OVERCURRENT);
*
* int16_t cmd = balance_pid_output();
* cmd = motor_current_apply_limit(cmd);
* motor_driver_update(&g_motor, cmd, steer, now_ms);
*
* motor_current_send_tlm(now_ms); // rate-limited to MOTOR_CURR_TLM_HZ
* }
*/
/* ---- Thresholds ---- */
#define MOTOR_CURR_HARD_MA 5000u /* 5 A — hard cutoff level */
#define MOTOR_CURR_SOFT_MA 4000u /* 4 A — soft-limit onset (80% of hard) */
#define MOTOR_CURR_OVERLOAD_MS 2000u /* sustained over HARD_MA before fault */
#define MOTOR_CURR_COOLDOWN_MS 10000u /* zero-output recovery period (ms) */
#define MOTOR_CURR_TLM_HZ 5u /* JLINK_TLM_MOTOR_CURRENT publish rate */
/* ---- State enum ---- */
typedef enum {
MC_NORMAL = 0,
MC_SOFT_LIMIT = 1,
MC_COOLDOWN = 2,
} MotorCurrentState;
/* ---- API ---- */
/*
* motor_current_init() — reset all state.
* Call once during system init, after battery_adc_init().
*/
void motor_current_init(void);
/*
* motor_current_tick(now_ms) — evaluate ADC reading, update state machine.
* Call from main loop after battery_adc_tick(), at any rate ≥ 10 Hz.
* Non-blocking (<1 µs).
*/
void motor_current_tick(uint32_t now_ms);
/*
* motor_current_apply_limit(cmd) — scale motor command by current-limit factor.
* MC_NORMAL: returns cmd unchanged.
* MC_SOFT_LIMIT: returns cmd scaled down linearly.
* MC_COOLDOWN: returns 0.
* Call after motor_current_tick() each loop iteration.
*/
int16_t motor_current_apply_limit(int16_t cmd);
/*
* motor_current_is_faulted() — true while in MC_COOLDOWN (output zeroed).
*/
bool motor_current_is_faulted(void);
/*
* motor_current_state() — current state machine state.
*/
MotorCurrentState motor_current_state(void);
/*
* motor_current_ma() — most recent ADC reading used by the state machine (mA).
*/
int32_t motor_current_ma(void);
/*
* motor_current_fault_count() — lifetime hard-cutoff trip counter (0..255).
*/
uint8_t motor_current_fault_count(void);
/*
* motor_current_fault_pending() — true for exactly one tick after a hard
* cutoff trip fires. Main loop should append a fault log entry and then the
* flag clears automatically on the next call.
*/
bool motor_current_fault_pending(void);
/*
* motor_current_send_tlm(now_ms) — transmit JLINK_TLM_MOTOR_CURRENT (0x86)
* frame to Jetson. Rate-limited to MOTOR_CURR_TLM_HZ; safe to call every tick.
*/
void motor_current_send_tlm(uint32_t now_ms);
#endif /* MOTOR_CURRENT_H */

47
legacy/stm32/include/motor_driver.h
View File

@ -1,47 +0,0 @@
#ifndef MOTOR_DRIVER_H
#define MOTOR_DRIVER_H
#include <stdint.h>
#include <stdbool.h>
/*
* SaltyLab Motor Driver
*
* Sits between the balance PID and the raw hoverboard UART driver.
* Responsibilities:
* - Differential drive mixing: speed = balance_cmd, steer mixed in
* - Steer ramping to avoid sudden yaw torque disturbing balance
* - Headroom clamping: |speed| + |steer| <= MOTOR_CMD_MAX
* - Emergency stop: immediate zero + latch until explicitly cleared
*
* Balance PID output is NOT ramped — it needs full immediate authority.
* Only the steer channel is ramped.
*
* Call motor_driver_update() at the ESC send rate (50Hz / every 20ms).
*/
typedef struct {
int16_t steer_actual; /* Ramped steer command currently sent */
bool estop; /* Emergency stop latched */
} motor_driver_t;
void motor_driver_init(motor_driver_t *m);
/*
* Update and send to ESC.
* balance_cmd : PID output, -1000..+1000
* steer_cmd : desired yaw/steer, -1000..+1000 (future RC/autonomous input)
* now : HAL_GetTick() timestamp (ms) for ramp delta-time
*/
void motor_driver_update(motor_driver_t *m,
int16_t balance_cmd,
int16_t steer_cmd,
uint32_t now);
/* Latch emergency stop — sends zero immediately */
void motor_driver_estop(motor_driver_t *m);
/* Clear emergency stop latch (only call from armed/ready context) */
void motor_driver_estop_clear(motor_driver_t *m);
#endif

43
legacy/stm32/include/mpu6000.h
View File

@ -1,43 +0,0 @@
#ifndef MPU6000_H
#define MPU6000_H
#include <stdint.h>
#include <stdbool.h>
typedef struct {
float pitch; // degrees, filtered (complementary filter)
float pitch_rate; // degrees/sec (raw gyro pitch axis)
float roll; // degrees, filtered (complementary filter)
float yaw; // degrees, gyro-integrated (drifts — no magnetometer)
float yaw_rate; // degrees/sec (raw gyro Z / board_gz, Issue #616)
float accel_x; // g
float accel_z; // g
} IMUData;
bool mpu6000_init(void);
/*
* Sample gyro for ~1s to compute per-axis bias offsets.
* Must be called after mpu6000_init() with the board held still.
* Blocks ~1s. LED1+LED2 solid during calibration.
* IWDG must NOT be running when this is called (call before safety_init()).
*/
void mpu6000_calibrate(void);
/* Returns true once mpu6000_calibrate() has completed. */
bool mpu6000_is_calibrated(void);
void mpu6000_read(IMUData *data);
/*
* mpu6000_set_mount_offset(pitch_deg, roll_deg) — set mount angle offsets.
* These are subtracted from the pitch and roll outputs in mpu6000_read().
* Load via imu_cal_flash_load() on boot; update after 'O' CDC command.
* Issue #680.
*/
void mpu6000_set_mount_offset(float pitch_deg, float roll_deg);
/* Returns true if non-zero mount offsets have been applied (Issue #680). */
bool mpu6000_has_mount_offset(void);
#endif

191
legacy/stm32/include/orin_can.h
View File

@ -1,191 +0,0 @@
#ifndef ORIN_CAN_H
#define ORIN_CAN_H
#include <stdint.h>
#include <stdbool.h>
/*
* orin_can — Orin↔FC CAN protocol driver (Issue #674).
*
* Standard 11-bit CAN IDs on CAN2, FIFO0.
*
* Orin → FC commands:
* 0x300 HEARTBEAT : uint32 sequence counter (4 bytes)
* 0x301 DRIVE : int16 speed (−1000..+1000), int16 steer (−1000..+1000)
* 0x302 MODE : uint8 mode (0=RC_MANUAL, 1=ASSISTED, 2=AUTONOMOUS)
* 0x303 ESTOP : uint8 action (1=ESTOP, 0=CLEAR)
*
* FC → Orin telemetry (broadcast at ORIN_TLM_HZ):
* 0x400 FC_STATUS : int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
* uint8 balance_state, uint8 flags [bit0=estop, bit1=armed]
* 0x401 FC_VESC : int16 left_rpm_x10 (RPM/10), int16 right_rpm_x10,
* int16 left_current_x10, int16 right_current_x10
*
* Balance independence: if no Orin heartbeat for ORIN_HB_TIMEOUT_MS, the FC
* continues balancing in-place — Orin commands are simply not injected.
* The balance PID loop runs entirely on Mamba and never depends on Orin.
*/
/* ---- Orin → FC command IDs ---- */
#define ORIN_CAN_ID_HEARTBEAT 0x300u
#define ORIN_CAN_ID_DRIVE 0x301u
#define ORIN_CAN_ID_MODE 0x302u
#define ORIN_CAN_ID_ESTOP 0x303u
#define ORIN_CAN_ID_LED_CMD 0x304u /* LED pattern override (Issue #685) */
#define ORIN_CAN_ID_PID_SET 0x305u /* PID gain update: kp/ki/kd (Issue #693) */
/* ---- FC → Orin telemetry IDs ---- */
#define ORIN_CAN_ID_FC_STATUS 0x400u /* balance state + pitch + vbat at 10 Hz */
#define ORIN_CAN_ID_FC_VESC 0x401u /* VESC RPM + current at 10 Hz */
#define ORIN_CAN_ID_FC_IMU 0x402u /* full IMU angles + cal status at 50 Hz (Issue #680) */
#define ORIN_CAN_ID_FC_BARO 0x403u /* barometer pressure/temp/altitude at 1 Hz (Issue #672) */
#define ORIN_CAN_ID_FC_BTN 0x404u /* button event on-demand (Issue #682) */
#define ORIN_CAN_ID_FC_PID_ACK 0x405u /* PID gain ACK: echoes applied kp/ki/kd (Issue #693) */
/* ---- Timing ---- */
#define ORIN_HB_TIMEOUT_MS 500u /* Orin offline after 500 ms without any frame */
#define ORIN_TLM_HZ 10u /* FC_STATUS + FC_VESC broadcast rate (Hz) */
#define ORIN_IMU_TLM_HZ 50u /* FC_IMU broadcast rate (Hz) */
#define ORIN_BARO_TLM_HZ 1u /* FC_BARO broadcast rate (Hz) */
/* ---- Volatile state updated by orin_can_on_frame(), read by main loop ---- */
typedef struct {
volatile int16_t speed; /* DRIVE: −1000..+1000 */
volatile int16_t steer; /* DRIVE: −1000..+1000 */
volatile uint8_t mode; /* MODE: robot_mode_t value */
volatile uint8_t drive_updated; /* set on DRIVE, cleared by main */
volatile uint8_t mode_updated; /* set on MODE, cleared by main */
volatile uint8_t estop_req; /* set on ESTOP(1), cleared by main */
volatile uint8_t estop_clear_req; /* set on ESTOP(0), cleared by main */
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last received frame */
/* PID_SET (Issue #693) -- set by orin_can_on_frame(), consumed by main */
volatile uint8_t pid_updated; /* set on PID_SET, cleared by main */
volatile uint16_t pid_kp_x100; /* Kp * 100 (0..50000) */
volatile uint16_t pid_ki_x100; /* Ki * 100 (0..5000) */
volatile uint16_t pid_kd_x100; /* Kd * 100 (0..5000) */
} OrinCanState;
extern volatile OrinCanState orin_can_state;
/* ---- FC → Orin broadcast payloads (packed, 8 bytes each) ---- */
typedef struct __attribute__((packed)) {
int16_t pitch_x10; /* pitch degrees × 10 */
int16_t motor_cmd; /* balance PID output −1000..+1000 */
uint16_t vbat_mv; /* battery voltage (mV) */
uint8_t balance_state; /* BalanceState: 0=DISARMED,1=ARMED,2=TILT_FAULT */
uint8_t flags; /* bit0=estop_active, bit1=armed */
} orin_can_fc_status_t; /* 8 bytes */
typedef struct __attribute__((packed)) {
int16_t left_rpm_x10; /* left wheel RPM / 10 (±32767 × 10 = ±327k RPM) */
int16_t right_rpm_x10; /* right wheel RPM / 10 */
int16_t left_current_x10; /* left phase current × 10 (A) */
int16_t right_current_x10; /* right phase current × 10 (A) */
} orin_can_fc_vesc_t; /* 8 bytes */
/* FC_IMU (0x402) — full IMU angles at 50 Hz (Issue #680) */
typedef struct __attribute__((packed)) {
int16_t pitch_x10; /* pitch degrees × 10 (mount-offset corrected) */
int16_t roll_x10; /* roll degrees × 10 (mount-offset corrected) */
int16_t yaw_x10; /* yaw degrees × 10 (gyro-integrated, drifts) */
uint8_t cal_status; /* 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */
uint8_t balance_state; /* BalanceState: 0=DISARMED, 1=ARMED, 2=TILT_FAULT */
} orin_can_fc_imu_t; /* 8 bytes */
/* FC_BARO (0x403) — barometer at 1 Hz (Issue #672) */
typedef struct __attribute__((packed)) {
int32_t pressure_pa; /* barometric pressure in Pa */
int16_t temp_x10; /* temperature × 10 (°C) */
int16_t alt_cm; /* altitude in cm (reference = pressure at boot) */
} orin_can_fc_baro_t; /* 8 bytes */
/* FC_BTN (0x404) — button event, sent on demand (Issue #682)
* event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED (pitch too large) */
typedef struct __attribute__((packed)) {
uint8_t event_id; /* 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED */
uint8_t balance_state; /* balance_state_t value after the event */
} orin_can_fc_btn_t; /* 2 bytes */
/* LED_CMD (0x304) — Orin → FC LED pattern override (Issue #685)
* duration_ms = 0: hold until next state change; >0: revert after duration */
typedef struct __attribute__((packed)) {
uint8_t pattern; /* 0=state_auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse */
uint8_t brightness; /* 0-255 (0 = both LEDs off) */
uint16_t duration_ms; /* override duration; 0 = permanent until state change */
} orin_can_led_cmd_t; /* 4 bytes */
/* LED override state (updated by orin_can_on_frame, read by main loop) */
extern volatile orin_can_led_cmd_t orin_can_led_override;
extern volatile uint8_t orin_can_led_updated;
/* ---- API ---- */
/*
* orin_can_init() — zero state, register orin_can_on_frame as std_cb with
* can_driver. Call after can_driver_init().
*/
void orin_can_init(void);
/*
* orin_can_on_frame(std_id, data, len) — dispatched by can_driver for each
* standard-ID frame in FIFO0. Updates orin_can_state.
*/
void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len);
/*
* orin_can_is_alive(now_ms) — true if a frame from Orin arrived within
* ORIN_HB_TIMEOUT_MS of now_ms.
*/
bool orin_can_is_alive(uint32_t now_ms);
/*
* orin_can_broadcast(now_ms, status, vesc) — rate-limited broadcast of
* FC_STATUS (0x400) and FC_VESC (0x401) at ORIN_TLM_HZ (10 Hz).
* Safe to call every ms; internally rate-limited.
*/
void orin_can_broadcast(uint32_t now_ms,
const orin_can_fc_status_t *status,
const orin_can_fc_vesc_t *vesc);
/*
* orin_can_broadcast_imu(now_ms, imu_tlm) — rate-limited broadcast of
* FC_IMU (0x402) at ORIN_IMU_TLM_HZ (50 Hz).
* Safe to call every ms; internally rate-limited. Issue #680.
*/
void orin_can_broadcast_imu(uint32_t now_ms,
const orin_can_fc_imu_t *imu_tlm);
/*
* orin_can_broadcast_baro(now_ms, baro_tlm) — rate-limited broadcast of
* FC_BARO (0x403) at ORIN_BARO_TLM_HZ (1 Hz).
* Pass NULL to skip transmission. Issue #672.
*/
void orin_can_broadcast_baro(uint32_t now_ms,
const orin_can_fc_baro_t *baro_tlm);
/*
* orin_can_send_btn_event(event_id, balance_state) — send FC_BTN (0x404)
* immediately. Call on button park/unpark events. Issue #682.
* event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED.
*/
void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state);
/* orin_can_send_pid_ack() -- send FC_PID_ACK (0x405). Issue #693. */
void orin_can_send_pid_ack(float kp, float ki, float kd);
/* PID_SET (0x305) -- 6-byte payload: kp*100, ki*100, kd*100 (uint16 BE each) */
typedef struct __attribute__((packed)) {
uint16_t kp_x100;
uint16_t ki_x100;
uint16_t kd_x100;
} orin_can_pid_set_t;
/* FC_PID_ACK (0x405) -- FC -> Orin echo of applied gains */
typedef struct __attribute__((packed)) {
uint16_t kp_x100;
uint16_t ki_x100;
uint16_t kd_x100;
} orin_can_fc_pid_ack_t;
#endif /* ORIN_CAN_H */

63
legacy/stm32/include/ota.h
View File

@ -1,63 +0,0 @@
#ifndef OTA_H
#define OTA_H
#include <stdint.h>
#include <stdbool.h>
/*
* OTA firmware update — Issue #124
*
* DFU entry triggered by JLINK_CMD_DFU_ENTER (0x06) or USB CDC 'R' command.
* Uses RTC backup register OTA_DFU_BKP_IDX to pass magic across the soft reset.
*
* RTC BKP register map:
* BKP0R–BKP5R : BNO055 calibration offsets (PR #150)
* BKP6R : BNO055 magic (0xB055CA10, PR #150)
* BKP7R–BKP14R : Reserved
* BKP15R : OTA DFU magic (this module)
*
* Using BKP15R avoids collision with BNO055 (BKP0–6) and the old BKP0R
* that the original request_bootloader() used before this module.
*
* Dual-bank note: STM32F722 has single-bank flash (512 KB). Hardware A/B
* rollback is not supported without a custom bootloader. DFU via the ST
* system bootloader at 0x1FF00000 is the supported update path. Rollback
* is handled by the host-side flash_firmware.py script, which keeps a
* backup of the previous binary.
*/
/* RTC backup register index used for DFU magic — avoids BNO055 BKP0–6 */
#define OTA_DFU_BKP_IDX 15u
/* Magic value written before reset to trigger DFU entry on next boot */
#define OTA_DFU_MAGIC 0xDEADBEEFu
/* STM32F722 internal flash: 512 KB starting at 0x08000000 */
#define OTA_FLASH_BASE 0x08000000u
#define OTA_FLASH_SIZE 0x00080000u /* 512 KB */
/*
* ota_enter_dfu(is_armed)
*
* Request entry to USB DFU mode (ST system bootloader at 0x1FF00000).
* Returns false without side effects if is_armed is true.
* Otherwise: enables backup domain, writes OTA_DFU_MAGIC to BKP15R,
* disables IRQs, calls NVIC_SystemReset(). Never returns on success.
*
* Call from the main loop only (not from ISR context).
*/
bool ota_enter_dfu(bool is_armed);
/*
* ota_fw_crc32()
*
* Compute a CRC-32/MPEG-2 checksum of the full flash region using the
* STM32 hardware CRC peripheral (poly 0x04C11DB7, init 0xFFFFFFFF,
* 32-bit words, no reflection). Covers OTA_FLASH_SIZE bytes from
* OTA_FLASH_BASE including erased padding.
*
* Takes ~0.5 ms at 216 MHz. Call only while disarmed.
*/
uint32_t ota_fw_crc32(void);
#endif /* OTA_H */

95
legacy/stm32/include/pid_flash.h
View File

@ -1,95 +0,0 @@
#ifndef PID_FLASH_H
#define PID_FLASH_H
#include <stdint.h>
#include <stdbool.h>
/*
* pid_flash — persistent PID storage for Issue #531 (auto-tune).
*
* Stores Kp, Ki, Kd in the last 64 bytes of STM32F722 flash sector 7
* (0x0807FFC0). Magic word validates presence of saved params.
* Sector 7 is 128KB starting at 0x08060000; firmware never exceeds sector 6.
*
* Flash writes require an erase of the full sector (128KB) before re-writing.
* The store address is the very last 64-byte block so future expansion can
* grow toward lower addresses within sector 7 without conflict.
*/
#define PID_FLASH_SECTOR FLASH_SECTOR_7
#define PID_FLASH_SECTOR_VOLTAGE VOLTAGE_RANGE_3 /* 2.7V-3.6V, 32-bit parallelism */
/* Sector 7: 128KB at 0x08060000; store in last 64 bytes */
#define PID_FLASH_STORE_ADDR 0x0807FFC0UL
#define PID_FLASH_MAGIC 0x534C5401UL /* 'SLT\x01' — version 1 */
typedef struct __attribute__((packed)) {
uint32_t magic; /* PID_FLASH_MAGIC when valid */
float kp;
float ki;
float kd;
uint8_t _pad[48]; /* padding to 64 bytes */
} pid_flash_t;
/* ---- Gain schedule flash storage (Issue #550) ---- */
/* Maximum number of speed-band entries in the gain schedule table */
#define PID_SCHED_MAX_BANDS 6u
/*
* Sector 7 layout (128KB at 0x08060000):
* 0x0807FF40 pid_sched_flash_t (128 bytes) — gain schedule record
* 0x0807FFC0 pid_flash_t ( 64 bytes) — single PID record (existing)
* Both records are written in a single sector erase via pid_flash_save_all().
*/
#define PID_SCHED_FLASH_ADDR 0x0807FF40UL
#define PID_SCHED_MAGIC 0x534C5402UL /* 'SLT\x02' — version 2 */
typedef struct __attribute__((packed)) {
float speed_mps; /* velocity breakpoint (m/s) */
float kp;
float ki;
float kd;
} pid_sched_entry_t; /* 16 bytes */
typedef struct __attribute__((packed)) {
uint32_t magic; /* PID_SCHED_MAGIC when valid */
uint8_t num_bands; /* valid entries (1..PID_SCHED_MAX_BANDS) */
uint8_t flags; /* reserved, must be 0 */
uint8_t _pad0[2];
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* 6 × 16 = 96 bytes */
uint8_t _pad1[24]; /* total = 4+1+1+2+96+24 = 128 bytes */
} pid_sched_flash_t; /* 128 bytes */
/*
* pid_flash_load() — read saved PID from flash.
* Returns true and fills *kp/*ki/*kd if magic is valid.
* Returns false if no valid params stored (caller keeps defaults).
*/
bool pid_flash_load(float *kp, float *ki, float *kd);
/*
* pid_flash_save() — erase sector 7 and write Kp/Ki/Kd (single-PID only).
* Use pid_flash_save_all() to save both single-PID and schedule atomically.
* Must not be called while armed (flash erase takes ~1s and stalls the CPU).
* Returns true on success.
*/
bool pid_flash_save(float kp, float ki, float kd);
/*
* pid_flash_load_schedule() — read gain schedule from flash.
* Returns true and fills out_entries[0..n-1] and *out_n if magic is valid.
* Returns false if no valid schedule stored.
*/
bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n);
/*
* pid_flash_save_all() — erase sector 7 once and atomically write both:
* - pid_sched_flash_t at PID_SCHED_FLASH_ADDR (0x0807FF40)
* - pid_flash_t at PID_FLASH_STORE_ADDR (0x0807FFC0)
* Must not be called while armed. Returns true on success.
*/
bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
const pid_sched_entry_t *entries, uint8_t num_bands);
#endif /* PID_FLASH_H */

122
legacy/stm32/include/pid_schedule.h
View File

@ -1,122 +0,0 @@
/*
* pid_schedule.h — Speed-dependent PID gain scheduling (Issue #550)
*
* Maps robot velocity to PID gain triplets (Kp, Ki, Kd) using a lookup
* table with linear interpolation between adjacent entries. The table
* supports 1–PID_SCHED_MAX_BANDS entries, each associating a velocity
* breakpoint (m/s) with gains that apply AT that velocity.
*
* HOW IT WORKS:
* 1. Each entry in the table defines: {speed_mps, kp, ki, kd}.
* The table is sorted by speed_mps ascending (pid_schedule_set_table
* sorts automatically).
*
* 2. pid_schedule_get_gains(speed_mps, ...) finds the two adjacent entries
* that bracket the query speed and linearly interpolates:
* t = (speed - bands[i-1].speed_mps) /
* (bands[i].speed_mps - bands[i-1].speed_mps)
* kp = bands[i-1].kp + t * (bands[i].kp - bands[i-1].kp)
* Speeds below the first entry or above the last entry clamp to the
* nearest endpoint (no extrapolation).
* The query speed is ABS(motor_speed) — scheduling is symmetric.
*
* 3. Default 3-entry table (loaded when flash has no valid schedule):
* Band 0: speed=0.00 m/s kp=40.0 ki=1.5 kd=1.2 (stopped — tight)
* Band 1: speed=0.30 m/s kp=35.0 ki=1.0 kd=1.0 (slow — balanced)
* Band 2: speed=0.80 m/s kp=28.0 ki=0.5 kd=0.8 (fast — relaxed)
*
* 4. pid_schedule_apply(balance, speed_mps) interpolates and writes the
* result directly into balance->kp/ki/kd. Call from the main loop at
* the same rate as the balance PID update (1 kHz) or slower (100 Hz
* for scheduling, 1 kHz for PID execution — gains change slowly enough).
*
* 5. Flash persistence: pid_schedule_flash_save() calls pid_flash_save_all()
* which erases sector 7 once and writes both the single-PID record at
* PID_FLASH_STORE_ADDR and the schedule at PID_SCHED_FLASH_ADDR.
*
* 6. JLINK interface (Issue #550):
* 0x0C SCHED_GET — no payload; triggers TLM_SCHED response
* 0x0D SCHED_SET — upload new table (num_bands + N×16-byte entries)
* 0x0E SCHED_SAVE — save current table + single PID to flash
* 0x85 TLM_SCHED — table dump response to SCHED_GET
*/
#ifndef PID_SCHEDULE_H
#define PID_SCHEDULE_H
#include <stdint.h>
#include <stdbool.h>
#include "pid_flash.h" /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */
#include "balance.h" /* balance_t */
/* ---- Default gain table ---- */
/* Motor ESC range is ±1000 counts; 1000 counts ≈ full drive.
* Speed scale: MOTOR_CMD_MAX=1000 → ~0.8 m/s max tangential velocity.
* Adjust PID_SCHED_SPEED_SCALE if odometry calibration changes this. */
#define PID_SCHED_SPEED_SCALE 0.0008f /* motor_cmd counts → m/s: 1000 × 0.0008 = 0.8 m/s */
/* ---- API ---- */
/*
* pid_schedule_init() — load table from flash (via pid_flash_load_schedule).
* Falls back to the built-in 3-band default if flash is empty or invalid.
* Call once after flash init during system startup.
*/
void pid_schedule_init(void);
/*
* pid_schedule_get_gains(speed_mps, *kp, *ki, *kd) — interpolate gains.
* |speed_mps| is used (scheduling is symmetric for forward/reverse).
* Clamps to table endpoints; does not extrapolate outside the table range.
*/
void pid_schedule_get_gains(float speed_mps, float *kp, float *ki, float *kd);
/*
* pid_schedule_apply(b, speed_mps) — compute interpolated gains and write
* them into b->kp, b->ki, b->kd. b->integral is reset to 0 when the
* active band changes to avoid integrator windup on transitions.
*/
void pid_schedule_apply(balance_t *b, float speed_mps);
/*
* pid_schedule_set_table(entries, n) — replace the active gain table.
* Entries are copied and sorted by speed_mps ascending.
* n is clamped to [1, PID_SCHED_MAX_BANDS].
* Does NOT automatically save to flash — call pid_schedule_flash_save().
*/
void pid_schedule_set_table(const pid_sched_entry_t *entries, uint8_t n);
/*
* pid_schedule_get_table(out_entries, out_n) — copy current table out.
* out_entries must have room for PID_SCHED_MAX_BANDS entries.
*/
void pid_schedule_get_table(pid_sched_entry_t *out_entries, uint8_t *out_n);
/*
* pid_schedule_get_num_bands() — return current number of table entries.
*/
uint8_t pid_schedule_get_num_bands(void);
/*
* pid_schedule_flash_save(kp_single, ki_single, kd_single) — save the
* current schedule table PLUS the caller-supplied single-PID values to
* flash in one atomic sector erase (pid_flash_save_all).
* Must NOT be called while armed (sector erase takes ~1s).
* Returns true on success.
*/
bool pid_schedule_flash_save(float kp_single, float ki_single, float kd_single);
/*
* pid_schedule_active_band_idx() — index (0-based) of the lower bracket
* entry used in the most recent interpolation. Useful for telemetry.
* Returns 0 if speed is below the first entry.
*/
uint8_t pid_schedule_active_band_idx(void);
/*
* pid_schedule_get_default_table(out_entries, out_n) — fill the 3-band
* default table into caller's buffer. Used for factory-reset.
*/
void pid_schedule_get_default_table(pid_sched_entry_t *out_entries, uint8_t *out_n);
#endif /* PID_SCHEDULE_H */

103
legacy/stm32/include/power_mgmt.h
View File

@ -1,103 +0,0 @@
#ifndef POWER_MGMT_H
#define POWER_MGMT_H
#include <stdint.h>
#include <stdbool.h>
/*
* power_mgmt — STM32F7 STOP-mode sleep/wake manager (Issue #178).
*
* State machine:
* PM_ACTIVE ──(idle ≥ PM_IDLE_TIMEOUT_MS or sleep cmd)──► PM_SLEEP_PENDING
* PM_SLEEP_PENDING ──(fade complete, ≥ PM_FADE_MS)──► PM_SLEEPING (WFI)
* PM_SLEEPING ──(EXTI wake)──► PM_WAKING ──(clocks restored)──► PM_ACTIVE
*
* Any call to power_mgmt_activity() during SLEEP_PENDING or SLEEPING
* immediately transitions back toward PM_ACTIVE.
*
* Wake sources (EXTI, falling edge on UART idle-high RX pin or IMU INT):
* EXTI1 PA1 UART4_RX — CRSF/ELRS start bit
* EXTI7 PB7 USART1_RX — JLink start bit
* EXTI4 PC4 MPU6000 INT — IMU motion (handler owned by mpu6000.c)
*
* Peripheral gating on sleep entry (clock disable, state preserved):
* Disabled: SPI3/I2S3 (audio amp), SPI2 (OSD), USART6, UART5 (debug)
* Active: SPI1 (IMU), UART4 (CRSF), USART1 (JLink), I2C1 (baro/mag)
*
* Sleep LED (LED1, active-low PC15):
* PM_SLEEP_PENDING: triangle-wave pulse, period PM_LED_PERIOD_MS
* All other states: 0 (caller uses normal LED logic)
*
* IWDG:
* Fed immediately before WFI. STOP wakeup <10 ms typical — well within
* WATCHDOG_TIMEOUT_MS (50 ms).
*
* Safety interlock:
* Caller MUST NOT call power_mgmt_tick() while armed; call
* power_mgmt_activity() instead to keep the idle timer reset.
*
* JLink integration:
* JLINK_CMD_SLEEP (0x09) → power_mgmt_request_sleep()
* Any valid JLink frame → power_mgmt_activity() (handled in main loop)
*/
typedef enum {
PM_ACTIVE = 0, /* Normal, all peripherals running */
PM_SLEEP_PENDING = 1, /* Idle timeout reached; LED fade-out in progress */
PM_SLEEPING = 2, /* In STOP mode (WFI); execution blocked in tick() */
PM_WAKING = 3, /* Transitional; clocks/peripherals being restored */
} PowerState;
/* ---- API ---- */
/*
* power_mgmt_init() — configure wake EXTI lines (EXTI1, EXTI7).
* Call after crsf_init() and jlink_init().
*/
void power_mgmt_init(void);
/*
* power_mgmt_activity() — record cmd_vel event (CRSF frame, JLink frame).
* Resets idle timer; aborts any pending/active sleep.
*/
void power_mgmt_activity(void);
/*
* power_mgmt_request_sleep() — force sleep regardless of idle timer
* (called on JLINK_CMD_SLEEP). Next tick() enters PM_SLEEP_PENDING.
*/
void power_mgmt_request_sleep(void);
/*
* power_mgmt_tick(now_ms) — drive state machine. May block in WFI during
* STOP mode. Returns state after this tick.
* MUST NOT be called while balance_state == BALANCE_ARMED.
*/
PowerState power_mgmt_tick(uint32_t now_ms);
/* power_mgmt_state() — non-blocking read of current state. */
PowerState power_mgmt_state(void);
/*
* power_mgmt_led_brightness() — 0-255 brightness for sleep-pending pulse.
* Returns 0 when not in PM_SLEEP_PENDING; caller uses normal LED logic.
*/
uint8_t power_mgmt_led_brightness(void);
/*
* power_mgmt_current_ma() — estimated total current draw (mA) based on
* gating state; populated in JLINK_TLM_POWER telemetry.
*/
uint16_t power_mgmt_current_ma(void);
/* power_mgmt_idle_ms() — ms elapsed since last power_mgmt_activity() call. */
uint32_t power_mgmt_idle_ms(void);
/*
* power_mgmt_notify_battery(vbat_mv) — Issue #467 integration.
* Called by battery_adc_check_pm() after BATTERY_ADC_LOW_HOLD_MS of sustained
* critical voltage. Forces PM_SLEEP_PENDING to protect the LiPo.
*/
void power_mgmt_notify_battery(uint32_t vbat_mv);
#endif /* POWER_MGMT_H */

124
legacy/stm32/include/rgb_fsm.h
View File

@ -1,124 +0,0 @@
#ifndef RGB_FSM_H
#define RGB_FSM_H
#include <stdint.h>
#include <stdbool.h>
/*
* rgb_fsm.h — RGB Status LED State Machine (Issue #290)
*
* Manages an 8-LED WS2812 NeoPixel ring with 8 operational states.
* Each state has a specific color pattern and animation.
*
* States:
* BOOT — Blue pulse (startup sequence, 0.5 Hz)
* IDLE — Green breathe (standby, smooth 0.5 Hz pulse)
* ARMED — Solid green (ready to move)
* NAV — Cyan spin (autonomous navigation active, rotating pattern)
* ERROR — Red flash (fault detected, 2 Hz blink)
* LOW_BATT — Orange blink (battery low, 1 Hz blink)
* CHARGING — Green fill (charging, progressive LEDs filling)
* ESTOP — Red solid (emergency stop, full red, no animation)
*
* Transitions via UART command from Jetson.
* Non-blocking operation with tick-based timing.
*/
/* LED State Machine States */
typedef enum {
LED_STATE_BOOT = 0,
LED_STATE_IDLE,
LED_STATE_ARMED,
LED_STATE_NAV,
LED_STATE_ERROR,
LED_STATE_LOW_BATT,
LED_STATE_CHARGING,
LED_STATE_ESTOP,
LED_STATE_COUNT
} LedState;
/* RGB Color (8-bit per channel) */
typedef struct {
uint8_t r; /* Red (0-255) */
uint8_t g; /* Green (0-255) */
uint8_t b; /* Blue (0-255) */
} RgbColor;
/*
* rgb_fsm_init()
*
* Initialize LED state machine:
* - PB4 as TIM3_CH1 PWM output for WS2812 driver
* - Configure TIM3 for 800 kHz PWM frequency
* - Set initial state to BOOT
* - Initialize all LEDs to off
*/
void rgb_fsm_init(void);
/*
* rgb_fsm_set_state(state)
*
* Transition to a new LED state immediately.
* Resets animation timing for the new state.
*
* Arguments:
* - state: Target LED state (LedState enum)
*
* Returns: true if state changed, false if already in that state
*/
bool rgb_fsm_set_state(LedState state);
/*
* rgb_fsm_get_state()
*
* Get current LED state.
*
* Returns: Current LED state (LedState enum)
*/
LedState rgb_fsm_get_state(void);
/*
* rgb_fsm_tick(now_ms)
*
* Update function called periodically (recommended: every 10-50ms).
* Processes animations and timing for current state.
* Updates LED strip via PWM.
*
* Arguments:
* - now_ms: Current time in milliseconds (from HAL_GetTick() or similar)
*/
void rgb_fsm_tick(uint32_t now_ms);
/*
* rgb_fsm_set_color(led_index, color)
*
* Set color of a specific LED (for testing and manual control).
* Bypasses current animation.
*
* Arguments:
* - led_index: 0-7 (LED ring has 8 LEDs)
* - color: RgbColor with R, G, B values (0-255)
*
* Returns: true if set, false if index out of range
*/
bool rgb_fsm_set_color(uint8_t led_index, RgbColor color);
/*
* rgb_fsm_all_off()
*
* Turn off all LEDs immediately.
* Useful for shutdown or error conditions.
*/
void rgb_fsm_all_off(void);
/*
* rgb_fsm_get_animation_frame()
*
* Get current animation progress (0-255).
* Useful for testing and debugging animation timing.
*
* Returns: Current frame value for animation (0-255 represents full cycle)
*/
uint8_t rgb_fsm_get_animation_frame(void);
#endif /* RGB_FSM_H */

65
legacy/stm32/include/safety.h
View File

@ -1,65 +0,0 @@
#ifndef SAFETY_H
#define SAFETY_H
#include <stdint.h>
#include <stdbool.h>
/*
* SaltyLab Safety Systems
*
* Covers:
* - IWDG hardware watchdog (MCU reset if main loop hangs)
* - RC signal timeout monitoring
* - Tilt fault alert via buzzer
* - Arm hold interlock (must hold arm for ARMING_HOLD_MS)
* - Remote e-stop over 4G MQTT (CDC 'E'/'F'/'Z' commands)
*/
typedef enum {
ESTOP_CLEAR = 0,
ESTOP_TILT = 1,
ESTOP_RC_KILL = 2,
ESTOP_REMOTE = 3,
ESTOP_CELLULAR_TIMEOUT = 4,
} EstopSource;
/*
* safety_init() — call once in main() after HAL_Init().
* Starts IWDG with WATCHDOG_TIMEOUT_MS timeout from config.h.
* Starts ARMING_HOLD_MS countdown from config.h.
*/
void safety_init(void);
/*
* safety_refresh() — call every main loop iteration.
* Resets IWDG counter. If not called within WATCHDOG_TIMEOUT_MS,
* the MCU will reset (independent of software — cannot be disabled).
*/
void safety_refresh(void);
/*
* safety_rc_alive() — returns true if RC receiver has sent a frame
* within RC_TIMEOUT_MS. Call from the balance loop.
*/
bool safety_rc_alive(uint32_t now);
/*
* safety_alert_tilt_fault() — one-shot buzzer beep for tilt fault.
* Safe to call repeatedly; only fires once per fault.
*/
void safety_alert_tilt_fault(bool faulted);
/*
* safety_arm_interlock() — returns true once arm button has been
* held for ARMING_HOLD_MS from the moment safety_arm_start() was called.
*/
void safety_arm_start(uint32_t now); /* Call when arm requested */
bool safety_arm_ready(uint32_t now); /* Poll until true, then arm */
void safety_arm_cancel(void); /* Cancel pending arm */
void safety_remote_estop(EstopSource src);
void safety_remote_estop_clear(void);
EstopSource safety_get_estop(void);
bool safety_remote_estop_active(void);
#endif /* SAFETY_H */

114
legacy/stm32/include/servo.h
View File

@ -1,114 +0,0 @@
#ifndef SERVO_H
#define SERVO_H
#include <stdint.h>
#include <stdbool.h>
/*
* servo.h — Pan-tilt servo driver for camera head (Issue #206)
*
* Hardware: TIM4 PWM at 50 Hz (20 ms period)
* - CH1 (PB6): Pan servo (0-180°)
* - CH2 (PB7): Tilt servo (0-180°)
*
* Servo pulse mapping:
* - 500 µs → 0° (full left/down)
* - 1500 µs → 90° (center)
* - 2500 µs → 180° (full right/up)
*
* Smooth sweeping via servo_sweep() for camera motion.
*/
/* Servo channels */
typedef enum {
SERVO_PAN = 0, /* CH1 (PB6) */
SERVO_TILT = 1, /* CH2 (PB7) */
SERVO_COUNT
} ServoChannel;
/* Servo state */
typedef struct {
uint16_t current_angle_deg[SERVO_COUNT]; /* Current angle in degrees (0-180) */
uint16_t target_angle_deg[SERVO_COUNT]; /* Target angle in degrees */
uint16_t pulse_us[SERVO_COUNT]; /* Pulse width in microseconds (500-2500) */
/* Sweep state (per-servo) */
uint32_t sweep_start_ms[SERVO_COUNT];
uint32_t sweep_duration_ms[SERVO_COUNT];
uint16_t sweep_start_deg[SERVO_COUNT];
uint16_t sweep_end_deg[SERVO_COUNT];
bool is_sweeping[SERVO_COUNT];
} ServoState;
/*
* servo_init()
*
* Initialize TIM4 PWM on PB6 (CH1, pan) and PB7 (CH2, tilt) at 50 Hz.
* Centers both servos at 90° (1500 µs). Call once at startup.
*/
void servo_init(void);
/*
* servo_set_angle(channel, degrees)
*
* Set target angle for a servo (0-180°).
* Immediately updates PWM without motion ramping.
* Valid channels: SERVO_PAN, SERVO_TILT
*
* Examples:
* servo_set_angle(SERVO_PAN, 0); // Pan left
* servo_set_angle(SERVO_PAN, 90); // Pan center
* servo_set_angle(SERVO_TILT, 180); // Tilt up
*/
void servo_set_angle(ServoChannel channel, uint16_t degrees);
/*
* servo_get_angle(channel)
*
* Return current servo angle in degrees (0-180).
*/
uint16_t servo_get_angle(ServoChannel channel);
/*
* servo_set_pulse_us(channel, pulse_us)
*
* Set servo pulse width directly in microseconds (500-2500).
* Used for fine-tuning or direct control.
*/
void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us);
/*
* servo_sweep(channel, start_deg, end_deg, duration_ms)
*
* Smooth linear sweep from start to end angle over duration_ms.
* Non-blocking: must call servo_tick() every ~10 ms to update PWM.
*
* Examples:
* servo_sweep(SERVO_PAN, 0, 180, 2000); // Pan left-to-right in 2 seconds
* servo_sweep(SERVO_TILT, 45, 135, 1000); // Tilt up-down in 1 second
*/
void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms);
/*
* servo_tick(now_ms)
*
* Update servo sweep animation (if active). Call every ~10 ms from main loop.
* No-op if not currently sweeping.
*/
void servo_tick(uint32_t now_ms);
/*
* servo_is_sweeping()
*
* Returns true if any servo is currently sweeping.
*/
bool servo_is_sweeping(void);
/*
* servo_stop_sweep(channel)
*
* Stop sweep immediately, hold current position.
*/
void servo_stop_sweep(ServoChannel channel);
#endif /* SERVO_H */

97
legacy/stm32/include/servo_bus.h
View File

@ -1,97 +0,0 @@
#ifndef SERVO_BUS_H
#define SERVO_BUS_H
#include <stdint.h>
#include <stdbool.h>
/*
* servo_bus.h — Feetech/ST3215 serial bus servo driver (Issue #547)
*
* Half-duplex single-wire UART protocol at 1 Mbps.
* Hardware: USART3 half-duplex on PB10 (USART3_TX, AF7).
* No separate RX pin — the TX line is bidirectional with HDSEL.
*
* Packet format (host to servo):
* [0xFF][0xFF][ID][LEN][INSTR][PARAMS...][CKSUM]
* CKSUM = ~(ID + LEN + INSTR + PARAMS) & 0xFF
*
* Response (servo to host):
* [0xFF][0xFF][ID][LEN][ERROR][DATA...][CKSUM]
*
* Key ST3215 registers:
* 0x28 Torque Enable (1=on, 0=off)
* 0x2A Goal Position L (0-4095)
* 0x2B Goal Position H
* 0x2E Moving Speed L (0=max, 1-4095)
* 0x2F Moving Speed H
* 0x38 Present Position L (0-4095)
* 0x39 Present Position H
* 0x3A Present Speed L (sign+magnitude: bit15=dir)
* 0x3B Present Speed H
*
* Position encoding: 0-4095 maps to 0-360 degrees.
* Center (180 deg) = 2048 raw.
*/
/* ST3215 register addresses */
#define SB_REG_TORQUE_EN 0x28u
#define SB_REG_GOAL_POS_L 0x2Au
#define SB_REG_MOVING_SPD_L 0x2Eu
#define SB_REG_PRES_POS_L 0x38u
#define SB_REG_PRES_SPD_L 0x3Au
/* ST3215 instructions */
#define SB_INSTR_PING 0x01u
#define SB_INSTR_READ 0x02u
#define SB_INSTR_WRITE 0x03u
/* Position encoding */
#define SB_POS_CENTER 2048u /* 180 deg */
#define SB_POS_MAX 4095u /* 360 deg */
#define SB_SPEED_MAX 4095u /* counts/sec (0 = max speed) */
/* Timeout for servo response (ms) */
#define SB_RX_TIMEOUT_MS 5u
/*
* servo_bus_init() - configure USART3 in half-duplex mode (PB10, AF7) at
* SB_BAUD (1 Mbps, 8N1). Call once at startup before gimbal_init().
*/
void servo_bus_init(void);
/*
* servo_bus_write_pos(id, raw_pos, speed) - write goal position and moving
* speed in a single WRITE DATA packet. raw_pos: 0-4095. speed: 0=max, 1-4095.
* Returns true on successful TX (no response expected on write).
*/
bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed);
/*
* servo_bus_write_torque(id, enable) - enable or disable servo torque.
*/
bool servo_bus_write_torque(uint8_t id, bool enable);
/*
* servo_bus_read_pos(id, raw_pos) - read present position.
* Returns true on success; raw_pos is 0-4095.
*/
bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos);
/*
* servo_bus_read_speed(id, speed) - read present speed.
* speed bit 15 is direction. Returns magnitude in lower 15 bits.
*/
bool servo_bus_read_speed(uint8_t id, uint16_t *speed);
/*
* servo_bus_deg_to_raw(deg) - convert degree (-180..+180) to raw position.
* Center (0 deg) = 2048. Clamps to 0-4095.
*/
uint16_t servo_bus_deg_to_raw(float deg);
/*
* servo_bus_raw_to_deg(raw) - convert raw position (0-4095) to degree (-180..+180).
*/
float servo_bus_raw_to_deg(uint16_t raw);
#endif /* SERVO_BUS_H */

101
legacy/stm32/include/slope_estimator.h
View File

@ -1,101 +0,0 @@
#ifndef SLOPE_ESTIMATOR_H
#define SLOPE_ESTIMATOR_H
#include <stdint.h>
#include <stdbool.h>
/*
* slope_estimator — slow-adapting terrain slope estimator for Issue #600.
*
* On a slope the robot must lean slightly into the hill to stay balanced.
* The IMU pitch reading therefore includes both the robot's balance offset
* and the ground incline. This module decouples the two by tracking the
* slowly-changing DC component of the pitch signal with a first-order IIR
* low-pass filter (time constant SLOPE_TAU_S = 5 s).
*
* HOW IT WORKS:
* Every call to slope_estimator_update(pitch_deg, dt):
*
* alpha = dt / (SLOPE_TAU_S + dt) // ≈ 0.0002 at 1 kHz
* raw = slope * (1 - alpha) + pitch * alpha
* slope = clamp(raw, -SLOPE_MAX_DEG, +SLOPE_MAX_DEG)
*
* The IIR converges to the steady-state pitch in ~5 s. Fast tilt
* transients (balance corrections, steps, bumps) are attenuated by
* the long time constant and do not corrupt the estimate.
*
* INTEGRATION IN BALANCE PID:
* Subtract slope_estimate from the measured pitch before computing
* the PID error so the controller balances around the slope surface
* rather than absolute vertical:
*
* tilt_corrected = pitch_deg - slope_estimate_deg
* error = setpoint - tilt_corrected
*
* This is equivalent to continuously adjusting the balance setpoint
* to track the incline.
*
* SAFETY:
* - Estimate is clamped to ±SLOPE_MAX_DEG (15°) to prevent drift from
* extreme falls being mistaken for genuine slopes.
* - slope_estimator_reset() zeroes the state; call on disarm or after
* a tilt fault so re-arming starts fresh.
*
* TELEMETRY:
* JLINK_TLM_SLOPE (0x88) published at SLOPE_TLM_HZ (1 Hz):
* jlink_tlm_slope_t { int16 slope_x100, uint8 active, uint8 _pad }
*/
/* ---- Configuration ---- */
#define SLOPE_TAU_S 5.0f /* IIR time constant (seconds) */
#define SLOPE_MAX_DEG 15.0f /* Maximum estimate magnitude (degrees) */
#define SLOPE_TLM_HZ 1u /* JLINK_TLM_SLOPE publish rate (Hz) */
/* ---- State ---- */
typedef struct {
float estimate_deg; /* current slope estimate (degrees, + = nose-up) */
bool enabled; /* compensation on/off; off = estimate frozen */
uint32_t last_tlm_ms; /* timestamp of last TLM transmission */
} slope_estimator_t;
/* ---- API ---- */
/*
* slope_estimator_init(se) — zero state, enable estimation.
* Call once during system init.
*/
void slope_estimator_init(slope_estimator_t *se);
/*
* slope_estimator_reset(se) — zero estimate without changing enabled flag.
* Call on disarm or after BALANCE_TILT_FAULT to avoid stale state on rearm.
*/
void slope_estimator_reset(slope_estimator_t *se);
/*
* slope_estimator_update(se, pitch_deg, dt) — advance the IIR filter.
* pitch_deg : current fused pitch angle from IMU (degrees)
* dt : loop interval (seconds)
* No-op if se->enabled == false or dt <= 0.
*/
void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt);
/*
* slope_estimator_get_deg(se) — return current estimate (degrees).
* Returns 0 if disabled.
*/
float slope_estimator_get_deg(const slope_estimator_t *se);
/*
* slope_estimator_set_enabled(se, en) — enable or disable compensation.
* Disabling freezes the estimate at its current value.
*/
void slope_estimator_set_enabled(slope_estimator_t *se, bool en);
/*
* slope_estimator_send_tlm(se, now_ms) — transmit JLINK_TLM_SLOPE (0x88)
* frame to Jetson. Rate-limited to SLOPE_TLM_HZ; safe to call every tick.
*/
void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms);
#endif /* SLOPE_ESTIMATOR_H */

20
legacy/stm32/include/status.h
View File

@ -1,20 +0,0 @@
#ifndef STATUS_H
#define STATUS_H
#include <stdint.h>
void status_init(void);
void status_boot_beep(void);
/*
* status_update() — call every main loop iteration.
* Controls LED1 (PC15) and LED2 (PC14), both active-low.
*
* Solid ON = good (normal operation)
* Slow blink (~1 Hz) = needs attention (error or fault)
*
* LED1 solid + LED2 off → disarmed, IMU OK
* LED1 solid + LED2 solid → armed
* Both slow blink → tilt fault
* Both fast blink (200ms) -- remote e-stop active (highest priority)
* LED1 slow blink + LED2 solid → IMU error (solid LED2 = always-on indicator)
*/
void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault, int remote_estop);
#endif

134
legacy/stm32/include/steering_pid.h
View File

@ -1,134 +0,0 @@
#ifndef STEERING_PID_H
#define STEERING_PID_H
#include <stdint.h>
#include <stdbool.h>
/*
* steering_pid — closed-loop yaw-rate controller for differential drive
* (Issue #616).
*
* OVERVIEW:
* Converts a desired yaw rate (from Jetson Twist.angular.z) into a
* differential wheel speed offset. The balance PID remains the primary
* controller; the steering PID generates a small differential term that
* is added to the balance command inside motor_driver:
*
* left_speed = balance_cmd - steer_out
* right_speed = balance_cmd + steer_out
*
* This is the standard differential-drive mixing already performed by
* the ESC backend (hoverboard/VESC).
*
* INPUT SIGNALS:
* target_omega_dps : desired yaw rate in deg/s (+ = clockwise from above)
* Derived from JLINK_CMD_DRIVE steer field:
* target_omega_dps = steer * STEER_OMEGA_SCALE
* (steer is int16 -1000..+1000 from Jetson)
* actual_omega_dps : measured yaw rate from IMU gyro Z (deg/s)
* = IMUData.yaw_rate
*
* PID ALGORITHM:
* error = target_omega - actual_omega
* integral = clamp(integral + error*dt, ±STEER_INTEGRAL_MAX)
* raw_out = Kp*error + Ki*integral + Kd*(error - prev_error)/dt
* raw_out = clamp(raw_out, ±STEER_OUTPUT_MAX)
* output = rate_limit(raw_out, STEER_RAMP_RATE_PER_MS * dt_ms)
*
* ANTI-WINDUP:
* Integral is clamped to ±STEER_INTEGRAL_MAX counts before the Ki
* multiply, bounding the integrator contribution independently of Kp/Kd.
*
* RATE LIMITER:
* Output changes at most STEER_RAMP_RATE_PER_MS counts per millisecond.
* Prevents a sudden step in steering demand from disturbing the balance
* PID (which has no knowledge of the steering channel).
*
* OMEGA SCALING:
* STEER_OMEGA_SCALE = 0.1 deg/s per steer unit.
* Range: steer -1000..+1000 → omega -100..+100 deg/s.
* 100 deg/s ≈ 1.75 rad/s — covers aggressive turns without exceeding
* the hoverboard ESC differential authority (STEER_OUTPUT_MAX = 400).
*
* DISABLING:
* steering_pid_set_enabled(s, false) zeroes target_omega and integral,
* then freezes output at 0. Use when Jetson is not active or in
* RC_MANUAL mode to avoid fighting the RC steer channel.
*
* TELEMETRY:
* JLINK_TLM_STEERING (0x8A) published at STEER_TLM_HZ (10 Hz):
* jlink_tlm_steering_t { int16 target_x10, int16 actual_x10,
* int16 output, uint8 enabled, uint8 _pad }
* 8 bytes total.
*/
/* ---- Configuration ---- */
#define STEER_KP 2.0f /* proportional gain (counts / (deg/s)) */
#define STEER_KI 0.5f /* integral gain (counts / (deg)) */
#define STEER_KD 0.05f /* derivative gain (counts / (deg/s²)) */
#define STEER_INTEGRAL_MAX 200.0f /* integrator clamp (motor counts) */
#define STEER_OUTPUT_MAX 400 /* peak differential output (counts) */
#define STEER_RAMP_RATE_PER_MS 10 /* max output change per ms (counts/ms) */
#define STEER_OMEGA_SCALE 0.1f /* steer units → deg/s (0.1 deg/s/unit) */
#define STEER_TLM_HZ 10u /* JLINK_TLM_STEERING publish rate (Hz) */
/* ---- State ---- */
typedef struct {
float target_omega_dps; /* setpoint: desired yaw rate (deg/s) */
float actual_omega_dps; /* feedback: measured yaw rate (deg/s) */
float integral; /* PID integrator (motor counts·s) */
float prev_error; /* error at last update (deg/s) */
int16_t output; /* rate-limited differential output */
bool enabled; /* false = output held at 0 */
uint32_t last_tlm_ms; /* rate-limit for TLM */
} steering_pid_t;
/* ---- API ---- */
/*
* steering_pid_init(s) — zero state, enable controller.
* Call once during system init.
*/
void steering_pid_init(steering_pid_t *s);
/*
* steering_pid_reset(s) — zero integrator, setpoint and output.
* Preserves enabled flag. Call on disarm.
*/
void steering_pid_reset(steering_pid_t *s);
/*
* steering_pid_set_target(s, omega_dps) — update setpoint.
* omega_dps : desired yaw rate in deg/s.
* Converts from JLINK_CMD_DRIVE steer field: omega = steer * STEER_OMEGA_SCALE.
* No-op if disabled (output remains 0).
*/
void steering_pid_set_target(steering_pid_t *s, float omega_dps);
/*
* steering_pid_update(s, actual_omega_dps, dt) — advance PID one step.
* actual_omega_dps : IMU gyro Z rate (deg/s) — use IMUData.yaw_rate.
* dt : loop interval (seconds).
* Returns differential output (-STEER_OUTPUT_MAX..+STEER_OUTPUT_MAX).
* Returns 0 if disabled or dt <= 0.
*/
int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt);
/*
* steering_pid_get_output(s) — last computed differential output.
*/
int16_t steering_pid_get_output(const steering_pid_t *s);
/*
* steering_pid_set_enabled(s, en) — enable or disable the controller.
* Disabling resets integrator and zeroes output.
*/
void steering_pid_set_enabled(steering_pid_t *s, bool en);
/*
* steering_pid_send_tlm(s, now_ms) — transmit JLINK_TLM_STEERING (0x8A)
* frame to Jetson. Rate-limited to STEER_TLM_HZ; safe to call every tick.
*/
void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms);
#endif /* STEERING_PID_H */

96
legacy/stm32/include/uart_protocol.h
View File

@ -1,96 +0,0 @@
#ifndef UART_PROTOCOL_H
#define UART_PROTOCOL_H
/*
* uart_protocol.h — UART command protocol for Jetson-STM32 communication (Issue #629)
*
* Frame format:
* [STX][LEN][CMD][PAYLOAD...][CRC8][ETX]
* 0x02 1B 1B 0-12 B 1B 0x03
*
* CRC8-SMBUS: poly=0x07, init=0x00, computed over CMD+PAYLOAD bytes.
*
* Physical layer: UART5 (PC12=TX / PD2=RX), GPIO_AF8_UART5, 115200 baud, no hw flow.
* NOTE: Spec requested USART1 @ 115200, but USART1 is occupied by JLink @ 921600.
* Implemented on UART5 instead; Jetson must connect to PC12/PD2.
*
* DMA: DMA1_Stream0_Channel4, circular 256-byte ring buffer.
* Heartbeat: if no frame received in UART_PROT_HB_TIMEOUT_MS (500 ms), Jetson is
* considered lost; caller must handle estop if needed.
*/
#include <stdint.h>
#include <stdbool.h>
/* ── Frame delimiters ─────────────────────────────────────────────────────── */
#define UPROT_STX 0x02u
#define UPROT_ETX 0x03u
/* ── Command IDs (host → STM32) ───────────────────────────────────────────── */
#define UCMD_SET_VELOCITY 0x01u /* payload: int16 left_rpm, int16 right_rpm (4 B) */
#define UCMD_GET_STATUS 0x02u /* payload: none */
#define UCMD_SET_PID 0x03u /* payload: float kp, float ki, float kd (12 B) */
#define UCMD_ESTOP 0x04u /* payload: none */
#define UCMD_CLEAR_ESTOP 0x05u /* payload: none */
/* ── Response IDs (STM32 → host) ──────────────────────────────────────────── */
#define URESP_ACK 0x80u /* payload: 1 B — echoed CMD */
#define URESP_NACK 0x81u /* payload: 2 B — CMD, error_code */
#define URESP_STATUS 0x82u /* payload: sizeof(uart_prot_status_t) = 8 B */
/* ── NACK error codes ─────────────────────────────────────────────────────── */
#define UERR_BAD_CRC 0x01u
#define UERR_BAD_LEN 0x02u
#define UERR_BAD_ETX 0x03u
#define UERR_ESTOP 0x04u /* command rejected — estop active */
#define UERR_DISARMED 0x05u /* velocity rejected — not armed */
/* ── STATUS payload (URESP_STATUS, 8 bytes packed) ───────────────────────── */
typedef struct __attribute__((packed)) {
int16_t pitch_x10; /* pitch angle ×10 deg (balance controller) */
int16_t motor_cmd; /* ESC motor command -1000..+1000 */
uint16_t vbat_mv; /* battery voltage in mV */
uint8_t balance_state; /* BalanceState enum (0=DISARMED, 1=ARMED, …) */
uint8_t estop_active; /* non-zero if remote estop is latched */
} uart_prot_status_t;
/* ── Shared state (read by main.c) ────────────────────────────────────────── */
typedef struct {
volatile uint8_t vel_updated; /* 1 when SET_VELOCITY received */
volatile int16_t left_rpm;
volatile int16_t right_rpm;
volatile uint8_t pid_updated; /* 1 when SET_PID received */
volatile float pid_kp;
volatile float pid_ki;
volatile float pid_kd;
volatile uint8_t estop_req; /* 1 on UCMD_ESTOP */
volatile uint8_t estop_clear_req; /* 1 on UCMD_CLEAR_ESTOP */
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame */
} UartProtState;
extern UartProtState uart_prot_state;
/* ── API ───────────────────────────────────────────────────────────────────── */
/**
* uart_protocol_init() — configure UART5 + DMA, start circular receive.
* Must be called once during system init, before main loop.
*/
void uart_protocol_init(void);
/**
* uart_protocol_process() — drain DMA ring buffer, parse frames, dispatch commands.
* Call once per main loop iteration (every ~1 ms).
*/
void uart_protocol_process(void);
/**
* uart_protocol_send_status() — build and TX a URESP_STATUS frame.
* @param s Pointer to status payload to send.
*/
void uart_protocol_send_status(const uart_prot_status_t *s);
#endif /* UART_PROTOCOL_H */

101
legacy/stm32/include/ultrasonic.h
View File

@ -1,101 +0,0 @@
#ifndef ULTRASONIC_H
#define ULTRASONIC_H
#include <stdint.h>
#include <stdbool.h>
/*
* ultrasonic.h — HC-SR04 ultrasonic distance sensor driver (Issue #243)
*
* STM32F722 driver for HC-SR04 ultrasonic ranger with TIM1 input capture.
* Trigger: PA0 (GPIO output, active high pulse 10µs)
* Echo: PA1 (TIM1_CH2, input capture on rising/falling edges)
*
* Non-blocking operation: trigger measurement, get result via callback.
* Distance calculated from echo pulse width: distance_mm = (pulse_us / 2) / 29.1
* Typical range: 20-4000mm (accuracy ±3mm above 30mm)
*/
/* Ultrasonic sensor states */
typedef enum {
ULTRASONIC_IDLE, /* Ready for new measurement */
ULTRASONIC_TRIGGERED, /* Trigger pulse sent, waiting for echo */
ULTRASONIC_MEASURING, /* Echo rising edge detected, measuring */
ULTRASONIC_COMPLETE, /* Measurement complete */
ULTRASONIC_ERROR /* Timeout or out-of-range */
} UltrasonicState;
/* Measurement result callback: called when measurement completes
* Arguments:
* - distance_mm: measured distance in mm (0 if error/timeout)
* - is_valid: true if measurement valid, false if timeout/error
*/
typedef void (*ultrasonic_callback_t)(uint16_t distance_mm, bool is_valid);
/*
* ultrasonic_init()
*
* Initialize HC-SR04 driver:
* - PA0 as GPIO output (trigger pin)
* - PA1 as TIM1_CH2 input capture (echo pin)
* - Configure TIM1 for input capture on both edges (rising/falling)
* - Enable timer interrupt for echo measurement
*/
void ultrasonic_init(void);
/*
* ultrasonic_trigger()
*
* Start a non-blocking distance measurement.
* Sends 10µs trigger pulse on PA0, sets up echo measurement.
* Measurement completes asynchronously (typically 25-300ms depending on distance).
* Call ultrasonic_get_state() to check status or wait for callback.
*
* Returns: true if triggered successfully, false if still measuring previous result
*/
bool ultrasonic_trigger(void);
/*
* ultrasonic_set_callback(callback)
*
* Register callback to be called when measurement completes.
* Callback receives: distance_mm (0 if error), is_valid (true if successful)
* Callback is optional; can poll with ultrasonic_get_result() instead.
*/
void ultrasonic_set_callback(ultrasonic_callback_t callback);
/*
* ultrasonic_get_state()
*
* Returns current measurement state (IDLE, TRIGGERED, MEASURING, COMPLETE, ERROR).
* Useful for non-blocking polling.
*/
UltrasonicState ultrasonic_get_state(void);
/*
* ultrasonic_get_result(distance_mm, is_valid)
*
* Retrieve result of last measurement (only valid when state == ULTRASONIC_COMPLETE).
* Resets state to IDLE after reading.
*
* Arguments:
* - distance_mm: pointer to store distance in mm
* - is_valid: pointer to store validity flag
*
* Returns: true if result retrieved, false if no measurement available
*/
bool ultrasonic_get_result(uint16_t *distance_mm, bool *is_valid);
/*
* ultrasonic_tick(now_ms)
*
* Update function called periodically (recommended: every 1-10ms in main loop).
* Handles timeout detection for echo measurement.
* Must be called regularly for non-blocking operation.
*
* Arguments:
* - now_ms: current time in milliseconds (from HAL_GetTick() or similar)
*/
void ultrasonic_tick(uint32_t now_ms);
#endif /* ULTRASONIC_H */

117
legacy/stm32/include/vesc_can.h
View File

@ -1,117 +0,0 @@
#ifndef VESC_CAN_H
#define VESC_CAN_H
#include <stdint.h>
#include <stdbool.h>
/*
* vesc_can — VESC CAN protocol driver for FSESC 6.7 Pro Mini Dual (Issue #674).
*
* VESC uses 29-bit extended CAN IDs:
* arbitration_id = (packet_type << 8) | vesc_node_id
*
* Wire format is big-endian throughout (matches VESC FW 6.x).
*
* Physical layer: CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps.
*
* NOTE ON PA11/PA12 vs PB12/PB13:
* PA11/PA12 carry CAN1_RX/TX (AF9) BUT are also USB_OTG_FS DM/DP (AF10).
* USB CDC is active on this board, so PA11/PA12 are occupied.
* PB8/PB9 (CAN1 alternate) are occupied by I2C1 (barometer).
* CAN2 on PB12/PB13 is the only conflict-free choice.
* If the SN65HVD230 is wired to the pads labelled RX6/TX6 on the Mamba
* silkscreen, those pads connect to PB12/PB13 (SPI2/OSD, repurposed).
*
* VESC frames arrive in FIFO1 (extended-ID filter, bank 15).
* Orin standard frames arrive in FIFO0 (standard-ID filter, bank 14).
*/
/* ---- VESC packet type IDs (upper byte of 29-bit arb ID) ---- */
#define VESC_PKT_SET_DUTY 0u /* int32 duty × 100000 */
#define VESC_PKT_SET_CURRENT 1u /* int32 current (mA) */
#define VESC_PKT_SET_CURRENT_BRAKE 2u /* int32 brake current (mA) */
#define VESC_PKT_SET_RPM 3u /* int32 target RPM */
#define VESC_PKT_STATUS 9u /* int32 RPM, int16 I×10, int16 duty×1000 */
#define VESC_PKT_STATUS_4 16u /* int16 T_fet×10, T_mot×10, I_in×10 */
#define VESC_PKT_STATUS_5 27u /* int32 tacho, int16 V_in×10 */
/* ---- Default VESC node IDs (configurable via vesc_can_init) ---- */
#define VESC_CAN_ID_LEFT 56u
#define VESC_CAN_ID_RIGHT 68u
/* ---- Alive timeout ---- */
#define VESC_ALIVE_TIMEOUT_MS 1000u /* node offline if no STATUS for 1 s */
/* ---- JLink telemetry rate ---- */
#define VESC_TLM_HZ 1u
/* ---- Fault codes (VESC FW 6.6) ---- */
#define VESC_FAULT_NONE 0u
#define VESC_FAULT_OVER_VOLTAGE 1u
#define VESC_FAULT_UNDER_VOLTAGE 2u
#define VESC_FAULT_DRV 3u
#define VESC_FAULT_ABS_OVER_CURRENT 4u
#define VESC_FAULT_OVER_TEMP_FET 5u
#define VESC_FAULT_OVER_TEMP_MOTOR 6u
#define VESC_FAULT_GATE_DRIVER_OVER_VOLTAGE 7u
#define VESC_FAULT_GATE_DRIVER_UNDER_VOLTAGE 8u
#define VESC_FAULT_MCU_UNDER_VOLTAGE 9u
#define VESC_FAULT_WATCHDOG_RESET 10u
/* ---- Telemetry state per VESC node ---- */
typedef struct {
int32_t rpm; /* actual RPM (STATUS pkt, int32 BE) */
int16_t current_x10; /* phase current (A × 10; STATUS pkt) */
int16_t duty_x1000; /* duty cycle (× 1000; –1000..+1000) */
int16_t temp_fet_x10; /* FET temperature (°C × 10; STATUS_4) */
int16_t temp_motor_x10; /* motor temperature (°C × 10; STATUS_4) */
int16_t current_in_x10; /* input (battery) current (A × 10; STATUS_4) */
int16_t voltage_x10; /* input voltage (V × 10; STATUS_5) */
uint8_t fault_code; /* VESC fault code (0 = none) */
uint8_t _pad;
uint32_t last_rx_ms; /* HAL_GetTick() of last received STATUS frame */
} vesc_state_t;
/* ---- API ---- */
/*
* vesc_can_init(id_left, id_right) — store VESC node IDs and register the
* extended-frame callback with can_driver.
* Call after can_driver_init().
*/
void vesc_can_init(uint8_t id_left, uint8_t id_right);
/*
* vesc_can_send_rpm(vesc_id, rpm) — transmit VESC_PKT_SET_RPM (3) to the
* target VESC. arb_id = (3 << 8) | vesc_id. Payload: int32 big-endian.
*/
void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm);
/*
* vesc_can_on_frame(ext_id, data, len) — called by can_driver when an
* extended-ID frame arrives (registered via can_driver_set_ext_cb).
* Parses STATUS / STATUS_4 / STATUS_5 into the matching vesc_state_t.
*/
void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len);
/*
* vesc_can_get_state(vesc_id, out) — copy latest telemetry snapshot.
* vesc_id must match id_left or id_right passed to vesc_can_init.
* Returns false if vesc_id unknown or no frame has arrived yet.
*/
bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out);
/*
* vesc_can_is_alive(vesc_id, now_ms) — true if a STATUS frame arrived
* within VESC_ALIVE_TIMEOUT_MS of now_ms.
*/
bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms);
/*
* vesc_can_send_tlm(now_ms) — rate-limited JLINK_TLM_VESC_STATE (0x8E)
* telemetry to Orin over JLink. Safe to call every ms; internally
* rate-limited to VESC_TLM_HZ (1 Hz).
*/
void vesc_can_send_tlm(uint32_t now_ms);
#endif /* VESC_CAN_H */

100
legacy/stm32/include/watchdog.h
View File

@ -1,100 +0,0 @@
#ifndef WATCHDOG_H
#define WATCHDOG_H
#include <stdint.h>
#include <stdbool.h>
/*
* watchdog.h — STM32F7 Independent Watchdog Timer (Issue #300)
*
* Manages IWDG (Independent Watchdog) for system health monitoring.
* Detects communication stalls from Jetson and resets the MCU.
*
* Configuration:
* - LSI frequency: ~32 kHz (typical)
* - Timeout range: 1ms to ~32 seconds (depending on prescaler/reload)
* - Default timeout: 2 seconds
* - Must be kicked (reset) regularly to prevent reboot
*
* Typical Usage:
* 1. Call watchdog_init(2000) in system startup
* 2. Call watchdog_kick() regularly from main loop (e.g., every 100ms)
* 3. If watchdog_kick() is not called for >= timeout, MCU resets
* 4. Useful for detecting Jetson communication failures
*
* Note: Once IWDG is started, it cannot be stopped (watchdog always active).
* It can only be reset via watchdog_kick() or by MCU reset/power cycle.
*/
/* Watchdog timeout presets (in milliseconds) */
typedef enum {
WATCHDOG_TIMEOUT_1S = 1000, /* 1 second timeout */
WATCHDOG_TIMEOUT_2S = 2000, /* 2 seconds (default) */
WATCHDOG_TIMEOUT_4S = 4000, /* 4 seconds */
WATCHDOG_TIMEOUT_8S = 8000, /* 8 seconds */
WATCHDOG_TIMEOUT_16S = 16000 /* 16 seconds */
} WatchdogTimeout;
/*
* watchdog_init(timeout_ms)
*
* Initialize the Independent Watchdog Timer.
*
* - Configures IWDG with specified timeout
* - Starts the watchdog timer (cannot be stopped)
* - Must call watchdog_kick() regularly to prevent reset
*
* Arguments:
* - timeout_ms: Timeout in milliseconds (e.g., 2000 for 2 seconds)
* Typical range: 1-16000 ms
* Will clamp to valid range
*
* Returns: true if initialized, false if invalid timeout
*/
bool watchdog_init(uint32_t timeout_ms);
/*
* watchdog_kick()
*
* Reset the watchdog timer counter.
* Call this regularly from the main loop (e.g., every 100ms or faster).
* If not called within the configured timeout period, MCU resets.
*
* Note: This is typically called from a high-priority timer interrupt
* or the main application loop to ensure timing is deterministic.
*/
void watchdog_kick(void);
/*
* watchdog_get_timeout()
*
* Get the configured watchdog timeout in milliseconds.
*
* Returns: Timeout value in ms
*/
uint32_t watchdog_get_timeout(void);
/*
* watchdog_is_running()
*
* Check if watchdog timer is running.
* Once started, watchdog cannot be stopped (only reset via kick).
*
* Returns: true if watchdog is active, false if not initialized
*/
bool watchdog_is_running(void);
/*
* watchdog_was_reset_by_watchdog()
*
* Detect if the last MCU reset was caused by watchdog timeout.
* Useful for diagnosing system failures (e.g., Jetson communication loss).
*
* Call this in early startup (before watchdog_init) to check reset reason.
* Typically used to log or report watchdog resets to debugging systems.
*
* Returns: true if last reset was by watchdog, false otherwise
*/
bool watchdog_was_reset_by_watchdog(void);
#endif /* WATCHDOG_H */

184
legacy/stm32/lib/USB_CDC/include/usbd_cdc.h
View File

@ -1,184 +0,0 @@
/**
******************************************************************************
* @file usbd_cdc.h
* @author MCD Application Team
* @brief header file for the usbd_cdc.c file.
******************************************************************************
* @attention
*
* Copyright (c) 2015 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USB_CDC_H
#define __USB_CDC_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "usbd_ioreq.h"
/** @addtogroup STM32_USB_DEVICE_LIBRARY
* @{
*/
/** @defgroup usbd_cdc
* @brief This file is the Header file for usbd_cdc.c
* @{
*/
/** @defgroup usbd_cdc_Exported_Defines
* @{
*/
#ifndef CDC_IN_EP
#define CDC_IN_EP 0x81U /* EP1 for data IN */
#endif /* CDC_IN_EP */
#ifndef CDC_OUT_EP
#define CDC_OUT_EP 0x01U /* EP1 for data OUT */
#endif /* CDC_OUT_EP */
#ifndef CDC_CMD_EP
#define CDC_CMD_EP 0x82U /* EP2 for CDC commands */
#endif /* CDC_CMD_EP */
#ifndef CDC_HS_BINTERVAL
#define CDC_HS_BINTERVAL 0x10U
#endif /* CDC_HS_BINTERVAL */
#ifndef CDC_FS_BINTERVAL
#define CDC_FS_BINTERVAL 0x10U
#endif /* CDC_FS_BINTERVAL */
/* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */
#define CDC_DATA_HS_MAX_PACKET_SIZE 512U /* Endpoint IN & OUT Packet size */
#define CDC_DATA_FS_MAX_PACKET_SIZE 64U /* Endpoint IN & OUT Packet size */
#define CDC_CMD_PACKET_SIZE 8U /* Control Endpoint Packet size */
#define USB_CDC_CONFIG_DESC_SIZ 67U
#define CDC_DATA_HS_IN_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE
#define CDC_DATA_HS_OUT_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE
#define CDC_DATA_FS_IN_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE
#define CDC_DATA_FS_OUT_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE
#define CDC_REQ_MAX_DATA_SIZE 0x7U
/*---------------------------------------------------------------------*/
/* CDC definitions */
/*---------------------------------------------------------------------*/
#define CDC_SEND_ENCAPSULATED_COMMAND 0x00U
#define CDC_GET_ENCAPSULATED_RESPONSE 0x01U
#define CDC_SET_COMM_FEATURE 0x02U
#define CDC_GET_COMM_FEATURE 0x03U
#define CDC_CLEAR_COMM_FEATURE 0x04U
#define CDC_SET_LINE_CODING 0x20U
#define CDC_GET_LINE_CODING 0x21U
#define CDC_SET_CONTROL_LINE_STATE 0x22U
#define CDC_SEND_BREAK 0x23U
/**
* @}
*/
/** @defgroup USBD_CORE_Exported_TypesDefinitions
* @{
*/
/**
* @}
*/
typedef struct
{
uint32_t bitrate;
uint8_t format;
uint8_t paritytype;
uint8_t datatype;
} USBD_CDC_LineCodingTypeDef;
typedef struct _USBD_CDC_Itf
{
int8_t (* Init)(void);
int8_t (* DeInit)(void);
int8_t (* Control)(uint8_t cmd, uint8_t *pbuf, uint16_t length);
int8_t (* Receive)(uint8_t *Buf, uint32_t *Len);
int8_t (* TransmitCplt)(uint8_t *Buf, uint32_t *Len, uint8_t epnum);
} USBD_CDC_ItfTypeDef;
typedef struct
{
uint32_t data[CDC_DATA_HS_MAX_PACKET_SIZE / 4U]; /* Force 32-bit alignment */
uint8_t CmdOpCode;
uint8_t CmdLength;
uint8_t *RxBuffer;
uint8_t *TxBuffer;
uint32_t RxLength;
uint32_t TxLength;
__IO uint32_t TxState;
__IO uint32_t RxState;
} USBD_CDC_HandleTypeDef;
/** @defgroup USBD_CORE_Exported_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USBD_CORE_Exported_Variables
* @{
*/
extern USBD_ClassTypeDef USBD_CDC;
#define USBD_CDC_CLASS &USBD_CDC
/**
* @}
*/
/** @defgroup USB_CORE_Exported_Functions
* @{
*/
uint8_t USBD_CDC_RegisterInterface(USBD_HandleTypeDef *pdev,
USBD_CDC_ItfTypeDef *fops);
#ifdef USE_USBD_COMPOSITE
uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff,
uint32_t length, uint8_t ClassId);
uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t ClassId);
#else
uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff,
uint32_t length);
uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev);
#endif /* USE_USBD_COMPOSITE */
uint8_t USBD_CDC_SetRxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff);
uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev);
/**
* @}
*/
#ifdef __cplusplus
}
#endif
#endif /* __USB_CDC_H */
/**
* @}
*/
/**
* @}
*/

20
legacy/stm32/lib/USB_CDC/include/usbd_cdc_if.h
View File

@ -1,20 +0,0 @@
#ifndef USBD_CDC_IF_H
#define USBD_CDC_IF_H
#include "usbd_cdc.h"
extern USBD_CDC_ItfTypeDef USBD_CDC_fops;
/* Send data over USB CDC */
uint8_t CDC_Transmit(uint8_t *buf, uint16_t len);
/* Betaflight-style DFU reboot check — call early in main() */
void checkForBootloader(void);
/* PID tuning command interface (written by USB IRQ, read by main loop) */
extern volatile uint8_t cdc_cmd_ready;
extern volatile char cdc_cmd_buf[32];
extern volatile uint8_t cdc_estop_request;
extern volatile uint8_t cdc_estop_clear_request;
#endif

27
legacy/stm32/lib/USB_CDC/include/usbd_conf.h
View File

@ -1,27 +0,0 @@
#ifndef USBD_CONF_H
#define USBD_CONF_H
/* Match Betaflight's usbd_conf.h exactly */
#include "stm32f7xx_hal.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define USBD_MAX_NUM_INTERFACES 3
#define USBD_MAX_NUM_CONFIGURATION 1
#define USBD_MAX_STR_DESC_SIZ 0x100
#define USBD_SUPPORT_USER_STRING 0
#define USBD_SELF_POWERED 1
#define USBD_DEBUG_LEVEL 0
#define USE_USB_FS
#define USBD_malloc malloc
#define USBD_free free
#define USBD_memset memset
#define USBD_memcpy memcpy
#define USBD_UsrLog(...)
#define USBD_ErrLog(...)
#define USBD_DbgLog(...)
#endif

175
legacy/stm32/lib/USB_CDC/include/usbd_core.h
View File

@ -1,175 +0,0 @@
/**
******************************************************************************
* @file usbd_core.h
* @author MCD Application Team
* @brief Header file for usbd_core.c file
******************************************************************************
* @attention
*
* Copyright (c) 2015 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USBD_CORE_H
#define __USBD_CORE_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "usbd_conf.h"
#include "usbd_def.h"
#include "usbd_ioreq.h"
#include "usbd_ctlreq.h"
/** @addtogroup STM32_USB_DEVICE_LIBRARY
* @{
*/
/** @defgroup USBD_CORE
* @brief This file is the Header file for usbd_core.c file
* @{
*/
/** @defgroup USBD_CORE_Exported_Defines
* @{
*/
#ifndef USBD_DEBUG_LEVEL
#define USBD_DEBUG_LEVEL 0U
#endif /* USBD_DEBUG_LEVEL */
/**
* @}
*/
/** @defgroup USBD_CORE_Exported_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @defgroup USBD_CORE_Exported_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USBD_CORE_Exported_Variables
* @{
*/
#define USBD_SOF USBD_LL_SOF
/**
* @}
*/
/** @defgroup USBD_CORE_Exported_FunctionsPrototype
* @{
*/
USBD_StatusTypeDef USBD_Init(USBD_HandleTypeDef *pdev, USBD_DescriptorsTypeDef *pdesc, uint8_t id);
USBD_StatusTypeDef USBD_DeInit(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_Start(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_Stop(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_RegisterClass(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass);
#if (USBD_USER_REGISTER_CALLBACK == 1U)
USBD_StatusTypeDef USBD_RegisterDevStateCallback(USBD_HandleTypeDef *pdev, USBD_DevStateCallbackTypeDef pUserCallback);
#endif /* USBD_USER_REGISTER_CALLBACK */
#ifdef USE_USBD_COMPOSITE
USBD_StatusTypeDef USBD_RegisterClassComposite(USBD_HandleTypeDef *pdev, USBD_ClassTypeDef *pclass,
USBD_CompositeClassTypeDef classtype, uint8_t *EpAddr);
USBD_StatusTypeDef USBD_UnRegisterClassComposite(USBD_HandleTypeDef *pdev);
uint8_t USBD_CoreGetEPAdd(USBD_HandleTypeDef *pdev, uint8_t ep_dir, uint8_t ep_type, uint8_t ClassId);
#endif /* USE_USBD_COMPOSITE */
uint8_t USBD_CoreFindIF(USBD_HandleTypeDef *pdev, uint8_t index);
uint8_t USBD_CoreFindEP(USBD_HandleTypeDef *pdev, uint8_t index);
USBD_StatusTypeDef USBD_RunTestMode(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_SetClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx);
USBD_StatusTypeDef USBD_ClrClassConfig(USBD_HandleTypeDef *pdev, uint8_t cfgidx);
USBD_StatusTypeDef USBD_LL_SetupStage(USBD_HandleTypeDef *pdev, uint8_t *psetup);
USBD_StatusTypeDef USBD_LL_DataOutStage(USBD_HandleTypeDef *pdev, uint8_t epnum, uint8_t *pdata);
USBD_StatusTypeDef USBD_LL_DataInStage(USBD_HandleTypeDef *pdev, uint8_t epnum, uint8_t *pdata);
USBD_StatusTypeDef USBD_LL_Reset(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_SetSpeed(USBD_HandleTypeDef *pdev, USBD_SpeedTypeDef speed);
USBD_StatusTypeDef USBD_LL_Suspend(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_Resume(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_SOF(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_IsoINIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum);
USBD_StatusTypeDef USBD_LL_IsoOUTIncomplete(USBD_HandleTypeDef *pdev, uint8_t epnum);
USBD_StatusTypeDef USBD_LL_DevConnected(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_DevDisconnected(USBD_HandleTypeDef *pdev);
/* USBD Low Level Driver */
USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_DeInit(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_Start(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_Stop(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_LL_OpenEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr,
uint8_t ep_type, uint16_t ep_mps);
USBD_StatusTypeDef USBD_LL_CloseEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr);
USBD_StatusTypeDef USBD_LL_FlushEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr);
USBD_StatusTypeDef USBD_LL_StallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr);
USBD_StatusTypeDef USBD_LL_ClearStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr);
USBD_StatusTypeDef USBD_LL_SetUSBAddress(USBD_HandleTypeDef *pdev, uint8_t dev_addr);
USBD_StatusTypeDef USBD_LL_Transmit(USBD_HandleTypeDef *pdev, uint8_t ep_addr,
uint8_t *pbuf, uint32_t size);
USBD_StatusTypeDef USBD_LL_PrepareReceive(USBD_HandleTypeDef *pdev, uint8_t ep_addr,
uint8_t *pbuf, uint32_t size);
#ifdef USBD_HS_TESTMODE_ENABLE
USBD_StatusTypeDef USBD_LL_SetTestMode(USBD_HandleTypeDef *pdev, uint8_t testmode);
#endif /* USBD_HS_TESTMODE_ENABLE */
uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr);
uint32_t USBD_LL_GetRxDataSize(USBD_HandleTypeDef *pdev, uint8_t ep_addr);
void USBD_LL_Delay(uint32_t Delay);
void *USBD_GetEpDesc(uint8_t *pConfDesc, uint8_t EpAddr);
USBD_DescHeaderTypeDef *USBD_GetNextDesc(uint8_t *pbuf, uint16_t *ptr);
/**
* @}
*/
#ifdef __cplusplus
}
#endif
#endif /* __USBD_CORE_H */
/**
* @}
*/
/**
* @}
*/

101
legacy/stm32/lib/USB_CDC/include/usbd_ctlreq.h
View File

@ -1,101 +0,0 @@
/**
******************************************************************************
* @file usbd_req.h
* @author MCD Application Team
* @brief Header file for the usbd_req.c file
******************************************************************************
* @attention
*
* Copyright (c) 2015 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USB_REQUEST_H
#define __USB_REQUEST_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "usbd_def.h"
/** @addtogroup STM32_USB_DEVICE_LIBRARY
* @{
*/
/** @defgroup USBD_REQ
* @brief header file for the usbd_req.c file
* @{
*/
/** @defgroup USBD_REQ_Exported_Defines
* @{
*/
/**
* @}
*/
/** @defgroup USBD_REQ_Exported_Types
* @{
*/
/**
* @}
*/
/** @defgroup USBD_REQ_Exported_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USBD_REQ_Exported_Variables
* @{
*/
/**
* @}
*/
/** @defgroup USBD_REQ_Exported_FunctionsPrototype
* @{
*/
USBD_StatusTypeDef USBD_StdDevReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req);
USBD_StatusTypeDef USBD_StdItfReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req);
USBD_StatusTypeDef USBD_StdEPReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req);
void USBD_CtlError(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req);
void USBD_ParseSetupRequest(USBD_SetupReqTypedef *req, uint8_t *pdata);
void USBD_GetString(uint8_t *desc, uint8_t *unicode, uint16_t *len);
/**
* @}
*/
#ifdef __cplusplus
}
#endif
#endif /* __USB_REQUEST_H */
/**
* @}
*/
/**
* @}
*/

523
legacy/stm32/lib/USB_CDC/include/usbd_def.h
View File

@ -1,523 +0,0 @@
/**
******************************************************************************
* @file usbd_def.h
* @author MCD Application Team
* @brief General defines for the usb device library
******************************************************************************
* @attention
*
* Copyright (c) 2015 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USBD_DEF_H
#define __USBD_DEF_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "usbd_conf.h"
/** @addtogroup STM32_USBD_DEVICE_LIBRARY
* @{
*/
/** @defgroup USB_DEF
* @brief general defines for the usb device library file
* @{
*/
/** @defgroup USB_DEF_Exported_Defines
* @{
*/
#ifndef NULL
#define NULL 0U
#endif /* NULL */
#ifndef USBD_MAX_NUM_INTERFACES
#define USBD_MAX_NUM_INTERFACES 1U
#endif /* USBD_MAX_NUM_CONFIGURATION */
#ifndef USBD_MAX_NUM_CONFIGURATION
#define USBD_MAX_NUM_CONFIGURATION 1U
#endif /* USBD_MAX_NUM_CONFIGURATION */
#ifdef USE_USBD_COMPOSITE
#ifndef USBD_MAX_SUPPORTED_CLASS
#define USBD_MAX_SUPPORTED_CLASS 4U
#endif /* USBD_MAX_SUPPORTED_CLASS */
#else
#ifndef USBD_MAX_SUPPORTED_CLASS
#define USBD_MAX_SUPPORTED_CLASS 1U
#endif /* USBD_MAX_SUPPORTED_CLASS */
#endif /* USE_USBD_COMPOSITE */
#ifndef USBD_MAX_CLASS_ENDPOINTS
#define USBD_MAX_CLASS_ENDPOINTS 5U
#endif /* USBD_MAX_CLASS_ENDPOINTS */
#ifndef USBD_MAX_CLASS_INTERFACES
#define USBD_MAX_CLASS_INTERFACES 5U
#endif /* USBD_MAX_CLASS_INTERFACES */
#ifndef USBD_LPM_ENABLED
#define USBD_LPM_ENABLED 0U
#endif /* USBD_LPM_ENABLED */
#ifndef USBD_SELF_POWERED
#define USBD_SELF_POWERED 1U
#endif /*USBD_SELF_POWERED */
#ifndef USBD_MAX_POWER
#define USBD_MAX_POWER 0x32U /* 100 mA */
#endif /* USBD_MAX_POWER */
#ifndef USBD_SUPPORT_USER_STRING_DESC
#define USBD_SUPPORT_USER_STRING_DESC 0U
#endif /* USBD_SUPPORT_USER_STRING_DESC */
#ifndef USBD_CLASS_USER_STRING_DESC
#define USBD_CLASS_USER_STRING_DESC 0U
#endif /* USBD_CLASS_USER_STRING_DESC */
#define USB_LEN_DEV_QUALIFIER_DESC 0x0AU
#define USB_LEN_DEV_DESC 0x12U
#define USB_LEN_CFG_DESC 0x09U
#define USB_LEN_IF_DESC 0x09U
#define USB_LEN_EP_DESC 0x07U
#define USB_LEN_OTG_DESC 0x03U
#define USB_LEN_LANGID_STR_DESC 0x04U
#define USB_LEN_OTHER_SPEED_DESC_SIZ 0x09U
#define USBD_IDX_LANGID_STR 0x00U
#define USBD_IDX_MFC_STR 0x01U
#define USBD_IDX_PRODUCT_STR 0x02U
#define USBD_IDX_SERIAL_STR 0x03U
#define USBD_IDX_CONFIG_STR 0x04U
#define USBD_IDX_INTERFACE_STR 0x05U
#define USB_REQ_TYPE_STANDARD 0x00U
#define USB_REQ_TYPE_CLASS 0x20U
#define USB_REQ_TYPE_VENDOR 0x40U
#define USB_REQ_TYPE_MASK 0x60U
#define USB_REQ_RECIPIENT_DEVICE 0x00U
#define USB_REQ_RECIPIENT_INTERFACE 0x01U
#define USB_REQ_RECIPIENT_ENDPOINT 0x02U
#define USB_REQ_RECIPIENT_MASK 0x03U
#define USB_REQ_GET_STATUS 0x00U
#define USB_REQ_CLEAR_FEATURE 0x01U
#define USB_REQ_SET_FEATURE 0x03U
#define USB_REQ_SET_ADDRESS 0x05U
#define USB_REQ_GET_DESCRIPTOR 0x06U
#define USB_REQ_SET_DESCRIPTOR 0x07U
#define USB_REQ_GET_CONFIGURATION 0x08U
#define USB_REQ_SET_CONFIGURATION 0x09U
#define USB_REQ_GET_INTERFACE 0x0AU
#define USB_REQ_SET_INTERFACE 0x0BU
#define USB_REQ_SYNCH_FRAME 0x0CU
#define USB_DESC_TYPE_DEVICE 0x01U
#define USB_DESC_TYPE_CONFIGURATION 0x02U
#define USB_DESC_TYPE_STRING 0x03U
#define USB_DESC_TYPE_INTERFACE 0x04U
#define USB_DESC_TYPE_ENDPOINT 0x05U
#define USB_DESC_TYPE_DEVICE_QUALIFIER 0x06U
#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION 0x07U
#define USB_DESC_TYPE_IAD 0x0BU
#define USB_DESC_TYPE_BOS 0x0FU
#define USB_CONFIG_REMOTE_WAKEUP 0x02U
#define USB_CONFIG_SELF_POWERED 0x01U
#define USB_FEATURE_EP_HALT 0x00U
#define USB_FEATURE_REMOTE_WAKEUP 0x01U
#define USB_FEATURE_TEST_MODE 0x02U
#define USB_DEVICE_CAPABITY_TYPE 0x10U
#define USB_CONF_DESC_SIZE 0x09U
#define USB_IF_DESC_SIZE 0x09U
#define USB_EP_DESC_SIZE 0x07U
#define USB_IAD_DESC_SIZE 0x08U
#define USB_HS_MAX_PACKET_SIZE 512U
#define USB_FS_MAX_PACKET_SIZE 64U
#define USB_MAX_EP0_SIZE 64U
/* Device Status */
#define USBD_STATE_DEFAULT 0x01U
#define USBD_STATE_ADDRESSED 0x02U
#define USBD_STATE_CONFIGURED 0x03U
#define USBD_STATE_SUSPENDED 0x04U
/* EP0 State */
#define USBD_EP0_IDLE 0x00U
#define USBD_EP0_SETUP 0x01U
#define USBD_EP0_DATA_IN 0x02U
#define USBD_EP0_DATA_OUT 0x03U
#define USBD_EP0_STATUS_IN 0x04U
#define USBD_EP0_STATUS_OUT 0x05U
#define USBD_EP0_STALL 0x06U
#define USBD_EP_TYPE_CTRL 0x00U
#define USBD_EP_TYPE_ISOC 0x01U
#define USBD_EP_TYPE_BULK 0x02U
#define USBD_EP_TYPE_INTR 0x03U
#ifdef USE_USBD_COMPOSITE
#define USBD_EP_IN 0x80U
#define USBD_EP_OUT 0x00U
#define USBD_FUNC_DESCRIPTOR_TYPE 0x24U
#define USBD_DESC_SUBTYPE_ACM 0x0FU
#define USBD_DESC_ECM_BCD_LOW 0x00U
#define USBD_DESC_ECM_BCD_HIGH 0x10U
#endif /* USE_USBD_COMPOSITE */
/**
* @}
*/
/** @defgroup USBD_DEF_Exported_TypesDefinitions
* @{
*/
typedef struct usb_setup_req
{
uint8_t bmRequest;
uint8_t bRequest;
uint16_t wValue;
uint16_t wIndex;
uint16_t wLength;
} USBD_SetupReqTypedef;
typedef struct
{
uint8_t bLength;
uint8_t bDescriptorType;
uint16_t wTotalLength;
uint8_t bNumInterfaces;
uint8_t bConfigurationValue;
uint8_t iConfiguration;
uint8_t bmAttributes;
uint8_t bMaxPower;
} __PACKED USBD_ConfigDescTypeDef;
typedef struct
{
uint8_t bLength;
uint8_t bDescriptorType;
uint16_t wTotalLength;
uint8_t bNumDeviceCaps;
} USBD_BosDescTypeDef;
typedef struct
{
uint8_t bLength;
uint8_t bDescriptorType;
uint8_t bEndpointAddress;
uint8_t bmAttributes;
uint16_t wMaxPacketSize;
uint8_t bInterval;
} __PACKED USBD_EpDescTypeDef;
typedef struct
{
uint8_t bLength;
uint8_t bDescriptorType;
uint8_t bDescriptorSubType;
} USBD_DescHeaderTypeDef;
struct _USBD_HandleTypeDef;
typedef struct _Device_cb
{
uint8_t (*Init)(struct _USBD_HandleTypeDef *pdev, uint8_t cfgidx);
uint8_t (*DeInit)(struct _USBD_HandleTypeDef *pdev, uint8_t cfgidx);
/* Control Endpoints*/
uint8_t (*Setup)(struct _USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req);
uint8_t (*EP0_TxSent)(struct _USBD_HandleTypeDef *pdev);
uint8_t (*EP0_RxReady)(struct _USBD_HandleTypeDef *pdev);
/* Class Specific Endpoints*/
uint8_t (*DataIn)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum);
uint8_t (*DataOut)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum);
uint8_t (*SOF)(struct _USBD_HandleTypeDef *pdev);
uint8_t (*IsoINIncomplete)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum);
uint8_t (*IsoOUTIncomplete)(struct _USBD_HandleTypeDef *pdev, uint8_t epnum);
uint8_t *(*GetHSConfigDescriptor)(uint16_t *length);
uint8_t *(*GetFSConfigDescriptor)(uint16_t *length);
uint8_t *(*GetOtherSpeedConfigDescriptor)(uint16_t *length);
uint8_t *(*GetDeviceQualifierDescriptor)(uint16_t *length);
#if (USBD_SUPPORT_USER_STRING_DESC == 1U)
uint8_t *(*GetUsrStrDescriptor)(struct _USBD_HandleTypeDef *pdev, uint8_t index, uint16_t *length);
#endif /* USBD_SUPPORT_USER_STRING_DESC */
} USBD_ClassTypeDef;
/* Following USB Device Speed */
typedef enum
{
USBD_SPEED_HIGH = 0U,
USBD_SPEED_FULL = 1U,
USBD_SPEED_LOW = 2U,
} USBD_SpeedTypeDef;
/* Following USB Device status */
typedef enum
{
USBD_OK = 0U,
USBD_BUSY,
USBD_EMEM,
USBD_FAIL,
} USBD_StatusTypeDef;
/* USB Device descriptors structure */
typedef struct
{
uint8_t *(*GetDeviceDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length);
uint8_t *(*GetLangIDStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length);
uint8_t *(*GetManufacturerStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length);
uint8_t *(*GetProductStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length);
uint8_t *(*GetSerialStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length);
uint8_t *(*GetConfigurationStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length);
uint8_t *(*GetInterfaceStrDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length);
#if (USBD_CLASS_USER_STRING_DESC == 1)
uint8_t *(*GetUserStrDescriptor)(USBD_SpeedTypeDef speed, uint8_t idx, uint16_t *length);
#endif /* USBD_CLASS_USER_STRING_DESC */
#if ((USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1))
uint8_t *(*GetBOSDescriptor)(USBD_SpeedTypeDef speed, uint16_t *length);
#endif /* (USBD_LPM_ENABLED == 1U) || (USBD_CLASS_BOS_ENABLED == 1) */
} USBD_DescriptorsTypeDef;
/* USB Device handle structure */
typedef struct
{
uint32_t status;
uint32_t total_length;
uint32_t rem_length;
uint32_t maxpacket;
uint16_t is_used;
uint16_t bInterval;
} USBD_EndpointTypeDef;
#ifdef USE_USBD_COMPOSITE
typedef enum
{
CLASS_TYPE_NONE = 0,
CLASS_TYPE_HID = 1,
CLASS_TYPE_CDC = 2,
CLASS_TYPE_MSC = 3,
CLASS_TYPE_DFU = 4,
CLASS_TYPE_CHID = 5,
CLASS_TYPE_AUDIO = 6,
CLASS_TYPE_ECM = 7,
CLASS_TYPE_RNDIS = 8,
CLASS_TYPE_MTP = 9,
CLASS_TYPE_VIDEO = 10,
CLASS_TYPE_PRINTER = 11,
CLASS_TYPE_CCID = 12,
} USBD_CompositeClassTypeDef;
/* USB Device handle structure */
typedef struct
{
uint8_t add;
uint8_t type;
uint8_t size;
uint8_t is_used;
} USBD_EPTypeDef;
/* USB Device handle structure */
typedef struct
{
USBD_CompositeClassTypeDef ClassType;
uint32_t ClassId;
uint32_t Active;
uint32_t NumEps;
USBD_EPTypeDef Eps[USBD_MAX_CLASS_ENDPOINTS];
uint8_t *EpAdd;
uint32_t NumIf;
uint8_t Ifs[USBD_MAX_CLASS_INTERFACES];
uint32_t CurrPcktSze;
} USBD_CompositeElementTypeDef;
#endif /* USE_USBD_COMPOSITE */
/* USB Device handle structure */
typedef struct _USBD_HandleTypeDef
{
uint8_t id;
uint32_t dev_config;
uint32_t dev_default_config;
uint32_t dev_config_status;
USBD_SpeedTypeDef dev_speed;
USBD_EndpointTypeDef ep_in[16];
USBD_EndpointTypeDef ep_out[16];
__IO uint32_t ep0_state;
uint32_t ep0_data_len;
__IO uint8_t dev_state;
__IO uint8_t dev_old_state;
uint8_t dev_address;
uint8_t dev_connection_status;
uint8_t dev_test_mode;
uint32_t dev_remote_wakeup;
uint8_t ConfIdx;
USBD_SetupReqTypedef request;
USBD_DescriptorsTypeDef *pDesc;
USBD_ClassTypeDef *pClass[USBD_MAX_SUPPORTED_CLASS];
void *pClassData;
void *pClassDataCmsit[USBD_MAX_SUPPORTED_CLASS];
void *pUserData[USBD_MAX_SUPPORTED_CLASS];
void *pData;
void *pBosDesc;
void *pConfDesc;
uint32_t classId;
uint32_t NumClasses;
#ifdef USE_USBD_COMPOSITE
USBD_CompositeElementTypeDef tclasslist[USBD_MAX_SUPPORTED_CLASS];
#endif /* USE_USBD_COMPOSITE */
#if (USBD_USER_REGISTER_CALLBACK == 1U)
void (* DevStateCallback)(uint8_t dev_state, uint8_t cfgidx); /*!< User Notification callback */
#endif /* USBD_USER_REGISTER_CALLBACK */
} USBD_HandleTypeDef;
#if (USBD_USER_REGISTER_CALLBACK == 1U)
typedef void (*USBD_DevStateCallbackTypeDef)(uint8_t dev_state, uint8_t cfgidx); /*!< pointer to User callback function */
#endif /* USBD_USER_REGISTER_CALLBACK */
/* USB Device endpoint direction */
typedef enum
{
OUT = 0x00,
IN = 0x80,
} USBD_EPDirectionTypeDef;
typedef enum
{
NETWORK_CONNECTION = 0x00,
RESPONSE_AVAILABLE = 0x01,
CONNECTION_SPEED_CHANGE = 0x2A
} USBD_CDC_NotifCodeTypeDef;
/**
* @}
*/
/** @defgroup USBD_DEF_Exported_Macros
* @{
*/
__STATIC_INLINE uint16_t SWAPBYTE(uint8_t *addr)
{
uint16_t _SwapVal;
uint16_t _Byte1;
uint16_t _Byte2;
uint8_t *_pbuff = addr;
_Byte1 = *(uint8_t *)_pbuff;
_pbuff++;
_Byte2 = *(uint8_t *)_pbuff;
_SwapVal = (_Byte2 << 8) | _Byte1;
return _SwapVal;
}
#ifndef LOBYTE
#define LOBYTE(x) ((uint8_t)((x) & 0x00FFU))
#endif /* LOBYTE */
#ifndef HIBYTE
#define HIBYTE(x) ((uint8_t)(((x) & 0xFF00U) >> 8U))
#endif /* HIBYTE */
#ifndef MIN
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#endif /* MIN */
#ifndef MAX
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#endif /* MAX */
#if defined ( __GNUC__ )
#ifndef __weak
#define __weak __attribute__((weak))
#endif /* __weak */
#ifndef __packed
#define __packed __attribute__((__packed__))
#endif /* __packed */
#endif /* __GNUC__ */
/* In HS mode and when the DMA is used, all variables and data structures dealing
with the DMA during the transaction process should be 4-bytes aligned */
#if defined ( __GNUC__ ) && !defined (__CC_ARM) /* GNU Compiler */
#ifndef __ALIGN_END
#define __ALIGN_END __attribute__ ((aligned (4U)))
#endif /* __ALIGN_END */
#ifndef __ALIGN_BEGIN
#define __ALIGN_BEGIN
#endif /* __ALIGN_BEGIN */
#else
#ifndef __ALIGN_END
#define __ALIGN_END
#endif /* __ALIGN_END */
#ifndef __ALIGN_BEGIN
#if defined (__CC_ARM) /* ARM Compiler */
#define __ALIGN_BEGIN __align(4U)
#elif defined (__ICCARM__) /* IAR Compiler */
#define __ALIGN_BEGIN
#endif /* __CC_ARM */
#endif /* __ALIGN_BEGIN */
#endif /* __GNUC__ */
/**
* @}
*/
/** @defgroup USBD_DEF_Exported_Variables
* @{
*/
/**
* @}
*/
/** @defgroup USBD_DEF_Exported_FunctionsPrototype
* @{
*/
/**
* @}
*/
#ifdef __cplusplus
}
#endif
#endif /* __USBD_DEF_H */
/**
* @}
*/
/**
* @}
*/

5
legacy/stm32/lib/USB_CDC/include/usbd_desc.h
View File

@ -1,5 +0,0 @@
#ifndef USBD_DESC_H
#define USBD_DESC_H
#include "usbd_def.h"
extern USBD_DescriptorsTypeDef SaltyLab_Desc;
#endif

113
legacy/stm32/lib/USB_CDC/include/usbd_ioreq.h
View File

@ -1,113 +0,0 @@
/**
******************************************************************************
* @file usbd_ioreq.h
* @author MCD Application Team
* @brief Header file for the usbd_ioreq.c file
******************************************************************************
* @attention
*
* Copyright (c) 2015 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USBD_IOREQ_H
#define __USBD_IOREQ_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "usbd_def.h"
#include "usbd_core.h"
/** @addtogroup STM32_USB_DEVICE_LIBRARY
* @{
*/
/** @defgroup USBD_IOREQ
* @brief header file for the usbd_ioreq.c file
* @{
*/
/** @defgroup USBD_IOREQ_Exported_Defines
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Exported_Types
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Exported_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Exported_Variables
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Exported_FunctionsPrototype
* @{
*/
USBD_StatusTypeDef USBD_CtlSendData(USBD_HandleTypeDef *pdev,
uint8_t *pbuf, uint32_t len);
USBD_StatusTypeDef USBD_CtlContinueSendData(USBD_HandleTypeDef *pdev,
uint8_t *pbuf, uint32_t len);
USBD_StatusTypeDef USBD_CtlPrepareRx(USBD_HandleTypeDef *pdev,
uint8_t *pbuf, uint32_t len);
USBD_StatusTypeDef USBD_CtlContinueRx(USBD_HandleTypeDef *pdev,
uint8_t *pbuf, uint32_t len);
USBD_StatusTypeDef USBD_CtlSendStatus(USBD_HandleTypeDef *pdev);
USBD_StatusTypeDef USBD_CtlReceiveStatus(USBD_HandleTypeDef *pdev);
uint32_t USBD_GetRxCount(USBD_HandleTypeDef *pdev, uint8_t ep_addr);
/**
* @}
*/
#ifdef __cplusplus
}
#endif
#endif /* __USBD_IOREQ_H */
/**
* @}
*/
/**
* @}
*/

893
legacy/stm32/lib/USB_CDC/src/usbd_cdc.c
View File

@ -1,893 +0,0 @@
/**
******************************************************************************
* @file usbd_cdc.c
* @author MCD Application Team
* @brief This file provides the high layer firmware functions to manage the
* following functionalities of the USB CDC Class:
* - Initialization and Configuration of high and low layer
* - Enumeration as CDC Device (and enumeration for each implemented memory interface)
* - OUT/IN data transfer
* - Command IN transfer (class requests management)
* - Error management
*
******************************************************************************
* @attention
*
* Copyright (c) 2015 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
* @verbatim
*
* ===================================================================
* CDC Class Driver Description
* ===================================================================
* This driver manages the "Universal Serial Bus Class Definitions for Communications Devices
* Revision 1.2 November 16, 2007" and the sub-protocol specification of "Universal Serial Bus
* Communications Class Subclass Specification for PSTN Devices Revision 1.2 February 9, 2007"
* This driver implements the following aspects of the specification:
* - Device descriptor management
* - Configuration descriptor management
* - Enumeration as CDC device with 2 data endpoints (IN and OUT) and 1 command endpoint (IN)
* - Requests management (as described in section 6.2 in specification)
* - Abstract Control Model compliant
* - Union Functional collection (using 1 IN endpoint for control)
* - Data interface class
*
* These aspects may be enriched or modified for a specific user application.
*
* This driver doesn't implement the following aspects of the specification
* (but it is possible to manage these features with some modifications on this driver):
* - Any class-specific aspect relative to communication classes should be managed by user application.
* - All communication classes other than PSTN are not managed
*
* @endverbatim
*
******************************************************************************
*/
/* BSPDependencies
- "stm32xxxxx_{eval}{discovery}{nucleo_144}.c"
- "stm32xxxxx_{eval}{discovery}_io.c"
EndBSPDependencies */
/* Includes ------------------------------------------------------------------*/
#include "usbd_cdc.h"
#include "usbd_ctlreq.h"
/** @addtogroup STM32_USB_DEVICE_LIBRARY
* @{
*/
/** @defgroup USBD_CDC
* @brief usbd core module
* @{
*/
/** @defgroup USBD_CDC_Private_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @defgroup USBD_CDC_Private_Defines
* @{
*/
/**
* @}
*/
/** @defgroup USBD_CDC_Private_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USBD_CDC_Private_FunctionPrototypes
* @{
*/
static uint8_t USBD_CDC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx);
static uint8_t USBD_CDC_DeInit(USBD_HandleTypeDef *pdev, uint8_t cfgidx);
static uint8_t USBD_CDC_Setup(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req);
static uint8_t USBD_CDC_DataIn(USBD_HandleTypeDef *pdev, uint8_t epnum);
static uint8_t USBD_CDC_DataOut(USBD_HandleTypeDef *pdev, uint8_t epnum);
static uint8_t USBD_CDC_EP0_RxReady(USBD_HandleTypeDef *pdev);
#ifndef USE_USBD_COMPOSITE
static uint8_t *USBD_CDC_GetFSCfgDesc(uint16_t *length);
static uint8_t *USBD_CDC_GetHSCfgDesc(uint16_t *length);
static uint8_t *USBD_CDC_GetOtherSpeedCfgDesc(uint16_t *length);
uint8_t *USBD_CDC_GetDeviceQualifierDescriptor(uint16_t *length);
#endif /* USE_USBD_COMPOSITE */
#ifndef USE_USBD_COMPOSITE
/* USB Standard Device Descriptor */
__ALIGN_BEGIN static uint8_t USBD_CDC_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALIGN_END =
{
USB_LEN_DEV_QUALIFIER_DESC,
USB_DESC_TYPE_DEVICE_QUALIFIER,
0x00,
0x02,
0x00,
0x00,
0x00,
0x40,
0x01,
0x00,
};
#endif /* USE_USBD_COMPOSITE */
/**
* @}
*/
/** @defgroup USBD_CDC_Private_Variables
* @{
*/
/* CDC interface class callbacks structure */
USBD_ClassTypeDef USBD_CDC =
{
USBD_CDC_Init,
USBD_CDC_DeInit,
USBD_CDC_Setup,
NULL, /* EP0_TxSent */
USBD_CDC_EP0_RxReady,
USBD_CDC_DataIn,
USBD_CDC_DataOut,
NULL,
NULL,
NULL,
#ifdef USE_USBD_COMPOSITE
NULL,
NULL,
NULL,
NULL,
#else
USBD_CDC_GetHSCfgDesc,
USBD_CDC_GetFSCfgDesc,
USBD_CDC_GetOtherSpeedCfgDesc,
USBD_CDC_GetDeviceQualifierDescriptor,
#endif /* USE_USBD_COMPOSITE */
};
#ifndef USE_USBD_COMPOSITE
/* USB CDC device Configuration Descriptor */
__ALIGN_BEGIN static uint8_t USBD_CDC_CfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIGN_END =
{
/* Configuration Descriptor */
0x09, /* bLength: Configuration Descriptor size */
USB_DESC_TYPE_CONFIGURATION, /* bDescriptorType: Configuration */
USB_CDC_CONFIG_DESC_SIZ, /* wTotalLength */
0x00,
0x02, /* bNumInterfaces: 2 interfaces */
0x01, /* bConfigurationValue: Configuration value */
0x00, /* iConfiguration: Index of string descriptor
describing the configuration */
#if (USBD_SELF_POWERED == 1U)
0xC0, /* bmAttributes: Bus Powered according to user configuration */
#else
0x80, /* bmAttributes: Bus Powered according to user configuration */
#endif /* USBD_SELF_POWERED */
USBD_MAX_POWER, /* MaxPower (mA) */
/*---------------------------------------------------------------------------*/
/* Interface Descriptor */
0x09, /* bLength: Interface Descriptor size */
USB_DESC_TYPE_INTERFACE, /* bDescriptorType: Interface */
/* Interface descriptor type */
0x00, /* bInterfaceNumber: Number of Interface */
0x00, /* bAlternateSetting: Alternate setting */
0x01, /* bNumEndpoints: One endpoint used */
0x02, /* bInterfaceClass: Communication Interface Class */
0x02, /* bInterfaceSubClass: Abstract Control Model */
0x01, /* bInterfaceProtocol: Common AT commands */
0x00, /* iInterface */
/* Header Functional Descriptor */
0x05, /* bLength: Endpoint Descriptor size */
0x24, /* bDescriptorType: CS_INTERFACE */
0x00, /* bDescriptorSubtype: Header Func Desc */
0x10, /* bcdCDC: spec release number */
0x01,
/* Call Management Functional Descriptor */
0x05, /* bFunctionLength */
0x24, /* bDescriptorType: CS_INTERFACE */
0x01, /* bDescriptorSubtype: Call Management Func Desc */
0x00, /* bmCapabilities: D0+D1 */
0x01, /* bDataInterface */
/* ACM Functional Descriptor */
0x04, /* bFunctionLength */
0x24, /* bDescriptorType: CS_INTERFACE */
0x02, /* bDescriptorSubtype: Abstract Control Management desc */
0x02, /* bmCapabilities */
/* Union Functional Descriptor */
0x05, /* bFunctionLength */
0x24, /* bDescriptorType: CS_INTERFACE */
0x06, /* bDescriptorSubtype: Union func desc */
0x00, /* bMasterInterface: Communication class interface */
0x01, /* bSlaveInterface0: Data Class Interface */
/* Endpoint 2 Descriptor */
0x07, /* bLength: Endpoint Descriptor size */
USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */
CDC_CMD_EP, /* bEndpointAddress */
0x03, /* bmAttributes: Interrupt */
LOBYTE(CDC_CMD_PACKET_SIZE), /* wMaxPacketSize */
HIBYTE(CDC_CMD_PACKET_SIZE),
CDC_FS_BINTERVAL, /* bInterval */
/*---------------------------------------------------------------------------*/
/* Data class interface descriptor */
0x09, /* bLength: Endpoint Descriptor size */
USB_DESC_TYPE_INTERFACE, /* bDescriptorType: */
0x01, /* bInterfaceNumber: Number of Interface */
0x00, /* bAlternateSetting: Alternate setting */
0x02, /* bNumEndpoints: Two endpoints used */
0x0A, /* bInterfaceClass: CDC */
0x00, /* bInterfaceSubClass */
0x00, /* bInterfaceProtocol */
0x00, /* iInterface */
/* Endpoint OUT Descriptor */
0x07, /* bLength: Endpoint Descriptor size */
USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */
CDC_OUT_EP, /* bEndpointAddress */
0x02, /* bmAttributes: Bulk */
LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize */
HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE),
0x00, /* bInterval */
/* Endpoint IN Descriptor */
0x07, /* bLength: Endpoint Descriptor size */
USB_DESC_TYPE_ENDPOINT, /* bDescriptorType: Endpoint */
CDC_IN_EP, /* bEndpointAddress */
0x02, /* bmAttributes: Bulk */
LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE), /* wMaxPacketSize */
HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE),
0x00 /* bInterval */
};
#endif /* USE_USBD_COMPOSITE */
static uint8_t CDCInEpAdd = CDC_IN_EP;
static uint8_t CDCOutEpAdd = CDC_OUT_EP;
static uint8_t CDCCmdEpAdd = CDC_CMD_EP;
/**
* @}
*/
/** @defgroup USBD_CDC_Private_Functions
* @{
*/
/**
* @brief USBD_CDC_Init
* Initialize the CDC interface
* @param pdev: device instance
* @param cfgidx: Configuration index
* @retval status
*/
static uint8_t USBD_CDC_Init(USBD_HandleTypeDef *pdev, uint8_t cfgidx)
{
UNUSED(cfgidx);
USBD_CDC_HandleTypeDef *hcdc;
hcdc = (USBD_CDC_HandleTypeDef *)USBD_malloc(sizeof(USBD_CDC_HandleTypeDef));
if (hcdc == NULL)
{
pdev->pClassDataCmsit[pdev->classId] = NULL;
return (uint8_t)USBD_EMEM;
}
(void)USBD_memset(hcdc, 0, sizeof(USBD_CDC_HandleTypeDef));
pdev->pClassDataCmsit[pdev->classId] = (void *)hcdc;
pdev->pClassData = pdev->pClassDataCmsit[pdev->classId];
#ifdef USE_USBD_COMPOSITE
/* Get the Endpoints addresses allocated for this class instance */
CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId);
CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId);
CDCCmdEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_INTR, (uint8_t)pdev->classId);
#endif /* USE_USBD_COMPOSITE */
if (pdev->dev_speed == USBD_SPEED_HIGH)
{
/* Open EP IN */
(void)USBD_LL_OpenEP(pdev, CDCInEpAdd, USBD_EP_TYPE_BULK,
CDC_DATA_HS_IN_PACKET_SIZE);
pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 1U;
/* Open EP OUT */
(void)USBD_LL_OpenEP(pdev, CDCOutEpAdd, USBD_EP_TYPE_BULK,
CDC_DATA_HS_OUT_PACKET_SIZE);
pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 1U;
/* Set bInterval for CDC CMD Endpoint */
pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = CDC_HS_BINTERVAL;
}
else
{
/* Open EP IN */
(void)USBD_LL_OpenEP(pdev, CDCInEpAdd, USBD_EP_TYPE_BULK,
CDC_DATA_FS_IN_PACKET_SIZE);
pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 1U;
/* Open EP OUT */
(void)USBD_LL_OpenEP(pdev, CDCOutEpAdd, USBD_EP_TYPE_BULK,
CDC_DATA_FS_OUT_PACKET_SIZE);
pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 1U;
/* Set bInterval for CMD Endpoint */
pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = CDC_FS_BINTERVAL;
}
/* Open Command IN EP */
(void)USBD_LL_OpenEP(pdev, CDCCmdEpAdd, USBD_EP_TYPE_INTR, CDC_CMD_PACKET_SIZE);
pdev->ep_in[CDCCmdEpAdd & 0xFU].is_used = 1U;
hcdc->RxBuffer = NULL;
/* Init physical Interface components */
((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Init();
/* Init Xfer states */
hcdc->TxState = 0U;
hcdc->RxState = 0U;
if (hcdc->RxBuffer == NULL)
{
return (uint8_t)USBD_EMEM;
}
if (pdev->dev_speed == USBD_SPEED_HIGH)
{
/* Prepare Out endpoint to receive next packet */
(void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer,
CDC_DATA_HS_OUT_PACKET_SIZE);
}
else
{
/* Prepare Out endpoint to receive next packet */
(void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer,
CDC_DATA_FS_OUT_PACKET_SIZE);
}
return (uint8_t)USBD_OK;
}
/**
* @brief USBD_CDC_Init
* DeInitialize the CDC layer
* @param pdev: device instance
* @param cfgidx: Configuration index
* @retval status
*/
static uint8_t USBD_CDC_DeInit(USBD_HandleTypeDef *pdev, uint8_t cfgidx)
{
UNUSED(cfgidx);
#ifdef USE_USBD_COMPOSITE
/* Get the Endpoints addresses allocated for this CDC class instance */
CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId);
CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId);
CDCCmdEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_INTR, (uint8_t)pdev->classId);
#endif /* USE_USBD_COMPOSITE */
/* Close EP IN */
(void)USBD_LL_CloseEP(pdev, CDCInEpAdd);
pdev->ep_in[CDCInEpAdd & 0xFU].is_used = 0U;
/* Close EP OUT */
(void)USBD_LL_CloseEP(pdev, CDCOutEpAdd);
pdev->ep_out[CDCOutEpAdd & 0xFU].is_used = 0U;
/* Close Command IN EP */
(void)USBD_LL_CloseEP(pdev, CDCCmdEpAdd);
pdev->ep_in[CDCCmdEpAdd & 0xFU].is_used = 0U;
pdev->ep_in[CDCCmdEpAdd & 0xFU].bInterval = 0U;
/* DeInit physical Interface components */
if (pdev->pClassDataCmsit[pdev->classId] != NULL)
{
((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->DeInit();
(void)USBD_free(pdev->pClassDataCmsit[pdev->classId]);
pdev->pClassDataCmsit[pdev->classId] = NULL;
pdev->pClassData = NULL;
}
return (uint8_t)USBD_OK;
}
/**
* @brief USBD_CDC_Setup
* Handle the CDC specific requests
* @param pdev: instance
* @param req: usb requests
* @retval status
*/
static uint8_t USBD_CDC_Setup(USBD_HandleTypeDef *pdev,
USBD_SetupReqTypedef *req)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId];
uint16_t len;
uint8_t ifalt = 0U;
uint16_t status_info = 0U;
USBD_StatusTypeDef ret = USBD_OK;
if (hcdc == NULL)
{
return (uint8_t)USBD_FAIL;
}
switch (req->bmRequest & USB_REQ_TYPE_MASK)
{
case USB_REQ_TYPE_CLASS:
if (req->wLength != 0U)
{
if ((req->bmRequest & 0x80U) != 0U)
{
((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(req->bRequest,
(uint8_t *)hcdc->data,
req->wLength);
len = MIN(CDC_REQ_MAX_DATA_SIZE, req->wLength);
(void)USBD_CtlSendData(pdev, (uint8_t *)hcdc->data, len);
}
else
{
hcdc->CmdOpCode = req->bRequest;
hcdc->CmdLength = (uint8_t)MIN(req->wLength, USB_MAX_EP0_SIZE);
(void)USBD_CtlPrepareRx(pdev, (uint8_t *)hcdc->data, hcdc->CmdLength);
}
}
else
{
((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(req->bRequest,
(uint8_t *)req, 0U);
}
break;
case USB_REQ_TYPE_STANDARD:
switch (req->bRequest)
{
case USB_REQ_GET_STATUS:
if (pdev->dev_state == USBD_STATE_CONFIGURED)
{
(void)USBD_CtlSendData(pdev, (uint8_t *)&status_info, 2U);
}
else
{
USBD_CtlError(pdev, req);
ret = USBD_FAIL;
}
break;
case USB_REQ_GET_INTERFACE:
if (pdev->dev_state == USBD_STATE_CONFIGURED)
{
(void)USBD_CtlSendData(pdev, &ifalt, 1U);
}
else
{
USBD_CtlError(pdev, req);
ret = USBD_FAIL;
}
break;
case USB_REQ_SET_INTERFACE:
if (pdev->dev_state != USBD_STATE_CONFIGURED)
{
USBD_CtlError(pdev, req);
ret = USBD_FAIL;
}
break;
case USB_REQ_CLEAR_FEATURE:
break;
default:
USBD_CtlError(pdev, req);
ret = USBD_FAIL;
break;
}
break;
default:
USBD_CtlError(pdev, req);
ret = USBD_FAIL;
break;
}
return (uint8_t)ret;
}
/**
* @brief USBD_CDC_DataIn
* Data sent on non-control IN endpoint
* @param pdev: device instance
* @param epnum: endpoint number
* @retval status
*/
static uint8_t USBD_CDC_DataIn(USBD_HandleTypeDef *pdev, uint8_t epnum)
{
USBD_CDC_HandleTypeDef *hcdc;
PCD_HandleTypeDef *hpcd = (PCD_HandleTypeDef *)pdev->pData;
if (pdev->pClassDataCmsit[pdev->classId] == NULL)
{
return (uint8_t)USBD_FAIL;
}
hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId];
if ((pdev->ep_in[epnum & 0xFU].total_length > 0U) &&
((pdev->ep_in[epnum & 0xFU].total_length % hpcd->IN_ep[epnum & 0xFU].maxpacket) == 0U))
{
/* Update the packet total length */
pdev->ep_in[epnum & 0xFU].total_length = 0U;
/* Send ZLP */
(void)USBD_LL_Transmit(pdev, epnum, NULL, 0U);
}
else
{
hcdc->TxState = 0U;
if (((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->TransmitCplt != NULL)
{
((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->TransmitCplt(hcdc->TxBuffer, &hcdc->TxLength, epnum);
}
}
return (uint8_t)USBD_OK;
}
/**
* @brief USBD_CDC_DataOut
* Data received on non-control Out endpoint
* @param pdev: device instance
* @param epnum: endpoint number
* @retval status
*/
static uint8_t USBD_CDC_DataOut(USBD_HandleTypeDef *pdev, uint8_t epnum)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId];
if (pdev->pClassDataCmsit[pdev->classId] == NULL)
{
return (uint8_t)USBD_FAIL;
}
/* Get the received data length */
hcdc->RxLength = USBD_LL_GetRxDataSize(pdev, epnum);
/* USB data will be immediately processed, this allow next USB traffic being
NAKed till the end of the application Xfer */
((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Receive(hcdc->RxBuffer, &hcdc->RxLength);
return (uint8_t)USBD_OK;
}
/**
* @brief USBD_CDC_EP0_RxReady
* Handle EP0 Rx Ready event
* @param pdev: device instance
* @retval status
*/
static uint8_t USBD_CDC_EP0_RxReady(USBD_HandleTypeDef *pdev)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId];
if (hcdc == NULL)
{
return (uint8_t)USBD_FAIL;
}
if ((pdev->pUserData[pdev->classId] != NULL) && (hcdc->CmdOpCode != 0xFFU))
{
((USBD_CDC_ItfTypeDef *)pdev->pUserData[pdev->classId])->Control(hcdc->CmdOpCode,
(uint8_t *)hcdc->data,
(uint16_t)hcdc->CmdLength);
hcdc->CmdOpCode = 0xFFU;
}
return (uint8_t)USBD_OK;
}
#ifndef USE_USBD_COMPOSITE
/**
* @brief USBD_CDC_GetFSCfgDesc
* Return configuration descriptor
* @param length : pointer data length
* @retval pointer to descriptor buffer
*/
static uint8_t *USBD_CDC_GetFSCfgDesc(uint16_t *length)
{
USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP);
USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP);
USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP);
if (pEpCmdDesc != NULL)
{
pEpCmdDesc->bInterval = CDC_FS_BINTERVAL;
}
if (pEpOutDesc != NULL)
{
pEpOutDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE;
}
if (pEpInDesc != NULL)
{
pEpInDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE;
}
*length = (uint16_t)sizeof(USBD_CDC_CfgDesc);
return USBD_CDC_CfgDesc;
}
/**
* @brief USBD_CDC_GetHSCfgDesc
* Return configuration descriptor
* @param length : pointer data length
* @retval pointer to descriptor buffer
*/
static uint8_t *USBD_CDC_GetHSCfgDesc(uint16_t *length)
{
USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP);
USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP);
USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP);
if (pEpCmdDesc != NULL)
{
pEpCmdDesc->bInterval = CDC_HS_BINTERVAL;
}
if (pEpOutDesc != NULL)
{
pEpOutDesc->wMaxPacketSize = CDC_DATA_HS_MAX_PACKET_SIZE;
}
if (pEpInDesc != NULL)
{
pEpInDesc->wMaxPacketSize = CDC_DATA_HS_MAX_PACKET_SIZE;
}
*length = (uint16_t)sizeof(USBD_CDC_CfgDesc);
return USBD_CDC_CfgDesc;
}
/**
* @brief USBD_CDC_GetOtherSpeedCfgDesc
* Return configuration descriptor
* @param length : pointer data length
* @retval pointer to descriptor buffer
*/
static uint8_t *USBD_CDC_GetOtherSpeedCfgDesc(uint16_t *length)
{
USBD_EpDescTypeDef *pEpCmdDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_CMD_EP);
USBD_EpDescTypeDef *pEpOutDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_OUT_EP);
USBD_EpDescTypeDef *pEpInDesc = USBD_GetEpDesc(USBD_CDC_CfgDesc, CDC_IN_EP);
if (pEpCmdDesc != NULL)
{
pEpCmdDesc->bInterval = CDC_FS_BINTERVAL;
}
if (pEpOutDesc != NULL)
{
pEpOutDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE;
}
if (pEpInDesc != NULL)
{
pEpInDesc->wMaxPacketSize = CDC_DATA_FS_MAX_PACKET_SIZE;
}
*length = (uint16_t)sizeof(USBD_CDC_CfgDesc);
return USBD_CDC_CfgDesc;
}
/**
* @brief USBD_CDC_GetDeviceQualifierDescriptor
* return Device Qualifier descriptor
* @param length : pointer data length
* @retval pointer to descriptor buffer
*/
uint8_t *USBD_CDC_GetDeviceQualifierDescriptor(uint16_t *length)
{
*length = (uint16_t)sizeof(USBD_CDC_DeviceQualifierDesc);
return USBD_CDC_DeviceQualifierDesc;
}
#endif /* USE_USBD_COMPOSITE */
/**
* @brief USBD_CDC_RegisterInterface
* @param pdev: device instance
* @param fops: CD Interface callback
* @retval status
*/
uint8_t USBD_CDC_RegisterInterface(USBD_HandleTypeDef *pdev,
USBD_CDC_ItfTypeDef *fops)
{
if (fops == NULL)
{
return (uint8_t)USBD_FAIL;
}
pdev->pUserData[pdev->classId] = fops;
return (uint8_t)USBD_OK;
}
/**
* @brief USBD_CDC_SetTxBuffer
* @param pdev: device instance
* @param pbuff: Tx Buffer
* @param length: length of data to be sent
* @param ClassId: The Class ID
* @retval status
*/
#ifdef USE_USBD_COMPOSITE
uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev,
uint8_t *pbuff, uint32_t length, uint8_t ClassId)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[ClassId];
#else
uint8_t USBD_CDC_SetTxBuffer(USBD_HandleTypeDef *pdev,
uint8_t *pbuff, uint32_t length)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId];
#endif /* USE_USBD_COMPOSITE */
if (hcdc == NULL)
{
return (uint8_t)USBD_FAIL;
}
hcdc->TxBuffer = pbuff;
hcdc->TxLength = length;
return (uint8_t)USBD_OK;
}
/**
* @brief USBD_CDC_SetRxBuffer
* @param pdev: device instance
* @param pbuff: Rx Buffer
* @retval status
*/
uint8_t USBD_CDC_SetRxBuffer(USBD_HandleTypeDef *pdev, uint8_t *pbuff)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId];
if (hcdc == NULL)
{
return (uint8_t)USBD_FAIL;
}
hcdc->RxBuffer = pbuff;
return (uint8_t)USBD_OK;
}
/**
* @brief USBD_CDC_TransmitPacket
* Transmit packet on IN endpoint
* @param pdev: device instance
* @param ClassId: The Class ID
* @retval status
*/
#ifdef USE_USBD_COMPOSITE
uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t ClassId)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[ClassId];
#else
uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId];
#endif /* USE_USBD_COMPOSITE */
USBD_StatusTypeDef ret = USBD_BUSY;
#ifdef USE_USBD_COMPOSITE
/* Get the Endpoints addresses allocated for this class instance */
CDCInEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_IN, USBD_EP_TYPE_BULK, ClassId);
#endif /* USE_USBD_COMPOSITE */
if (hcdc == NULL)
{
return (uint8_t)USBD_FAIL;
}
if (hcdc->TxState == 0U)
{
/* Tx Transfer in progress */
hcdc->TxState = 1U;
/* Update the packet total length */
pdev->ep_in[CDCInEpAdd & 0xFU].total_length = hcdc->TxLength;
/* Transmit next packet */
(void)USBD_LL_Transmit(pdev, CDCInEpAdd, hcdc->TxBuffer, hcdc->TxLength);
ret = USBD_OK;
}
return (uint8_t)ret;
}
/**
* @brief USBD_CDC_ReceivePacket
* prepare OUT Endpoint for reception
* @param pdev: device instance
* @retval status
*/
uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)pdev->pClassDataCmsit[pdev->classId];
#ifdef USE_USBD_COMPOSITE
/* Get the Endpoints addresses allocated for this class instance */
CDCOutEpAdd = USBD_CoreGetEPAdd(pdev, USBD_EP_OUT, USBD_EP_TYPE_BULK, (uint8_t)pdev->classId);
#endif /* USE_USBD_COMPOSITE */
if (pdev->pClassDataCmsit[pdev->classId] == NULL)
{
return (uint8_t)USBD_FAIL;
}
if (pdev->dev_speed == USBD_SPEED_HIGH)
{
/* Prepare Out endpoint to receive next packet */
(void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer,
CDC_DATA_HS_OUT_PACKET_SIZE);
}
else
{
/* Prepare Out endpoint to receive next packet */
(void)USBD_LL_PrepareReceive(pdev, CDCOutEpAdd, hcdc->RxBuffer,
CDC_DATA_FS_OUT_PACKET_SIZE);
}
return (uint8_t)USBD_OK;
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/

211
legacy/stm32/lib/USB_CDC/src/usbd_cdc_if.c
View File

@ -1,211 +0,0 @@
#include "usbd_cdc_if.h"
#include "stm32f7xx_hal.h"
#include "ota.h"
extern USBD_HandleTypeDef hUsbDevice;
volatile uint8_t cdc_streaming = 1; /* auto-stream */
static volatile uint8_t cdc_port_open = 0; /* set when host asserts DTR */
volatile uint8_t cdc_arm_request = 0; /* set by A command */
volatile uint8_t cdc_disarm_request = 0; /* set by D command */
volatile uint8_t cdc_recal_request = 0; /* set by G command — gyro recalibration */
volatile uint8_t cdc_imu_cal_request = 0; /* set by O command — mount offset calibration (Issue #680) */
volatile uint32_t cdc_rx_count = 0; /* total CDC packets received from host */
volatile uint8_t cdc_estop_request = 0;
volatile uint8_t cdc_estop_clear_request = 0;
/*
* PID tuning command buffer.
* CDC_Receive (USB IRQ) copies multi-char commands here.
* Main loop polls cdc_cmd_ready, parses, and clears.
* Commands: P<kp> I<ki> D<kd> T<setpoint> M<max_speed> ?
*/
volatile uint8_t cdc_cmd_ready = 0;
volatile char cdc_cmd_buf[32];
/*
* Jetson command buffer (bidirectional protocol).
* 'H'\n — heartbeat, ISR updates jetson_hb_tick only (no buf copy needed).
* 'C'<s>,<t>\n — drive command: ISR copies to buf, main loop parses with sscanf.
* jetson_hb_tick is also refreshed on every C command.
*/
volatile uint8_t jetson_cmd_ready = 0;
volatile char jetson_cmd_buf[32];
volatile uint32_t jetson_hb_tick = 0; /* HAL_GetTick() of last H or C */
/*
* USB TX/RX buffers grouped into a single 512-byte aligned struct so that
* one MPU region (configured in usbd_conf.c) can mark them non-cacheable.
* Size must be a power-of-2 >= total size for MPU RASR SIZE encoding.
*/
static struct {
uint8_t tx[256];
uint8_t rx[256];
} __attribute__((aligned(512))) usb_nc_buf;
#define UserTxBuffer usb_nc_buf.tx
#define UserRxBuffer usb_nc_buf.rx
/* Exported so usbd_conf.c USB_NC_MPU_Config() can set the region base */
void * const usb_nc_buf_base = &usb_nc_buf;
/*
* Betaflight-proven DFU reboot:
* 1. Write magic to RTC backup register (persists across soft reset)
* 2. NVIC_SystemReset() — clean hardware reset
* 3. Early startup checks magic, clears it, jumps to system bootloader
*
* Magic is written to BKP15R (OTA_DFU_BKP_IDX) — not BKP0R — so that
* BKP0R–BKP6R are available for BNO055 calibration offsets (PR #150).
* The magic check in checkForBootloader() reads the same BKP15R register.
*/
static void request_bootloader(void) {
/* Betaflight-proven: write magic, disable IRQs, reset.
* checkForBootloader() runs on next boot before anything else. */
__HAL_RCC_PWR_CLK_ENABLE();
HAL_PWR_EnableBkUpAccess();
__HAL_RCC_RTC_ENABLE();
/* Write magic to BKP15R via OTA module constants (avoids BNO055 BKP0–6) */
(&RTC->BKP0R)[OTA_DFU_BKP_IDX] = OTA_DFU_MAGIC;
__disable_irq();
NVIC_SystemReset();
}
/*
* Call this VERY early in main(), before HAL_Init().
* Checks RTC backup register for magic value left by request_bootloader().
* If found: clear magic, jump to STM32F7 system bootloader at 0x1FF00000.
*/
void checkForBootloader(void) {
/*
* Betaflight-proven bootloader jump for STM32F7.
* Called VERY early, before HAL_Init/caches/clocks.
* At this point only RCC PWR is needed to read RTC backup regs.
*/
/* Enable backup domain access to read RTC backup register */
__HAL_RCC_PWR_CLK_ENABLE();
HAL_PWR_EnableBkUpAccess();
__HAL_RCC_RTC_ENABLE();
uint32_t magic = (&RTC->BKP0R)[OTA_DFU_BKP_IDX]; /* read BKP15R */
if (magic != OTA_DFU_MAGIC) {
return; /* Normal boot */
}
/* Clear magic so next boot is normal */
(&RTC->BKP0R)[OTA_DFU_BKP_IDX] = 0;
/* Jump to STM32F7 system bootloader at 0x1FF00000.
* Exactly as Betaflight does it — no cache/VTOR/MEMRMP games needed
* because we run before any of that is configured. */
__HAL_RCC_SYSCFG_CLK_ENABLE();
__set_MSP(*(uint32_t *)0x1FF00000);
((void (*)(void))(*(uint32_t *)0x1FF00004))();
while (1);
}
static int8_t CDC_Init(void) {
USBD_CDC_SetTxBuffer(&hUsbDevice, UserTxBuffer, 0);
USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer);
USBD_CDC_ReceivePacket(&hUsbDevice);
/* Reset TxState so CDC_Transmit works after host (re)connects.
* Without this, if transmits happen before host opens port,
* TxState stays BUSY forever since host never ACKs. */
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)hUsbDevice.pClassData;
if (hcdc) hcdc->TxState = 0;
return USBD_OK;
}
static int8_t CDC_DeInit(void) { return USBD_OK; }
static int8_t CDC_Control(uint8_t cmd, uint8_t *pbuf, uint16_t length) {
(void)pbuf; (void)length;
if (cmd == 0x22) { /* CDC_SET_CONTROL_LINE_STATE — host opened port */
cdc_port_open = 1;
cdc_streaming = 1;
}
return USBD_OK;
}
static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
if (*len < 1) goto done;
switch (buf[0]) {
case 'S': cdc_streaming = !cdc_streaming; break;
case 'A': cdc_arm_request = 1; break;
case 'D': cdc_disarm_request = 1; break;
case 'G': cdc_recal_request = 1; break; /* gyro recalibration */
case 'O': cdc_imu_cal_request = 1; break; /* mount offset cal (Issue #680) */
case 'R': request_bootloader(); break; /* never returns */
case 'E': cdc_estop_request = 1; break;
case 'F': cdc_estop_request = 2; break;
case 'Z': cdc_estop_clear_request = 1; break;
/*
* PID tuning: P<kp> I<ki> D<kd> T<setpoint> M<max_speed> ?
* Copy to cmd buffer; main loop parses float (avoids sscanf in IRQ).
*/
case 'P': case 'I': case 'K': case 'T': case 'M': case '?': {
uint32_t copy_len = *len < 31 ? *len : 31;
for (uint32_t i = 0; i < copy_len; i++) cdc_cmd_buf[i] = (char)buf[i];
cdc_cmd_buf[copy_len] = '\0';
cdc_cmd_ready = 1;
break;
}
/* Jetson heartbeat — just refresh the tick, no buffer copy needed */
case 'H':
jetson_hb_tick = HAL_GetTick();
break;
/* Jetson drive command: C<speed>,<steer>\n
* Copy to buffer; main loop parses ints (keeps sscanf out of ISR). */
case 'C': {
uint32_t copy_len = *len < 31 ? *len : 31;
for (uint32_t i = 0; i < copy_len; i++) jetson_cmd_buf[i] = (char)buf[i];
jetson_cmd_buf[copy_len] = '\0';
jetson_hb_tick = HAL_GetTick(); /* C command also refreshes heartbeat */
jetson_cmd_ready = 1;
break;
}
default: break;
}
done:
cdc_rx_count++;
USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer);
USBD_CDC_ReceivePacket(&hUsbDevice);
return USBD_OK;
}
USBD_CDC_ItfTypeDef USBD_CDC_fops = { CDC_Init, CDC_DeInit, CDC_Control, CDC_Receive };
uint8_t CDC_Transmit(uint8_t *buf, uint16_t len) {
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)hUsbDevice.pClassData;
if (hcdc == NULL) return USBD_FAIL;
if (hcdc->TxState != 0) {
/* If stuck busy (no host ACK), force reset after a while */
static uint32_t busy_count = 0;
if (++busy_count > 100) { hcdc->TxState = 0; busy_count = 0; }
return USBD_BUSY;
}
/* Always copy into the static UserTxBuffer so the USB hardware reads
* from a known fixed SRAM address — never from the caller's stack.
* The USB TXFE IRQ fires asynchronously; a stack buffer could be
* overwritten by the time the FIFO is loaded. */
if (len > sizeof(UserTxBuffer)) len = sizeof(UserTxBuffer);
memcpy(UserTxBuffer, buf, len);
USBD_CDC_SetTxBuffer(&hUsbDevice, UserTxBuffer, len);
return USBD_CDC_TransmitPacket(&hUsbDevice);
}

125
legacy/stm32/lib/USB_CDC/src/usbd_conf.c
View File

@ -1,125 +0,0 @@
/* Taken directly from Betaflight: usbd_conf_stm32f7xx.c */
#include "stm32f7xx_hal.h"
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_cdc.h"
#include "usbd_conf.h"
/*
* Mark USB TX/RX buffers non-cacheable via MPU Region 0.
* Cortex-M7 TEX=1, C=0, B=0 → Normal Non-cacheable.
* Called before HAL_PCD_Init() so the region is active before the USB
* hardware ever touches the buffers.
*/
extern void * const usb_nc_buf_base; /* defined in usbd_cdc_if.c */
static void USB_NC_MPU_Config(void)
{
MPU_Region_InitTypeDef r = {0};
HAL_MPU_Disable();
r.Enable = MPU_REGION_ENABLE;
r.Number = MPU_REGION_NUMBER0;
r.BaseAddress = (uint32_t)usb_nc_buf_base;
r.Size = MPU_REGION_SIZE_512B;
r.SubRegionDisable = 0x00;
r.TypeExtField = MPU_TEX_LEVEL1; /* TEX=1 */
r.AccessPermission = MPU_REGION_FULL_ACCESS;
r.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
r.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
r.IsCacheable = MPU_ACCESS_NOT_CACHEABLE; /* C=0 */
r.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE; /* B=0 */
HAL_MPU_ConfigRegion(&r);
HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
}
PCD_HandleTypeDef hpcd;
void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
{
GPIO_InitTypeDef GPIO_InitStruct;
if (hpcd->Instance == USB_OTG_FS) {
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_InitStruct.Pin = (GPIO_PIN_11 | GPIO_PIN_12);
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
__HAL_RCC_USB_OTG_FS_CLK_ENABLE();
HAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0);
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
}
}
void HAL_PCD_MspDeInit(PCD_HandleTypeDef *hpcd)
{
if (hpcd->Instance == USB_OTG_FS) {
__HAL_RCC_USB_OTG_FS_CLK_DISABLE();
__HAL_RCC_SYSCFG_CLK_DISABLE();
}
}
void HAL_PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_SetupStage(hpcd->pData, (uint8_t *)hpcd->Setup); }
void HAL_PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_DataOutStage(hpcd->pData, epnum, hpcd->OUT_ep[epnum].xfer_buff); }
void HAL_PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_DataInStage(hpcd->pData, epnum, hpcd->IN_ep[epnum].xfer_buff); }
void HAL_PCD_SOFCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_SOF(hpcd->pData); }
void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd) {
USBD_LL_Reset(hpcd->pData);
USBD_LL_SetSpeed(hpcd->pData, USBD_SPEED_FULL);
}
void HAL_PCD_SuspendCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_Suspend(hpcd->pData); }
void HAL_PCD_ResumeCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_Resume(hpcd->pData); }
void HAL_PCD_ISOOUTIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_IsoOUTIncomplete(hpcd->pData, epnum); }
void HAL_PCD_ISOINIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum) { USBD_LL_IsoINIncomplete(hpcd->pData, epnum); }
void HAL_PCD_ConnectCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_DevConnected(hpcd->pData); }
void HAL_PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd) { USBD_LL_DevDisconnected(hpcd->pData); }
USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev)
{
hpcd.Instance = USB_OTG_FS;
hpcd.Init.dev_endpoints = 4;
hpcd.Init.use_dedicated_ep1 = 0;
hpcd.Init.ep0_mps = 0x40;
hpcd.Init.dma_enable = 0;
hpcd.Init.low_power_enable = 0;
hpcd.Init.phy_itface = PCD_PHY_EMBEDDED;
hpcd.Init.Sof_enable = 0;
hpcd.Init.speed = PCD_SPEED_FULL;
hpcd.Init.vbus_sensing_enable = 0;
hpcd.Init.lpm_enable = 0;
hpcd.pData = pdev;
pdev->pData = &hpcd;
USB_NC_MPU_Config(); /* Mark USB buffers non-cacheable before USB hardware init */
HAL_PCD_Init(&hpcd);
HAL_PCDEx_SetRxFiFo(&hpcd, 0x80);
HAL_PCDEx_SetTxFiFo(&hpcd, 0, 0x40);
HAL_PCDEx_SetTxFiFo(&hpcd, 1, 0x80);
return USBD_OK;
}
USBD_StatusTypeDef USBD_LL_DeInit(USBD_HandleTypeDef *pdev) { HAL_PCD_DeInit(pdev->pData); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_Start(USBD_HandleTypeDef *pdev) { HAL_PCD_Start(pdev->pData); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_Stop(USBD_HandleTypeDef *pdev) { HAL_PCD_Stop(pdev->pData); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_OpenEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t ep_type, uint16_t ep_mps) { HAL_PCD_EP_Open(pdev->pData, ep_addr, ep_mps, ep_type); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_CloseEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_Close(pdev->pData, ep_addr); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_FlushEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_Flush(pdev->pData, ep_addr); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_StallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_SetStall(pdev->pData, ep_addr); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_ClearStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { HAL_PCD_EP_ClrStall(pdev->pData, ep_addr); return USBD_OK; }
uint8_t USBD_LL_IsStallEP(USBD_HandleTypeDef *pdev, uint8_t ep_addr) {
PCD_HandleTypeDef *hpcd = pdev->pData;
if ((ep_addr & 0x80) == 0x80) return hpcd->IN_ep[ep_addr & 0x7F].is_stall;
else return hpcd->OUT_ep[ep_addr & 0x7F].is_stall;
}
USBD_StatusTypeDef USBD_LL_SetUSBAddress(USBD_HandleTypeDef *pdev, uint8_t dev_addr) { HAL_PCD_SetAddress(pdev->pData, dev_addr); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_Transmit(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t *pbuf, uint32_t size) { HAL_PCD_EP_Transmit(pdev->pData, ep_addr, pbuf, size); return USBD_OK; }
USBD_StatusTypeDef USBD_LL_PrepareReceive(USBD_HandleTypeDef *pdev, uint8_t ep_addr, uint8_t *pbuf, uint32_t size) { HAL_PCD_EP_Receive(pdev->pData, ep_addr, pbuf, size); return USBD_OK; }
uint32_t USBD_LL_GetRxDataSize(USBD_HandleTypeDef *pdev, uint8_t ep_addr) { return HAL_PCD_EP_GetRxCount(pdev->pData, ep_addr); }
void USBD_LL_Delay(uint32_t Delay) { HAL_Delay(Delay); }

1215
legacy/stm32/lib/USB_CDC/src/usbd_core.c
View File

File diff suppressed because it is too large Load Diff

1058
legacy/stm32/lib/USB_CDC/src/usbd_ctlreq.c
View File

File diff suppressed because it is too large Load Diff

63
legacy/stm32/lib/USB_CDC/src/usbd_desc.c
View File

@ -1,63 +0,0 @@
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_conf.h"
#define USBD_VID 0x0483 /* STMicroelectronics */
#define USBD_PID_FS 0x5740 /* CDC Virtual COM Port */
#define USBD_LANGID_STRING 0x0409 /* English US */
#define USBD_MFR_STRING "SaltyLab"
#define USBD_PRODUCT_STRING "SaltyLab IMU"
#define USBD_SERIAL_STRING "SALTY001"
static uint8_t *USBD_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
static uint8_t *USBD_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
static uint8_t *USBD_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
static uint8_t *USBD_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
static uint8_t *USBD_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
static uint8_t *USBD_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
static uint8_t *USBD_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length);
USBD_DescriptorsTypeDef SaltyLab_Desc = {
USBD_DeviceDescriptor,
USBD_LangIDStrDescriptor,
USBD_ManufacturerStrDescriptor,
USBD_ProductStrDescriptor,
USBD_SerialStrDescriptor,
USBD_ConfigStrDescriptor,
USBD_InterfaceStrDescriptor,
};
static uint8_t USBD_DeviceDesc[USB_LEN_DEV_DESC] = {
0x12, USB_DESC_TYPE_DEVICE, 0x00, 0x02, 0x02, 0x02, 0x00,
64, LOBYTE(USBD_VID), HIBYTE(USBD_VID), LOBYTE(USBD_PID_FS), HIBYTE(USBD_PID_FS),
0x00, 0x02, 1, 2, 3, 1
};
static uint8_t USBD_LangIDDesc[USB_LEN_LANGID_STR_DESC] = {
USB_LEN_LANGID_STR_DESC, USB_DESC_TYPE_STRING,
LOBYTE(USBD_LANGID_STRING), HIBYTE(USBD_LANGID_STRING)
};
static uint8_t USBD_StrDesc[USBD_MAX_STR_DESC_SIZ];
static uint8_t *USBD_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) {
(void)speed; *length = sizeof(USBD_DeviceDesc); return USBD_DeviceDesc;
}
static uint8_t *USBD_LangIDStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) {
(void)speed; *length = sizeof(USBD_LangIDDesc); return USBD_LangIDDesc;
}
static uint8_t *USBD_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) {
USBD_GetString((uint8_t *)USBD_MFR_STRING, USBD_StrDesc, length); return USBD_StrDesc;
}
static uint8_t *USBD_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) {
USBD_GetString((uint8_t *)USBD_PRODUCT_STRING, USBD_StrDesc, length); return USBD_StrDesc;
}
static uint8_t *USBD_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) {
USBD_GetString((uint8_t *)USBD_SERIAL_STRING, USBD_StrDesc, length); return USBD_StrDesc;
}
static uint8_t *USBD_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) {
USBD_GetString((uint8_t *)"CDC Config", USBD_StrDesc, length); return USBD_StrDesc;
}
static uint8_t *USBD_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) {
USBD_GetString((uint8_t *)"CDC Interface", USBD_StrDesc, length); return USBD_StrDesc;
}

224
legacy/stm32/lib/USB_CDC/src/usbd_ioreq.c
View File

@ -1,224 +0,0 @@
/**
******************************************************************************
* @file usbd_ioreq.c
* @author MCD Application Team
* @brief This file provides the IO requests APIs for control endpoints.
******************************************************************************
* @attention
*
* Copyright (c) 2015 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "usbd_ioreq.h"
/** @addtogroup STM32_USB_DEVICE_LIBRARY
* @{
*/
/** @defgroup USBD_IOREQ
* @brief control I/O requests module
* @{
*/
/** @defgroup USBD_IOREQ_Private_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Private_Defines
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Private_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Private_Variables
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Private_FunctionPrototypes
* @{
*/
/**
* @}
*/
/** @defgroup USBD_IOREQ_Private_Functions
* @{
*/
/**
* @brief USBD_CtlSendData
* send data on the ctl pipe
* @param pdev: device instance
* @param buff: pointer to data buffer
* @param len: length of data to be sent
* @retval status
*/
USBD_StatusTypeDef USBD_CtlSendData(USBD_HandleTypeDef *pdev,
uint8_t *pbuf, uint32_t len)
{
/* Set EP0 State */
pdev->ep0_state = USBD_EP0_DATA_IN;
pdev->ep_in[0].total_length = len;
#ifdef USBD_AVOID_PACKET_SPLIT_MPS
pdev->ep_in[0].rem_length = 0U;
#else
pdev->ep_in[0].rem_length = len;
#endif /* USBD_AVOID_PACKET_SPLIT_MPS */
/* Start the transfer */
(void)USBD_LL_Transmit(pdev, 0x00U, pbuf, len);
return USBD_OK;
}
/**
* @brief USBD_CtlContinueSendData
* continue sending data on the ctl pipe
* @param pdev: device instance
* @param buff: pointer to data buffer
* @param len: length of data to be sent
* @retval status
*/
USBD_StatusTypeDef USBD_CtlContinueSendData(USBD_HandleTypeDef *pdev,
uint8_t *pbuf, uint32_t len)
{
/* Start the next transfer */
(void)USBD_LL_Transmit(pdev, 0x00U, pbuf, len);
return USBD_OK;
}
/**
* @brief USBD_CtlPrepareRx
* receive data on the ctl pipe
* @param pdev: device instance
* @param buff: pointer to data buffer
* @param len: length of data to be received
* @retval status
*/
USBD_StatusTypeDef USBD_CtlPrepareRx(USBD_HandleTypeDef *pdev,
uint8_t *pbuf, uint32_t len)
{
/* Set EP0 State */
pdev->ep0_state = USBD_EP0_DATA_OUT;
pdev->ep_out[0].total_length = len;
#ifdef USBD_AVOID_PACKET_SPLIT_MPS
pdev->ep_out[0].rem_length = 0U;
#else
pdev->ep_out[0].rem_length = len;
#endif /* USBD_AVOID_PACKET_SPLIT_MPS */
/* Start the transfer */
(void)USBD_LL_PrepareReceive(pdev, 0U, pbuf, len);
return USBD_OK;
}
/**
* @brief USBD_CtlContinueRx
* continue receive data on the ctl pipe
* @param pdev: device instance
* @param buff: pointer to data buffer
* @param len: length of data to be received
* @retval status
*/
USBD_StatusTypeDef USBD_CtlContinueRx(USBD_HandleTypeDef *pdev,
uint8_t *pbuf, uint32_t len)
{
(void)USBD_LL_PrepareReceive(pdev, 0U, pbuf, len);
return USBD_OK;
}
/**
* @brief USBD_CtlSendStatus
* send zero lzngth packet on the ctl pipe
* @param pdev: device instance
* @retval status
*/
USBD_StatusTypeDef USBD_CtlSendStatus(USBD_HandleTypeDef *pdev)
{
/* Set EP0 State */
pdev->ep0_state = USBD_EP0_STATUS_IN;
/* Start the transfer */
(void)USBD_LL_Transmit(pdev, 0x00U, NULL, 0U);
return USBD_OK;
}
/**
* @brief USBD_CtlReceiveStatus
* receive zero lzngth packet on the ctl pipe
* @param pdev: device instance
* @retval status
*/
USBD_StatusTypeDef USBD_CtlReceiveStatus(USBD_HandleTypeDef *pdev)
{
/* Set EP0 State */
pdev->ep0_state = USBD_EP0_STATUS_OUT;
/* Start the transfer */
(void)USBD_LL_PrepareReceive(pdev, 0U, NULL, 0U);
return USBD_OK;
}
/**
* @brief USBD_GetRxCount
* returns the received data length
* @param pdev: device instance
* @param ep_addr: endpoint address
* @retval Rx Data blength
*/
uint32_t USBD_GetRxCount(USBD_HandleTypeDef *pdev, uint8_t ep_addr)
{
return USBD_LL_GetRxDataSize(pdev, ep_addr);
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/

19
legacy/stm32/platformio.ini
View File

@ -1,19 +0,0 @@
[env:f722]
platform = ststm32
board = nucleo_f722ze
framework = stm32cube
upload_protocol = dfu
upload_command = /opt/homebrew/bin/dfu-util -a 0 -s 0x08000000:leave -D $SOURCE
monitor_speed = 115200
board_build.mcu = stm32f722ret6
board_build.f_cpu = 216000000L
build_flags =
-DESP32xx
-DUSE_HAL_DRIVER
-DHSE_VALUE=8000000U
-DUSE_USB_FS
-I include
-Os
-Wl,--defsym,_Min_Heap_Size=0x2000
-Wl,--defsym,_Min_Stack_Size=0x1000
-Wl,--defsym,__stack_end=_estack-0x1000

238
legacy/stm32/scripts/flash_firmware.py
View File

@ -1,238 +0,0 @@
#!/usr/bin/env python3
"""SaltyLab Firmware OTA Flash Script — Issue #124
Flashes firmware via USB DFU using dfu-util.
Supports CRC32 integrity verification and host-side backup/rollback.
Usage:
python flash_firmware.py firmware.bin [options]
python flash_firmware.py --rollback
python flash_firmware.py firmware.bin --trigger-dfu /dev/ttyUSB0
Options:
--vid HEX USB vendor ID (default: 0x0483 — STMicroelectronics)
--pid HEX USB product ID (default: 0xDF11 — DFU mode)
--alt N DFU alt setting (default: 0 — internal flash)
--rollback Flash the previous firmware backup
--trigger-dfu PORT Send DFU_ENTER over JLink UART before flashing
--dry-run Print dfu-util command but do not execute
Requirements:
pip install pyserial (only if using --trigger-dfu)
dfu-util >= 0.9 installed and in PATH
Dual-bank note:
ESP32 has single-bank 512 KB flash; hardware A/B rollback is not
supported. Rollback is implemented here by saving a backup of the
previous binary (.firmware_backup.bin) before each flash.
"""
import argparse
import binascii
import os
import shutil
import struct
import subprocess
import sys
import time
# ---- ESP32 flash constants ----
FLASH_BASE = 0x08000000
FLASH_SIZE = 0x80000 # 512 KB
# ---- DFU device defaults (ESP32/STM32 system bootloader) ----
DFU_VID = 0x0483 # STMicroelectronics
DFU_PID = 0xDF11 # DFU mode
BACKUP_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)),
'.firmware_backup.bin')
# ---- CRC utilities ----
def crc32_file(path: str) -> int:
"""
Compute CRC-32/ISO-HDLC (standard Python binascii.crc32) of a file.
Used for pre-flash integrity verification; consistent across runs.
"""
with open(path, 'rb') as fh:
data = fh.read()
return binascii.crc32(data) & 0xFFFFFFFF
def stm32_crc32(data: bytes) -> int:
"""
Compute CRC-32/MPEG-2 matching ESP32 hardware CRC unit.
ESP32/STM32 algorithm:
Polynomial : 0x04C11DB7
Initial : 0xFFFFFFFF
Width : 32 bits
Reflection : none (MSB-first)
Feed size : 32-bit words from flash (little-endian CPU read)
When the ESP32 BALANCE reads a flash word it gets a little-endian uint32;
the hardware CRC unit processes bits[31:24] first, then [23:16],
[15:8], [7:0]. This Python implementation replicates that behaviour.
data should be padded to a 4-byte boundary with 0xFF before calling.
"""
if len(data) % 4:
data += b'\xff' * (4 - len(data) % 4)
crc = 0xFFFFFFFF
for i in range(0, len(data), 4):
# Little-endian: byte0 is LSB, byte3 is MSB of the 32-bit word
word = struct.unpack_from('<I', data, i)[0]
crc ^= word
for _ in range(32):
if crc & 0x80000000:
crc = ((crc << 1) ^ 0x04C11DB7) & 0xFFFFFFFF
else:
crc = (crc << 1) & 0xFFFFFFFF
return crc
# ---- JLink protocol helpers ----
def _crc16_xmodem(data: bytes) -> int:
"""CRC-16/XMODEM (poly 0x1021, init 0x0000) — JLink frame CRC."""
crc = 0x0000
for b in data:
crc ^= b << 8
for _ in range(8):
if crc & 0x8000:
crc = ((crc << 1) ^ 0x1021) & 0xFFFF
else:
crc = (crc << 1) & 0xFFFF
return crc
def _build_jlink_frame(cmd: int, payload: bytes = b'') -> bytes:
"""Build a JLink binary frame: [STX][LEN][CMD][PAYLOAD][CRC_hi][CRC_lo][ETX]."""
STX, ETX = 0x02, 0x03
body = bytes([cmd]) + payload
length = len(body)
crc = _crc16_xmodem(body)
return bytes([STX, length, cmd]) + payload + bytes([crc >> 8, crc & 0xFF, ETX])
def trigger_dfu_via_jlink(port: str, baud: int = 921600) -> None:
"""Send JLINK_CMD_DFU_ENTER (0x06) over USART1 to put device in DFU mode."""
try:
import serial
except ImportError:
print("ERROR: pyserial not installed. Run: pip install pyserial",
file=sys.stderr)
sys.exit(1)
frame = _build_jlink_frame(0x06) # JLINK_CMD_DFU_ENTER, no payload
with serial.Serial(port, baud, timeout=2) as ser:
ser.write(frame)
time.sleep(0.1)
print(f"DFU_ENTER sent to {port} ({len(frame)} bytes)")
# ---- Flash ----
def flash(bin_path: str, vid: int, pid: int, alt: int = 0,
dry_run: bool = False) -> int:
"""
Flash firmware using dfu-util. Returns the process exit code.
Uses --dfuse-address with :leave to reset into application after flash.
"""
addr = f'0x{FLASH_BASE:08X}'
cmd = [
'dfu-util',
'--device', f'{vid:04x}:{pid:04x}',
'--alt', str(alt),
'--dfuse-address', f'{addr}:leave',
'--download', bin_path,
]
print('Running:', ' '.join(cmd))
if dry_run:
print('[dry-run] skipping dfu-util execution')
return 0
return subprocess.call(cmd)
# ---- Main ----
def main() -> int:
parser = argparse.ArgumentParser(
description='SaltyLab firmware OTA flash via USB DFU (Issue #124)'
)
parser.add_argument('firmware', nargs='?',
help='Firmware .bin file to flash')
parser.add_argument('--vid', type=lambda x: int(x, 0), default=DFU_VID,
help=f'USB vendor ID (default: 0x{DFU_VID:04X})')
parser.add_argument('--pid', type=lambda x: int(x, 0), default=DFU_PID,
help=f'USB product ID (default: 0x{DFU_PID:04X})')
parser.add_argument('--alt', type=int, default=0,
help='DFU alt setting (default: 0 — internal flash)')
parser.add_argument('--rollback', action='store_true',
help='Flash the previous firmware backup')
parser.add_argument('--trigger-dfu', metavar='PORT',
help='Trigger DFU via JLink UART before flashing '
'(e.g. /dev/ttyUSB0 or COM3)')
parser.add_argument('--dry-run', action='store_true',
help='Print dfu-util command without executing it')
args = parser.parse_args()
# Optionally trigger DFU mode over JLink serial
if args.trigger_dfu:
trigger_dfu_via_jlink(args.trigger_dfu)
print('Waiting 3 s for USB DFU enumeration…')
time.sleep(3)
# Determine target binary
if args.rollback:
if not os.path.exists(BACKUP_PATH):
print(f'ERROR: No backup found at {BACKUP_PATH}', file=sys.stderr)
return 1
target = BACKUP_PATH
print(f'Rolling back to {BACKUP_PATH}')
elif args.firmware:
target = args.firmware
else:
parser.print_help()
return 1
if not os.path.exists(target):
print(f'ERROR: File not found: {target}', file=sys.stderr)
return 1
# CRC32 integrity check
crc_std = crc32_file(target)
size = os.path.getsize(target)
print(f'Binary : {target} ({size} bytes)')
print(f'CRC-32 : 0x{crc_std:08X} (ISO-HDLC)')
if size > FLASH_SIZE:
print(f'ERROR: Binary ({size} bytes) exceeds flash size '
f'({FLASH_SIZE} bytes)', file=sys.stderr)
return 1
# ESP32/STM32 hardware CRC (for cross-checking with firmware telemetry)
with open(target, 'rb') as fh:
bin_data = fh.read()
crc_hw = stm32_crc32(bin_data.ljust(FLASH_SIZE, b'\xff'))
print(f'CRC-32 : 0x{crc_hw:08X} (MPEG-2 / ESP32/STM32 HW, padded to {FLASH_SIZE // 1024} KB)')
# Save backup before flashing (skip when rolling back)
if not args.rollback:
shutil.copy2(target, BACKUP_PATH)
print(f'Backup : {BACKUP_PATH}')
# Flash
rc = flash(target, args.vid, args.pid, args.alt, args.dry_run)
if rc == 0:
print('Flash complete — device should reset into application.')
else:
print(f'ERROR: dfu-util exited with code {rc}', file=sys.stderr)
return rc
if __name__ == '__main__':
sys.exit(main())

352
legacy/stm32/src/audio.c
View File

@ -1,352 +0,0 @@
#include "audio.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
/* ================================================================
* Buffer layout
* ================================================================
* AUDIO_BUF_HALF samples per DMA half (config.h: 441 at 22050 Hz → 20 ms).
* DMA runs in circular mode over 2 halves; TxHalfCplt refills [0] and
* TxCplt refills [AUDIO_BUF_HALF]. Both callbacks simply call fill_half().
*/
#define AUDIO_BUF_SIZE (AUDIO_BUF_HALF * 2u)
/* DMA buffer: must be in non-cached SRAM or flushed before DMA access.
* Placed in SRAM1 (default .bss section on STM32F722 — below 512 KB).
* The I-cache / D-cache is on by default; DMA1 is an AHB master that
* bypasses cache, so we keep this buffer in DTCM-uncacheable SRAM. */
static int16_t s_dma_buf[AUDIO_BUF_SIZE];
/* ================================================================
* PCM FIFO — Jetson audio (main-loop writer, ISR reader)
* ================================================================
* Single-producer / single-consumer lock-free ring buffer.
* Power-of-2 size so wrap uses bitwise AND (safe across ISR boundary).
* 4096 samples ≈ 185 ms at 22050 Hz — enough to absorb JLink jitter.
*/
#define PCM_FIFO_SIZE 4096u
#define PCM_FIFO_MASK (PCM_FIFO_SIZE - 1u)
static int16_t s_pcm_fifo[PCM_FIFO_SIZE];
static volatile uint16_t s_pcm_rd = 0; /* consumer (ISR advances) */
static volatile uint16_t s_pcm_wr = 0; /* producer (main loop advances) */
/* ================================================================
* Tone sequencer
* ================================================================
* Each AudioTone maps to a const ToneStep array. Steps are played
* sequentially; a gap_ms of 0 means no silence between steps.
* audio_tick() in the main loop advances the state machine and
* writes the volatile active-tone params read by the ISR fill path.
*/
typedef struct {
uint16_t freq_hz;
uint16_t dur_ms;
uint16_t gap_ms; /* silence after this step */
} ToneStep;
/* ---- Tone definitions ---- */
static const ToneStep s_def_beep_short[] = {{880, 100, 0}};
static const ToneStep s_def_beep_long[] = {{880, 500, 0}};
static const ToneStep s_def_startup[] = {{523, 120, 60},
{659, 120, 60},
{784, 200, 0}};
static const ToneStep s_def_arm[] = {{880, 80, 60},
{1047, 100, 0}};
static const ToneStep s_def_disarm[] = {{880, 80, 60},
{659, 100, 0}};
static const ToneStep s_def_fault[] = {{200, 500, 0}};
typedef struct {
const ToneStep *steps;
uint8_t n_steps;
} ToneDef;
static const ToneDef s_tone_defs[AUDIO_TONE_COUNT] = {
[AUDIO_TONE_BEEP_SHORT] = {s_def_beep_short, 1},
[AUDIO_TONE_BEEP_LONG] = {s_def_beep_long, 1},
[AUDIO_TONE_STARTUP] = {s_def_startup, 3},
[AUDIO_TONE_ARM] = {s_def_arm, 2},
[AUDIO_TONE_DISARM] = {s_def_disarm, 2},
[AUDIO_TONE_FAULT] = {s_def_fault, 1},
};
/* Active tone queue */
#define TONE_QUEUE_DEPTH 4u
typedef struct {
const ToneDef *def;
uint8_t step; /* current step index within def */
bool in_gap; /* true while playing inter-step silence */
uint32_t step_end_ms; /* abs HAL_GetTick() when step/gap expires */
} ToneSeq;
static ToneSeq s_tone_q[TONE_QUEUE_DEPTH];
static uint8_t s_tq_head = 0; /* consumer (audio_tick reads) */
static uint8_t s_tq_tail = 0; /* producer (audio_play_tone writes) */
/* Volatile parameters written by audio_tick(), read by ISR fill_half() */
static volatile uint16_t s_active_freq = 0; /* 0 = silence / gap */
static volatile uint32_t s_active_phase = 0; /* sample phase counter */
/* ================================================================
* Volume
* ================================================================ */
static uint8_t s_volume = AUDIO_VOLUME_DEFAULT; /* 0–100 */
/* ================================================================
* HAL handles
* ================================================================ */
static I2S_HandleTypeDef s_i2s;
static DMA_HandleTypeDef s_dma_tx;
/* ================================================================
* fill_half() — called from ISR, O(AUDIO_BUF_HALF)
* ================================================================
* Priority: PCM FIFO (Jetson TTS) > square-wave tone > silence.
* Volume applied via integer scaling — no float in ISR.
*/
static void fill_half(int16_t *buf, uint16_t n)
{
uint16_t rd = s_pcm_rd;
uint16_t wr = s_pcm_wr;
uint16_t avail = (uint16_t)((wr - rd) & PCM_FIFO_MASK);
if (avail >= n) {
/* ---- Drain Jetson PCM FIFO ---- */
for (uint16_t i = 0; i < n; i++) {
int32_t s = (int32_t)s_pcm_fifo[rd] * (int32_t)s_volume / 100;
buf[i] = (s > 32767) ? (int16_t)32767 :
(s < -32768) ? (int16_t)-32768 : (int16_t)s;
rd = (rd + 1u) & PCM_FIFO_MASK;
}
s_pcm_rd = rd;
} else if (s_active_freq) {
/* ---- Square wave tone generator ---- */
uint32_t half_p = (uint32_t)AUDIO_SAMPLE_RATE / (2u * s_active_freq);
int16_t amp = (int16_t)(16384u * (uint32_t)s_volume / 100u);
uint32_t ph = s_active_phase;
uint32_t period = 2u * half_p;
for (uint16_t i = 0; i < n; i++) {
buf[i] = ((ph % period) < half_p) ? amp : (int16_t)(-amp);
ph++;
}
s_active_phase = ph;
} else {
/* ---- Silence ---- */
memset(buf, 0, (size_t)(n * 2u));
}
}
/* ================================================================
* DMA callbacks (ISR context)
* ================================================================ */
void HAL_I2S_TxHalfCpltCallback(I2S_HandleTypeDef *hi2s)
{
if (hi2s->Instance == SPI3)
fill_half(s_dma_buf, AUDIO_BUF_HALF);
}
void HAL_I2S_TxCpltCallback(I2S_HandleTypeDef *hi2s)
{
if (hi2s->Instance == SPI3)
fill_half(s_dma_buf + AUDIO_BUF_HALF, AUDIO_BUF_HALF);
}
/* DMA1 Stream7 IRQ (SPI3/I2S3 TX) */
void DMA1_Stream7_IRQHandler(void)
{
HAL_DMA_IRQHandler(&s_dma_tx);
}
/* ================================================================
* audio_init()
* ================================================================ */
void audio_init(void)
{
/* ---- PLLI2S: N=192, R=2 → 96 MHz I2S clock ----
* With SPI3/I2S3 and I2S_DATAFORMAT_16B (16-bit, 32-bit frame slot):
* FS = 96 MHz / (32 × 2 × I2SDIV) where HAL picks I2SDIV = 68
* → 96 000 000 / (32 × 2 × 68) = 22 058 Hz (< 0.04 % error)
*/
RCC_PeriphCLKInitTypeDef pclk = {0};
pclk.PeriphClockSelection = RCC_PERIPHCLK_I2S;
pclk.I2sClockSelection = RCC_I2SCLKSOURCE_PLLI2S;
pclk.PLLI2S.PLLI2SN = 192; /* VCO = (HSE/PLLM) × 192 = 192 MHz */
pclk.PLLI2S.PLLI2SR = 2; /* I2S clock = 96 MHz */
HAL_RCCEx_PeriphCLKConfig(&pclk);
/* ---- GPIO ---- */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
GPIO_InitTypeDef gpio = {0};
/* PA15: I2S3_WS (LRCLK), AF6 */
gpio.Pin = AUDIO_LRCK_PIN;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_NOPULL;
gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
gpio.Alternate = GPIO_AF6_SPI3;
HAL_GPIO_Init(AUDIO_LRCK_PORT, &gpio);
/* PC10: I2S3_CK (BCLK), AF6 */
gpio.Pin = AUDIO_BCLK_PIN;
HAL_GPIO_Init(AUDIO_BCLK_PORT, &gpio);
/* PB5: I2S3_SD (DIN), AF6 */
gpio.Pin = AUDIO_DOUT_PIN;
HAL_GPIO_Init(AUDIO_DOUT_PORT, &gpio);
/* PC5: AUDIO_MUTE GPIO output — drive low (muted) initially */
gpio.Pin = AUDIO_MUTE_PIN;
gpio.Mode = GPIO_MODE_OUTPUT_PP;
gpio.Pull = GPIO_NOPULL;
gpio.Speed = GPIO_SPEED_FREQ_LOW;
gpio.Alternate = 0;
HAL_GPIO_Init(AUDIO_MUTE_PORT, &gpio);
HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_RESET);
/* ---- DMA1 Stream7 Channel0 (SPI3/I2S3 TX) ---- */
__HAL_RCC_DMA1_CLK_ENABLE();
s_dma_tx.Instance = DMA1_Stream7;
s_dma_tx.Init.Channel = DMA_CHANNEL_0;
s_dma_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
s_dma_tx.Init.PeriphInc = DMA_PINC_DISABLE;
s_dma_tx.Init.MemInc = DMA_MINC_ENABLE;
s_dma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
s_dma_tx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
s_dma_tx.Init.Mode = DMA_CIRCULAR;
s_dma_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
s_dma_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
HAL_DMA_Init(&s_dma_tx);
__HAL_LINKDMA(&s_i2s, hdmatx, s_dma_tx);
HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 7, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn);
/* ---- SPI3 in I2S3 master TX mode ---- */
__HAL_RCC_SPI3_CLK_ENABLE();
s_i2s.Instance = SPI3;
s_i2s.Init.Mode = I2S_MODE_MASTER_TX;
s_i2s.Init.Standard = I2S_STANDARD_PHILIPS;
s_i2s.Init.DataFormat = I2S_DATAFORMAT_16B;
s_i2s.Init.MCLKOutput = I2S_MCLKOUTPUT_DISABLE;
s_i2s.Init.AudioFreq = I2S_AUDIOFREQ_22K;
s_i2s.Init.CPOL = I2S_CPOL_LOW;
s_i2s.Init.ClockSource = I2S_CLOCK_PLL;
HAL_I2S_Init(&s_i2s);
/* Pre-fill with silence and start circular DMA TX */
memset(s_dma_buf, 0, sizeof(s_dma_buf));
HAL_I2S_Transmit_DMA(&s_i2s, (uint16_t *)s_dma_buf, AUDIO_BUF_SIZE);
/* Unmute amp after DMA is running — avoids start-up click */
HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_SET);
}
/* ================================================================
* Public API
* ================================================================ */
void audio_mute(bool active)
{
HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN,
active ? GPIO_PIN_SET : GPIO_PIN_RESET);
}
void audio_set_volume(uint8_t vol)
{
s_volume = (vol > 100u) ? 100u : vol;
}
bool audio_play_tone(AudioTone tone)
{
if (tone >= AUDIO_TONE_COUNT) return false;
uint8_t next = (s_tq_tail + 1u) % TONE_QUEUE_DEPTH;
if (next == s_tq_head) return false; /* queue full */
s_tone_q[s_tq_tail].def = &s_tone_defs[tone];
s_tone_q[s_tq_tail].step = 0;
s_tone_q[s_tq_tail].in_gap = false;
s_tone_q[s_tq_tail].step_end_ms = 0; /* audio_tick() sets this on first run */
s_tq_tail = next;
return true;
}
uint16_t audio_write_pcm(const int16_t *samples, uint16_t n)
{
uint16_t wr = s_pcm_wr;
uint16_t rd = s_pcm_rd;
uint16_t space = (uint16_t)((rd - wr - 1u) & PCM_FIFO_MASK);
uint16_t accept = (n < space) ? n : space;
for (uint16_t i = 0; i < accept; i++) {
s_pcm_fifo[wr] = samples[i];
wr = (wr + 1u) & PCM_FIFO_MASK;
}
s_pcm_wr = wr;
return accept;
}
void audio_tick(uint32_t now_ms)
{
/* Nothing to do if queue is empty */
if (s_tq_head == s_tq_tail) {
s_active_freq = 0;
return;
}
ToneSeq *seq = &s_tone_q[s_tq_head];
/* First call for this sequence entry: arm the first step */
if (seq->step_end_ms == 0u) {
const ToneStep *st = &seq->def->steps[0];
seq->in_gap = false;
seq->step_end_ms = now_ms + st->dur_ms;
s_active_freq = st->freq_hz;
s_active_phase = 0;
return;
}
/* Step / gap still running */
if (now_ms < seq->step_end_ms) return;
/* Current step or gap has expired */
const ToneStep *st = &seq->def->steps[seq->step];
if (!seq->in_gap && st->gap_ms) {
/* Transition: tone → inter-step gap (silence) */
seq->in_gap = true;
seq->step_end_ms = now_ms + st->gap_ms;
s_active_freq = 0;
return;
}
/* Advance to next step */
seq->step++;
seq->in_gap = false;
if (seq->step >= seq->def->n_steps) {
/* Sequence complete — pop from queue */
s_tq_head = (s_tq_head + 1u) % TONE_QUEUE_DEPTH;
s_active_freq = 0;
return;
}
/* Start next step */
st = &seq->def->steps[seq->step];
seq->step_end_ms = now_ms + st->dur_ms;
s_active_freq = st->freq_hz;
s_active_phase = 0;
}
bool audio_is_playing(void)
{
return (s_i2s.State == HAL_I2S_STATE_BUSY_TX);
}

121
legacy/stm32/src/balance.c
View File

@ -1,121 +0,0 @@
#include "balance.h"
#include "slope_estimator.h"
#include "config.h"
#include <math.h>
void balance_init(balance_t *b) {
b->state = BALANCE_DISARMED;
b->pitch_deg = 0.0f;
b->pitch_rate = 0.0f;
b->integral = 0.0f;
b->prev_error = 0.0f;
b->motor_cmd = 0;
/* Default PID — conservative starting point */
b->kp = PID_KP;
b->ki = PID_KI;
b->kd = PID_KD;
b->setpoint = 0.0f;
/* Safety defaults from config */
b->max_tilt = MAX_TILT_DEG;
b->max_speed = MAX_SPEED_LIMIT;
/* Slope estimator */
slope_estimator_init(&b->slope);
}
void balance_update(balance_t *b, const IMUData *imu, float dt) {
if (dt <= 0.0f || dt > 0.1f) return; /* Sanity check dt */
/* Consume fused angle from mpu6000 complementary filter */
b->pitch_deg = imu->pitch;
b->pitch_rate = imu->pitch_rate;
/* Advance slope estimator (no-op when not armed) */
slope_estimator_update(&b->slope, b->pitch_deg, dt);
/* Safety: tilt cutoff */
if (b->state == BALANCE_ARMED) {
if (fabsf(b->pitch_deg) > b->max_tilt) {
b->state = BALANCE_TILT_FAULT;
b->motor_cmd = 0;
b->integral = 0.0f;
slope_estimator_reset(&b->slope);
return;
}
}
if (b->state != BALANCE_ARMED) {
b->motor_cmd = 0;
b->integral = 0.0f;
b->prev_error = 0.0f;
return;
}
/* PID — subtract slope estimate so controller balances on incline */
float tilt_corrected = b->pitch_deg - slope_estimator_get_deg(&b->slope);
float error = b->setpoint - tilt_corrected;
/* Proportional */
float p_term = b->kp * error;
/* Integral with anti-windup */
b->integral += error * dt;
if (b->integral > PID_INTEGRAL_MAX) b->integral = PID_INTEGRAL_MAX;
if (b->integral < -PID_INTEGRAL_MAX) b->integral = -PID_INTEGRAL_MAX;
float i_term = b->ki * b->integral;
/* Derivative on measurement (avoids setpoint kick) */
float d_term = b->kd * (-b->pitch_rate);
/* Sum and clamp */
float output = p_term + i_term + d_term;
if (output > (float)b->max_speed) output = (float)b->max_speed;
if (output < -(float)b->max_speed) output = -(float)b->max_speed;
b->motor_cmd = (int16_t)output;
b->prev_error = error;
}
void balance_arm(balance_t *b) {
if (b->state == BALANCE_DISARMED) {
/* Only arm if roughly upright */
if (fabsf(b->pitch_deg) < 10.0f) {
b->integral = 0.0f;
b->prev_error = 0.0f;
b->motor_cmd = 0;
b->state = BALANCE_ARMED;
}
}
}
void balance_disarm(balance_t *b) {
b->state = BALANCE_DISARMED;
b->motor_cmd = 0;
b->integral = 0.0f;
slope_estimator_reset(&b->slope);
}
void balance_park(balance_t *b) {
/* Suspend balancing from ARMED state only — keeps robot stationary on flat ground */
if (b->state == BALANCE_ARMED) {
b->state = BALANCE_PARKED;
b->motor_cmd = 0;
b->integral = 0.0f;
b->prev_error = 0.0f;
slope_estimator_reset(&b->slope);
}
}
void balance_unpark(balance_t *b) {
/* Quick re-arm from PARKED — only if pitch is safe (< 20 deg) */
if (b->state == BALANCE_PARKED) {
if (fabsf(b->pitch_deg) < 20.0f) {
b->motor_cmd = 0;
b->prev_error = 0.0f;
b->state = BALANCE_ARMED;
}
/* If pitch too large, stay PARKED — caller checks resulting state */
}
}

90
legacy/stm32/src/baro.c
View File

@ -1,90 +0,0 @@
/*
* baro.c — BME280/BMP280 barometric pressure & ambient temperature module
* (Issue #672).
*
* Reads pressure, temperature, and (on BME280) humidity from the sensor at
* BARO_READ_HZ (1 Hz). Computes pressure altitude using bmp280_pressure_to_alt_cm()
* (ISA barometric formula, p0 = 101325 Pa). Publishes JLINK_TLM_BARO (0x8D)
* telemetry to the Orin at BARO_TLM_HZ (1 Hz).
*
* Runs entirely on the Mamba F722S. No Orin dependency.
* baro_get_alt_cm() exposes altitude for slope compensation in the balance PID.
*/
#include "baro.h"
#include "bmp280.h"
#include "jlink.h"
static int s_chip_id = 0; /* 0x58=BMP280, 0x60=BME280, 0=absent */
static baro_data_t s_data; /* latest reading */
static uint32_t s_last_read_ms; /* timestamp of last I2C read */
static uint32_t s_last_tlm_ms; /* timestamp of last telemetry TX */
/* ---- baro_init() ---- */
void baro_init(int chip_id)
{
s_chip_id = chip_id;
s_data.pressure_pa = 0;
s_data.temp_x10 = 0;
s_data.alt_cm = 0;
s_data.humidity_pct_x10 = -1;
s_data.valid = false;
/*
* Initialise timestamps so the first baro_tick() call fires immediately
* (same convention as slope_estimator_init and steering_pid_init).
*/
const uint32_t interval_ms = 1000u / BARO_READ_HZ;
s_last_read_ms = (uint32_t)(-(uint32_t)interval_ms);
s_last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / BARO_TLM_HZ));
}
/* ---- baro_tick() ---- */
void baro_tick(uint32_t now_ms)
{
if (s_chip_id == 0) return;
const uint32_t read_interval_ms = 1000u / BARO_READ_HZ;
if ((now_ms - s_last_read_ms) < read_interval_ms) return;
s_last_read_ms = now_ms;
/* Read pressure (Pa) and temperature (°C × 10) */
bmp280_read(&s_data.pressure_pa, &s_data.temp_x10);
/* Compute pressure altitude: ISA formula, p0 = 101325 Pa */
s_data.alt_cm = bmp280_pressure_to_alt_cm(s_data.pressure_pa);
/* Humidity: BME280 (0x60) only; BMP280 returns -1 */
s_data.humidity_pct_x10 = (s_chip_id == 0x60)
? bmp280_read_humidity()
: (int16_t)-1;
s_data.valid = true;
/* Publish telemetry to Orin via JLink (JLINK_TLM_BARO = 0x8D) */
const uint32_t tlm_interval_ms = 1000u / BARO_TLM_HZ;
if ((now_ms - s_last_tlm_ms) >= tlm_interval_ms) {
s_last_tlm_ms = now_ms;
jlink_tlm_baro_t tlm;
tlm.pressure_pa = s_data.pressure_pa;
tlm.temp_x10 = s_data.temp_x10;
tlm.alt_cm = s_data.alt_cm;
tlm.humidity_pct_x10 = s_data.humidity_pct_x10;
jlink_send_baro_tlm(&tlm);
}
}
/* ---- baro_get() ---- */
bool baro_get(baro_data_t *out)
{
if (!s_data.valid) return false;
*out = s_data;
return true;
}
/* ---- baro_get_alt_cm() ---- */
int32_t baro_get_alt_cm(void)
{
return s_data.valid ? s_data.alt_cm : 0;
}

131
legacy/stm32/src/battery.c
View File

@ -1,131 +0,0 @@
/*
* battery.c — Vbat ADC reading for CRSF telemetry uplink (Issue #103)
*
* Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722S FC).
* Voltage divider: 10 kΩ (upper) / 1 kΩ (lower) → VBAT_SCALE_NUM = 11.
*
* Vbat_mV = (raw × VBAT_AREF_MV × VBAT_SCALE_NUM) >> VBAT_ADC_BITS
* = (raw × 3300 × 11) / 4096
*/
#include "battery.h"
#include "coulomb_counter.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include "ina219.h"
#include <stdbool.h>
static ADC_HandleTypeDef s_hadc;
static bool s_ready = false;
static bool s_coulomb_valid = false;
/* Default battery capacity: 2200 mAh (typical lab 3S LiPo) */
#define DEFAULT_BATTERY_CAPACITY_MAH 2200u
void battery_init(void) {
__HAL_RCC_ADC3_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
/* PC1 → analog input (no pull, no speed) */
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_1;
gpio.Mode = GPIO_MODE_ANALOG;
gpio.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &gpio);
/* ADC3 — single-conversion, software trigger, 12-bit right-aligned */
s_hadc.Instance = ADC3;
s_hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; /* APB2/8 */
s_hadc.Init.Resolution = ADC_RESOLUTION_12B;
s_hadc.Init.ScanConvMode = DISABLE;
s_hadc.Init.ContinuousConvMode = DISABLE;
s_hadc.Init.DiscontinuousConvMode = DISABLE;
s_hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
s_hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START;
s_hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
s_hadc.Init.NbrOfConversion = 1;
s_hadc.Init.DMAContinuousRequests = DISABLE;
s_hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
if (HAL_ADC_Init(&s_hadc) != HAL_OK) return;
/* Channel IN11 (PC1) with 480-cycle sampling for stability */
ADC_ChannelConfTypeDef ch = {0};
ch.Channel = ADC_CHANNEL_11;
ch.Rank = 1;
ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;
/* Initialize coulomb counter with default battery capacity */
coulomb_counter_init(DEFAULT_BATTERY_CAPACITY_MAH);
s_coulomb_valid = true;
s_ready = true;
}
uint32_t battery_read_mv(void) {
if (!s_ready) return 0u;
HAL_ADC_Start(&s_hadc);
if (HAL_ADC_PollForConversion(&s_hadc, 2u) != HAL_OK) return 0u;
uint32_t raw = HAL_ADC_GetValue(&s_hadc);
HAL_ADC_Stop(&s_hadc);
/* Vbat_mV = raw × (VREF_mV × scale) / ADC_counts */
return (raw * (uint32_t)VBAT_AREF_MV * VBAT_SCALE_NUM) /
((1u << VBAT_ADC_BITS));
}
/*
* Coarse SoC estimate (voltage-based fallback).
* 3S LiPo: 9.9 V (0%) – 12.6 V (100%) — detect by Vbat < 13 V
* 4S LiPo: 13.2 V (0%) – 16.8 V (100%) — detect by Vbat ≥ 13 V
*/
uint8_t battery_estimate_pct(uint32_t voltage_mv) {
uint32_t v_min_mv, v_max_mv;
if (voltage_mv >= 13000u) {
/* 4S LiPo */
v_min_mv = 13200u;
v_max_mv = 16800u;
} else {
/* 3S LiPo */
v_min_mv = 9900u;
v_max_mv = 12600u;
}
if (voltage_mv <= v_min_mv) return 0u;
if (voltage_mv >= v_max_mv) return 100u;
return (uint8_t)(((voltage_mv - v_min_mv) * 100u) / (v_max_mv - v_min_mv));
}
/*
* battery_accumulate_coulombs() — call periodically (50-100 Hz) to track
* battery current and integrate coulombs. Reads motor currents via INA219.
*/
void battery_accumulate_coulombs(void) {
if (!s_coulomb_valid) return;
/* Sum left + right motor currents as proxy for battery draw
* (simple approach; doesn't include subsystem drain like OSD, audio) */
int16_t left_ma = 0, right_ma = 0;
ina219_read_current_ma(INA219_LEFT_MOTOR, &left_ma);
ina219_read_current_ma(INA219_RIGHT_MOTOR, &right_ma);
/* Total battery current ≈ motors + subsystem baseline (~200 mA) */
int16_t total_ma = left_ma + right_ma + 200;
/* Accumulate to coulomb counter */
coulomb_counter_accumulate(total_ma);
}
/*
* battery_get_soc_coulomb() — get coulomb-based SoC (0-100, 255=invalid).
* Preferred over voltage-based when available.
*/
uint8_t battery_get_soc_coulomb(void) {
if (!s_coulomb_valid || !coulomb_counter_is_valid()) {
return 255; /* Invalid */
}
return coulomb_counter_get_soc_pct();
}

394
legacy/stm32/src/battery_adc.c
View File

@ -1,394 +0,0 @@
/*
* battery_adc.c — DMA-based battery voltage/current ADC driver (Issue #533)
*
* Voltage divider calibration, DMA-based continuous sampling, IIR low-pass
* filter, and USART telemetry publish to Jetson via jlink (JLINK_TLM_BATTERY).
* Integrates with power_mgmt for low-battery sleep (Issue #467).
*
* DMA mapping (STM32F7 RM Table 27):
* DMA2_Stream0 Channel2 → ADC3 (no conflict: Stream2/Ch4 = USART1_RX)
*
* ADC3 scan sequence:
* Rank 1: IN11 (PC1) — Vbat
* Rank 2: IN13 (PC3) — Ibat
* Continuous mode, 480-cycle sampling → ~14 kSPS per channel @ 13.5 MHz ADC
*
* DMA circular buffer layout (8 × uint16_t, pairs repeat continuously):
* [vbat0, ibat0, vbat1, ibat1, vbat2, ibat2, vbat3, ibat3]
*
* On each battery_adc_tick() call the 4 Vbat and 4 Ibat samples are averaged
* (4× hardware oversampling) then fed into an IIR low-pass filter.
*/
#include "battery_adc.h"
#include "jlink.h"
#include "power_mgmt.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
/* ---- DMA buffer ---- */
#define ADC_DMA_BUF_LEN 8u /* 4 Vbat/Ibat pairs */
#define ADC_OVERSAMPLE 4u /* pairs per tick average */
static volatile uint16_t s_dma_buf[ADC_DMA_BUF_LEN];
/* ---- HAL handles ---- */
static ADC_HandleTypeDef s_hadc;
static DMA_HandleTypeDef s_hdma;
/* ---- Filtered state (Q0 integer, units: raw ADC counts × 8 for sub-LSB) ---- */
/*
* IIR accumulator: s_lpf_vbat = filtered Vbat in (raw_counts × 8).
* Multiply by 8 gives 3 bits of sub-LSB precision with integer arithmetic.
* On each tick: s_lpf_vbat += (raw_avg - (s_lpf_vbat >> 3)) >> LPF_SHIFT
* Simplified: s_lpf_vbat8 = s_lpf_vbat8 + (raw_avg_x8 - s_lpf_vbat8) >> SHIFT
*/
static uint32_t s_lpf_vbat8 = 0; /* Vbat accumulator × 8 */
static int32_t s_lpf_ibat8 = 0; /* Ibat accumulator × 8 (signed) */
/* ---- Calibrated outputs (updated each tick) ---- */
static uint32_t s_vbat_mv = 0; /* calibrated LPF Vbat (mV) */
static int32_t s_ibat_ma = 0; /* calibrated LPF Ibat (mA) */
static uint32_t s_vbat_raw_mv = 0; /* unfiltered last-tick average (mV) */
/* ---- Calibration ---- */
static battery_adc_cal_t s_cal = {
.vbat_offset_mv = 0,
.ibat_offset_ma = 0,
.vbat_scale_num = 0, /* 0 = use VBAT_SCALE_NUM from config.h */
.vbat_scale_den = 0, /* 0 = use 1 */
};
/* ---- Low-voltage tracking ---- */
static uint32_t s_low_since_ms = 0; /* tick when Vbat first went low */
static bool s_low_active = false; /* currently below LOW_MV threshold */
static bool s_critical_notified = false; /* pm notified for this event */
static uint32_t s_low_mv = BATTERY_ADC_LOW_MV;
static uint32_t s_critical_mv = BATTERY_ADC_CRITICAL_MV;
/* ---- Ready flag ---- */
static bool s_ready = false;
/* ---- Telemetry rate limiting ---- */
static uint32_t s_last_publish_ms = 0;
static bool s_published_once = false; /* force send on first call */
/* ---- Helper: convert raw ADC count to mV ---- */
/*
* Apply voltage divider and calibration:
* vbat_mv = (raw × Vref_mV × scale_num) / (4096 × scale_den)
* + cal.vbat_offset_mv
* Default: Vref=3300 mV, scale_num=VBAT_SCALE_NUM=11, scale_den=1.
*/
static uint32_t raw_to_vbat_mv(uint32_t raw_counts)
{
uint16_t snum = (s_cal.vbat_scale_num != 0u) ? s_cal.vbat_scale_num
: (uint16_t)VBAT_SCALE_NUM;
uint16_t sden = (s_cal.vbat_scale_den != 0u) ? s_cal.vbat_scale_den : 1u;
/* raw_counts × (3300 × 11) / 4096 — keep 32-bit intermediate */
uint32_t mv = (raw_counts * (uint32_t)VBAT_AREF_MV * snum) /
((1u << VBAT_ADC_BITS) * sden);
int32_t cal_mv = (int32_t)mv + (int32_t)s_cal.vbat_offset_mv;
if (cal_mv < 0) cal_mv = 0;
return (uint32_t)cal_mv;
}
/* ---- Helper: convert raw ADC count to mA ---- */
/*
* ADC_IBAT_SCALE = 115 (betaflight ibata_scale units: mA per count × 10).
* Formula: ibat_ma = raw_counts × ADC_IBAT_SCALE / 10 + cal.ibat_offset_ma
* Betaflight ibata_scale is defined as: current = raw * scale / (4096 * 10)
* in units of 100mA per count → ibat_ma = raw × ibata_scale * 100 / 4096
*
* With ibata_scale=115: ibat_ma = raw × 115 × 100 / 4096
* ≈ raw × 2.808 mA/count
* For raw=4095 (full-scale): ~11,500 mA (11.5 A) — appropriate for 3S robot.
*/
static int32_t raw_to_ibat_ma(uint32_t raw_counts)
{
int32_t ma = (int32_t)((raw_counts * (uint32_t)ADC_IBAT_SCALE * 100u) /
(1u << VBAT_ADC_BITS));
ma += (int32_t)s_cal.ibat_offset_ma;
return ma;
}
/* ---- battery_adc_init() ---- */
void battery_adc_init(void)
{
/* ---- GPIO: PC1 (Vbat) and PC3 (Ibat) as analog input ---- */
__HAL_RCC_GPIOC_CLK_ENABLE();
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_1 | GPIO_PIN_3;
gpio.Mode = GPIO_MODE_ANALOG;
gpio.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &gpio);
/* ---- DMA2_Stream0 Channel2 → ADC3 (circular, 16-bit, 8 words) ---- */
__HAL_RCC_DMA2_CLK_ENABLE();
s_hdma.Instance = DMA2_Stream0;
s_hdma.Init.Channel = DMA_CHANNEL_2;
s_hdma.Init.Direction = DMA_PERIPH_TO_MEMORY;
s_hdma.Init.PeriphInc = DMA_PINC_DISABLE;
s_hdma.Init.MemInc = DMA_MINC_ENABLE;
s_hdma.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
s_hdma.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
s_hdma.Init.Mode = DMA_CIRCULAR;
s_hdma.Init.Priority = DMA_PRIORITY_LOW;
s_hdma.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&s_hdma) != HAL_OK) return;
/* ---- ADC3: continuous scan, DMA, 12-bit, 2 channels ---- */
__HAL_RCC_ADC3_CLK_ENABLE();
s_hadc.Instance = ADC3;
s_hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; /* 108/8 ≈ 13.5 MHz */
s_hadc.Init.Resolution = ADC_RESOLUTION_12B;
s_hadc.Init.ScanConvMode = ENABLE; /* scan both channels */
s_hadc.Init.ContinuousConvMode = ENABLE; /* free-running */
s_hadc.Init.DiscontinuousConvMode = DISABLE;
s_hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
s_hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START;
s_hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
s_hadc.Init.NbrOfConversion = 2;
s_hadc.Init.DMAContinuousRequests = ENABLE; /* DMA circular */
s_hadc.Init.EOCSelection = ADC_EOC_SEQ_CONV;
__HAL_LINKDMA(&s_hadc, DMA_Handle, s_hdma);
if (HAL_ADC_Init(&s_hadc) != HAL_OK) return;
/* ---- Rank 1: IN11 (PC1) — Vbat ---- */
ADC_ChannelConfTypeDef ch = {0};
ch.Channel = ADC_CHANNEL_11;
ch.Rank = 1;
ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;
/* ---- Rank 2: IN13 (PC3) — Ibat ---- */
ch.Channel = ADC_CHANNEL_13;
ch.Rank = 2;
ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;
/* ---- Detect battery cell count at init for correct thresholds ---- */
/*
* Note: at init Vbat may not be settled; thresholds are updated after
* first tick once LPF has stabilised. Set 4S thresholds if voltage
* detected ≥ 13 V in battery_adc_tick() on first valid reading.
*/
/* ---- Start continuous DMA conversion ---- */
if (HAL_ADC_Start_DMA(&s_hadc, (uint32_t *)s_dma_buf, ADC_DMA_BUF_LEN)
!= HAL_OK) return;
s_ready = true;
}
/* ---- battery_adc_tick() ---- */
void battery_adc_tick(uint32_t now_ms)
{
if (!s_ready) return;
(void)now_ms; /* captured for caller context; may be used for future rate ctrl */
/* ---- Snapshot DMA buffer with IRQ guard to prevent torn reads ---- */
uint16_t snap[ADC_DMA_BUF_LEN];
uint32_t primask = __get_PRIMASK();
__disable_irq();
memcpy(snap, (void *)s_dma_buf, sizeof(snap));
__set_PRIMASK(primask);
/* ---- Average 4 Vbat and 4 Ibat samples (interleaved pairs) ---- */
uint32_t vbat_sum = 0u;
uint32_t ibat_sum = 0u;
for (uint32_t i = 0u; i < ADC_DMA_BUF_LEN; i += 2u) {
vbat_sum += snap[i]; /* even indices: Vbat (rank 1) */
ibat_sum += snap[i + 1u]; /* odd indices: Ibat (rank 2) */
}
uint32_t vbat_avg = vbat_sum / ADC_OVERSAMPLE; /* 0–4095 */
uint32_t ibat_avg = ibat_sum / ADC_OVERSAMPLE;
/* ---- Raw (unfiltered) calibrated voltage — for calibration query ---- */
s_vbat_raw_mv = raw_to_vbat_mv(vbat_avg);
/* ---- IIR low-pass filter ---- */
/*
* Accumulator stores value × 8 for sub-LSB precision.
* Update: acc8 += (raw × 8 - acc8) >> LPF_SHIFT
* Result: filtered_raw = acc8 >> 3
*
* With LPF_SHIFT=3: α = 1/(1+8) ≈ 0.111 → cutoff ≈ 4 Hz at 100 Hz tick.
*/
s_lpf_vbat8 += ((vbat_avg << 3u) - s_lpf_vbat8) >> BATTERY_ADC_LPF_SHIFT;
s_lpf_ibat8 += (((int32_t)ibat_avg << 3u) - s_lpf_ibat8)
>> BATTERY_ADC_LPF_SHIFT;
uint32_t vbat_filtered = s_lpf_vbat8 >> 3u; /* back to ADC counts */
uint32_t ibat_filtered = (uint32_t)((s_lpf_ibat8 < 0 ? 0 : s_lpf_ibat8) >> 3u);
/* ---- Apply calibration and convert to engineering units ---- */
s_vbat_mv = raw_to_vbat_mv(vbat_filtered);
s_ibat_ma = raw_to_ibat_ma(ibat_filtered);
/* ---- Auto-set thresholds based on detected cell count ---- */
/*
* Detect 4S (Vbat ≥ 13 V) vs 3S (<13 V). Only update thresholds once
* after initial settling (first non-zero filtered reading).
*/
static bool s_thresholds_set = false;
if (!s_thresholds_set && s_vbat_mv > 500u) {
if (s_vbat_mv >= 13000u) {
s_low_mv = BATTERY_ADC_LOW_MV_4S;
s_critical_mv = BATTERY_ADC_CRITICAL_MV_4S;
}
/* else keep 3S defaults set at file scope */
s_thresholds_set = true;
}
}
/* ---- battery_adc_get_voltage_mv() ---- */
uint32_t battery_adc_get_voltage_mv(void)
{
return s_vbat_mv;
}
/* ---- battery_adc_get_current_ma() ---- */
int32_t battery_adc_get_current_ma(void)
{
return s_ibat_ma;
}
/* ---- battery_adc_get_raw_voltage_mv() ---- */
uint32_t battery_adc_get_raw_voltage_mv(void)
{
return s_vbat_raw_mv;
}
/* ---- battery_adc_calibrate() ---- */
void battery_adc_calibrate(const battery_adc_cal_t *cal)
{
if (cal == NULL) {
/* Reset to defaults */
s_cal.vbat_offset_mv = 0;
s_cal.ibat_offset_ma = 0;
s_cal.vbat_scale_num = 0;
s_cal.vbat_scale_den = 0;
return;
}
/* Clamp offsets to prevent runaway calibration values */
int16_t voff = cal->vbat_offset_mv;
if (voff > 500) voff = 500;
if (voff < -500) voff = -500;
int16_t ioff = cal->ibat_offset_ma;
if (ioff > 200) ioff = 200;
if (ioff < -200) ioff = -200;
s_cal.vbat_offset_mv = voff;
s_cal.ibat_offset_ma = ioff;
s_cal.vbat_scale_num = cal->vbat_scale_num;
s_cal.vbat_scale_den = cal->vbat_scale_den;
}
/* ---- battery_adc_get_calibration() ---- */
void battery_adc_get_calibration(battery_adc_cal_t *out_cal)
{
if (out_cal != NULL) {
*out_cal = s_cal;
}
}
/* ---- battery_adc_publish() ---- */
/*
* Sends JLINK_TLM_BATTERY (0x82) frame to Jetson at BATTERY_ADC_PUBLISH_HZ.
*
* Payload (jlink_tlm_battery_t, 10 bytes packed):
* vbat_mv uint16 — calibrated LPF voltage (mV)
* ibat_ma int16 — calibrated LPF current (mA)
* vbat_raw_mv uint16 — unfiltered last-tick voltage (mV)
* flags uint8 — bit0=low, bit1=critical, bit2=4S detected, bit3=ready
* cal_offset int8 — vbat_offset_mv / 4 (compressed, ±127 → ±508 mV)
* lpf_shift uint8 — BATTERY_ADC_LPF_SHIFT value (for Jetson info)
* soc_pct uint8 — voltage-based SoC estimate (0-100, 255=unknown)
*/
void battery_adc_publish(uint32_t now_ms)
{
if (!s_ready) return;
uint32_t period_ms = 1000u / BATTERY_ADC_PUBLISH_HZ;
if (s_published_once && (now_ms - s_last_publish_ms) < period_ms) return;
s_last_publish_ms = now_ms;
s_published_once = true;
/* Build SoC estimate (voltage-based) */
uint8_t soc = 255u; /* unknown */
if (s_vbat_mv > 500u) {
uint32_t v_min, v_max;
if (s_vbat_mv >= 13000u) {
v_min = 13200u; v_max = 16800u; /* 4S */
} else {
v_min = 9900u; v_max = 12600u; /* 3S */
}
if (s_vbat_mv <= v_min) {
soc = 0u;
} else if (s_vbat_mv >= v_max) {
soc = 100u;
} else {
soc = (uint8_t)(((s_vbat_mv - v_min) * 100u) / (v_max - v_min));
}
}
/* Flags byte */
uint8_t flags = 0u;
if (s_vbat_mv > 0u && s_vbat_mv < s_low_mv) flags |= 0x01u;
if (s_vbat_mv > 0u && s_vbat_mv < s_critical_mv) flags |= 0x02u;
if (s_low_mv == BATTERY_ADC_LOW_MV_4S) flags |= 0x04u;
if (s_ready) flags |= 0x08u;
/* Build JLINK_TLM_BATTERY frame via jlink_send_battery_telemetry() */
jlink_tlm_battery_t tlm = {
.vbat_mv = (uint16_t)(s_vbat_mv & 0xFFFFu),
.ibat_ma = (int16_t) (s_ibat_ma & 0xFFFF),
.vbat_raw_mv = (uint16_t)(s_vbat_raw_mv & 0xFFFFu),
.flags = flags,
.cal_offset = (int8_t) (s_cal.vbat_offset_mv / 4),
.lpf_shift = (uint8_t) BATTERY_ADC_LPF_SHIFT,
.soc_pct = soc,
};
jlink_send_battery_telemetry(&tlm);
}
/* ---- battery_adc_check_pm() ---- */
void battery_adc_check_pm(uint32_t now_ms)
{
if (!s_ready || s_vbat_mv == 0u) return;
bool below_low = (s_vbat_mv < s_low_mv);
bool below_critical = (s_vbat_mv < s_critical_mv);
/* Track first entry below low threshold */
if (below_low && !s_low_active) {
s_low_active = true;
s_low_since_ms = now_ms;
s_critical_notified = false;
} else if (!below_low) {
s_low_active = false;
s_critical_notified = false;
}
/* Notify power management on sustained critical voltage */
if (below_critical && !s_critical_notified &&
(now_ms - s_low_since_ms) >= BATTERY_ADC_LOW_HOLD_MS) {
s_critical_notified = true;
power_mgmt_notify_battery(s_vbat_mv);
}
}
/* ---- battery_adc_is_low() ---- */
bool battery_adc_is_low(void)
{
return (s_ready && s_vbat_mv > 0u && s_vbat_mv < s_low_mv);
}
/* ---- battery_adc_is_critical() ---- */
bool battery_adc_is_critical(void)
{
return (s_ready && s_vbat_mv > 0u && s_vbat_mv < s_critical_mv);
}

169
legacy/stm32/src/bmp280.c
View File

@ -1,169 +0,0 @@
/*
* bmp280.c — BMP280/BME280 barometer driver (I2C1, shared bus)
*
* Probes 0x76 first, then 0x77. Requires i2c1_init() before bmp280_init().
* Returns chip_id on success (0x58=BMP280, 0x60=BME280), negative if absent.
*
* All HAL_I2C_Mem_Read/Write calls use 100ms timeouts — init cannot hang
* indefinitely even if the I2C bus is stuck or the breakout is absent.
*
* BME280 (chip_id 0x60): bmp280_read_humidity() returns %RH × 10.
* Call bmp280_read() first to refresh t_fine, then bmp280_read_humidity().
*/
#include "bmp280.h"
#include "i2c1.h"
#include <math.h>
static uint16_t s_addr;
static int s_chip_id; /* 0x58=BMP280, 0x60=BME280, 0=none */
/* Shared temp/pressure calibration */
static uint16_t dig_T1;
static int16_t dig_T2, dig_T3;
static uint16_t dig_P1;
static int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
static int32_t t_fine; /* updated by bmp280_read(); used by bmp280_read_humidity() */
/* BME280-only humidity calibration (chip_id 0x60) */
static uint8_t dig_H1;
static int16_t dig_H2;
static uint8_t dig_H3;
static int16_t dig_H4, dig_H5;
static int8_t dig_H6;
static uint8_t i2c_read(uint8_t reg) {
uint8_t val = 0;
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &val, 1, 100);
return val;
}
static void i2c_read_burst(uint8_t reg, uint8_t *buf, uint8_t len) {
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 100);
}
static void i2c_write(uint8_t reg, uint8_t val) {
HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100);
}
static int try_init(uint16_t addr) {
s_addr = addr;
uint8_t id = i2c_read(0xD0);
if (id != 0x58 && id != 0x60) return -(int)id;
s_chip_id = (int)id;
/* Temp/pressure calibration (0x88–0x9D, identical layout on BMP280 and BME280) */
uint8_t cal[26];
i2c_read_burst(0x88, cal, 26);
dig_T1 = (uint16_t)(cal[1] << 8 | cal[0]);
dig_T2 = (int16_t) (cal[3] << 8 | cal[2]);
dig_T3 = (int16_t) (cal[5] << 8 | cal[4]);
dig_P1 = (uint16_t)(cal[7] << 8 | cal[6]);
dig_P2 = (int16_t) (cal[9] << 8 | cal[8]);
dig_P3 = (int16_t) (cal[11] << 8 | cal[10]);
dig_P4 = (int16_t) (cal[13] << 8 | cal[12]);
dig_P5 = (int16_t) (cal[15] << 8 | cal[14]);
dig_P6 = (int16_t) (cal[17] << 8 | cal[16]);
dig_P7 = (int16_t) (cal[19] << 8 | cal[18]);
dig_P8 = (int16_t) (cal[21] << 8 | cal[20]);
dig_P9 = (int16_t) (cal[23] << 8 | cal[22]);
if (id == 0x60) {
/* BME280: humidity calibration.
* dig_H1 : 0xA1 (uint8)
* dig_H2 : 0xE1–0xE2 (int16, LSB first)
* dig_H3 : 0xE3 (uint8)
* dig_H4 : 0xE4[7:0] | 0xE5[3:0] (int12)
* dig_H5 : 0xE5[7:4] | 0xE6[7:0] (int12)
* dig_H6 : 0xE7 (int8)
*/
dig_H1 = i2c_read(0xA1);
uint8_t hcal[7];
i2c_read_burst(0xE1, hcal, 7);
dig_H2 = (int16_t)((hcal[1] << 8) | hcal[0]);
dig_H3 = hcal[2];
dig_H4 = (int16_t)((hcal[3] << 4) | (hcal[4] & 0x0F));
dig_H5 = (int16_t)((hcal[5] << 4) | (hcal[4] >> 4));
dig_H6 = (int8_t)hcal[6];
/* ctrl_hum (0xF2) MUST be written before ctrl_meas (0xF4) — hardware req */
i2c_write(0xF2, 0x05); /* osrs_h = ×16 */
}
i2c_write(0xF5, 0x00); /* config: standby=0.5ms, filter=off */
i2c_write(0xF4, 0xB7); /* ctrl_meas: osrs_t=×16, osrs_p=×16, normal mode */
return (int)id;
}
int bmp280_init(void) {
int ret = try_init(0x76 << 1);
if (ret > 0) return ret;
return try_init(0x77 << 1);
}
void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) {
uint8_t buf[6];
i2c_read_burst(0xF7, buf, 6);
int32_t adc_P = (int32_t)((buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4));
int32_t adc_T = (int32_t)((buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4));
/* Temperature compensation (BME280/BMP280 datasheet Section 4.2.3) */
int32_t v1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
int32_t v2 = (((((adc_T >> 4) - (int32_t)dig_T1) *
((adc_T >> 4) - (int32_t)dig_T1)) >> 12) * (int32_t)dig_T3) >> 14;
t_fine = v1 + v2;
*temp_x10 = (int16_t)((t_fine * 5 + 128) >> 8); /* 0.1 °C */
/* Pressure compensation */
int64_t p1 = ((int64_t)t_fine) - 128000;
int64_t p2 = p1 * p1 * (int64_t)dig_P6;
p2 += (p1 * (int64_t)dig_P5) << 17;
p2 += ((int64_t)dig_P4) << 35;
p1 = ((p1 * p1 * (int64_t)dig_P3) >> 8) + ((p1 * (int64_t)dig_P2) << 12);
p1 = ((((int64_t)1 << 47) + p1)) * ((int64_t)dig_P1) >> 33;
if (p1 == 0) { *pressure_pa = 0; return; }
int64_t p = 1048576 - adc_P;
p = (((p << 31) - p2) * 3125) / p1;
p1 = ((int64_t)dig_P9 * (p >> 13) * (p >> 13)) >> 25;
p2 = ((int64_t)dig_P8 * p) >> 19;
*pressure_pa = (int32_t)(((p + p1 + p2) >> 8) + ((int64_t)dig_P7 << 4)) / 256;
}
/*
* BME280-only humidity readout. MUST be called after bmp280_read() (uses t_fine).
*
* Compensation: BME280 datasheet section 4.2.3 integer formula.
* Result is Q22.10 fixed-point: 1024 units = 1 %RH.
*
* Returns humidity in %RH × 10 (e.g. 500 = 50.0 %RH).
* Returns -1 if chip is BMP280 (no humidity sensor).
*/
int16_t bmp280_read_humidity(void) {
if (s_chip_id != 0x60) return -1;
uint8_t hbuf[2];
i2c_read_burst(0xFD, hbuf, 2);
int32_t adc_H = (int32_t)((hbuf[0] << 8) | hbuf[1]);
/* BME280 datasheet section 4.2.3 — floating-point compensation.
* Single-precision float is hardware-accelerated on STM32F7 (FPv5-SP FPU).
* Called at 50 Hz — negligible overhead.
*/
float var_H = ((float)t_fine) - 76800.0f;
var_H = (adc_H - (((float)dig_H4) * 64.0f + ((float)dig_H5) / 16384.0f * var_H)) *
(((float)dig_H2) / 65536.0f *
(1.0f + ((float)dig_H6) / 67108864.0f * var_H *
(1.0f + ((float)dig_H3) / 67108864.0f * var_H)));
var_H *= (1.0f - (float)dig_H1 * var_H / 524288.0f);
if (var_H > 100.0f) var_H = 100.0f;
if (var_H < 0.0f) var_H = 0.0f;
return (int16_t)(var_H * 10.0f + 0.5f); /* %RH × 10, rounded */
}
int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa) {
/* Barometric formula: h = 44330 * (1 - (p/p0)^(1/5.255)) metres */
float ratio = (float)pressure_pa / 101325.0f;
float alt_m = 44330.0f * (1.0f - powf(ratio, 0.1902949f));
return (int32_t)(alt_m * 100.0f); /* cm */
}

316
legacy/stm32/src/bno055.c
View File

@ -1,316 +0,0 @@
/*
* bno055.c — Bosch BNO055 NDOF IMU driver over I2C1
*
* Issue #135: auto-detect alongside MPU6000; fallback + yaw augmentation.
*
* Calibration offsets are persisted in STM32F7 RTC backup registers
* (BKP0R–BKP6R) and restored automatically on next power-on, so the
* sensor re-enters a calibrated state immediately after reset.
*
* Internal temperature compensation:
* The BNO055 silicon compensates all three sensors (accel, gyro, mag)
* continuously against its onboard thermometer. No external correction
* is required. bno055_temperature() exposes the reading for telemetry.
*/
#include "bno055.h"
#include "i2c1.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
#include <math.h>
/* ---- I2C addresses (7-bit, shifted left 1 for HAL) ---- */
#define BNO055_ADDR_LOW (0x28u << 1)
#define BNO055_ADDR_HIGH (0x29u << 1)
#define BNO055_CHIP_ID_VAL 0xA0u
/* ---- Register map (page 0) ---- */
#define REG_CHIP_ID 0x00u
#define REG_PAGE_ID 0x07u
#define REG_GYRO_X_LSB 0x14u /* 6 bytes: Gx, Gy, Gz */
#define REG_EULER_H_LSB 0x1Au /* 6 bytes: H, R, P (each int16) */
#define REG_LIA_X_LSB 0x28u /* 6 bytes: linear accel X,Y,Z */
#define REG_GRV_X_LSB 0x2Eu /* 6 bytes: gravity X,Y,Z */
#define REG_TEMP 0x34u
#define REG_CALIB_STAT 0x35u
#define REG_OPR_MODE 0x3Du
#define REG_PWR_MODE 0x3Eu
#define REG_SYS_TRIGGER 0x3Fu
#define REG_TEMP_SOURCE 0x40u
#define REG_OFFSET_BASE 0x55u /* 22 bytes: accel(6)+mag(6)+gyro(6)+radii(4) */
/* ---- Operating modes ---- */
#define MODE_CONFIG 0x00u
#define MODE_IMUPLUS 0x08u /* 6DOF, no mag */
#define MODE_NDOF 0x0Cu /* 9DOF with mag */
/* ---- RTC backup register magic for saved offsets ---- */
/*
* Layout: BKP0R..BKP5R hold 22 bytes of calibration offsets (packed
* 4 bytes per register; BKP5R only uses its low 2 bytes).
* BKP6R holds the magic word to validate the stored data.
*/
#define BNO055_BKP_MAGIC 0xB055CA10u
#define BKP_REG(n) (*(volatile uint32_t *)(&RTC->BKP0R + (n)))
/* ---- Scaling constants ---- */
/*
* BNO055 default unit selection (UNIT_SEL=0x00):
* Euler angles: 1 degree = 16 LSB → divide by 16.0f
* Gyro rates: 1 °/s = 16 LSB → divide by 16.0f
* Accel/LIA/GRV: 1 m/s² = 100 LSB → divide by 100.0f
*/
#define EULER_SCALE 16.0f
#define GYRO_SCALE 16.0f
#define ACCEL_SCALE 100.0f
#define G_MS2 9.80665f /* m/s² per g */
/* ---- Module state ---- */
static uint16_t s_addr = 0;
static bool s_present = false;
static bool s_offsets_ok = false; /* true if offsets restored */
static uint8_t s_calib = 0; /* cached CALIB_STAT */
static int8_t s_temp = 25; /* cached temperature °C */
static uint16_t s_temp_ctr = 0; /* temp read divider */
/* ---- Low-level I2C helpers ---- */
static uint8_t rd(uint8_t reg)
{
uint8_t v = 0;
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &v, 1, 100);
return v;
}
static void wr(uint8_t reg, uint8_t val)
{
HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100);
}
static void rdblk(uint8_t reg, uint8_t *buf, uint8_t len)
{
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 200);
}
static void wrblk(uint8_t reg, const uint8_t *buf, uint8_t len)
{
HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, (uint8_t *)buf, len, 200);
}
/* ---- bno055_restore_offsets() ---- */
bool bno055_restore_offsets(void)
{
__HAL_RCC_PWR_CLK_ENABLE();
HAL_PWR_EnableBkUpAccess();
if (BKP_REG(6) != BNO055_BKP_MAGIC) return false;
/* Unpack 22 bytes from BKP0R..BKP5R */
uint8_t offsets[22];
for (int i = 0; i < 5; i++) {
uint32_t w = BKP_REG(i);
offsets[i * 4 + 0] = (uint8_t)(w & 0xFFu);
offsets[i * 4 + 1] = (uint8_t)((w >> 8) & 0xFFu);
offsets[i * 4 + 2] = (uint8_t)((w >> 16) & 0xFFu);
offsets[i * 4 + 3] = (uint8_t)((w >> 24) & 0xFFu);
}
uint32_t w5 = BKP_REG(5);
offsets[20] = (uint8_t)(w5 & 0xFFu);
offsets[21] = (uint8_t)((w5 >> 8) & 0xFFu);
/* Write offsets — must be in CONFIG mode */
wr(REG_OPR_MODE, MODE_CONFIG);
HAL_Delay(25);
wr(REG_PAGE_ID, 0x00);
wrblk(REG_OFFSET_BASE, offsets, 22);
/* Caller switches back to NDOF after this returns */
return true;
}
/* ---- bno055_save_offsets() ---- */
bool bno055_save_offsets(void)
{
if (!s_present) return false;
/* Switch to CONFIG mode to read offsets */
wr(REG_OPR_MODE, MODE_CONFIG);
HAL_Delay(25);
wr(REG_PAGE_ID, 0x00);
uint8_t offsets[22];
rdblk(REG_OFFSET_BASE, offsets, 22);
/* Restore to NDOF */
wr(REG_OPR_MODE, MODE_NDOF);
HAL_Delay(10);
/* Pack into RTC backup registers */
__HAL_RCC_PWR_CLK_ENABLE();
HAL_PWR_EnableBkUpAccess();
for (int i = 0; i < 5; i++) {
BKP_REG(i) = ((uint32_t)offsets[i * 4 + 0])
| ((uint32_t)offsets[i * 4 + 1] << 8)
| ((uint32_t)offsets[i * 4 + 2] << 16)
| ((uint32_t)offsets[i * 4 + 3] << 24);
}
BKP_REG(5) = (uint32_t)offsets[20] | ((uint32_t)offsets[21] << 8);
BKP_REG(6) = BNO055_BKP_MAGIC;
return true;
}
/* ---- bno055_init() ---- */
int bno055_init(void)
{
/* Probe 0x28 (ADR=0) first, then 0x29 (ADR=1) */
const uint16_t candidates[2] = { BNO055_ADDR_LOW, BNO055_ADDR_HIGH };
s_present = false;
for (int i = 0; i < 2; i++) {
s_addr = candidates[i];
if (rd(REG_CHIP_ID) == BNO055_CHIP_ID_VAL) {
s_present = true;
break;
}
}
if (!s_present) return -1;
/* Software reset via SYS_TRIGGER */
wr(REG_PAGE_ID, 0x00);
wr(REG_SYS_TRIGGER, 0x20u);
HAL_Delay(700); /* POR settle time from datasheet */
/* Confirm chip ID after reset */
if (rd(REG_CHIP_ID) != BNO055_CHIP_ID_VAL) {
s_present = false;
return -1;
}
wr(REG_PAGE_ID, 0x00);
/* CONFIGMODE before any register writes */
wr(REG_OPR_MODE, MODE_CONFIG);
HAL_Delay(25);
/* Normal power mode */
wr(REG_PWR_MODE, 0x00u);
HAL_Delay(10);
/*
* Unit selection (UNIT_SEL = 0x00, default):
* Euler = degrees, Gyro = dps, Accel = m/s², Temp = °C
* No write needed — defaults match what we want.
*/
/* Temperature source = gyro (more stable than accel) */
wr(REG_TEMP_SOURCE, 0x01u);
/* Try to restore saved calibration offsets */
s_offsets_ok = bno055_restore_offsets();
/* Enter NDOF fusion mode */
wr(REG_OPR_MODE, MODE_NDOF);
HAL_Delay(20);
s_calib = 0;
s_temp = 25;
s_temp_ctr = 0;
return 0;
}
/* ---- bno055_read() ---- */
/*
* Performs two I2C burst reads (~3ms total at 100kHz):
* 1. REG_GYRO_X_LSB → 12 bytes: gyro(6) + euler(6)
* 2. REG_LIA_X_LSB → 12 bytes: linear_accel(6) + gravity(6)
*
* BNO055 coordinate frame (NDOF, no remapping):
* X: right, Y: forward, Z: up
* Euler H: yaw (0–360°, clockwise from north)
* Euler R: roll (–180°..+180°, right bank positive)
* Euler P: pitch (–90°..+90°, nose up positive)
* Gyro Y: pitch rate (deg/s, nose-up positive)
*
* Linear accel X (m/s²): forward-backward acceleration (gravity removed)
* Gravity Z (m/s²): gravity component on Z axis (≈9.81 when level)
*/
void bno055_read(IMUData *data)
{
uint8_t buf[12];
/* ---- Burst 1: Gyro (0x14–0x19) + Euler (0x1A–0x1F) ---- */
rdblk(REG_GYRO_X_LSB, buf, 12);
/* Gyro: bytes 0..5 = [Gx_L, Gx_H, Gy_L, Gy_H, Gz_L, Gz_H]
* Gy = pitch rate (Y axis = pitch axis in BNO055 frame) */
float pitch_rate = (float)(int16_t)((uint16_t)buf[3] << 8 | buf[2]) / GYRO_SCALE;
/* Euler: bytes 6..11 = [H_L, H_H, R_L, R_H, P_L, P_H] */
float heading = (float)(int16_t)((uint16_t)buf[7] << 8 | buf[6]) / EULER_SCALE;
float roll = (float)(int16_t)((uint16_t)buf[9] << 8 | buf[8]) / EULER_SCALE;
float pitch = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / EULER_SCALE;
/* ---- Burst 2: Linear accel (0x28–0x2D) + Gravity (0x2E–0x33) ---- */
rdblk(REG_LIA_X_LSB, buf, 12);
/* Linear accel X (forward, m/s²) — gravity-compensated */
float lia_x_ms2 = (float)(int16_t)((uint16_t)buf[1] << 8 | buf[0]) / ACCEL_SCALE;
/* Gravity Z (up axis, m/s²) — should be ≈+9.81 when level */
float grv_z_ms2 = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / ACCEL_SCALE;
/* Fill IMUData in same units as mpu6000_read() */
data->pitch = pitch;
data->pitch_rate = pitch_rate;
data->roll = roll;
data->yaw = heading;
data->accel_x = lia_x_ms2 / G_MS2; /* m/s² → g */
data->accel_z = grv_z_ms2 / G_MS2; /* m/s² → g (≈1.0 when level) */
/* Periodically update calibration status and temperature (~1Hz at 250Hz loop) */
if (++s_temp_ctr >= 250u) {
s_temp_ctr = 0;
s_calib = rd(REG_CALIB_STAT);
s_temp = (int8_t)rd(REG_TEMP);
/*
* Auto-save offsets once when full calibration is achieved for
* the first time this session (sys=3, acc=3, gyr=3, don't wait
* for mag=3 as it is harder to achieve and not critical for balance).
*/
static bool s_saved = false;
if (!s_saved) {
uint8_t sys_cal = (s_calib & BNO055_CAL_SYS_MASK) >> 6;
uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2;
uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4;
if (sys_cal >= 1u && acc_cal >= 3u && gyr_cal >= 3u) {
bno055_save_offsets();
s_saved = true;
}
}
}
}
/* ---- bno055_is_ready() ---- */
bool bno055_is_ready(void)
{
if (!s_present) return false;
/* If we restored saved offsets, trust them immediately */
if (s_offsets_ok) return true;
/* Otherwise require gyro ≥ 2 and accel ≥ 2 */
uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2;
uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4;
return (acc_cal >= 2u) && (gyr_cal >= 2u);
}
/* ---- bno055_calib_status() ---- */
uint8_t bno055_calib_status(void)
{
return s_calib;
}
/* ---- bno055_temperature() ---- */
int8_t bno055_temperature(void)
{
return s_temp;
}

293
legacy/stm32/src/buzzer.c
View File

@ -1,293 +0,0 @@
#include "buzzer.h"
#include "stm32f7xx_hal.h"
#include "config.h"
#include <string.h>
/* ================================================================
* Buzzer Hardware Configuration
* ================================================================ */
#define BUZZER_PIN GPIO_PIN_8
#define BUZZER_PORT GPIOA
#define BUZZER_TIM TIM1
#define BUZZER_TIM_CHANNEL TIM_CHANNEL_1
#define BUZZER_BASE_FREQ_HZ 1000 /* Base PWM frequency (1kHz) */
/* ================================================================
* Predefined Melodies
* ================================================================ */
/* Startup jingle: C-E-G ascending pattern */
const MelodyNote melody_startup[] = {
{NOTE_C4, DURATION_QUARTER},
{NOTE_E4, DURATION_QUARTER},
{NOTE_G4, DURATION_QUARTER},
{NOTE_C5, DURATION_HALF},
{NOTE_REST, 0} /* Terminator */
};
/* Low battery warning: two descending beeps */
const MelodyNote melody_low_battery[] = {
{NOTE_A5, DURATION_EIGHTH},
{NOTE_REST, DURATION_EIGHTH},
{NOTE_A5, DURATION_EIGHTH},
{NOTE_REST, DURATION_EIGHTH},
{NOTE_F5, DURATION_EIGHTH},
{NOTE_REST, DURATION_EIGHTH},
{NOTE_F5, DURATION_EIGHTH},
{NOTE_REST, 0}
};
/* Error alert: rapid repeating tone */
const MelodyNote melody_error[] = {
{NOTE_E5, DURATION_SIXTEENTH},
{NOTE_REST, DURATION_SIXTEENTH},
{NOTE_E5, DURATION_SIXTEENTH},
{NOTE_REST, DURATION_SIXTEENTH},
{NOTE_E5, DURATION_SIXTEENTH},
{NOTE_REST, DURATION_SIXTEENTH},
{NOTE_REST, 0}
};
/* Docking complete: cheerful ascending chime */
const MelodyNote melody_docking_complete[] = {
{NOTE_C4, DURATION_EIGHTH},
{NOTE_E4, DURATION_EIGHTH},
{NOTE_G4, DURATION_EIGHTH},
{NOTE_C5, DURATION_QUARTER},
{NOTE_REST, DURATION_QUARTER},
{NOTE_G4, DURATION_EIGHTH},
{NOTE_C5, DURATION_HALF},
{NOTE_REST, 0}
};
/* ================================================================
* Melody Queue
* ================================================================ */
#define MELODY_QUEUE_SIZE 4
typedef struct {
const MelodyNote *notes; /* Melody sequence pointer */
uint16_t note_index; /* Current note in sequence */
uint32_t note_start_ms; /* When current note started */
uint32_t note_duration_ms; /* Duration of current note */
uint16_t current_frequency; /* Current tone frequency (Hz) */
bool is_custom; /* Is this a custom melody? */
} MelodyPlayback;
typedef struct {
MelodyPlayback queue[MELODY_QUEUE_SIZE];
uint8_t write_index;
uint8_t read_index;
uint8_t count;
} MelodyQueue;
static MelodyQueue s_queue = {0};
static MelodyPlayback s_current = {0};
static uint32_t s_last_tick_ms = 0;
/* ================================================================
* Hardware Initialization
* ================================================================ */
void buzzer_init(void)
{
/* Enable GPIO and timer clocks */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_TIM1_CLK_ENABLE();
/* Configure PA8 as TIM1_CH1 PWM output */
GPIO_InitTypeDef gpio_init = {0};
gpio_init.Pin = BUZZER_PIN;
gpio_init.Mode = GPIO_MODE_AF_PP;
gpio_init.Pull = GPIO_NOPULL;
gpio_init.Speed = GPIO_SPEED_HIGH;
gpio_init.Alternate = GPIO_AF1_TIM1;
HAL_GPIO_Init(BUZZER_PORT, &gpio_init);
/* Configure TIM1 for PWM:
* Clock: 216MHz / PSC = output frequency
* For 1kHz base frequency: PSC = 216, ARR = 1000
* Duty cycle = CCR / ARR (e.g., 500/1000 = 50%)
*/
TIM_HandleTypeDef htim1 = {0};
htim1.Instance = BUZZER_TIM;
htim1.Init.Prescaler = 216 - 1; /* 216MHz / 216 = 1MHz clock */
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = (1000000 / BUZZER_BASE_FREQ_HZ) - 1; /* 1kHz = 1000 counts */
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
HAL_TIM_PWM_Init(&htim1);
/* Configure PWM on CH1: 50% duty cycle initially (silence will be 0%) */
TIM_OC_InitTypeDef oc_init = {0};
oc_init.OCMode = TIM_OCMODE_PWM1;
oc_init.Pulse = 0; /* Start at 0% duty (silence) */
oc_init.OCPolarity = TIM_OCPOLARITY_HIGH;
oc_init.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_PWM_ConfigChannel(&htim1, &oc_init, BUZZER_TIM_CHANNEL);
/* Start PWM generation */
HAL_TIM_PWM_Start(BUZZER_TIM, BUZZER_TIM_CHANNEL);
/* Initialize queue */
memset(&s_queue, 0, sizeof(s_queue));
memset(&s_current, 0, sizeof(s_current));
s_last_tick_ms = 0;
}
/* ================================================================
* Public API
* ================================================================ */
bool buzzer_play_melody(MelodyType melody_type)
{
const MelodyNote *notes = NULL;
switch (melody_type) {
case MELODY_STARTUP:
notes = melody_startup;
break;
case MELODY_LOW_BATTERY:
notes = melody_low_battery;
break;
case MELODY_ERROR:
notes = melody_error;
break;
case MELODY_DOCKING_COMPLETE:
notes = melody_docking_complete;
break;
default:
return false;
}
return buzzer_play_custom(notes);
}
bool buzzer_play_custom(const MelodyNote *notes)
{
if (!notes || s_queue.count >= MELODY_QUEUE_SIZE) {
return false;
}
MelodyPlayback *playback = &s_queue.queue[s_queue.write_index];
memset(playback, 0, sizeof(*playback));
playback->notes = notes;
playback->note_index = 0;
playback->is_custom = true;
s_queue.write_index = (s_queue.write_index + 1) % MELODY_QUEUE_SIZE;
s_queue.count++;
return true;
}
bool buzzer_play_tone(uint16_t frequency, uint16_t duration_ms)
{
if (s_queue.count >= MELODY_QUEUE_SIZE) {
return false;
}
/* Create a simple 2-note melody: tone + rest */
static MelodyNote temp_notes[3];
temp_notes[0].frequency = frequency;
temp_notes[0].duration_ms = duration_ms;
temp_notes[1].frequency = NOTE_REST;
temp_notes[1].duration_ms = 0;
MelodyPlayback *playback = &s_queue.queue[s_queue.write_index];
memset(playback, 0, sizeof(*playback));
playback->notes = temp_notes;
playback->note_index = 0;
playback->is_custom = true;
s_queue.write_index = (s_queue.write_index + 1) % MELODY_QUEUE_SIZE;
s_queue.count++;
return true;
}
void buzzer_stop(void)
{
/* Clear queue and current playback */
memset(&s_queue, 0, sizeof(s_queue));
memset(&s_current, 0, sizeof(s_current));
/* Silence buzzer (0% duty cycle) */
TIM1->CCR1 = 0;
}
bool buzzer_is_playing(void)
{
return (s_current.notes != NULL) || (s_queue.count > 0);
}
/* ================================================================
* Timer Update and PWM Frequency Control
* ================================================================ */
static void buzzer_set_frequency(uint16_t frequency)
{
if (frequency == 0) {
/* Silence: 0% duty cycle */
TIM1->CCR1 = 0;
return;
}
/* Set PWM frequency and 50% duty cycle
* TIM1 clock: 1MHz (after prescaler)
* ARR = 1MHz / frequency
* CCR1 = ARR / 2 (50% duty)
*/
uint32_t arr = (1000000 / frequency);
if (arr > 65535) arr = 65535; /* Clamp to 16-bit */
TIM1->ARR = arr - 1;
TIM1->CCR1 = arr / 2; /* 50% duty cycle for all tones */
}
void buzzer_tick(uint32_t now_ms)
{
/* Check if current note has finished */
if (s_current.notes != NULL) {
uint32_t elapsed = now_ms - s_current.note_start_ms;
if (elapsed >= s_current.note_duration_ms) {
/* Move to next note */
s_current.note_index++;
if (s_current.notes[s_current.note_index].duration_ms == 0) {
/* End of melody sequence */
s_current.notes = NULL;
buzzer_set_frequency(0);
/* Start next queued melody if available */
if (s_queue.count > 0) {
s_current = s_queue.queue[s_queue.read_index];
s_queue.read_index = (s_queue.read_index + 1) % MELODY_QUEUE_SIZE;
s_queue.count--;
s_current.note_start_ms = now_ms;
s_current.note_duration_ms = s_current.notes[0].duration_ms;
buzzer_set_frequency(s_current.notes[0].frequency);
}
} else {
/* Play next note */
s_current.note_start_ms = now_ms;
s_current.note_duration_ms = s_current.notes[s_current.note_index].duration_ms;
uint16_t frequency = s_current.notes[s_current.note_index].frequency;
buzzer_set_frequency(frequency);
}
}
} else if (s_queue.count > 0 && s_current.notes == NULL) {
/* Start first queued melody */
s_current = s_queue.queue[s_queue.read_index];
s_queue.read_index = (s_queue.read_index + 1) % MELODY_QUEUE_SIZE;
s_queue.count--;
s_current.note_start_ms = now_ms;
s_current.note_duration_ms = s_current.notes[0].duration_ms;
buzzer_set_frequency(s_current.notes[0].frequency);
}
s_last_tick_ms = now_ms;
}

253
legacy/stm32/src/can_driver.c
View File

@ -1,253 +0,0 @@
/* CAN bus driver (Issues #597, #676, #674, #694) */
#include "can_driver.h"
#include "stm32f7xx_hal.h"
#include <string.h>
static CAN_HandleTypeDef s_can;
static volatile can_feedback_t s_feedback[CAN_NUM_MOTORS];
static volatile can_stats_t s_stats;
static can_ext_frame_cb_t s_ext_cb = NULL;
static can_std_frame_cb_t s_std_cb = NULL;
static volatile can_wdog_t s_wdog;
#ifdef TEST_HOST
static volatile uint32_t s_test_esr = 0u;
void can_driver_inject_esr(uint32_t v) { s_test_esr = v; }
static uint32_t _read_esr(void) { return s_test_esr; }
static HAL_StatusTypeDef _can_restart(void) {
HAL_CAN_Stop(&s_can); s_test_esr = 0u; return HAL_CAN_Start(&s_can);
}
#else
static uint32_t _read_esr(void) { return s_can.Instance->ESR; }
static HAL_StatusTypeDef _can_restart(void) {
HAL_CAN_Stop(&s_can); return HAL_CAN_Start(&s_can);
}
#endif
void can_driver_init(void)
{
__HAL_RCC_CAN1_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9;
gpio.Mode = GPIO_MODE_AF_PP; gpio.Pull = GPIO_NOPULL;
gpio.Speed = GPIO_SPEED_FREQ_HIGH; gpio.Alternate = GPIO_AF9_CAN1;
HAL_GPIO_Init(GPIOB, &gpio);
s_can.Instance = CAN1;
s_can.Init.Prescaler = CAN_PRESCALER;
s_can.Init.Mode = CAN_MODE_NORMAL;
s_can.Init.SyncJumpWidth = CAN_SJW_1TQ;
s_can.Init.TimeSeg1 = CAN_BS1_13TQ;
s_can.Init.TimeSeg2 = CAN_BS2_4TQ;
s_can.Init.TimeTriggeredMode = DISABLE;
s_can.Init.AutoBusOff = ENABLE;
s_can.Init.AutoWakeUp = DISABLE;
s_can.Init.AutoRetransmission = ENABLE;
s_can.Init.ReceiveFifoLocked = DISABLE;
s_can.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&s_can) != HAL_OK) { s_stats.bus_off = 1u; return; }
CAN_FilterTypeDef flt = {0};
flt.FilterBank = 0u; flt.FilterMode = CAN_FILTERMODE_IDMASK;
flt.FilterScale = CAN_FILTERSCALE_32BIT;
flt.FilterFIFOAssignment = CAN_RX_FIFO0;
flt.FilterActivation = CAN_FILTER_ENABLE;
flt.SlaveStartFilterBank = 14u;
if (HAL_CAN_ConfigFilter(&s_can, &flt) != HAL_OK) { s_stats.bus_off = 1u; return; }
CAN_FilterTypeDef flt2 = {0};
flt2.FilterBank = 15u; flt2.FilterMode = CAN_FILTERMODE_IDMASK;
flt2.FilterScale = CAN_FILTERSCALE_32BIT;
flt2.FilterIdLow = 0x0004u; flt2.FilterMaskIdLow = 0x0004u;
flt2.FilterFIFOAssignment = CAN_RX_FIFO1;
flt2.FilterActivation = CAN_FILTER_ENABLE;
flt2.SlaveStartFilterBank = 14u;
if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) { s_stats.bus_off = 1u; return; }
HAL_CAN_Start(&s_can);
#ifndef TEST_HOST
HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 5u, 0u);
HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn);
HAL_CAN_ActivateNotification(&s_can, CAN_IT_BUSOFF | CAN_IT_ERROR_PASSIVE | CAN_IT_ERROR_WARNING);
#endif
memset((void *)s_feedback, 0, sizeof(s_feedback));
memset((void *)&s_stats, 0, sizeof(s_stats));
memset((void *)&s_wdog, 0, sizeof(s_wdog));
}
void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd)
{
if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) return;
uint8_t data[4];
data[0] = (uint8_t)((uint16_t)cmd->velocity_rpm & 0xFFu);
data[1] = (uint8_t)((uint16_t)cmd->velocity_rpm >> 8u);
data[2] = (uint8_t)((uint16_t)cmd->torque_x100 & 0xFFu);
data[3] = (uint8_t)((uint16_t)cmd->torque_x100 >> 8u);
CAN_TxHeaderTypeDef hdr = {0};
hdr.StdId = CAN_ID_VEL_CMD_BASE + (uint32_t)node_id;
hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = 4u;
uint32_t mailbox;
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK)
s_stats.tx_count++;
else s_stats.err_count++;
}
}
void can_driver_send_enable(uint8_t node_id, bool enable)
{
if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) return;
uint8_t data[1] = { enable ? 1u : 0u };
CAN_TxHeaderTypeDef hdr = {0};
hdr.StdId = CAN_ID_ENABLE_CMD_BASE + (uint32_t)node_id;
hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = 1u;
uint32_t mailbox;
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK)
s_stats.tx_count++;
else s_stats.err_count++;
}
}
void can_driver_process(void)
{
if (_read_esr() & CAN_ESR_BOFF) { s_stats.bus_off = 1u; return; }
s_stats.bus_off = 0u;
while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) {
CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8];
if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO0, &rxhdr, rxdata) != HAL_OK) {
s_stats.err_count++; break;
}
if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) continue;
if (s_std_cb != NULL) s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) {
uint8_t nid = (uint8_t)nid_u;
s_feedback[nid].velocity_rpm = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
s_feedback[nid].current_ma = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
s_feedback[nid].temperature_c = (int8_t)rxdata[6];
s_feedback[nid].fault = rxdata[7];
s_feedback[nid].last_rx_ms = HAL_GetTick();
}
s_stats.rx_count++;
}
while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) {
CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8];
if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) {
s_stats.err_count++; break;
}
if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) continue;
if (s_ext_cb != NULL) s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
s_stats.rx_count++;
}
}
void can_driver_set_ext_cb(can_ext_frame_cb_t cb) { s_ext_cb = cb; }
void can_driver_set_std_cb(can_std_frame_cb_t cb) { s_std_cb = cb; }
void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len)
{
if (s_stats.bus_off || len > 8u) return;
CAN_TxHeaderTypeDef hdr = {0};
hdr.ExtId = ext_id; hdr.IDE = CAN_ID_EXT; hdr.RTR = CAN_RTR_DATA; hdr.DLC = len;
uint32_t mailbox;
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK)
s_stats.tx_count++;
else s_stats.err_count++;
}
}
void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len)
{
if (s_stats.bus_off || len > 8u) return;
CAN_TxHeaderTypeDef hdr = {0};
hdr.StdId = std_id; hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = len;
uint32_t mailbox;
if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK)
s_stats.tx_count++;
else s_stats.err_count++;
}
}
bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out)
{
if (node_id >= CAN_NUM_MOTORS || out == NULL) return false;
if (s_feedback[node_id].last_rx_ms == 0u) return false;
memcpy(out, (const void *)&s_feedback[node_id], sizeof(can_feedback_t));
return true;
}
bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms)
{
if (node_id >= CAN_NUM_MOTORS || s_feedback[node_id].last_rx_ms == 0u) return false;
return (now_ms - s_feedback[node_id].last_rx_ms) < CAN_NODE_TIMEOUT_MS;
}
void can_driver_get_stats(can_stats_t *out)
{
if (out == NULL) return;
memcpy(out, (const void *)&s_stats, sizeof(can_stats_t));
}
/* SCE interrupt handler -- Issue #694 */
#ifndef TEST_HOST
void CAN1_SCE_IRQHandler(void)
{
uint32_t esr = s_can.Instance->ESR;
if (esr & CAN_ESR_BOFF) {
if (s_wdog.error_state != CAN_ERR_BUS_OFF) s_wdog.busoff_count++;
s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u;
} else if (esr & CAN_ESR_EPVF) {
if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) {
s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE;
}
} else if (esr & CAN_ESR_EWGF) {
if (s_wdog.error_state < CAN_ERR_WARNING) {
s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING;
}
}
__HAL_CAN_CLEAR_FLAG(&s_can, CAN_FLAG_ERRI);
HAL_CAN_IRQHandler(&s_can);
}
#endif
/* Watchdog tick -- Issue #694 */
can_error_state_t can_driver_watchdog_tick(uint32_t now_ms)
{
uint32_t esr = _read_esr();
if (esr & CAN_ESR_BOFF) {
if (s_wdog.error_state != CAN_ERR_BUS_OFF) {
s_wdog.busoff_count++; s_wdog.busoff_ms = now_ms; s_wdog.busoff_pending = 1u;
}
s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u;
} else if (esr & CAN_ESR_EPVF) {
if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) {
s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE;
}
} else if (esr & CAN_ESR_EWGF) {
if (s_wdog.error_state < CAN_ERR_WARNING) {
s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING;
}
} else {
if (s_wdog.error_state > CAN_ERR_NOMINAL && s_wdog.error_state < CAN_ERR_BUS_OFF)
s_wdog.error_state = CAN_ERR_NOMINAL;
}
if (s_wdog.busoff_pending && s_wdog.error_state == CAN_ERR_BUS_OFF &&
(now_ms - s_wdog.busoff_ms) >= CAN_WDOG_RESTART_MS) {
if (_can_restart() == HAL_OK) {
s_wdog.restart_count++; s_wdog.error_state = CAN_ERR_NOMINAL;
s_wdog.busoff_pending = 0u; s_stats.bus_off = 0u;
} else {
s_wdog.busoff_ms = now_ms;
}
}
s_wdog.tec = (uint8_t)((esr >> CAN_ESR_TEC_Pos) & 0xFFu);
s_wdog.rec = (uint8_t)((esr >> CAN_ESR_REC_Pos) & 0xFFu);
return s_wdog.error_state;
}
void can_driver_get_wdog(can_wdog_t *out)
{
if (out == NULL) return;
memcpy(out, (const void *)&s_wdog, sizeof(can_wdog_t));
}

118
legacy/stm32/src/coulomb_counter.c
View File

@ -1,118 +0,0 @@
/*
* coulomb_counter.c — Battery coulomb counter (Issue #325)
*
* Tracks Ah consumed from current readings, provides SoC independent of load.
* Time integration: consumed_mah += current_ma * dt_ms / 3600000
*/
#include "coulomb_counter.h"
#include "stm32f7xx_hal.h"
/* State structure */
static struct {
bool initialized;
bool valid; /* At least one measurement taken */
uint16_t capacity_mah; /* Battery capacity in mAh */
uint32_t accumulated_mah_x100; /* Accumulated coulombs in mAh×100 (fixed-point) */
uint32_t last_tick_ms; /* Last update timestamp (ms) */
} s_state = {0};
void coulomb_counter_init(uint16_t capacity_mah) {
if (capacity_mah == 0 || capacity_mah > 20000) {
/* Sanity check: reasonable battery is 100–20000 mAh */
return;
}
s_state.capacity_mah = capacity_mah;
s_state.accumulated_mah_x100 = 0;
s_state.last_tick_ms = HAL_GetTick();
s_state.initialized = true;
s_state.valid = false;
}
void coulomb_counter_accumulate(int16_t current_ma) {
if (!s_state.initialized) return;
uint32_t now_ms = HAL_GetTick();
uint32_t dt_ms = now_ms - s_state.last_tick_ms;
/* Handle tick wraparound (~49.7 days at 32-bit ms) */
if (dt_ms > 86400000UL) {
/* If jump > 1 day, likely wraparound; skip this sample */
s_state.last_tick_ms = now_ms;
return;
}
/* Prevent negative dt or dt=0 */
if (dt_ms == 0) return;
if (dt_ms > 1000) {
/* Cap to 1 second max per call to prevent overflow */
dt_ms = 1000;
}
/* Accumulate: mAh += mA × dt_ms / 3600000
* Using fixed-point (×100): accumulated_mah_x100 += mA × dt_ms / 36000 */
int32_t coulomb_x100 = (int32_t)current_ma * (int32_t)dt_ms / 36000;
/* Only accumulate if discharging (positive current) or realistic charging */
if (coulomb_x100 > 0) {
s_state.accumulated_mah_x100 += (uint32_t)coulomb_x100;
} else if (coulomb_x100 < 0 && s_state.accumulated_mah_x100 > 0) {
/* Allow charging (negative current) to reduce accumulated coulombs */
int32_t new_val = (int32_t)s_state.accumulated_mah_x100 + coulomb_x100;
if (new_val < 0) {
s_state.accumulated_mah_x100 = 0;
} else {
s_state.accumulated_mah_x100 = (uint32_t)new_val;
}
}
/* Clamp to capacity */
if (s_state.accumulated_mah_x100 > (uint32_t)s_state.capacity_mah * 100) {
s_state.accumulated_mah_x100 = (uint32_t)s_state.capacity_mah * 100;
}
s_state.last_tick_ms = now_ms;
s_state.valid = true;
}
uint8_t coulomb_counter_get_soc_pct(void) {
if (!s_state.valid) return 255; /* 255 = invalid/not measured */
/* SoC = 100 - (consumed_mah / capacity_mah) * 100 */
uint32_t consumed_mah = s_state.accumulated_mah_x100 / 100;
if (consumed_mah >= s_state.capacity_mah) {
return 0; /* Fully discharged */
}
uint32_t remaining_mah = s_state.capacity_mah - consumed_mah;
uint8_t soc = (uint8_t)((remaining_mah * 100u) / s_state.capacity_mah);
return soc;
}
uint16_t coulomb_counter_get_consumed_mah(void) {
return (uint16_t)(s_state.accumulated_mah_x100 / 100);
}
uint16_t coulomb_counter_get_remaining_mah(void) {
if (!s_state.valid) return s_state.capacity_mah;
uint32_t consumed = s_state.accumulated_mah_x100 / 100;
if (consumed >= s_state.capacity_mah) {
return 0;
}
return (uint16_t)(s_state.capacity_mah - consumed);
}
void coulomb_counter_reset(void) {
if (!s_state.initialized) return;
s_state.accumulated_mah_x100 = 0;
s_state.last_tick_ms = HAL_GetTick();
}
bool coulomb_counter_is_valid(void) {
return s_state.valid;
}

361
legacy/stm32/src/crsf.c
View File

@ -1,361 +0,0 @@
/*
* crsf.c — CRSF/ExpressLRS RC receiver driver
*
* Hardware: UART4, PA0=TX PA1=RX (GPIO_AF8_UART4), 420000 baud 8N1
* DMA: DMA1 Stream2 Channel4 (UART4_RX), circular 64-byte buffer
* UART IDLE interrupt → drain; DMA half/complete callbacks → drain
*
* CRSF frame layout:
* [0xC8] [LEN] [TYPE] [PAYLOAD...] [CRC8-DVB-S2]
* LEN = bytes after itself = TYPE + PAYLOAD + CRC (max 62)
* CRC covers TYPE through last PAYLOAD byte (not SYNC, not LEN, not CRC)
*
* Supported frame types:
* 0x16 — RC channels (22 bytes, 16 ch × 11 bit packed)
* 0x14 — Link statistics (RSSI, LQ, SNR)
*
* Channel mapping (0-indexed):
* CH1 [0] = steering (-1000..+1000)
* CH2 [1] = speed/lean override (future use)
* CH5 [4] = arm switch (> CRSF_ARM_THRESHOLD = armed)
* CH6 [5] = mode (< 992 = RC, > 992 = auto)
*
* Protocol reference: ExpressLRS/CRSF_Spec, verified against Betaflight src/main/rx/crsf.c
*/
#include "crsf.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
/* ------------------------------------------------------------------ */
/* DMA circular receive buffer */
/* ------------------------------------------------------------------ */
#define CRSF_DMA_BUF_SIZE 64u /* must be power-of-2 >= 2× max frame (26) */
static uint8_t s_dma_buf[CRSF_DMA_BUF_SIZE];
static volatile uint16_t s_dma_head = 0; /* last processed position */
static UART_HandleTypeDef s_uart;
static DMA_HandleTypeDef s_dma_rx;
/* ------------------------------------------------------------------ */
/* Frame parser state */
/* ------------------------------------------------------------------ */
#define CRSF_SYNC 0xC8u
#define CRSF_FRAME_RC 0x16u /* RC channels packed */
#define CRSF_FRAME_LINK 0x14u /* Link statistics */
#define CRSF_MAX_FRAME_LEN 64u
typedef enum { ST_SYNC, ST_LEN, ST_DATA } parse_state_t;
static parse_state_t s_ps = ST_SYNC;
static uint8_t s_frame[CRSF_MAX_FRAME_LEN];
static uint8_t s_flen = 0; /* total expected frame bytes */
static uint8_t s_fpos = 0; /* bytes received so far */
/* ------------------------------------------------------------------ */
/* Public state */
/* ------------------------------------------------------------------ */
volatile CRSFState crsf_state = {0};
/* ------------------------------------------------------------------ */
/* CRC8 DVB-S2 — polynomial 0xD5 */
/* ------------------------------------------------------------------ */
static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a) {
crc ^= a;
for (int i = 0; i < 8; i++) {
crc = (crc & 0x80u) ? ((crc << 1) ^ 0xD5u) : (crc << 1);
}
return crc;
}
static uint8_t crsf_frame_crc(const uint8_t *frame, uint8_t frame_len) {
/* CRC covers frame[2] (type) .. frame[frame_len-2] (last payload byte) */
uint8_t crc = 0;
for (uint8_t i = 2; i < frame_len - 1; i++) {
crc = crc8_dvb_s2(crc, frame[i]);
}
return crc;
}
/* ------------------------------------------------------------------ */
/* 11-bit channel unpacking — 16 channels from 22 bytes */
/* ------------------------------------------------------------------ */
static void unpack_channels(const uint8_t *payload, uint16_t *ch) {
uint32_t bits = 0;
int loaded = 0, idx = 0, src = 0;
while (idx < 16) {
while (loaded < 11 && src < 22) {
bits |= (uint32_t)payload[src++] << loaded;
loaded += 8;
}
ch[idx++] = bits & 0x7FFu;
bits >>= 11;
loaded -= 11;
}
}
/* ------------------------------------------------------------------ */
/* Frame processing */
/* ------------------------------------------------------------------ */
static void process_frame(const uint8_t *frame, uint8_t frame_len) {
/* Validate minimum length and CRC */
if (frame_len < 4) return;
if (frame[frame_len - 1] != crsf_frame_crc(frame, frame_len)) return;
uint8_t type = frame[2];
const uint8_t *payload = &frame[3];
uint8_t payload_len = frame_len - 4; /* type + payload + crc = frame[1], minus type and crc */
switch (type) {
case CRSF_FRAME_RC:
if (payload_len < 22) return;
unpack_channels(payload, (uint16_t *)crsf_state.channels);
crsf_state.last_rx_ms = HAL_GetTick();
/* Update arm switch state from CH5 (index 4) */
crsf_state.armed = (crsf_state.channels[4] > CRSF_ARM_THRESHOLD);
break;
case CRSF_FRAME_LINK:
/* Link stats payload:
* [0] uplink RSSI ant1 (value = -dBm, so negate for dBm)
* [2] uplink link quality (0-100 %)
* [3] uplink SNR (signed dB)
*/
if (payload_len < 4) return;
crsf_state.rssi_dbm = -(int8_t)payload[0];
crsf_state.link_quality = payload[2];
crsf_state.snr = (int8_t)payload[3];
break;
default:
break;
}
}
/* ------------------------------------------------------------------ */
/* Byte-level parser state machine */
/* ------------------------------------------------------------------ */
static void parse_byte(uint8_t b) {
switch (s_ps) {
case ST_SYNC:
if (b == CRSF_SYNC) {
s_frame[0] = b;
s_ps = ST_LEN;
}
break;
case ST_LEN:
/* LEN = bytes remaining after this field (type + payload + crc), max 62 */
if (b >= 2 && b <= 62) {
s_frame[1] = b;
s_flen = b + 2u; /* total frame = SYNC + LEN + rest */
s_fpos = 2;
s_ps = ST_DATA;
} else {
s_ps = ST_SYNC; /* invalid length — resync */
}
break;
case ST_DATA:
s_frame[s_fpos++] = b;
if (s_fpos >= s_flen) {
process_frame(s_frame, s_flen);
s_ps = ST_SYNC;
}
break;
}
}
/* ------------------------------------------------------------------ */
/* DMA buffer drain — called from IDLE IRQ and DMA half/complete CBs */
/* ------------------------------------------------------------------ */
static void dma_drain(void) {
/* DMA CNDTR counts DOWN from buf size; write position = size - CNDTR */
uint16_t pos = (uint16_t)(CRSF_DMA_BUF_SIZE - __HAL_DMA_GET_COUNTER(&s_dma_rx));
uint16_t head = s_dma_head;
while (head != pos) {
parse_byte(s_dma_buf[head]);
head = (head + 1u) & (CRSF_DMA_BUF_SIZE - 1u);
}
s_dma_head = pos;
}
/* ------------------------------------------------------------------ */
/* IRQ handlers */
/* ------------------------------------------------------------------ */
void UART4_IRQHandler(void) {
/* IDLE line detection — fires when bus goes quiet between frames */
if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) {
__HAL_UART_CLEAR_IDLEFLAG(&s_uart);
dma_drain();
}
HAL_UART_IRQHandler(&s_uart);
}
void DMA1_Stream2_IRQHandler(void) {
HAL_DMA_IRQHandler(&s_dma_rx);
}
/* DMA half-complete: drain first half of circular buffer */
void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *h) {
if (h->Instance == UART4) dma_drain();
}
/* DMA complete: drain second half (buffer wrapped) */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *h) {
if (h->Instance == UART4) dma_drain();
if (h->Instance == USART6) {
extern void jetson_uart_rx_callback(UART_HandleTypeDef *huart);
jetson_uart_rx_callback(h);
}
}
/* ------------------------------------------------------------------ */
/* Public API */
/* ------------------------------------------------------------------ */
/*
* crsf_init() — configure UART4 + DMA1 and start circular receive.
*
* UART4: PA0=TX, PA1=RX, AF8_UART4, 420000 baud 8N1, oversampling×8.
* APB1 = 54 MHz → BRR = 0x101 → actual 418604 baud (0.33% error, within CRSF spec).
*
* DMA: DMA1 Stream2 Channel4, peripheral→memory, circular, byte width.
* IDLE interrupt + DMA half/complete callbacks drain the circular buffer.
*/
void crsf_init(void) {
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_UART4_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
/* PA0=TX, PA1=RX, AF8_UART4 */
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_0 | GPIO_PIN_1;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_PULLUP;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
gpio.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIOA, &gpio);
/* DMA1 Stream2 Channel4 — UART4_RX, circular byte transfers */
s_dma_rx.Instance = DMA1_Stream2;
s_dma_rx.Init.Channel = DMA_CHANNEL_4;
s_dma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
s_dma_rx.Init.PeriphInc = DMA_PINC_DISABLE;
s_dma_rx.Init.MemInc = DMA_MINC_ENABLE;
s_dma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
s_dma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
s_dma_rx.Init.Mode = DMA_CIRCULAR;
s_dma_rx.Init.Priority = DMA_PRIORITY_LOW;
s_dma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
HAL_DMA_Init(&s_dma_rx);
__HAL_LINKDMA(&s_uart, hdmarx, s_dma_rx);
/* UART4: 420000 8N1, oversampling×8 (better tolerance at high baud) */
s_uart.Instance = UART4;
s_uart.Init.BaudRate = 420000;
s_uart.Init.WordLength = UART_WORDLENGTH_8B;
s_uart.Init.StopBits = UART_STOPBITS_1;
s_uart.Init.Parity = UART_PARITY_NONE;
s_uart.Init.Mode = UART_MODE_TX_RX;
s_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
s_uart.Init.OverSampling = UART_OVERSAMPLING_8;
HAL_UART_Init(&s_uart);
/* Start circular DMA — runs indefinitely, no need to restart */
HAL_UART_Receive_DMA(&s_uart, s_dma_buf, CRSF_DMA_BUF_SIZE);
/* IDLE line interrupt — fires when UART bus goes quiet (end of frame) */
__HAL_UART_ENABLE_IT(&s_uart, UART_IT_IDLE);
HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 6, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn);
HAL_NVIC_SetPriority(UART4_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(UART4_IRQn);
}
/*
* crsf_parse_byte() — kept for compatibility; direct call path not used
* when DMA is active, but available for unit testing or UART-IT fallback.
*/
void crsf_parse_byte(uint8_t byte) {
parse_byte(byte);
}
/*
* crsf_to_range() — map raw CRSF value 172–1811 to [min, max].
* Midpoint 992 maps to (min+max)/2.
*/
int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max) {
int32_t v = (int32_t)val;
int32_t r = min + (v - 172) * (int32_t)(max - min) / (1811 - 172);
if (r < min) r = min;
if (r > max) r = max;
return (int16_t)r;
}
/* ------------------------------------------------------------------ */
/* Telemetry TX helpers */
/* ------------------------------------------------------------------ */
/*
* Build a CRSF frame in `buf` and return the total byte count.
* buf must be at least CRSF_MAX_FRAME_LEN bytes.
* frame_type : CRSF type byte (e.g. 0x08 battery, 0x21 flight mode)
* payload : frame payload bytes (excluding type, CRC)
* plen : payload length in bytes
*/
static uint8_t crsf_build_frame(uint8_t *buf, uint8_t frame_type,
const uint8_t *payload, uint8_t plen) {
/* Total frame = SYNC + LEN + TYPE + PAYLOAD + CRC */
uint8_t frame_len = 2u + 1u + plen + 1u; /* SYNC + LEN + TYPE + payload + CRC */
if (frame_len > CRSF_MAX_FRAME_LEN) return 0;
buf[0] = CRSF_SYNC; /* 0xC8 */
buf[1] = (uint8_t)(plen + 2u); /* LEN = TYPE + payload + CRC */
buf[2] = frame_type;
memcpy(&buf[3], payload, plen);
buf[frame_len - 1] = crsf_frame_crc(buf, frame_len);
return frame_len;
}
/*
* crsf_send_battery() — type 0x08 battery sensor.
* voltage_mv → units of 100 mV (big-endian uint16)
* capacity_mah → remaining capacity in mAh (Issue #325, coulomb counter)
* remaining_pct→ 0–100 % (uint8)
*/
void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah,
uint8_t remaining_pct) {
uint16_t v100 = (uint16_t)(voltage_mv / 100u); /* 100 mV units */
/* Convert capacity (mAh) to 3-byte big-endian: cap_hi, cap_mid, cap_lo */
uint32_t cap = capacity_mah & 0xFFFFFFu; /* 24-bit cap max */
/* Payload: [v_hi][v_lo][current_hi][current_lo][cap_hi][cap_mid][cap_lo][remaining] */
uint8_t payload[8] = {
(uint8_t)(v100 >> 8), (uint8_t)(v100 & 0xFF),
0, 0, /* current: not available on STM32, always 0 for now */
(uint8_t)((cap >> 16) & 0xFF), /* cap_hi */
(uint8_t)((cap >> 8) & 0xFF), /* cap_mid */
(uint8_t)(cap & 0xFF), /* cap_lo */
remaining_pct,
};
uint8_t frame[CRSF_MAX_FRAME_LEN];
uint8_t flen = crsf_build_frame(frame, 0x08u, payload, sizeof(payload));
if (flen) HAL_UART_Transmit(&s_uart, frame, flen, 5u);
}
/*
* crsf_send_flight_mode() — type 0x21 flight mode text.
* Displays on the handset's OSD/status bar.
* "ARMED\0" when armed (5 payload bytes + null)
* "DISARM\0" when not (7 payload bytes + null)
*/
void crsf_send_flight_mode(bool armed) {
const char *text = armed ? "ARMED" : "DISARM";
uint8_t plen = (uint8_t)(strlen(text) + 1u); /* include null terminator */
uint8_t frame[CRSF_MAX_FRAME_LEN];
uint8_t flen = crsf_build_frame(frame, 0x21u, (const uint8_t *)text, plen);
if (flen) HAL_UART_Transmit(&s_uart, frame, flen, 5u);
}

347
legacy/stm32/src/encoder_odom.c
View File

@ -1,347 +0,0 @@
/*
* encoder_odom.c — quadrature encoder reading and differential-drive
* odometry for Issue #632.
*
* TIM2 (32-bit) = left encoder, TIM3 (16-bit) = right encoder.
* Both configured in encoder mode 3 (count on both A and B edges × 4).
*
* RPM formula:
* rpm = delta_ticks * 60 / (ticks_per_rev * dt_s)
*
* Odometry (Euler-forward differential drive):
* d_l = delta_left * meters_per_tick
* d_r = delta_right * meters_per_tick
* d_c = (d_l + d_r) / 2
* dθ = (d_r - d_l) / wheel_base_m
* x += d_c * cos(θ)
* y += d_c * sin(θ)
* θ += dθ
*/
#include "encoder_odom.h"
#include "jlink.h"
#include <math.h>
#include <string.h>
/* ---- Platform abstraction (stubbed in TEST_HOST builds) ---- */
#ifndef TEST_HOST
#include "stm32f7xx_hal.h"
#include "config.h"
/* Read 32-bit left encoder counter (TIM2) */
static inline uint32_t enc_read_left(void)
{
return (uint32_t)TIM2->CNT;
}
/* Read 16-bit right encoder counter (TIM3) */
static inline uint16_t enc_read_right(void)
{
return (uint16_t)TIM3->CNT;
}
/* Configure TIM2 in encoder mode (left wheel) */
static void enc_tim2_init(void)
{
/* Enable peripheral clock */
__HAL_RCC_TIM2_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/* PA15 = TIM2_CH1 (AF1), PB3 = TIM2_CH2 (AF1) — floating input */
GPIO_InitTypeDef g = {0};
g.Mode = GPIO_MODE_AF_PP;
g.Pull = GPIO_PULLUP;
g.Speed = GPIO_SPEED_FREQ_LOW;
g.Alternate = GPIO_AF1_TIM2;
g.Pin = GPIO_PIN_15;
HAL_GPIO_Init(GPIOA, &g);
g.Pin = GPIO_PIN_3;
HAL_GPIO_Init(GPIOB, &g);
/* Encoder mode 3: count on both TI1 and TI2 edges */
TIM2->CR1 = 0;
TIM2->SMCR = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1; /* SMS = 0x3 */
TIM2->CCMR1 = (TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0); /* IC1→TI1, IC2→TI2 */
TIM2->CCER = 0; /* no polarity inversion (change CC1P/CC2P to reverse) */
TIM2->ARR = 0xFFFFFFFFUL;
TIM2->CNT = 0;
TIM2->CR1 |= TIM_CR1_CEN;
}
/* Configure TIM3 in encoder mode (right wheel) */
static void enc_tim3_init(void)
{
__HAL_RCC_TIM3_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
/* PC6 = TIM3_CH1 (AF2), PC7 = TIM3_CH2 (AF2) */
GPIO_InitTypeDef g = {0};
g.Mode = GPIO_MODE_AF_PP;
g.Pull = GPIO_PULLUP;
g.Speed = GPIO_SPEED_FREQ_LOW;
g.Alternate = GPIO_AF2_TIM3;
g.Pin = GPIO_PIN_6 | GPIO_PIN_7;
HAL_GPIO_Init(GPIOC, &g);
TIM3->CR1 = 0;
TIM3->SMCR = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1;
TIM3->CCMR1 = (TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0);
TIM3->CCER = 0;
TIM3->ARR = 0xFFFF;
TIM3->CNT = 0;
TIM3->CR1 |= TIM_CR1_CEN;
}
/* Flash helpers */
static bool flash_write_words(uint32_t addr, const void *data, uint32_t len)
{
const uint32_t *src = (const uint32_t *)data;
uint32_t words = (len + 3u) / 4u;
for (uint32_t i = 0; i < words; i++) {
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
addr + i * 4u, src[i]) != HAL_OK) {
return false;
}
}
return true;
}
static bool flash_erase_sector7(void)
{
FLASH_EraseInitTypeDef erase = {
.TypeErase = FLASH_TYPEERASE_SECTORS,
.Sector = FLASH_SECTOR_7,
.NbSectors = 1,
.VoltageRange = FLASH_VOLTAGE_RANGE_3,
};
uint32_t err = 0;
return (HAL_FLASHEx_Erase(&erase, &err) == HAL_OK) &&
(err == 0xFFFFFFFFUL);
}
#else /* TEST_HOST stubs */
/* Test-controlled counter values */
static uint32_t g_enc_left_cnt = 0;
static uint16_t g_enc_right_cnt = 0;
static inline uint32_t enc_read_left(void) { return g_enc_left_cnt; }
static inline uint16_t enc_read_right(void) { return g_enc_right_cnt; }
static void enc_tim2_init(void) {}
static void enc_tim3_init(void) {}
static bool g_flash_erase_ok = true;
static bool g_flash_write_ok = true;
static enc_flash_config_t g_flash_store;
static bool g_flash_has_data = false;
static bool flash_erase_sector7(void) { return g_flash_erase_ok; }
static bool flash_write_words(uint32_t addr, const void *data, uint32_t len)
{
(void)addr;
if (!g_flash_write_ok) return false;
memcpy(&g_flash_store, data, len < sizeof(g_flash_store) ? len : sizeof(g_flash_store));
g_flash_has_data = true;
return true;
}
#endif /* TEST_HOST */
/* ---- Config defaults ---- */
static void cfg_set_defaults(enc_config_t *cfg)
{
cfg->ticks_per_rev = ENC_TICKS_PER_REV_DEFAULT;
cfg->wheel_diam_mm = ENC_WHEEL_DIAM_MM_DEFAULT;
cfg->wheel_base_mm = ENC_WHEEL_BASE_MM_DEFAULT;
}
/* ---- Pre-compute derived constants from config ---- */
static void enc_update_precomputed(encoder_odom_t *eo)
{
eo->meters_per_tick = (3.14159265358979f * (float)eo->cfg.wheel_diam_mm * 0.001f)
/ (float)eo->cfg.ticks_per_rev;
eo->wheel_base_m = (float)eo->cfg.wheel_base_mm * 0.001f;
}
/* ---- encoder_odom_load_config() ---- */
bool encoder_odom_load_config(enc_config_t *cfg)
{
#ifndef TEST_HOST
const enc_flash_config_t *p = (const enc_flash_config_t *)ENC_FLASH_ADDR;
if (p->magic == ENC_FLASH_MAGIC &&
p->ticks_per_rev > 0u &&
p->wheel_diam_mm > 0u &&
p->wheel_base_mm > 0u)
{
cfg->ticks_per_rev = p->ticks_per_rev;
cfg->wheel_diam_mm = p->wheel_diam_mm;
cfg->wheel_base_mm = p->wheel_base_mm;
return true;
}
#else
if (g_flash_has_data && g_flash_store.magic == ENC_FLASH_MAGIC &&
g_flash_store.ticks_per_rev > 0u &&
g_flash_store.wheel_diam_mm > 0u &&
g_flash_store.wheel_base_mm > 0u)
{
cfg->ticks_per_rev = g_flash_store.ticks_per_rev;
cfg->wheel_diam_mm = g_flash_store.wheel_diam_mm;
cfg->wheel_base_mm = g_flash_store.wheel_base_mm;
return true;
}
#endif
cfg_set_defaults(cfg);
return false;
}
/* ---- encoder_odom_save_config() ---- */
bool encoder_odom_save_config(const enc_config_t *cfg)
{
enc_flash_config_t rec;
memset(&rec, 0xFF, sizeof(rec));
rec.magic = ENC_FLASH_MAGIC;
rec.ticks_per_rev = cfg->ticks_per_rev;
rec.wheel_diam_mm = cfg->wheel_diam_mm;
rec.wheel_base_mm = cfg->wheel_base_mm;
#ifndef TEST_HOST
HAL_FLASH_Unlock();
bool ok = flash_erase_sector7() &&
flash_write_words(ENC_FLASH_ADDR, &rec, sizeof(rec));
HAL_FLASH_Lock();
return ok;
#else
return flash_write_words(ENC_FLASH_ADDR, &rec, sizeof(rec));
#endif
}
/* ---- encoder_odom_init() ---- */
void encoder_odom_init(encoder_odom_t *eo)
{
memset(eo, 0, sizeof(*eo));
/* Load or default config */
encoder_odom_load_config(&eo->cfg);
enc_update_precomputed(eo);
/* Initialize timers */
enc_tim2_init();
enc_tim3_init();
/* Snapshot initial counter values (treat as zero reference) */
eo->cnt_left = enc_read_left();
eo->cnt_right = enc_read_right();
/* Initialize TLM timer so first call fires immediately */
eo->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (ENC_TLM_HZ > 0u ? ENC_TLM_HZ : 1u)));
}
/* ---- encoder_odom_reset_pose() ---- */
void encoder_odom_reset_pose(encoder_odom_t *eo)
{
eo->x_mm = 0.0f;
eo->y_mm = 0.0f;
eo->theta_rad = 0.0f;
}
/* ---- encoder_odom_tick() ---- */
void encoder_odom_tick(encoder_odom_t *eo, uint32_t now_ms)
{
/* Compute dt */
uint32_t elapsed_ms = now_ms - eo->last_tick_ms;
if (elapsed_ms == 0u) return; /* avoid divide-by-zero at high call rates */
if (elapsed_ms > 500u) elapsed_ms = 500u; /* clamp: stale data guard */
float dt_s = (float)elapsed_ms * 0.001f;
eo->last_tick_ms = now_ms;
/* Read counters */
uint32_t new_left = enc_read_left();
uint16_t new_right = enc_read_right();
/* Signed delta — handles 32/16-bit wrap correctly */
int32_t delta_left = (int32_t)(new_left - eo->cnt_left);
int16_t delta_right = (int16_t)(new_right - eo->cnt_right);
eo->cnt_left = new_left;
eo->cnt_right = (uint16_t)new_right;
/* RPM */
float rpm_scale = 60.0f / ((float)eo->cfg.ticks_per_rev * dt_s);
float rpm_l = (float)delta_left * rpm_scale;
float rpm_r = (float)delta_right * rpm_scale;
/* Clamp to int16 range */
if (rpm_l > 32767.0f) rpm_l = 32767.0f;
if (rpm_l < -32768.0f) rpm_l = -32768.0f;
if (rpm_r > 32767.0f) rpm_r = 32767.0f;
if (rpm_r < -32768.0f) rpm_r = -32768.0f;
eo->rpm_left = (int16_t)rpm_l;
eo->rpm_right = (int16_t)rpm_r;
/* Wheel displacements (metres) */
float d_left = (float)delta_left * eo->meters_per_tick;
float d_right = (float)delta_right * eo->meters_per_tick;
/* Linear / angular increments */
float d_center = (d_left + d_right) * 0.5f;
float d_theta = (d_right - d_left) / eo->wheel_base_m;
/* Linear speed (mm/s), clamped to int16 */
float speed_raw = (d_center / dt_s) * 1000.0f; /* m/s → mm/s */
if (speed_raw > 32767.0f) speed_raw = 32767.0f;
if (speed_raw < -32768.0f) speed_raw = -32768.0f;
eo->speed_mmps = (int16_t)speed_raw;
/* Integrate pose (Euler-forward) */
float cos_h = cosf(eo->theta_rad);
float sin_h = sinf(eo->theta_rad);
eo->x_mm += d_center * cos_h * 1000.0f;
eo->y_mm += d_center * sin_h * 1000.0f;
eo->theta_rad += d_theta;
/* Wrap theta to (-π, π] */
while (eo->theta_rad > 3.14159265358979f) eo->theta_rad -= 2.0f * 3.14159265358979f;
while (eo->theta_rad < -3.14159265358979f) eo->theta_rad += 2.0f * 3.14159265358979f;
}
/* ---- encoder_odom_send_tlm() ---- */
void encoder_odom_send_tlm(const encoder_odom_t *eo, uint32_t now_ms)
{
if (ENC_TLM_HZ == 0u) return;
uint32_t interval_ms = 1000u / ENC_TLM_HZ;
if ((now_ms - eo->last_tlm_ms) < interval_ms) return;
/* Cast away const for timestamp — only mutable field */
((encoder_odom_t *)eo)->last_tlm_ms = now_ms;
jlink_tlm_odom_t tlm;
tlm.rpm_left = eo->rpm_left;
tlm.rpm_right = eo->rpm_right;
/* x/y: float mm → int32_t mm (saturate at ±2147 km — more than adequate) */
float xf = eo->x_mm;
float yf = eo->y_mm;
if (xf > 2147483647.0f) xf = 2147483647.0f;
if (xf < -2147483648.0f) xf = -2147483648.0f;
if (yf > 2147483647.0f) yf = 2147483647.0f;
if (yf < -2147483648.0f) yf = -2147483648.0f;
tlm.x_mm = (int32_t)xf;
tlm.y_mm = (int32_t)yf;
/* theta: radians → centidegrees (0.01° steps) */
float theta_cdeg = eo->theta_rad * (18000.0f / 3.14159265358979f);
if (theta_cdeg > 32767.0f) theta_cdeg = 32767.0f;
if (theta_cdeg < -32768.0f) theta_cdeg = -32768.0f;
tlm.theta_cdeg = (int16_t)theta_cdeg;
tlm.speed_mmps = eo->speed_mmps;
jlink_send_odom_tlm(&tlm);
}

58
legacy/stm32/src/esc_backend.c
View File

@ -1,58 +0,0 @@
#include "esc_backend.h"
#include <stddef.h>
/* Global active backend (selected at runtime or compile-time) */
static const esc_backend_t *g_active_backend = NULL;
void esc_backend_register(const esc_backend_t *backend) {
g_active_backend = backend;
if (backend && backend->init) {
backend->init();
}
}
const esc_backend_t *esc_backend_get(void) {
return g_active_backend;
}
/* High-level convenience wrappers — call through vtable */
void esc_init(void) {
if (g_active_backend && g_active_backend->init) {
g_active_backend->init();
}
}
void esc_send(int16_t speed, int16_t steer) {
if (g_active_backend && g_active_backend->send) {
g_active_backend->send(speed, steer);
}
}
void esc_estop(void) {
if (g_active_backend && g_active_backend->estop) {
g_active_backend->estop();
}
}
void esc_resume(void) {
if (g_active_backend && g_active_backend->resume) {
g_active_backend->resume();
}
}
void esc_get_telemetry(esc_telemetry_t *out) {
if (out) {
/* Zero-fill by default */
out->speed = 0;
out->steer = 0;
out->voltage_mv = 0;
out->current_ma = 0;
out->temperature_c = 0;
out->fault = 0;
if (g_active_backend && g_active_backend->get_telemetry) {
g_active_backend->get_telemetry(out);
}
}
}

183
legacy/stm32/src/esc_hoverboard.c
View File

@ -1,183 +0,0 @@
#include "esc_backend.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#ifdef DEBUG_MOTOR_TEST
#include <stdio.h>
#endif
/*
* Hoverboard ESC Backend Implementation
*
* Adapts the Hoverboard EFeru FOC protocol to the ESC backend vtable.
* UART2: PA2=TX, PA3=RX @ 115200 baud
*
* Packet: [0xABCD] [steer:i16] [speed:i16] [checksum:u16]
* Checksum = start ^ steer ^ speed
* Speed range: -1000 to +1000
* Must send at >=50Hz or ESC times out.
*/
#define HOVERBOARD_START_FRAME 0xABCD
#define HOVERBOARD_BAUD 115200
typedef struct __attribute__((packed)) {
uint16_t start;
int16_t steer;
int16_t speed;
uint16_t checksum;
} hoverboard_cmd_t;
#ifdef DEBUG_MOTOR_TEST
UART_HandleTypeDef huart2; /* non-static: exposed for jetson_uart.c R command */
#else
static UART_HandleTypeDef huart2;
#endif
/* Backend vtable instance */
static const esc_backend_t hoverboard_backend;
/*
* Initialize UART2 for hoverboard communication.
* Called once at startup via backend registration.
*/
static void hoverboard_backend_init(void) {
/* Enable clocks */
__HAL_RCC_UART5_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
/* PA2=TX, PA3=RX, AF7 for USART2 */
GPIO_InitTypeDef gpio = {0};
// UART5: PC12=TX, PD2=RX
__HAL_RCC_GPIOD_CLK_ENABLE();
gpio.Pin = GPIO_PIN_12;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_PULLUP;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
gpio.Alternate = GPIO_AF8_UART5;
HAL_GPIO_Init(GPIOC, &gpio);
// RX: PD2
gpio.Pin = GPIO_PIN_2;
gpio.Alternate = GPIO_AF8_UART5;
HAL_GPIO_Init(GPIOD, &gpio);
/* USART2 config */
huart2.Instance = UART5;
huart2.Init.BaudRate = HOVERBOARD_BAUD;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
HAL_UART_Init(&huart2);
#ifdef DEBUG_MOTOR_TEST
/* Diagnostic: report UART5 register state on USART6 after init */
{
extern void jetson_uart_send(const uint8_t *data, uint16_t len);
char diag[128];
uint32_t brr = UART5->BRR;
uint32_t cr1 = UART5->CR1;
uint32_t isr = UART5->ISR;
uint32_t apb1 = HAL_RCC_GetPCLK1Freq();
/* Also read GPIOC MODER to verify PC12 is in AF mode (bits 25:24 = 10) */
uint32_t moder = GPIOC->MODER;
uint32_t pc12_mode = (moder >> 24) & 0x3; /* 0=input 1=output 2=AF 3=analog */
uint32_t afr = GPIOC->AFR[1]; /* AFR high for pins 8-15 */
uint32_t pc12_af = (afr >> 16) & 0xF; /* AF for pin 12 */
int n = snprintf(diag, sizeof(diag),
"UART5: BRR=%lu CR1=0x%lX ISR=0x%lX APB1=%luHz PC12mode=%lu AF=%lu\n",
(unsigned long)brr, (unsigned long)cr1, (unsigned long)isr,
(unsigned long)apb1, (unsigned long)pc12_mode, (unsigned long)pc12_af);
/* Delay to let USART6 finish boot banner first */
HAL_Delay(100);
jetson_uart_send((uint8_t*)diag, n);
}
#endif /* DEBUG_MOTOR_TEST */
}
/*
* Send motor command via hoverboard protocol.
* Called at ~50Hz from motor_driver_update().
*/
#ifdef DEBUG_MOTOR_TEST
static volatile uint32_t hover_tx_count = 0;
#endif
static void hoverboard_backend_send(int16_t speed, int16_t steer) {
hoverboard_cmd_t cmd;
cmd.start = HOVERBOARD_START_FRAME;
cmd.steer = steer;
cmd.speed = speed;
cmd.checksum = cmd.start ^ cmd.steer ^ cmd.speed;
#ifdef DEBUG_MOTOR_TEST
HAL_StatusTypeDef rc = HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
hover_tx_count++;
/* Debug: every 50th send, report status on USART6 */
if (hover_tx_count % 50 == 1) {
extern void jetson_uart_send(const uint8_t *data, uint16_t len);
char dbg[64];
int n = snprintf(dbg, sizeof(dbg), "ESC tx=%lu rc=%d spd=%d str=%d\n",
(unsigned long)hover_tx_count, (int)rc, speed, steer);
jetson_uart_send((uint8_t*)dbg, n);
}
#else
HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
#endif
}
/*
* Emergency stop: send zero and disable motors.
* Hoverboard will disable outputs on repeated zero packets.
*/
static void hoverboard_backend_estop(void) {
hoverboard_backend_send(0, 0);
}
/*
* Resume after estop (optional, hoverboard auto-resumes on non-zero command).
*/
static void hoverboard_backend_resume(void) {
/* No action needed — next non-zero send will resume */
}
/*
* Query telemetry from hoverboard.
* Hoverboard protocol is command-only (no RX in this implementation).
* Return zero telemetry stub; future: add RX feedback.
*/
static void hoverboard_backend_get_telemetry(esc_telemetry_t *out) {
if (out) {
out->speed = 0;
out->steer = 0;
out->voltage_mv = 0;
out->current_ma = 0;
out->temperature_c = 0;
out->fault = 0;
}
}
/* Hoverboard backend vtable */
static const esc_backend_t hoverboard_backend = {
.init = hoverboard_backend_init,
.send = hoverboard_backend_send,
.estop = hoverboard_backend_estop,
.resume = hoverboard_backend_resume,
.get_telemetry = hoverboard_backend_get_telemetry,
};
/*
* Public functions for backward compatibility.
* These remain for existing code that calls hoverboard_init/hoverboard_send directly.
*/
void hoverboard_init(void) {
esc_backend_register(&hoverboard_backend);
}
void hoverboard_send(int16_t speed, int16_t steer) {
esc_send(speed, steer);
}

70
legacy/stm32/src/esc_vesc.c
View File

@ -1,70 +0,0 @@
#include "esc_backend.h"
/*
* VESC ESC Backend Stub
*
* Placeholder implementation for FSESC 4.20 Plus (VESC-based dual ESC).
* UART: ttyTHS1 (Jetson Orin) → FC USART6 @ 921600 baud
* Protocol: pyvesc with CRC16-XModem checksum
*
* Issue #383: VESC integration (fills in this stub)
* Issue #388: ESC abstraction layer (provides this interface)
*
* TODO (Issue #383):
* - Implement vesc_init() with UART/GPIO config
* - Implement vesc_send() with pyvesc packet encoding (duty/RPM control)
* - Implement vesc_estop() to disable motor controller
* - Implement vesc_get_telemetry() to parse VESC state messages
* - Add balance mode configuration if using VESC balance app
*/
static const esc_backend_t vesc_backend;
/* Stub implementations — no-op until #383 fills them in */
static void vesc_backend_init(void) {
/* TODO (Issue #383): Initialize UART6, configure VESC balance mode */
}
static void vesc_backend_send(int16_t speed, int16_t steer) {
/* TODO (Issue #383): Encode speed/steer to pyvesc packet and send via UART6 */
(void)speed;
(void)steer;
}
static void vesc_backend_estop(void) {
/* TODO (Issue #383): Send VESC shutdown command to disable motors */
}
static void vesc_backend_resume(void) {
/* TODO (Issue #383): Resume from estop if needed */
}
static void vesc_backend_get_telemetry(esc_telemetry_t *out) {
/* TODO (Issue #383): Poll/parse VESC telemetry (voltage, current, RPM, temp, fault) */
if (out) {
out->speed = 0;
out->steer = 0;
out->voltage_mv = 0;
out->current_ma = 0;
out->temperature_c = 0;
out->fault = 0;
}
}
/* VESC backend vtable */
static const esc_backend_t vesc_backend = {
.init = vesc_backend_init,
.send = vesc_backend_send,
.estop = vesc_backend_estop,
.resume = vesc_backend_resume,
.get_telemetry = vesc_backend_get_telemetry,
};
/*
* Public function to register VESC backend.
* Called from main.c when configured with ESC_BACKEND=VESC.
*/
void vesc_backend_register_impl(void) {
esc_backend_register(&vesc_backend);
}

307
legacy/stm32/src/face_animation.c
View File

@ -1,307 +0,0 @@
/*
* face_animation.c — Face Emotion Renderer for LCD Display
*
* Implements expressive face animations with smooth transitions between emotions.
* Supports idle blinking and parameterized eye/mouth shapes for each emotion.
*/
#include "face_animation.h"
#include "face_lcd.h"
#include <math.h>
#include <string.h>
/* === Configuration === */
#define TRANSITION_FRAMES 15 /* ~0.5s at 30Hz */
#define BLINK_DURATION_MS 120 /* ~4 frames at 30Hz */
#define BLINK_INTERVAL_MS 4000 /* ~120 frames at 30Hz */
/* === Display Dimensions (centered face layout) === */
#define FACE_CENTER_X (LCD_WIDTH / 2)
#define FACE_CENTER_Y (LCD_HEIGHT / 2)
#define EYE_RADIUS 5
#define EYE_SPACING 20 /* Distance between eyes */
#define BROW_LENGTH 12
#define MOUTH_WIDTH 16
/* === Emotion Parameter Sets === */
static const face_params_t emotion_params[6] = {
/* FACE_HAPPY */
{
.eye_x = -EYE_SPACING/2, .eye_y = -10,
.eye_open_y = 5, .eye_close_y = 0,
.brow_angle = 15, .brow_y_offset = -6,
.mouth_x = 0, .mouth_y = 10,
.mouth_width = MOUTH_WIDTH, .mouth_curve = 4, /* Upturned smile */
.blink_interval_ms = 120,
},
/* FACE_SAD */
{
.eye_x = -EYE_SPACING/2, .eye_y = -8,
.eye_open_y = 5, .eye_close_y = 0,
.brow_angle = -15, .brow_y_offset = -8,
.mouth_x = 0, .mouth_y = 12,
.mouth_width = MOUTH_WIDTH, .mouth_curve = -3, /* Downturned frown */
.blink_interval_ms = 180, /* Slower blink when sad */
},
/* FACE_CURIOUS */
{
.eye_x = -EYE_SPACING/2, .eye_y = -12,
.eye_open_y = 7, .eye_close_y = 0, /* Wide eyes */
.brow_angle = 20, .brow_y_offset = -10, /* Raised brows */
.mouth_x = 2, .mouth_y = 10,
.mouth_width = 12, .mouth_curve = 1, /* Slight smile */
.blink_interval_ms = 150,
},
/* FACE_ANGRY */
{
.eye_x = -EYE_SPACING/2, .eye_y = -6,
.eye_open_y = 3, .eye_close_y = 0, /* Narrowed eyes */
.brow_angle = -20, .brow_y_offset = -5,
.mouth_x = 0, .mouth_y = 11,
.mouth_width = 14, .mouth_curve = -5, /* Strong frown */
.blink_interval_ms = 90, /* Angry blinks faster */
},
/* FACE_SLEEPING */
{
.eye_x = -EYE_SPACING/2, .eye_y = -8,
.eye_open_y = 0, .eye_close_y = -2, /* Closed/squinted */
.brow_angle = 5, .brow_y_offset = -4,
.mouth_x = 0, .mouth_y = 10,
.mouth_width = 10, .mouth_curve = 2, /* Peaceful smile */
.blink_interval_ms = 60, /* Not used when sleeping */
},
/* FACE_NEUTRAL */
{
.eye_x = -EYE_SPACING/2, .eye_y = -8,
.eye_open_y = 5, .eye_close_y = 0,
.brow_angle = 0, .brow_y_offset = -6,
.mouth_x = 0, .mouth_y = 10,
.mouth_width = 12, .mouth_curve = 0, /* Straight line */
.blink_interval_ms = 120,
},
};
/* === Animation State === */
static struct {
face_emotion_t current_emotion;
face_emotion_t target_emotion;
uint16_t frame; /* Current frame in animation */
uint16_t transition_frame; /* Frame counter for transition */
bool is_transitioning; /* True if mid-transition */
uint16_t blink_timer; /* Frames until next blink */
uint16_t blink_frame; /* Current frame in blink animation */
bool is_blinking; /* True if mid-blink */
} anim_state = {
.current_emotion = FACE_NEUTRAL,
.target_emotion = FACE_NEUTRAL,
.frame = 0,
.transition_frame = 0,
.is_transitioning = false,
.blink_timer = BLINK_INTERVAL_MS / 33, /* ~120 frames */
.blink_frame = 0,
.is_blinking = false,
};
/* === Easing Functions === */
/**
* Ease-in-out cubic interpolation [0, 1].
* Smooth acceleration/deceleration for transitions.
*/
static float ease_in_out_cubic(float t) {
if (t < 0.5f)
return 4.0f * t * t * t;
else {
float f = 2.0f * t - 2.0f;
return 0.5f * f * f * f + 1.0f;
}
}
/**
* Interpolate two emotion parameters by factor [0, 1].
*/
static face_params_t interpolate_params(const face_params_t *a,
const face_params_t *b,
float t) {
face_params_t result;
result.eye_x = (int16_t)(a->eye_x + (b->eye_x - a->eye_x) * t);
result.eye_y = (int16_t)(a->eye_y + (b->eye_y - a->eye_y) * t);
result.eye_open_y = (int16_t)(a->eye_open_y + (b->eye_open_y - a->eye_open_y) * t);
result.eye_close_y = (int16_t)(a->eye_close_y + (b->eye_close_y - a->eye_close_y) * t);
result.brow_angle = (int16_t)(a->brow_angle + (b->brow_angle - a->brow_angle) * t);
result.brow_y_offset = (int16_t)(a->brow_y_offset + (b->brow_y_offset - a->brow_y_offset) * t);
result.mouth_x = (int16_t)(a->mouth_x + (b->mouth_x - a->mouth_x) * t);
result.mouth_y = (int16_t)(a->mouth_y + (b->mouth_y - a->mouth_y) * t);
result.mouth_width = (int16_t)(a->mouth_width + (b->mouth_width - a->mouth_width) * t);
result.mouth_curve = (int16_t)(a->mouth_curve + (b->mouth_curve - a->mouth_curve) * t);
return result;
}
/* === Drawing Functions === */
/**
* Draw an eye (circle) with optional closure (eyelid).
*/
static void draw_eye(int16_t x, int16_t y, int16_t open_y, int16_t close_y,
bool is_blinking) {
lcd_color_t color = LCD_WHITE;
/* Eye position accounts for blink closure */
int16_t eye_h = is_blinking ? close_y : open_y;
if (eye_h <= 0) {
/* Closed: draw horizontal line instead */
face_lcd_line(x - EYE_RADIUS, y, x + EYE_RADIUS, y, color);
} else {
/* Open: draw circle (simplified ellipse) */
face_lcd_circle(x, y, EYE_RADIUS, color);
/* Fill iris pupil */
face_lcd_fill_rect(x - 2, y - 1, 4, 2, color);
}
}
/**
* Draw an eyebrow with angle and offset.
*/
static void draw_brow(int16_t x, int16_t y, int16_t angle, int16_t y_offset) {
/* Approximate angled line by adjusting endpoints */
int16_t brow_y = y + y_offset;
int16_t angle_offset = (angle * BROW_LENGTH) / 45; /* ~1 pixel per 45 degrees */
face_lcd_line(x - BROW_LENGTH/2 - angle_offset, brow_y,
x + BROW_LENGTH/2 + angle_offset, brow_y,
LCD_WHITE);
}
/**
* Draw mouth (curved line or bezier approximation).
*/
static void draw_mouth(int16_t x, int16_t y, int16_t width, int16_t curve) {
/* Simplified mouth: two diagonal lines forming a V or inverted V */
int16_t mouth_left = x - width / 2;
int16_t mouth_right = x + width / 2;
int16_t mouth_bottom = y + (curve > 0 ? 3 : 0);
if (curve > 0) {
/* Smile: V shape upturned */
face_lcd_line(mouth_left, y + 2, x, mouth_bottom, LCD_WHITE);
face_lcd_line(x, mouth_bottom, mouth_right, y + 2, LCD_WHITE);
} else if (curve < 0) {
/* Frown: ^ shape downturned */
face_lcd_line(mouth_left, y - 2, x, y + 2, LCD_WHITE);
face_lcd_line(x, y + 2, mouth_right, y - 2, LCD_WHITE);
} else {
/* Neutral: straight line */
face_lcd_line(mouth_left, y, mouth_right, y, LCD_WHITE);
}
}
/* === Public API Implementation === */
void face_animation_init(void) {
anim_state.current_emotion = FACE_NEUTRAL;
anim_state.target_emotion = FACE_NEUTRAL;
anim_state.frame = 0;
anim_state.transition_frame = 0;
anim_state.is_transitioning = false;
anim_state.blink_timer = BLINK_INTERVAL_MS / 33;
anim_state.blink_frame = 0;
anim_state.is_blinking = false;
}
void face_animation_set_emotion(face_emotion_t emotion) {
if (emotion < 6) {
anim_state.target_emotion = emotion;
anim_state.is_transitioning = true;
anim_state.transition_frame = 0;
}
}
void face_animation_tick(void) {
anim_state.frame++;
/* Handle transition */
if (anim_state.is_transitioning) {
anim_state.transition_frame++;
if (anim_state.transition_frame >= TRANSITION_FRAMES) {
/* Transition complete */
anim_state.current_emotion = anim_state.target_emotion;
anim_state.is_transitioning = false;
}
}
/* Handle idle blink */
if (!anim_state.is_blinking) {
anim_state.blink_timer--;
if (anim_state.blink_timer == 0) {
anim_state.is_blinking = true;
anim_state.blink_frame = 0;
/* Reset timer for next blink */
anim_state.blink_timer = BLINK_INTERVAL_MS / 33;
}
} else {
/* In blink */
anim_state.blink_frame++;
if (anim_state.blink_frame >= BLINK_DURATION_MS / 33) {
/* Blink complete */
anim_state.is_blinking = false;
anim_state.blink_frame = 0;
}
}
}
void face_animation_render(void) {
/* Clear display */
face_lcd_clear();
/* Get current emotion parameters (interpolated if transitioning) */
face_params_t params;
if (anim_state.is_transitioning) {
float t = ease_in_out_cubic((float)anim_state.transition_frame /
TRANSITION_FRAMES);
params = interpolate_params(
&emotion_params[anim_state.current_emotion],
&emotion_params[anim_state.target_emotion],
t);
} else {
params = emotion_params[anim_state.current_emotion];
}
/* Draw left eye */
draw_eye(FACE_CENTER_X + params.eye_x, FACE_CENTER_Y + params.eye_y,
params.eye_open_y, params.eye_close_y, anim_state.is_blinking);
/* Draw right eye */
draw_eye(FACE_CENTER_X - params.eye_x, FACE_CENTER_Y + params.eye_y,
params.eye_open_y, params.eye_close_y, anim_state.is_blinking);
/* Draw left brow */
draw_brow(FACE_CENTER_X + params.eye_x, FACE_CENTER_Y + params.brow_y_offset,
params.brow_angle, 0);
/* Draw right brow (mirrored) */
draw_brow(FACE_CENTER_X - params.eye_x, FACE_CENTER_Y + params.brow_y_offset,
-params.brow_angle, 0);
/* Draw mouth */
draw_mouth(FACE_CENTER_X + params.mouth_x, FACE_CENTER_Y + params.mouth_y,
params.mouth_width, params.mouth_curve);
/* Push framebuffer to display */
face_lcd_flush();
}
face_emotion_t face_animation_get_emotion(void) {
return anim_state.is_transitioning ? anim_state.target_emotion
: anim_state.current_emotion;
}
void face_animation_blink_now(void) {
anim_state.is_blinking = true;
anim_state.blink_frame = 0;
anim_state.blink_timer = BLINK_INTERVAL_MS / 33;
}
bool face_animation_is_idle(void) {
return !anim_state.is_transitioning && !anim_state.is_blinking;
}

191
legacy/stm32/src/face_lcd.c
View File

@ -1,191 +0,0 @@
/*
* face_lcd.c — STM32 LCD Display Driver for Face Animations
*
* Implements low-level LCD framebuffer management and display control.
* Supports 1-bit monochrome displays (SSD1306, etc.) via SPI.
*
* HARDWARE:
* - SPI2 (PB13=SCK, PB14=MISO, PB15=MOSI) — already configured for OSD
* - CS (GPIO) for LCD chip select
* - DC (GPIO) for data/command mode select
* - RES (GPIO) optional reset
*
* NOTE: SPI2 is currently used by OSD (MAX7456). For face LCD, we would
* typically use a separate SPI or I2C. This implementation assumes
* a dedicated I2C or separate SPI interface. Configure LCD_INTERFACE
* in face_lcd.h to match your hardware.
*/
#include "face_lcd.h"
#include <string.h>
/* === State Variables === */
static uint8_t lcd_framebuffer[LCD_FBSIZE];
static volatile uint32_t frame_counter = 0;
static volatile bool flush_requested = false;
static volatile bool transfer_busy = false;
/* === Private Functions === */
/**
* Initialize hardware (SPI/I2C) and LCD controller.
* Sends initialization sequence to put display in active mode.
*/
static void lcd_hardware_init(void) {
/* TODO: Implement hardware-specific initialization
* - Configure SPI/I2C pins and clock
* - Send controller init sequence (power on, set contrast, etc.)
* - Clear display
*
* For SSD1306 (common monochrome):
* - Send 0xAE (display off)
* - Set contrast 0x81, 0x7F
* - Set clock div ratio, precharge, comdesat
* - Set address mode, column/page range
* - Send 0xAF (display on)
*/
}
/**
* Push framebuffer to display via SPI/I2C DMA transfer.
* Handles paging for monochrome displays (8 pixels per byte, horizontal pages).
*/
static void lcd_transfer_fb(void) {
transfer_busy = true;
/* TODO: Implement DMA/blocking transfer
* For SSD1306 (8-pixel pages):
* For each page (8 rows):
* - Send page address command
* - Send column address command
* - DMA transfer 128 bytes (1 row of page data)
*
* Can use SPI DMA for async transfer or blocking transfer.
* Set transfer_busy=false when complete (in ISR or blocking).
*/
transfer_busy = false;
}
/* === Public API Implementation === */
void face_lcd_init(void) {
memset(lcd_framebuffer, 0, LCD_FBSIZE);
frame_counter = 0;
flush_requested = false;
transfer_busy = false;
lcd_hardware_init();
}
void face_lcd_clear(void) {
memset(lcd_framebuffer, 0, LCD_FBSIZE);
}
void face_lcd_pixel(uint16_t x, uint16_t y, lcd_color_t color) {
/* Bounds check */
if (x >= LCD_WIDTH || y >= LCD_HEIGHT)
return;
#if LCD_BPP == 1
/* Monochrome: pack 8 pixels per byte, LSB = leftmost pixel */
uint16_t byte_idx = (y / 8) * LCD_WIDTH + x;
uint8_t bit_pos = y % 8;
if (color)
lcd_framebuffer[byte_idx] |= (1 << bit_pos);
else
lcd_framebuffer[byte_idx] &= ~(1 << bit_pos);
#else
/* RGB565: 2 bytes per pixel */
uint16_t pixel_idx = y * LCD_WIDTH + x;
((uint16_t *)lcd_framebuffer)[pixel_idx] = color;
#endif
}
void face_lcd_line(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1,
lcd_color_t color) {
/* Bresenham line algorithm */
int16_t dx = (x1 > x0) ? (x1 - x0) : (x0 - x1);
int16_t dy = (y1 > y0) ? (y1 - y0) : (y0 - y1);
int16_t sx = (x0 < x1) ? 1 : -1;
int16_t sy = (y0 < y1) ? 1 : -1;
int16_t err = (dx > dy) ? (dx / 2) : -(dy / 2);
int16_t x = x0, y = y0;
while (1) {
face_lcd_pixel(x, y, color);
if (x == x1 && y == y1)
break;
int16_t e2 = err;
if (e2 > -dx) {
err -= dy;
x += sx;
}
if (e2 < dy) {
err += dx;
y += sy;
}
}
}
void face_lcd_circle(uint16_t cx, uint16_t cy, uint16_t r, lcd_color_t color) {
/* Midpoint circle algorithm */
int16_t x = r, y = 0;
int16_t err = 0;
while (x >= y) {
face_lcd_pixel(cx + x, cy + y, color);
face_lcd_pixel(cx + y, cy + x, color);
face_lcd_pixel(cx - y, cy + x, color);
face_lcd_pixel(cx - x, cy + y, color);
face_lcd_pixel(cx - x, cy - y, color);
face_lcd_pixel(cx - y, cy - x, color);
face_lcd_pixel(cx + y, cy - x, color);
face_lcd_pixel(cx + x, cy - y, color);
if (err <= 0) {
y++;
err += 2 * y + 1;
} else {
x--;
err -= 2 * x + 1;
}
}
}
void face_lcd_fill_rect(uint16_t x, uint16_t y, uint16_t w, uint16_t h,
lcd_color_t color) {
for (uint16_t row = y; row < y + h && row < LCD_HEIGHT; row++) {
for (uint16_t col = x; col < x + w && col < LCD_WIDTH; col++) {
face_lcd_pixel(col, row, color);
}
}
}
void face_lcd_flush(void) {
flush_requested = true;
}
bool face_lcd_is_busy(void) {
return transfer_busy;
}
void face_lcd_tick(void) {
frame_counter++;
/* Request flush every N frames to achieve LCD_REFRESH_HZ */
uint32_t frames_per_flush = (1000 / LCD_REFRESH_HZ) / 33; /* ~30ms per frame */
if (frame_counter % frames_per_flush == 0) {
flush_requested = true;
}
/* Perform transfer if requested and not busy */
if (flush_requested && !transfer_busy) {
flush_requested = false;
lcd_transfer_fb();
}
}
uint8_t *face_lcd_get_fb(void) {
return lcd_framebuffer;
}

175
legacy/stm32/src/face_uart.c
View File

@ -1,175 +0,0 @@
/*
* face_uart.c — UART Command Interface for Face Animations
*
* Receives emotion commands from Jetson Orin and triggers face animations.
* Text-based protocol over USART3 @ 115200 baud.
*/
#include "face_uart.h"
#include "face_animation.h"
#include <string.h>
#include <ctype.h>
#include <stdio.h>
/* === Ring Buffer State === */
static struct {
uint8_t buf[FACE_UART_RX_BUF_SZ];
uint16_t head; /* Write index (ISR) */
uint16_t tail; /* Read index (process) */
uint16_t count; /* Bytes in buffer */
} rx_buf = {0};
/* === Forward Declarations === */
static void uart_send_response(const char *cmd, const char *status);
/* === Private Functions === */
/**
* Extract a line from RX buffer (newline-terminated).
* Returns length if found, 0 otherwise.
*/
static uint16_t extract_line(char *line, uint16_t max_len) {
uint16_t len = 0;
uint16_t idx = rx_buf.tail;
/* Scan for newline */
while (idx != rx_buf.head && len < max_len - 1) {
uint8_t byte = rx_buf.buf[idx];
if (byte == '\n') {
/* Found end of line */
for (uint16_t i = 0; i < len; i++) {
line[i] = rx_buf.buf[(rx_buf.tail + i) % FACE_UART_RX_BUF_SZ];
}
line[len] = '\0';
/* Trim trailing whitespace */
while (len > 0 && (line[len - 1] == '\r' || isspace(line[len - 1]))) {
line[--len] = '\0';
}
/* Update tail and count */
rx_buf.tail = (idx + 1) % FACE_UART_RX_BUF_SZ;
rx_buf.count -= (len + 1 + 1); /* +1 for newline, +1 for any preceding data */
return len;
}
len++;
idx = (idx + 1) % FACE_UART_RX_BUF_SZ;
}
return 0; /* No complete line */
}
/**
* Convert string to uppercase for case-insensitive command matching.
*/
static void str_toupper(char *str) {
for (int i = 0; str[i]; i++)
str[i] = toupper((unsigned char)str[i]);
}
/**
* Parse and execute a command.
*/
static void parse_command(const char *cmd) {
if (!cmd || !cmd[0])
return;
char cmd_upper[32];
strncpy(cmd_upper, cmd, sizeof(cmd_upper) - 1);
cmd_upper[sizeof(cmd_upper) - 1] = '\0';
str_toupper(cmd_upper);
/* Command dispatch */
if (strcmp(cmd_upper, "HAPPY") == 0) {
face_animation_set_emotion(FACE_HAPPY);
uart_send_response(cmd_upper, "OK");
} else if (strcmp(cmd_upper, "SAD") == 0) {
face_animation_set_emotion(FACE_SAD);
uart_send_response(cmd_upper, "OK");
} else if (strcmp(cmd_upper, "CURIOUS") == 0) {
face_animation_set_emotion(FACE_CURIOUS);
uart_send_response(cmd_upper, "OK");
} else if (strcmp(cmd_upper, "ANGRY") == 0) {
face_animation_set_emotion(FACE_ANGRY);
uart_send_response(cmd_upper, "OK");
} else if (strcmp(cmd_upper, "SLEEP") == 0 ||
strcmp(cmd_upper, "SLEEPING") == 0) {
face_animation_set_emotion(FACE_SLEEPING);
uart_send_response(cmd_upper, "OK");
} else if (strcmp(cmd_upper, "NEUTRAL") == 0) {
face_animation_set_emotion(FACE_NEUTRAL);
uart_send_response(cmd_upper, "OK");
} else if (strcmp(cmd_upper, "BLINK") == 0) {
face_animation_blink_now();
uart_send_response(cmd_upper, "OK");
} else if (strcmp(cmd_upper, "STATUS") == 0) {
const char *emotion_names[] = {"HAPPY", "SAD", "CURIOUS", "ANGRY",
"SLEEPING", "NEUTRAL"};
face_emotion_t current = face_animation_get_emotion();
char status[64];
snprintf(status, sizeof(status), "EMOTION=%s, IDLE=%s",
emotion_names[current],
face_animation_is_idle() ? "true" : "false");
uart_send_response(cmd_upper, status);
} else {
uart_send_response(cmd_upper, "ERR: unknown command");
}
}
/**
* Send a response string to UART TX.
* Format: "CMD: status\n"
*/
static void uart_send_response(const char *cmd, const char *status) {
/* TODO: Implement UART TX
* Use HAL_UART_Transmit_IT or similar to send:
* "CMD: status\n"
*/
(void)cmd; /* Suppress unused warnings */
(void)status;
}
/* === Public API Implementation === */
void face_uart_init(void) {
/* TODO: Configure USART3 @ 115200 baud
* - Enable USART3 clock (RCC_APB1ENR)
* - Configure pins (PB10=TX, PB11=RX)
* - Set baud rate to 115200
* - Enable RX interrupt (NVIC + USART3_IRQn)
* - Enable USART
*/
rx_buf.head = 0;
rx_buf.tail = 0;
rx_buf.count = 0;
}
void face_uart_process(void) {
char line[128];
uint16_t len;
/* Extract and process complete commands */
while ((len = extract_line(line, sizeof(line))) > 0) {
parse_command(line);
}
}
void face_uart_rx_isr(uint8_t byte) {
/* Push byte into ring buffer */
if (rx_buf.count < FACE_UART_RX_BUF_SZ) {
rx_buf.buf[rx_buf.head] = byte;
rx_buf.head = (rx_buf.head + 1) % FACE_UART_RX_BUF_SZ;
rx_buf.count++;
}
/* Buffer overflow: silently discard oldest byte */
}
void face_uart_send(const char *str) {
/* TODO: Implement non-blocking UART TX
* Use HAL_UART_Transmit_IT() or DMA-based TX queue.
*/
(void)str; /* Suppress unused warnings */
}

277
legacy/stm32/src/fan.c
View File

@ -1,277 +0,0 @@
#include "fan.h"
#include "stm32f7xx_hal.h"
#include "config.h"
#include <string.h>
/* ================================================================
* Fan Hardware Configuration
* ================================================================ */
#define FAN_PIN GPIO_PIN_9
#define FAN_PORT GPIOA
#define FAN_TIM TIM1
#define FAN_TIM_CHANNEL TIM_CHANNEL_2
#define FAN_PWM_FREQ_HZ 25000 /* 25 kHz for brushless fan */
/* ================================================================
* Temperature Curve Parameters
* ================================================================ */
#define TEMP_OFF 40 /* Fan off below this (°C) */
#define TEMP_LOW 50 /* Low speed threshold (°C) */
#define TEMP_HIGH 70 /* High speed threshold (°C) */
#define SPEED_OFF 0 /* Speed at TEMP_OFF (%) */
#define SPEED_LOW 30 /* Speed at TEMP_LOW (%) */
#define SPEED_HIGH 100 /* Speed at TEMP_HIGH (%) */
/* ================================================================
* Internal State
* ================================================================ */
typedef struct {
uint8_t current_speed; /* Current speed 0-100% */
uint8_t target_speed; /* Target speed 0-100% */
int16_t last_temperature; /* Last temperature reading (°C) */
float ramp_rate_per_ms; /* Speed change rate (%/ms) */
uint32_t last_ramp_time_ms; /* When last ramp update occurred */
bool is_ramping; /* Speed is transitioning */
} FanState_t;
static FanState_t s_fan = {
.current_speed = 0,
.target_speed = 0,
.last_temperature = 0,
.ramp_rate_per_ms = 0.05f, /* 5% per 100ms default */
.last_ramp_time_ms = 0,
.is_ramping = false
};
/* ================================================================
* Hardware Initialization
* ================================================================ */
void fan_init(void)
{
/* Enable GPIO and timer clocks */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_TIM1_CLK_ENABLE();
/* Configure PA9 as TIM1_CH2 PWM output */
GPIO_InitTypeDef gpio_init = {0};
gpio_init.Pin = FAN_PIN;
gpio_init.Mode = GPIO_MODE_AF_PP;
gpio_init.Pull = GPIO_NOPULL;
gpio_init.Speed = GPIO_SPEED_HIGH;
gpio_init.Alternate = GPIO_AF1_TIM1;
HAL_GPIO_Init(FAN_PORT, &gpio_init);
/* Configure TIM1 for PWM:
* Clock: 216MHz / PSC = output frequency
* For 25kHz frequency: PSC = 346, ARR = 25
* Duty cycle = CCR / ARR (e.g., 12.5/25 = 50%)
*/
TIM_HandleTypeDef htim1 = {0};
htim1.Instance = FAN_TIM;
htim1.Init.Prescaler = 346 - 1; /* 216MHz / 346 ≈ 624kHz clock */
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = 25 - 1; /* 624kHz / 25 = 25kHz */
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
HAL_TIM_PWM_Init(&htim1);
/* Configure PWM on CH2: 0% duty initially (fan off) */
TIM_OC_InitTypeDef oc_init = {0};
oc_init.OCMode = TIM_OCMODE_PWM1;
oc_init.Pulse = 0; /* Start at 0% duty (off) */
oc_init.OCPolarity = TIM_OCPOLARITY_HIGH;
oc_init.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_PWM_ConfigChannel(&htim1, &oc_init, FAN_TIM_CHANNEL);
/* Start PWM generation */
HAL_TIM_PWM_Start(FAN_TIM, FAN_TIM_CHANNEL);
s_fan.current_speed = 0;
s_fan.target_speed = 0;
s_fan.last_ramp_time_ms = 0;
}
/* ================================================================
* Temperature Curve Calculation
* ================================================================ */
static uint8_t fan_calculate_speed_from_temp(int16_t temp_celsius)
{
if (temp_celsius < TEMP_OFF) {
return SPEED_OFF; /* Off below 40°C */
}
if (temp_celsius < TEMP_LOW) {
/* Linear ramp from 0% to 30% between 40-50°C */
int32_t temp_offset = temp_celsius - TEMP_OFF; /* 0-10 */
int32_t temp_range = TEMP_LOW - TEMP_OFF; /* 10 */
int32_t speed_range = SPEED_LOW - SPEED_OFF; /* 30 */
uint8_t speed = SPEED_OFF + (temp_offset * speed_range) / temp_range;
return (speed > 100) ? 100 : speed;
}
if (temp_celsius < TEMP_HIGH) {
/* Linear ramp from 30% to 100% between 50-70°C */
int32_t temp_offset = temp_celsius - TEMP_LOW; /* 0-20 */
int32_t temp_range = TEMP_HIGH - TEMP_LOW; /* 20 */
int32_t speed_range = SPEED_HIGH - SPEED_LOW; /* 70 */
uint8_t speed = SPEED_LOW + (temp_offset * speed_range) / temp_range;
return (speed > 100) ? 100 : speed;
}
return SPEED_HIGH; /* 100% at 70°C and above */
}
/* ================================================================
* PWM Duty Cycle Control
* ================================================================ */
static void fan_set_pwm_duty(uint8_t percentage)
{
/* Clamp to 0-100% */
if (percentage > 100) percentage = 100;
/* Convert percentage to PWM counts
* ARR = 25 (0-24 counts for 0-96%, scale up to 25 for 100%)
* Duty = (percentage * 25) / 100
*/
uint32_t duty = (percentage * 25) / 100;
if (duty > 25) duty = 25;
/* Update CCR2 for TIM1_CH2 */
TIM1->CCR2 = duty;
}
/* ================================================================
* Public API
* ================================================================ */
bool fan_set_speed(uint8_t percentage)
{
if (percentage > 100) {
return false;
}
s_fan.current_speed = percentage;
s_fan.target_speed = percentage;
s_fan.is_ramping = false;
fan_set_pwm_duty(percentage);
return true;
}
uint8_t fan_get_speed(void)
{
return s_fan.current_speed;
}
bool fan_set_target_speed(uint8_t percentage)
{
if (percentage > 100) {
return false;
}
s_fan.target_speed = percentage;
if (percentage == s_fan.current_speed) {
s_fan.is_ramping = false;
} else {
s_fan.is_ramping = true;
}
return true;
}
void fan_update_temperature(int16_t temp_celsius)
{
s_fan.last_temperature = temp_celsius;
/* Calculate target speed from temperature curve */
uint8_t new_target = fan_calculate_speed_from_temp(temp_celsius);
fan_set_target_speed(new_target);
}
int16_t fan_get_temperature(void)
{
return s_fan.last_temperature;
}
FanState fan_get_state(void)
{
if (s_fan.current_speed == 0) return FAN_OFF;
if (s_fan.current_speed <= 30) return FAN_LOW;
if (s_fan.current_speed <= 60) return FAN_MEDIUM;
if (s_fan.current_speed <= 99) return FAN_HIGH;
return FAN_FULL;
}
void fan_set_ramp_rate(float percentage_per_ms)
{
if (percentage_per_ms <= 0) {
s_fan.ramp_rate_per_ms = 0.01f; /* Minimum rate */
} else if (percentage_per_ms > 10.0f) {
s_fan.ramp_rate_per_ms = 10.0f; /* Maximum rate */
} else {
s_fan.ramp_rate_per_ms = percentage_per_ms;
}
}
bool fan_is_ramping(void)
{
return s_fan.is_ramping;
}
void fan_tick(uint32_t now_ms)
{
if (!s_fan.is_ramping) {
return;
}
/* Calculate time elapsed since last ramp */
if (s_fan.last_ramp_time_ms == 0) {
s_fan.last_ramp_time_ms = now_ms;
return;
}
uint32_t elapsed = now_ms - s_fan.last_ramp_time_ms;
if (elapsed == 0) {
return; /* No time has passed */
}
/* Calculate speed change allowed in this time interval */
float speed_change = s_fan.ramp_rate_per_ms * elapsed;
int32_t new_speed;
if (s_fan.target_speed > s_fan.current_speed) {
/* Ramp up */
new_speed = s_fan.current_speed + (int32_t)speed_change;
if (new_speed >= s_fan.target_speed) {
s_fan.current_speed = s_fan.target_speed;
s_fan.is_ramping = false;
} else {
s_fan.current_speed = (uint8_t)new_speed;
}
} else {
/* Ramp down */
new_speed = s_fan.current_speed - (int32_t)speed_change;
if (new_speed <= s_fan.target_speed) {
s_fan.current_speed = s_fan.target_speed;
s_fan.is_ramping = false;
} else {
s_fan.current_speed = (uint8_t)new_speed;
}
}
/* Update PWM duty cycle */
fan_set_pwm_duty(s_fan.current_speed);
s_fan.last_ramp_time_ms = now_ms;
}
void fan_disable(void)
{
fan_set_speed(0);
}

457
legacy/stm32/src/fault_handler.c
View File

@ -1,457 +0,0 @@
#include "fault_handler.h"
#include "config.h"
#include "pid_flash.h"
#include "stm32f7xx_hal.h"
#include <string.h>
#include <stdio.h>
/*
* fault_handler.c — STM32F7 fault detection and recovery (Issue #565)
*
* Recovery flow:
* Fault ISR (naked) → _capture_and_reset() captures registers into .noinit
* SRAM → sets FAULT_SRAM_MAGIC → NVIC_SystemReset().
* On next boot: fault_handler_init() sees FAULT_SRAM_MAGIC → persists to
* flash log → prints CDC dump → starts LED blink code.
*
* No flash writes occur inside fault ISRs. All flash operations happen safely
* in the normal boot context, well before safety_init() / IWDG start.
*/
/* ---- .noinit SRAM (preserved across NVIC_SystemReset) ---- */
/*
* GCC startup code only zeroes .bss and initialises .data. Variables in
* .noinit are left untouched. The magic word guards against cold-boot garbage.
*/
#define FAULT_SRAM_MAGIC 0xFADE5A01u
#define RESET_COUNT_MAGIC 0x1234ABCDu
static __attribute__((section(".noinit"))) volatile uint32_t s_fault_magic;
static __attribute__((section(".noinit"))) volatile fault_log_entry_t s_fault_sram;
static __attribute__((section(".noinit"))) volatile uint32_t s_reset_count_magic;
static __attribute__((section(".noinit"))) volatile uint32_t s_reset_count;
/* ---- LED blink sequencer ---- */
/*
* Each pattern is a 16-bit bitmask; bit 15 = first step.
* One step = period_ms milliseconds. LED2 (PC14) is active-low.
*/
typedef struct {
uint16_t pattern; /* bitmask: 1 = LED on */
uint8_t steps; /* number of valid bits to cycle */
uint16_t period_ms; /* ms per step */
} LedBlink;
/*
* Pattern table indexed by FaultType (0..8).
* NONE = silent
* HARDFAULT = 1010 1010 1010 1010 (3 fast blinks, 100 ms)
* WATCHDOG = 1111 0000 1111 0000 (2 slow pulses, 150 ms × 8 steps = 1.2 s)
* BROWNOUT = 1111 1111 0000 0000 (1 long pulse, 100 ms × 16 = 1.6 s)
* STACK_OVF = 1110 1110 1110 1110 (4 short bursts, 100 ms)
* BUS_FAULT = 1010 1111 1100 0000 (3+1 pattern)
* USAGE_FAULT = 1010 0000 0000 0000 (2 fast blinks)
* MEM_FAULT = 1010 1010 1000 0000 (3 blinks, slower tail)
* ASSERT = 1101 1011 0000 0000 (SOS-like)
*/
static const LedBlink s_blink_table[] = {
/* FAULT_NONE */ { 0x0000u, 16, 100 },
/* FAULT_HARDFAULT */ { 0xAAAAu, 16, 100 },
/* FAULT_WATCHDOG */ { 0xF0F0u, 16, 150 },
/* FAULT_BROWNOUT */ { 0xFF00u, 16, 100 },
/* FAULT_STACK_OVF */ { 0xEEEEu, 16, 100 },
/* FAULT_BUS_FAULT */ { 0xAFC0u, 16, 100 },
/* FAULT_USAGE_FAULT */ { 0xA000u, 16, 100 },
/* FAULT_MEM_FAULT */ { 0xAA80u, 16, 100 },
/* FAULT_ASSERT */ { 0xDB00u, 16, 100 },
};
#define BLINK_TABLE_SIZE (sizeof(s_blink_table) / sizeof(s_blink_table[0]))
static FaultType s_led_fault = FAULT_NONE;
static uint32_t s_led_start = 0;
static uint32_t s_led_last = 0;
static uint8_t s_led_step = 0;
/* ------------------------------------------------------------------ */
/* Flash helpers */
/* ------------------------------------------------------------------ */
static uint32_t _slot_addr(uint8_t idx)
{
return FAULT_LOG_BASE_ADDR + (uint32_t)idx * FAULT_LOG_ENTRY_SIZE;
}
static bool _slot_empty(uint8_t idx)
{
/* An erased 32-bit word reads as 0xFFFFFFFF */
const uint32_t *p = (const uint32_t *)_slot_addr(idx);
return (*p == 0xFFFFFFFFu);
}
static int _free_slot(void)
{
for (uint8_t i = 0; i < FAULT_LOG_MAX_ENTRIES; i++) {
if (_slot_empty(i)) return (int)i;
}
return -1;
}
static bool _erase_sector7(void)
{
FLASH_EraseInitTypeDef er = {0};
er.TypeErase = FLASH_TYPEERASE_SECTORS;
er.Sector = FLASH_SECTOR_7;
er.NbSectors = 1;
er.VoltageRange = FLASH_VOLTAGE_RANGE_3;
uint32_t err = 0;
return HAL_FLASHEx_Erase(&er, &err) == HAL_OK;
}
/*
* Write fault entry to the next free flash slot.
* When all 8 slots are occupied: erase sector 7, restore PID if valid,
* then write entry at slot 0. Sector 7 erase stalls CPU ~1 s — only
* called from fault_handler_init() before IWDG is started.
*/
static bool _fault_log_write(const fault_log_entry_t *entry)
{
int slot = _free_slot();
/* ---- Handle full log: erase sector 7 ---- */
if (slot < 0) {
float kp, ki, kd;
bool pid_ok = pid_flash_load(&kp, &ki, &kd);
HAL_FLASH_Unlock();
bool erased = _erase_sector7();
HAL_FLASH_Lock();
if (!erased) return false;
if (pid_ok) {
/* pid_flash_save() manages its own unlock/lock */
pid_flash_save(kp, ki, kd);
}
slot = 0;
}
/* ---- Write 64 bytes (16 × 32-bit words) to chosen slot ---- */
uint32_t addr = _slot_addr((uint8_t)slot);
const uint32_t *words = (const uint32_t *)entry;
HAL_FLASH_Unlock();
bool ok = true;
for (uint8_t w = 0; w < FAULT_LOG_ENTRY_SIZE / 4u; w++) {
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
addr + (uint32_t)w * 4u, words[w]) != HAL_OK) {
ok = false;
break;
}
}
HAL_FLASH_Lock();
return ok;
}
/* ------------------------------------------------------------------ */
/* LED blink */
/* ------------------------------------------------------------------ */
static void _led_start(FaultType type)
{
s_led_fault = type;
s_led_start = HAL_GetTick();
s_led_last = s_led_start;
s_led_step = 0;
}
/* ------------------------------------------------------------------ */
/* Public API */
/* ------------------------------------------------------------------ */
void fault_mpu_guard_init(void)
{
/*
* Configure MPU Region 0 as a 32-byte no-access guard page at
* __stack_end (lowest address of the main stack). The stack grows
* downward; when it overflows into this region a MemManage fault fires.
*
* MPU RASR SIZE field = log2(region_bytes) - 1 = log2(32) - 1 = 4.
* AP = 0b000 → no access in any mode.
*/
extern uint32_t __stack_end; /* defined in linker script */
HAL_MPU_Disable();
MPU_Region_InitTypeDef r = {0};
r.Enable = MPU_REGION_ENABLE;
r.Number = MPU_REGION_NUMBER0;
r.BaseAddress = (uint32_t)&__stack_end;
r.Size = MPU_REGION_SIZE_32B;
r.SubRegionDisable = 0x00u;
r.TypeExtField = MPU_TEX_LEVEL0;
r.AccessPermission = MPU_REGION_NO_ACCESS;
r.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
r.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
r.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
r.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
HAL_MPU_ConfigRegion(&r);
/* Enable MPU with default memory map for privileged access */
HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
/* Enable configurable fault handlers */
SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk
| SCB_SHCSR_BUSFAULTENA_Msk
| SCB_SHCSR_USGFAULTENA_Msk;
}
void fault_handler_init(void)
{
/* ---- Maintain lifetime reset counter ---- */
if (s_reset_count_magic != RESET_COUNT_MAGIC) {
s_reset_count_magic = RESET_COUNT_MAGIC;
s_reset_count = 0u;
}
s_reset_count++;
/* ---- Detect brownout via RCC_CSR ---- */
bool brownout = (RCC->CSR & RCC_CSR_BORRSTF) != 0u;
if (brownout) {
printf("[FAULT] Brownout reset detected (reset_count=%lu)\n",
(unsigned long)s_reset_count);
fault_log_entry_t e;
memset(&e, 0, sizeof(e));
e.magic = FAULT_LOG_MAGIC;
e.fault_type = (uint8_t)FAULT_BROWNOUT;
e.reset_count = (uint8_t)(s_reset_count & 0xFFu);
_fault_log_write(&e);
_led_start(FAULT_BROWNOUT);
}
/* ---- Clear all RCC reset source flags ---- */
RCC->CSR |= RCC_CSR_RMVF;
/* ---- Check for pending .noinit fault capture ---- */
if (s_fault_magic == FAULT_SRAM_MAGIC) {
s_fault_magic = 0u; /* consume once */
fault_log_entry_t e;
memcpy(&e, (const void *)&s_fault_sram, sizeof(e));
e.reset_count = (uint8_t)(s_reset_count & 0xFFu);
/* Print register dump over CDC/UART */
printf("[FAULT] *** FAULT RECOVERED ***\n");
printf("[FAULT] type=%u reset_count=%u ts=%lu ms\n",
e.fault_type, e.reset_count, (unsigned long)e.timestamp_ms);
printf("[FAULT] PC=0x%08lX LR=0x%08lX SP=0x%08lX\n",
(unsigned long)e.pc, (unsigned long)e.lr, (unsigned long)e.sp);
printf("[FAULT] R0=0x%08lX R1=0x%08lX R2=0x%08lX R3=0x%08lX\n",
(unsigned long)e.r0, (unsigned long)e.r1,
(unsigned long)e.r2, (unsigned long)e.r3);
printf("[FAULT] CFSR=0x%08lX HFSR=0x%08lX MMFAR=0x%08lX BFAR=0x%08lX\n",
(unsigned long)e.cfsr, (unsigned long)e.hfsr,
(unsigned long)e.mmfar, (unsigned long)e.bfar);
_fault_log_write(&e);
FaultType ft = (e.fault_type < (uint8_t)BLINK_TABLE_SIZE)
? (FaultType)e.fault_type : FAULT_HARDFAULT;
_led_start(ft);
}
/* ---- Install MPU stack guard & enable fault handlers ---- */
fault_mpu_guard_init();
}
FaultType fault_get_last_type(void)
{
for (int i = (int)FAULT_LOG_MAX_ENTRIES - 1; i >= 0; i--) {
if (_slot_empty((uint8_t)i)) continue;
const fault_log_entry_t *e =
(const fault_log_entry_t *)_slot_addr((uint8_t)i);
if (e->magic == FAULT_LOG_MAGIC)
return (FaultType)e->fault_type;
}
return FAULT_NONE;
}
bool fault_log_read(uint8_t idx, fault_log_entry_t *out)
{
if (idx >= FAULT_LOG_MAX_ENTRIES) return false;
if (_slot_empty(idx)) return false;
const fault_log_entry_t *e =
(const fault_log_entry_t *)_slot_addr(idx);
if (e->magic != FAULT_LOG_MAGIC) return false;
memcpy(out, e, sizeof(*out));
return true;
}
uint8_t fault_log_get_count(void)
{
uint8_t n = 0;
for (uint8_t i = 0; i < FAULT_LOG_MAX_ENTRIES; i++) {
if (!_slot_empty(i)) n++;
}
return n;
}
void fault_log_clear(void)
{
float kp, ki, kd;
bool pid_ok = pid_flash_load(&kp, &ki, &kd);
HAL_FLASH_Unlock();
_erase_sector7();
HAL_FLASH_Lock();
if (pid_ok) {
pid_flash_save(kp, ki, kd);
}
}
void fault_assert_impl(const char *file, int line)
{
(void)file; (void)line;
s_fault_sram.magic = FAULT_LOG_MAGIC;
s_fault_sram.fault_type = (uint8_t)FAULT_ASSERT;
s_fault_sram.timestamp_ms = HAL_GetTick();
s_fault_sram.pc = (uint32_t)__builtin_return_address(0);
s_fault_sram.lr = 0u;
s_fault_sram.r0 = (uint32_t)(uintptr_t)file;
s_fault_sram.r1 = (uint32_t)line;
s_fault_sram.cfsr = SCB->CFSR;
s_fault_sram.hfsr = 0u;
s_fault_sram.mmfar = 0u;
s_fault_sram.bfar = 0u;
s_fault_sram.sp = 0u;
s_fault_magic = FAULT_SRAM_MAGIC;
NVIC_SystemReset();
}
void fault_led_tick(uint32_t now_ms)
{
if (s_led_fault == FAULT_NONE) return;
/* Auto-disable after 10 s */
if ((now_ms - s_led_start) > 10000u) {
s_led_fault = FAULT_NONE;
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET); /* off */
return;
}
uint8_t fi = (uint8_t)s_led_fault;
if (fi >= BLINK_TABLE_SIZE) return;
const LedBlink *b = &s_blink_table[fi];
if ((now_ms - s_led_last) >= b->period_ms) {
s_led_last = now_ms;
bool on = ((b->pattern >> (15u - s_led_step)) & 1u) != 0u;
/* LED2 is active-low (GPIO_PIN_RESET = lit) */
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN,
on ? GPIO_PIN_RESET : GPIO_PIN_SET);
s_led_step = (uint8_t)((s_led_step + 1u) % b->steps);
}
}
/* ================================================================
* Fault vector hooks
* ================================================================
*
* Naked entry stubs determine whether the auto-saved stack frame is on
* MSP or PSP (bit 2 of EXC_RETURN in LR), then tail-call the C handler
* with the frame pointer in R0.
*
* Cortex-M auto-pushed stack frame layout (from [SP]):
* [0] R0 [1] R1 [2] R2 [3] R3
* [4] R12 [5] LR [6] PC [7] xPSR
*/
static void _capture_and_reset(FaultType type, uint32_t *frame)
{
s_fault_sram.magic = FAULT_LOG_MAGIC;
s_fault_sram.fault_type = (uint8_t)type;
s_fault_sram.timestamp_ms = HAL_GetTick();
s_fault_sram.r0 = frame[0];
s_fault_sram.r1 = frame[1];
s_fault_sram.r2 = frame[2];
s_fault_sram.r3 = frame[3];
/* frame[4] = R12 (unused in log), frame[5] = LR, frame[6] = PC */
s_fault_sram.lr = frame[5];
s_fault_sram.pc = frame[6];
s_fault_sram.sp = (uint32_t)(uintptr_t)(frame + 8); /* SP after push */
s_fault_sram.cfsr = SCB->CFSR;
s_fault_sram.hfsr = SCB->HFSR;
s_fault_sram.mmfar = SCB->MMFAR;
s_fault_sram.bfar = SCB->BFAR;
s_fault_magic = FAULT_SRAM_MAGIC;
/* Brief LED flash so a scope can catch it (≈50 ms at 216 MHz) */
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET); /* on */
for (volatile uint32_t i = 0u; i < 10800000u; i++) __NOP();
NVIC_SystemReset();
}
/* Determine if a MemManage is from stack overflow vs other memory fault */
static FaultType _mem_fault_type(void)
{
if ((SCB->CFSR & SCB_CFSR_MMARVALID_Msk) != 0u) {
extern uint32_t __stack_end;
uint32_t guard = (uint32_t)&__stack_end;
if (SCB->MMFAR >= guard && SCB->MMFAR < guard + 32u)
return FAULT_STACK_OVF;
}
return FAULT_MEM_FAULT;
}
/* C-level handlers — called from naked asm stubs */
void fault_hard_c(uint32_t *frame) { _capture_and_reset(FAULT_HARDFAULT, frame); }
void fault_mem_c(uint32_t *frame) { _capture_and_reset(_mem_fault_type(), frame); }
void fault_bus_c(uint32_t *frame) { _capture_and_reset(FAULT_BUS_FAULT, frame); }
void fault_usage_c(uint32_t *frame) { _capture_and_reset(FAULT_USAGE_FAULT, frame); }
/* ---- Naked asm entry stubs ---- */
__attribute__((naked)) void HardFault_Handler(void)
{
__asm volatile (
"tst lr, #4 \n" /* EXC_RETURN[2]: 0=MSP, 1=PSP */
"ite eq \n"
"mrseq r0, msp \n"
"mrsne r0, psp \n"
"b fault_hard_c \n"
);
}
__attribute__((naked)) void MemManage_Handler(void)
{
__asm volatile (
"tst lr, #4 \n"
"ite eq \n"
"mrseq r0, msp \n"
"mrsne r0, psp \n"
"b fault_mem_c \n"
);
}
__attribute__((naked)) void BusFault_Handler(void)
{
__asm volatile (
"tst lr, #4 \n"
"ite eq \n"
"mrseq r0, msp \n"
"mrsne r0, psp \n"
"b fault_bus_c \n"
);
}
__attribute__((naked)) void UsageFault_Handler(void)
{
__asm volatile (
"tst lr, #4 \n"
"ite eq \n"
"mrseq r0, msp \n"
"mrsne r0, psp \n"
"b fault_usage_c \n"
);
}

127
legacy/stm32/src/gimbal.c
View File

@ -1,127 +0,0 @@
#include "gimbal.h"
#include "servo_bus.h"
#include "config.h"
#include <stddef.h>
/*
* gimbal.c — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
*
* Tick rate: called every 1 ms from main loop; self-throttles to GIMBAL_TLM_HZ.
* Feedback polling alternates: pan on even ticks, tilt on odd ticks.
* This gives ~25 Hz per axis, keeping per-read latency under 2 ms total.
*
* Safety limits:
* Pan: -GIMBAL_PAN_LIMIT_DEG .. +GIMBAL_PAN_LIMIT_DEG (±180 deg)
* Tilt: -GIMBAL_TILT_LIMIT_DEG .. +GIMBAL_TILT_LIMIT_DEG (± 90 deg)
*/
#define TICK_PERIOD_MS (1000u / GIMBAL_TLM_HZ) /* 20 ms at 50 Hz */
/* Clamp int16 to [lo, hi] */
static int16_t _clamp16(int16_t v, int16_t lo, int16_t hi)
{
if (v < lo) return lo;
if (v > hi) return hi;
return v;
}
/* ---- gimbal_init() ---- */
void gimbal_init(gimbal_t *g)
{
g->cmd_pan_x10 = 0;
g->cmd_tilt_x10 = 0;
g->cmd_speed = 0; /* 0 = max speed */
g->fb_pan_x10 = 0;
g->fb_tilt_x10 = 0;
g->fb_pan_speed = 0;
g->fb_tilt_speed = 0;
g->rx_ok = 0;
g->rx_err = 0;
g->_last_tick_ms = 0;
g->_poll_phase = 0;
/* Enable torque and center both servos */
servo_bus_write_torque(GIMBAL_PAN_ID, true);
servo_bus_write_torque(GIMBAL_TILT_ID, true);
g->torque_enabled = true;
uint16_t center = servo_bus_deg_to_raw(0.0f);
servo_bus_write_pos(GIMBAL_PAN_ID, center, 0);
servo_bus_write_pos(GIMBAL_TILT_ID, center, 0);
}
/* ---- gimbal_set_pos() ---- */
void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
uint16_t speed)
{
/* Clamp to hardware limits */
pan_x10 = _clamp16(pan_x10,
-(int16_t)(GIMBAL_PAN_LIMIT_DEG * 10),
(int16_t)(GIMBAL_PAN_LIMIT_DEG * 10));
tilt_x10 = _clamp16(tilt_x10,
-(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10),
(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10));
if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX;
g->cmd_pan_x10 = pan_x10;
g->cmd_tilt_x10 = tilt_x10;
g->cmd_speed = speed;
float pan_deg = (float)pan_x10 / 10.0f;
float tilt_deg = (float)tilt_x10 / 10.0f;
servo_bus_write_pos(GIMBAL_PAN_ID,
servo_bus_deg_to_raw(pan_deg), speed);
servo_bus_write_pos(GIMBAL_TILT_ID,
servo_bus_deg_to_raw(tilt_deg), speed);
}
/* ---- gimbal_torque() ---- */
void gimbal_torque(gimbal_t *g, bool enable)
{
servo_bus_write_torque(GIMBAL_PAN_ID, enable);
servo_bus_write_torque(GIMBAL_TILT_ID, enable);
g->torque_enabled = enable;
}
/* ---- gimbal_tick() ---- */
void gimbal_tick(gimbal_t *g, uint32_t now_ms)
{
if ((now_ms - g->_last_tick_ms) < TICK_PERIOD_MS) return;
g->_last_tick_ms = now_ms;
uint16_t raw = 0;
if (g->_poll_phase == 0u) {
/* Poll pan position */
if (servo_bus_read_pos(GIMBAL_PAN_ID, &raw)) {
g->fb_pan_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
g->rx_ok++;
} else {
g->rx_err++;
}
/* Also refresh pan speed */
uint16_t spd = 0;
(void)servo_bus_read_speed(GIMBAL_PAN_ID, &spd);
g->fb_pan_speed = spd;
} else {
/* Poll tilt position */
if (servo_bus_read_pos(GIMBAL_TILT_ID, &raw)) {
g->fb_tilt_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
g->rx_ok++;
} else {
g->rx_err++;
}
uint16_t spd = 0;
(void)servo_bus_read_speed(GIMBAL_TILT_ID, &spd);
g->fb_tilt_speed = spd;
}
g->_poll_phase ^= 1u; /* toggle 0 / 1 */
}

179
legacy/stm32/src/hw_button.c
View File

@ -1,179 +0,0 @@
/* hw_button.c — hardware button debounce + gesture detection (Issue #682)
*
* Debounce FSM:
* IDLE → (raw press detected) → DEBOUNCING
* DEBOUNCING → (still pressed after BTN_DEBOUNCE_MS) → HELD
* HELD → (released) → classify press type, back to IDLE
*
* Press types:
* SHORT held < BTN_LONG_MIN_MS from confirmed start
* LONG held >= BTN_LONG_MIN_MS
*
* Sequence detection (operates on classified presses):
* Buffer up to 3 presses. Recognised patterns:
* [SHORT, SHORT, LONG] -> BTN_EVENT_REARM_COMBO (fires on LONG release)
* [SHORT] + BTN_COMMIT_MS timeout -> BTN_EVENT_PARK
* Sequence reset after BTN_SEQ_TIMEOUT_MS from first press.
*/
#include "hw_button.h"
#include "config.h"
#ifndef TEST_HOST
#include "stm32f7xx_hal.h"
#endif
/* ---- Timing defaults (override in config.h) ---- */
#ifndef BTN_DEBOUNCE_MS
#define BTN_DEBOUNCE_MS 20u
#endif
#ifndef BTN_LONG_MIN_MS
#define BTN_LONG_MIN_MS 1500u
#endif
#ifndef BTN_COMMIT_MS
#define BTN_COMMIT_MS 500u
#endif
#ifndef BTN_SEQ_TIMEOUT_MS
#define BTN_SEQ_TIMEOUT_MS 3000u
#endif
/* ---- Press type ---- */
typedef enum {
_PT_SHORT = 1u,
_PT_LONG = 2u,
} _press_type_t;
/* ---- Debounce state ---- */
typedef enum {
_BTN_IDLE,
_BTN_DEBOUNCING,
_BTN_HELD,
} _btn_state_t;
static _btn_state_t s_state = _BTN_IDLE;
static uint32_t s_trans_ms = 0u; /* timestamp of last FSM transition */
static bool s_pressed = false;
/* ---- Sequence buffer ---- */
#define _SEQ_MAX 3u
static _press_type_t s_seq[_SEQ_MAX];
static uint8_t s_seq_len = 0u;
static uint32_t s_seq_first_ms = 0u;
static uint32_t s_seq_last_ms = 0u;
/* ---- GPIO read ---- */
#ifdef TEST_HOST
static bool s_test_raw = false;
void hw_button_inject(bool pressed) { s_test_raw = pressed; }
static bool _read_raw(void) { return s_test_raw; }
#else
static bool _read_raw(void)
{
return HAL_GPIO_ReadPin(BTN_PORT, BTN_PIN) == GPIO_PIN_RESET; /* active-low */
}
#endif
void hw_button_init(void)
{
#ifndef TEST_HOST
__HAL_RCC_GPIOC_CLK_ENABLE(); /* BTN_PORT assumed GPIOC; adjust if needed */
GPIO_InitTypeDef g = {0};
g.Pin = BTN_PIN;
g.Mode = GPIO_MODE_INPUT;
g.Pull = GPIO_PULLUP;
HAL_GPIO_Init(BTN_PORT, &g);
#endif
s_state = _BTN_IDLE;
s_seq_len = 0u;
s_pressed = false;
}
bool hw_button_is_pressed(void)
{
return s_pressed;
}
/* Record a classified press into the sequence buffer and check for patterns. */
static hw_btn_event_t _record_press(_press_type_t pt, uint32_t now_ms)
{
if (s_seq_len == 0u) {
s_seq_first_ms = now_ms;
}
s_seq_last_ms = now_ms;
if (s_seq_len < _SEQ_MAX) {
s_seq[s_seq_len++] = pt;
}
/* Check REARM_COMBO: SHORT + SHORT + LONG */
if (s_seq_len == 3u &&
s_seq[0] == _PT_SHORT &&
s_seq[1] == _PT_SHORT &&
s_seq[2] == _PT_LONG) {
s_seq_len = 0u;
return BTN_EVENT_REARM_COMBO;
}
return BTN_EVENT_NONE;
}
hw_btn_event_t hw_button_tick(uint32_t now_ms)
{
bool raw = _read_raw();
/* ---- Debounce FSM ---- */
switch (s_state) {
case _BTN_IDLE:
if (raw) {
s_state = _BTN_DEBOUNCING;
s_trans_ms = now_ms;
}
break;
case _BTN_DEBOUNCING:
if (!raw) {
/* Released before debounce elapsed — bounce, ignore */
s_state = _BTN_IDLE;
} else if ((now_ms - s_trans_ms) >= BTN_DEBOUNCE_MS) {
s_state = _BTN_HELD;
s_trans_ms = now_ms; /* record confirmed press-start time */
s_pressed = true;
}
break;
case _BTN_HELD:
if (!raw) {
s_pressed = false;
uint32_t held_ms = now_ms - s_trans_ms;
_press_type_t pt = (held_ms >= BTN_LONG_MIN_MS) ? _PT_LONG : _PT_SHORT;
s_state = _BTN_IDLE;
hw_btn_event_t ev = _record_press(pt, now_ms);
if (ev != BTN_EVENT_NONE) {
return ev;
}
}
break;
}
/* ---- Sequence timeout / commit check (only when not currently held) ---- */
if (s_state == _BTN_IDLE && s_seq_len > 0u) {
uint32_t since_first = now_ms - s_seq_first_ms;
uint32_t since_last = now_ms - s_seq_last_ms;
/* Whole sequence window expired — abandon */
if (since_first >= BTN_SEQ_TIMEOUT_MS) {
s_seq_len = 0u;
return BTN_EVENT_NONE;
}
/* Single short press + BTN_COMMIT_MS of quiet -> PARK */
if (s_seq_len == 1u &&
s_seq[0] == _PT_SHORT &&
since_last >= BTN_COMMIT_MS) {
s_seq_len = 0u;
return BTN_EVENT_PARK;
}
}
return BTN_EVENT_NONE;
}

33
legacy/stm32/src/i2c1.c
View File

@ -1,33 +0,0 @@
/*
* i2c1.c — Shared I2C1 bus (PB8=SCL, PB9=SDA, 100 kHz)
*
* Used by barometer and magnetometer drivers.
* Call i2c1_init() once before any I2C probes.
*/
#include "i2c1.h"
#include "stm32f7xx_hal.h"
I2C_HandleTypeDef hi2c1;
int i2c1_init(void) {
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_I2C1_CLK_ENABLE();
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9; /* PB8=SCL, PB9=SDA */
gpio.Mode = GPIO_MODE_AF_OD;
gpio.Pull = GPIO_PULLUP;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
gpio.Alternate = GPIO_AF4_I2C1;
HAL_GPIO_Init(GPIOB, &gpio);
hi2c1.Instance = I2C1;
hi2c1.Init.Timing = 0x20404768; /* 100 kHz @ 54 MHz APB1 */
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
return (HAL_I2C_Init(&hi2c1) == HAL_OK) ? 0 : -1;
}

191
legacy/stm32/src/icm42688.c
View File

@ -1,191 +0,0 @@
/* MPU6000 + ICM-42688-P dual driver — auto-detects based on WHO_AM_I */
#include "stm32f7xx_hal.h"
#include "config.h"
#include "icm42688.h"
static SPI_HandleTypeDef hspi1;
static uint8_t imu_type = 0; /* 0=unknown, 1=MPU6000, 2=ICM42688 */
/* MPU6000 registers */
#define MPU_REG_SMPLRT_DIV 0x19
#define MPU_REG_CONFIG 0x1A
#define MPU_REG_GYRO_CONFIG 0x1B
#define MPU_REG_ACCEL_CONFIG 0x1C
#define MPU_REG_ACCEL_XOUT_H 0x3B
#define MPU_REG_PWR_MGMT_1 0x6B
#define MPU_REG_PWR_MGMT_2 0x6C
#define MPU_REG_WHO_AM_I 0x75
#define MPU6000_WHO 0x68
/* ICM-42688-P registers */
#define ICM_REG_DEVICE_CONFIG 0x11
#define ICM_REG_TEMP_DATA1 0x1D
#define ICM_REG_PWR_MGMT0 0x4E
#define ICM_REG_GYRO_CONFIG0 0x4F
#define ICM_REG_ACCEL_CONFIG0 0x50
#define ICM_REG_WHO_AM_I 0x75
#define ICM_REG_BANK_SEL 0x76
#define ICM42688_WHO 0x47
static void cs_low(void) { HAL_GPIO_WritePin(MPU_CS_PORT, MPU_CS_PIN, GPIO_PIN_RESET); }
static void cs_high(void) { HAL_GPIO_WritePin(MPU_CS_PORT, MPU_CS_PIN, GPIO_PIN_SET); }
static void wreg(uint8_t reg, uint8_t val) {
uint8_t tx[2] = { reg & 0x7F, val };
uint8_t rx[2];
cs_low();
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100);
cs_high();
HAL_Delay(1);
}
static uint8_t rreg(uint8_t reg) {
uint8_t tx[2] = { reg | 0x80, 0x00 };
uint8_t rx[2] = {0, 0};
cs_low();
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 100);
cs_high();
/* DCache coherency: invalidate rx so CPU reads SPI-written SRAM, not stale cache.
* No-op when DCache is disabled; required if DCache is on (e.g. SCB_EnableDCache). */
SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx));
return rx[1];
}
static uint8_t trace[16];
static int trace_idx = 0;
static void tr(uint8_t v) { if (trace_idx < 16) trace[trace_idx++] = v; }
static int init_mpu6000(void) {
/* Reset */
wreg(MPU_REG_PWR_MGMT_1, 0x80);
HAL_Delay(100);
/* Wake up, use PLL with X gyro ref */
wreg(MPU_REG_PWR_MGMT_1, 0x01);
HAL_Delay(10);
/* Sample rate = 1kHz (divider=0) */
wreg(MPU_REG_SMPLRT_DIV, 0x00);
/* DLPF = 42Hz (config=3) */
wreg(MPU_REG_CONFIG, 0x03);
/* Gyro: ±2000°/s (FS_SEL=3) */
wreg(MPU_REG_GYRO_CONFIG, 0x18);
/* Accel: ±16g (AFS_SEL=3) */
wreg(MPU_REG_ACCEL_CONFIG, 0x18);
/* Enable all axes */
wreg(MPU_REG_PWR_MGMT_2, 0x00);
HAL_Delay(50);
/* Verify */
uint8_t pwr = rreg(MPU_REG_PWR_MGMT_1);
tr(pwr); /* Should be 0x01 */
return (pwr == 0x01) ? 0 : -200 - (int)pwr;
}
int icm42688_init(void) {
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_SPI1_CLK_ENABLE();
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_NOPULL;
gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
gpio.Alternate = GPIO_AF5_SPI1;
HAL_GPIO_Init(GPIOA, &gpio);
/* CS on PA4 for MAMBA */
gpio.Pin = MPU_CS_PIN;
gpio.Mode = GPIO_MODE_OUTPUT_PP;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
gpio.Pull = GPIO_PULLUP;
HAL_GPIO_Init(MPU_CS_PORT, &gpio);
cs_high();
/* DCache: main.c does NOT call SCB_EnableDCache(), so DCache is currently OFF.
* If DCache is ever enabled, all SPI rx buffers need SCB_InvalidateDCache_by_Addr()
* (already added in rreg() and icm42688_read()) and USB buffers must remain mapped
* non-cacheable via MPU Region 0 in usbd_conf.c. */
hspi1.Instance = SPI1;
hspi1.Init.Mode = SPI_MODE_MASTER;
hspi1.Init.Direction = SPI_DIRECTION_2LINES;
hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.NSS = SPI_NSS_SOFT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
if (HAL_SPI_Init(&hspi1) != HAL_OK) return -1;
HAL_Delay(200);
/* Wake from sleep first - MPU6000 needs this before WHO_AM_I */
wreg(0x6B, 0x80); /* Reset */
HAL_Delay(100);
wreg(0x6B, 0x01); /* Wake, PLL */
HAL_Delay(50);
/* Retry WHO_AM_I up to 3 times: a single SPI glitch returning 0x00
* would otherwise abort init and prevent calibration from ever running. */
uint8_t who = 0;
for (int attempt = 0; attempt < 3 && who == 0; attempt++) {
if (attempt > 0) HAL_Delay(10);
who = rreg(MPU_REG_WHO_AM_I);
}
tr(who); /* trace[0] */
int ret;
if (who == MPU6000_WHO) {
imu_type = 1;
ret = init_mpu6000();
} else if (who == ICM42688_WHO) {
imu_type = 2;
ret = -99; /* TODO: ICM init */
} else {
/* who==0 means no SPI response — must not return 0 (false success) */
ret = (who != 0) ? -(int)who : -128;
}
/* Speed up SPI for reads */
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
HAL_SPI_Init(&hspi1);
tr((uint8_t)imu_type); /* trace[last] */
return ret;
}
void icm42688_read(icm42688_data_t *d) {
if (imu_type == 1) {
/* MPU6000: ACCEL_XOUT_H (0x3B) → 14 bytes: accel(6)+temp(2)+gyro(6) */
uint8_t tx[15] = {0};
uint8_t rx[15] = {0}; /* zero-init: failed SPI transfers return 0, not garbage */
tx[0] = MPU_REG_ACCEL_XOUT_H | 0x80;
cs_low();
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 15, 100);
cs_high();
/* DCache coherency: force CPU to read SPI-written SRAM, not a stale cache line.
* No-op when DCache is disabled; critical if SCB_EnableDCache() is called. */
SCB_InvalidateDCache_by_Addr((uint32_t *)(uintptr_t)rx, (int32_t)sizeof(rx));
d->ax = (int16_t)((rx[1] << 8) | rx[2]);
d->ay = (int16_t)((rx[3] << 8) | rx[4]);
d->az = (int16_t)((rx[5] << 8) | rx[6]);
int16_t temp_raw = (int16_t)((rx[7] << 8) | rx[8]);
d->temp_x10 = (int16_t)((temp_raw + 12421) / 34); /* MPU6000 formula */
d->gx = (int16_t)((rx[9] << 8) | rx[10]);
d->gy = (int16_t)((rx[11] << 8) | rx[12]);
d->gz = (int16_t)((rx[13] << 8) | rx[14]);
}
}
void icm42688_get_trace(uint8_t *out, int *len) {
*len = trace_idx;
for (int i = 0; i < trace_idx; i++) out[i] = trace[i];
}

100
legacy/stm32/src/imu_cal_flash.c
View File

@ -1,100 +0,0 @@
/* imu_cal_flash.c — IMU mount angle calibration flash storage (Issue #680)
*
* Stores pitch/roll mount offsets in STM32F722 flash sector 7 at 0x0807FF00.
* Preserves existing PID records (pid_sched_flash_t + pid_flash_t) across
* the mandatory sector erase by reading them into RAM before erasing.
*/
#include "imu_cal_flash.h"
#include "pid_flash.h"
#include "stm32f7xx_hal.h"
#include <string.h>
bool imu_cal_flash_load(float *pitch_offset, float *roll_offset)
{
const imu_cal_flash_t *p = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
if (p->magic != IMU_CAL_FLASH_MAGIC) return false;
/* Sanity-check: mount offsets beyond ±90° indicate a corrupt record */
if (p->pitch_offset < -90.0f || p->pitch_offset > 90.0f) return false;
if (p->roll_offset < -90.0f || p->roll_offset > 90.0f) return false;
*pitch_offset = p->pitch_offset;
*roll_offset = p->roll_offset;
return true;
}
/* Write 'len' bytes (multiple of 4) from 'src' to flash at 'addr'.
* Flash must be unlocked by caller. */
static HAL_StatusTypeDef write_words(uint32_t addr,
const void *src,
uint32_t len)
{
const uint32_t *p = (const uint32_t *)src;
for (uint32_t i = 0; i < len / 4u; i++) {
HAL_StatusTypeDef rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
addr, p[i]);
if (rc != HAL_OK) return rc;
addr += 4u;
}
return HAL_OK;
}
bool imu_cal_flash_save(float pitch_offset, float roll_offset)
{
/* Snapshot PID records BEFORE erasing so we can restore them */
pid_flash_t pid_snap;
pid_sched_flash_t sched_snap;
memcpy(&pid_snap, (const void *)PID_FLASH_STORE_ADDR, sizeof(pid_snap));
memcpy(&sched_snap, (const void *)PID_SCHED_FLASH_ADDR, sizeof(sched_snap));
HAL_StatusTypeDef rc;
rc = HAL_FLASH_Unlock();
if (rc != HAL_OK) return false;
/* Erase sector 7 (covers all three records) */
FLASH_EraseInitTypeDef erase = {
.TypeErase = FLASH_TYPEERASE_SECTORS,
.Sector = PID_FLASH_SECTOR,
.NbSectors = 1,
.VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
};
uint32_t sector_error = 0;
rc = HAL_FLASHEx_Erase(&erase, &sector_error);
if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
HAL_FLASH_Lock();
return false;
}
/* Write new IMU calibration record at 0x0807FF00 */
imu_cal_flash_t cal;
memset(&cal, 0xFF, sizeof(cal));
cal.magic = IMU_CAL_FLASH_MAGIC;
cal.pitch_offset = pitch_offset;
cal.roll_offset = roll_offset;
rc = write_words(IMU_CAL_FLASH_ADDR, &cal, sizeof(cal));
if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
/* Restore PID gain schedule if it was valid */
if (sched_snap.magic == PID_SCHED_MAGIC) {
rc = write_words(PID_SCHED_FLASH_ADDR, &sched_snap, sizeof(sched_snap));
if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
}
/* Restore single-PID record if it was valid */
if (pid_snap.magic == PID_FLASH_MAGIC) {
rc = write_words(PID_FLASH_STORE_ADDR, &pid_snap, sizeof(pid_snap));
if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
}
HAL_FLASH_Lock();
/* Verify readback */
const imu_cal_flash_t *v = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
return (v->magic == IMU_CAL_FLASH_MAGIC &&
v->pitch_offset == pitch_offset &&
v->roll_offset == roll_offset);
}

Some files were not shown because too many files have changed in this diff Show More