17 Commits

Author SHA1 Message Date
4a46fad002 feat(rc): CRSF/ELRS RC integration — telemetry uplink + channel fix (Issue #103)
## Summary
- config.h: CH1[0]=steer, CH2[1]=throttle (was CH4/CH3); CRSF_FAILSAFE_MS→500ms
- include/battery.h + src/battery.c: ADC3 Vbat reading on PC1 (11:1 divider)
  battery_read_mv(), battery_estimate_pct() for 3S/4S auto-detection
- include/crsf.h + src/crsf.c: CRSF telemetry TX uplink
  crsf_send_battery() — type 0x08, voltage/current/SoC to ELRS TX module
  crsf_send_flight_mode() — type 0x21, "ARMED\0"/"DISARM\0" for handset OSD
- src/main.c: battery_init() after crsf_init(); 1Hz telemetry tick calls
  crsf_send_battery(vbat_mv, 0, soc_pct) + crsf_send_flight_mode(armed)
- test/test_crsf_frames.py: 28 pytest tests — CRC8-DVB-S2, battery frame
  layout/encoding, flight-mode frame, battery_estimate_pct SoC math

Existing (already complete from crsf-elrs branch):
  CRSF frame decoder UART4 420000 baud DMA circular + IDLE interrupt
  Mode manager: RC↔autonomous blend, CH6 3-pos switch, 500ms smooth transition
  Failsafe in main.c: disarm if crsf_state.last_rx_ms stale > CRSF_FAILSAFE_MS
  CH5 arm switch with ARMING_HOLD_MS interlock + edge detection
  RC override: mode_manager blends steer/speed per mode (CH6)

Closes #103

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 08:35:48 -05:00
d41a9dfe10 feat(safety): remote e-stop over 4G MQTT (Issue #63)
STM32 firmware:
- safety.h/c: EstopSource enum, safety_remote_estop/clear/get/active()
  CDC 'E'=ESTOP_REMOTE, 'F'=ESTOP_CELLULAR_TIMEOUT, 'Z'=clear latch
- usbd_cdc_if: cdc_estop_request/cdc_estop_clear_request volatile flags
- status: status_update() +remote_estop param; both LEDs fast-blink 200ms
- main.c: immediate motor cutoff highest-priority; arming gated by
  !safety_remote_estop_active(); motor estop auto-clear gated; telemetry
  'es' field 0-4; status_update() updated to 5 args

Safety: IMMEDIATE motor cutoff, latched until explicit Z + DISARMED,
cannot re-arm via MQTT alone (requires RC arm hold). IWDG-safe.

Jetson bridge:
- remote_estop_node.py: paho-mqtt + pyserial, cellular watchdog 5s
- estop_params.yaml, remote_estop.launch.py
- setup.py / package.xml: register node + paho-mqtt dep
- docker-compose.yml: remote-estop service
- test_remote_estop.py: kill/clear/watchdog/latency unit tests

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-01 04:55:54 -05:00
fbfde24aba feat: CRSF/ELRS RC integration — 16ch input with failsafe (#Phase2)
Protocol choice: implemented from spec (CRSFforArduino needs Arduino
framework; Betaflight extraction has deep scheduler dependencies).
Protocol verified against Betaflight src/main/rx/crsf.c + CRSF spec.

crsf.c:
- UART4 PA0=TX/PA1=RX (GPIO_AF8_UART4), 420000 baud 8N1, oversampling×8
  APB1=54MHz → BRR=0x101 → 418604 baud (0.33% error, within spec)
- DMA1 Stream2 Channel4, circular 64-byte buffer, IDLE interrupt
  DMA half/complete callbacks drain buffer; IDLE fires at frame boundary
- CRC8 DVB-S2 (polynomial 0xD5) validated on every frame
- Parser state machine: SYNC(0xC8)→LEN→DATA with length sanity check
- 11-bit channel unpack for all 16 channels from 22-byte payload
- RC channels frame (0x16): unpacks 16ch, updates last_rx_ms + armed
- Link stats frame (0x14): captures RSSI dBm, LQ%, SNR dB

crsf.h: added rssi_dbm, link_quality, snr fields to CRSFState

config.h: CRSF_ARM_THRESHOLD=1750, CRSF_STEER_MAX=400, CRSF_FAILSAFE_MS=300

main.c:
- crsf_init() called after motor_driver_init()
- RC failsafe: disarm if (now - last_rx_ms) > CRSF_FAILSAFE_MS, but only
  after RC was first seen (last_rx_ms != 0) — USB-only mode unaffected
- RC arm: CH5 rising edge → safety_arm_start(); falling edge → disarm
  Same ARMING_HOLD_MS interlock as USB arm command
- RC steer: CH1 → crsf_to_range() → ±CRSF_STEER_MAX → motor_driver steer
- RSSI/LQ: appended to JSON when safety_rc_alive() ("rssi","lq" fields)

ui/index.html: hidden RC RSSI row revealed on first packet with rssi/lq

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 21:54:58 -05:00
seb
f867956b43 Merge pull request 'feat: Jetson command protocol — /cmd_vel to STM32 (Phase 2)' (#34) from sl-jetson/command-protocol into main 2026-02-28 21:43:03 -05:00
seb
14ac85bf57 Merge pull request 'feat: RC/Autonomous mode switch (Phase 2)' (#33) from sl-controls/mode-switch into main 2026-02-28 21:43:00 -05:00
22aaeb02cf feat: Jetson→STM32 command protocol — /cmd_vel to serial (Phase 2)
STM32 firmware (C):
- include/jetson_cmd.h: protocol constants (HB_TIMEOUT_MS=500,
  SPEED_MAX_DEG=4°), API for jetson_cmd_process/is_active/steer/sp_offset
- src/jetson_cmd.c: main-loop parser for buffered C<spd>,<str> frames;
  setpoint offset = speed/1000 * 4°; steer clamped ±1000
- lib/USB_CDC/src/usbd_cdc_if.c: add H (heartbeat) and C (drive cmd) to
  CDC_Receive ISR — follows existing pattern: H updates jetson_hb_tick in
  ISR, C copied to jetson_cmd_buf for main-loop sscanf (avoids sscanf in IRQ)
- src/main.c: integrate jetson_cmd — process buffered frame, apply setpoint
  offset around balance_update(), inject steer into motor_driver_update()
  only when heartbeat alive (fallback: steer=0, setpoint unchanged)

ROS2 (Python):
- saltybot_cmd_node.py: full bidirectional node — owns serial port, handles
  telemetry RX → topics AND /cmd_vel TX → C<spd>,<str>\n + H\n heartbeat
  200ms timer; sends C0,0\n on shutdown; speed/steer_scale configurable
- serial_bridge_node.py: add write_serial() helper for extensibility
- launch/bridge.launch.py: mode arg (bidirectional|rx_only) selects node
- config/bridge_params.yaml: heartbeat_period, speed_scale, steer_scale docs
- test/test_cmd.py: 13 tests — zero, full fwd/rev, turn clamping, combined
- setup.py: saltybot_cmd_node entry point

All 21 tests pass (8 parse + 13 cmd).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 21:07:15 -05:00
ea5e2dac72 feat: RC/autonomous mode manager with smooth handoff
Adds mode_manager.c/h: three operating modes selected by RC CH6 (3-pos
switch), smoothly interpolated over ~500ms to prevent jerky transitions.

Modes:
  RC_MANUAL   (blend=0.0) — RC CH4 steer + CH3 speed bias; balance PID active
  RC_ASSISTED (blend=0.5) — 50/50 blend of RC and Jetson autonomous commands
  AUTONOMOUS  (blend=1.0) — Jetson steer only; RC CH5 still kills motors

Key design:
- Single `blend` float (0=RC, 1=auto) drives all lerp; MANUAL→AUTO takes
  500ms, adjacent steps take ~250ms
- CH6 thresholds: <=600=MANUAL, >=1200=AUTONOMOUS, else ASSISTED
- CH4/CH3 read with ±30-count deadband around CRSF center (992)
- RC speed bias (CH3, ±300 counts) added to bal.motor_cmd AFTER PID
- RC CH5 kill: if rc_alive && !crsf_state.armed → disarm, regardless of mode
- Jetson steer fed via mode_manager_set_auto_cmd() → blended in get_steer()
- Telemetry: new "md" field (0/1/2) in USB JSON stream
- mode_manager_set_auto_cmd() API ready for Jetson serial bridge integration

config.h: CRSF channel indices, deadband, speed-bias max, blend timing.
Safe on USB-only build: CRSF stub keeps last_rx_ms=0 → rc_alive=false
→ RC inputs = 0, mode stays RC_MANUAL, CH5 kill never fires.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 21:06:26 -05:00
ca23407ceb feat: BME280 full readout — temp, humidity, pressure telemetry (#30)
- bmp280.c: detect BME280 (chip_id 0x60) vs BMP280 (0x58) at init
- bmp280.c: read humidity calibration (dig_H1–H6) from 0xA1 and 0xE1–0xE7
- bmp280.c: set ctrl_hum (0xF2, osrs_h=×16) before ctrl_meas — hardware req
- bmp280.c: add bmp280_read_humidity() — float compensation (FPv5-SP FPU),
  returns %RH × 10; -1 if chip is BMP280 or not initialised
- bmp280.h: add bmp280_read_humidity() declaration + timeout note
- main.c: baro_ok → baro_chip (stores chip_id for BME280 detection)
- main.c: telemetry adds t (°C×10), pa (hPa×10) for all barometers;
  adds h (%RH×10) for BME280 only; alt unchanged
- ui/index.html: hidden TEMP/HUMIDITY/PRESSURE rows, revealed on first
  packet containing t/h/pa fields; values shown with 1 dp

I2C hang safety: all HAL_I2C_Mem_Read/Write use 100ms timeouts, so
missing hardware (NAK) returns in <1ms, not after a hang.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 19:43:48 -05:00
seb
d37e9ab276 Merge pull request 'feat: Auto-detect magnetometer + barometer (#24)' (#27) from sl-firmware/mag-baro-detect into main 2026-02-28 18:45:25 -05:00
e21526327b feat: Auto-detect magnetometer + barometer (#24)
Shared I2C1 bus (i2c1.c/h, PB8=SCL PB9=SDA 100kHz):
- i2c1_init() called once in main() before sensor probes.
- hi2c1 exported globally; baro and mag drivers use it directly.

Barometer (bmp280.c):
- Probes I2C1 at 0x76 then 0x77 (covers both SDO options).
- bmp280_init() returns chip_id (0x58/0x60) on success, neg if absent.
- Added bmp280_pressure_to_alt_cm() — ISA barometric formula.
- Added bmp280.h (was missing).

Magnetometer (mag.c / mag.h):
- Auto-detects QMC5883L (0x0D, id=0xFF), HMC5883L (0x1E, id='H43'),
  IST8310 (0x0E, id=0x10) in that order.
- mag_read_heading() returns degrees×10 (0–3599) or -1 if not ready.
- HMC5883L: correct XZY byte order applied.
- IST8310: single-measurement trigger mode.

main.c:
- i2c1_init() + bmp280_init() + mag_init() after all other inits.
- Both skip gracefully (baro_ok=0, mag_type=MAG_NONE) if not present.
- Telemetry JSON: incremental builder appends ",\"hd\":<n>" when mag
  found and ",\"alt\":<n>" when baro found. No extra bytes when absent.

UI (index.html):
- HEADING and ALT rows hidden until first packet with that field.
- Heading shown in degrees, alt in metres (firmware sends cm).

Closes #24.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 17:48:53 -05:00
011f212056 fix: Status LEDs solid=OK blink=error (#22)
New LED behavior (active-low, PC15=LED1, PC14=LED2):
  Disarmed, IMU OK : LED1 solid ON  + LED2 off
  Armed            : LED1 solid ON  + LED2 solid ON
  Tilt fault       : LED1 blink 1Hz + LED2 blink 1Hz
  IMU error        : LED1 blink 1Hz + LED2 solid ON

Rule: solid = good, slow blink (~1Hz) = needs attention.
Removed the confusing fast-blink-at-5Hz-for-error and the
baro-flash-every-5s patterns.

status_update() signature changed: baro_ok → armed + tilt_fault
so the LED pattern can reflect arm state directly.

Closes #22.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 17:44:33 -05:00
f02ed8172a feat: gyro bias calibration on boot — fixes yaw drift (issues #21, #23)
On boot, before the main loop, sample 1000 gyro readings (~1s) while
board is held still. Compute per-axis mean offset (sensor-frame raw LSBs)
and subtract from all subsequent readings in mpu6000_read().

- mpu6000_calibrate(): LED1+LED2 solid ON during 1s sample window,
  resets filter state to zero once bias is known
- mpu6000_is_calibrated(): gate; main loop blocks arming and USB
  streaming until calibration completes
- Bias subtracted in sensor frame before CW270 axis transform + scale,
  so all three axes (pitch/roll/yaw rate) benefit
- config.h: GYRO_CAL_SAMPLES=1000
- No flash storage — recalibrate fresh each boot (bias varies with temp)

Closes #21 (3.5°/s yaw drift), #23 (gyro bias calibration on boot).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 17:42:24 -05:00
80a41e5008 feat: motor driver layer — differential drive, steer ramp, estop
Adds motor_driver.c/h between the balance PID and the raw
hoverboard UART driver:

- Differential drive: balance_cmd → speed, steer_cmd → steer
- Steer-only ramping at MOTOR_STEER_RAMP_RATE (balance PID keeps
  full immediate authority — no ramp on speed channel)
- Headroom clamp: reduces steer so |speed|+|steer|<=MOTOR_CMD_MAX
  ensuring ESC never clips the balance command
- Emergency stop: latches on TILT_FAULT, clears on BALANCE_DISARMED;
  send path stays in 50Hz ESC tick to avoid flooding UART

main.c: replace bare hoverboard_send() with motor_driver_update();
config.h: MOTOR_CMD_MAX=1000, MOTOR_STEER_RAMP_RATE=20 counts/ms

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 17:15:40 -05:00
6513b04e4e fix: correct roll axis mapping + add yaw telemetry (issues #12, #13)
Issue #12 — Roll displayed as pitch:
- Firmware was sending r=pitch_rate (wrong). Changed to r=roll_deg*10.
- mpu6000.c: add roll complementary filter (accel atan2(ay,az) +
  gyro gy integration, same COMP_ALPHA=0.98 as pitch).
- IMUData: add roll and yaw fields.
- UI: updateIMU() now uses data.r/10 for roll (not client-side filter
  that computed from ax/ay/az which firmware never sent).
- Three.js: roll -> rotation.z (banking), pitch -> rotation.x (tipping)
  — axes were already correct, fix was the firmware data.

Issue #13 — Add yaw telemetry:
- Firmware: gyro Z integration (gz * dt) → s_yaw, sent as y=yaw_deg*10.
  Gyro-only, drifts — no magnetometer.
- IMUData: yaw field added.
- UI: yaw -> rotation.y (spinning on vertical axis). Displayed in HUD.
- UI: YAW RESET button captures current yaw as new zero reference
  (client-side offset, no new firmware command needed).

Closes #12, #13.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 15:07:04 -05:00
4dd52b47dc feat(safety): IWDG watchdog, arm hold interlock, tilt alert (bd-3qh)
Safety systems implementation:

IWDG Hardware Watchdog (50ms timeout, config.h WATCHDOG_TIMEOUT_MS):
- safety_init() configures IWDG at PSC/32 (0.8ms tick), reload=62
- safety_refresh() must be called every loop iteration
- Cannot be disabled once started — MCU resets if loop hangs
- Started after 3s USB init delay (avoids spurious startup reset)

Arm Hold Interlock (3s, config.h ARMING_HOLD_MS):
- Arm command starts a hold timer, not immediate motor enable
- Motors only enable after ARMING_HOLD_MS consecutive hold
- Disarm or tilt > 10° cancels pending arm
- Prevents accidental arm from single keypress

Tilt Fault Alert:
- safety_alert_tilt_fault() fires one-shot buzzer on TILT_FAULT edge
- Rider hears alarm when tilt cutoff triggers
- Edge-detected (buzzer only fires once per fault event)

RC Timeout (infrastructure):
- safety_rc_alive() checks crsf_state.last_rx_ms vs RC_TIMEOUT_MS
- RC disarm wired but guarded (no CRSF yet) — remove guard when wired
- Compatible with future CRSF implementation

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 13:11:43 -05:00
398cbb9a55 feat(imu): MPU6000 sensor fusion with complementary filter
Add src/mpu6000.c implementing a complementary filter (α=0.98) on top of
the existing icm42688 SPI driver. Fixes wrong scale factors in balance.c
(was ±250°/s / ±2g; hardware is configured ±2000°/s / ±16g). Fusion now
lives in the IMU driver layer; balance_update() consumes IMUData directly.

- mpu6000_init(): calls icm42688_init(), seeds filter state
- mpu6000_read(): reads raw SPI, applies complementary filter, returns
  fused pitch (degrees) + pitch_rate (°/s) + accel_x/z (g)
- balance.c: removes duplicated fusion code, uses IMUData.pitch
- main.c: switches to mpu6000_init()/mpu6000_read(), updates telemetry

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 13:09:18 -05:00
Sebastien Vayrette
ba3e1161b9 Balance firmware + USB CDC bug 2026-02-28 11:58:23 -05:00