5f03e4cbef
feat: Tilt compensation for slopes (Issue #600 )
...
Adds a slow-adapting terrain slope estimator (IIR tau=5s) that decouples
the robot's balance offset from genuine ground incline. The balance
controller subtracts the slope estimate from measured pitch so the PID
balances around the slope surface rather than absolute vertical.
- include/slope_estimator.h + src/slope_estimator.c: first-order IIR
filter clamped to ±15°; JLINK_TLM_SLOPE (0x88) telemetry at 1 Hz
- include/jlink.h + src/jlink.c: add JLINK_TLM_SLOPE (0x88),
jlink_tlm_slope_t (4 bytes), jlink_send_slope_tlm()
- include/balance.h + src/balance.c: integrate slope_estimator into
balance_t; update, reset on tilt-fault and disarm
- test/test_slope_estimator.c: 35 unit tests, all passing
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-14 15:04:58 -04:00
8fbe7c0033
feat: STM32 watchdog and fault recovery handler (Issue #565 )
...
- New src/fault_handler.c + include/fault_handler.h:
- HardFault/MemManage/BusFault/UsageFault naked ISR stubs with
Cortex-M7 stack-frame capture (R0-R3, LR, PC, xPSR, CFSR, HFSR,
MMFAR, BFAR, SP) and NVIC_SystemReset()
- .noinit SRAM capture ring survives soft reset; persisted to flash
sector 7 (0x08060000, 8x64-byte slots) on subsequent boot
- MPU Region 0 stack guard (32 B at __stack_end, no-access) ->
MemManage fault detected as FAULT_STACK_OVF
- Brownout detect via RCC_CSR_BORRSTF on boot -> FAULT_BROWNOUT
- Watchdog reset detection delegates to existing watchdog.c
- LED blink codes on LED2 (PC14, active-low) for 10 s post-recovery:
HARDFAULT=3, WATCHDOG=2, BROWNOUT=1, STACK_OVF=4 fast blinks
- fault_led_tick(), fault_log_read(), fault_log_get_count(),
fault_get_last_type(), fault_log_clear(), FAULT_ASSERT() macro
- jlink.h: add JLINK_CMD_FAULT_LOG_GET (0x0F), JLINK_TLM_FAULT_LOG
(0x86), jlink_tlm_fault_log_t (20 bytes), fault_log_req in JLinkState,
jlink_send_fault_log() declaration
- jlink.c: dispatch JLINK_CMD_FAULT_LOG_GET; implement
jlink_send_fault_log() (26-byte CRC16-XModem framed response)
- main.c: call fault_handler_init() first in main(); send fault log
TLM on boot if prior fault recorded; fault_led_tick() in main loop;
handle fault_log_req flag to respond to Jetson queries
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-14 13:37:14 -04:00
2b06161cb4
feat: Motor current monitoring and overload protection (Issue #584 )
...
Adds ADC-based motor current sensing with configurable overload threshold,
soft PWM limiting, hard cutoff on sustained overload, and auto-recovery.
Changes:
- include/motor_current.h: MotorCurrentState enum (NORMAL/SOFT_LIMIT/COOLDOWN),
thresholds (5A hard, 4A soft, 2s overload, 10s cooldown), full API
- src/motor_current.c: reads battery_adc_get_current_ma() each tick (reuses
existing ADC3 IN13/PC3 DMA sampling); linear PWM scale in soft-limit zone
(scale256 fixed-point); fault counter + one-tick fault_pending flag for
main-loop fault log integration; telemetry at MOTOR_CURR_TLM_HZ (5 Hz)
- include/pid_flash.h: add pid_sched_entry_t (16 bytes), pid_sched_flash_t
(128 bytes at 0x0807FF40), PID_SCHED_MAX_BANDS=6, pid_flash_load_schedule(),
pid_flash_save_all() — fixes missing types needed by jlink.h (Issue #550 )
- src/pid_flash.c: implement flash_write_words() helper, pid_flash_load_schedule(),
pid_flash_save_all() — single sector-7 erase covers both schedule and PID records
- include/jlink.h: add JLINK_TLM_MOTOR_CURRENT (0x86), jlink_tlm_motor_current_t
(8 bytes: current_ma, limit_pct, state, fault_count), jlink_send_motor_current_tlm()
- src/jlink.c: implement jlink_send_motor_current_tlm() (14-byte frame)
Motor overload state machine:
MC_NORMAL : current_ma < 4000 mA — full PWM authority
MC_SOFT_LIMIT : 4000-5000 mA — linear reduction (0% at 4A → 100% at 5A)
MC_COOLDOWN : >5A sustained 2s → zero output for 10s then NORMAL
Main-loop integration:
motor_current_tick(now_ms);
if (motor_current_fault_pending()) fault_log_append(FAULT_MOTOR_OVERCURRENT);
cmd = motor_current_apply_limit(balance_pid_output());
motor_current_send_tlm(now_ms);
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-14 12:25:29 -04:00
8e03a209be
feat: ROS2 sensor health monitor (Issue #566 )
...
Add sensor_health_node to saltybot_health_monitor package. Monitors 8
sensor topics for staleness, publishing DiagnosticArray on
/saltybot/diagnostics and MQTT JSON on saltybot/health.
Sensors monitored (configurable thresholds):
/camera/color/image_raw, /camera/depth/image_rect_raw,
/camera/color/camera_info, /scan, /imu/data,
/saltybot/uwb/range, /saltybot/battery, /saltybot/motor_daemon/status
Each sensor: OK/WARN/ERROR based on topic age vs warn_s/error_s thresholds.
Critical sensors (camera, lidar, imu, motor_daemon) escalate overall status.
Files added:
sensor_health_node.py — SensorWatcher + SensorHealthNode
config/sensor_health_params.yaml — per-sensor thresholds
launch/sensor_health.launch.py
test/test_sensor_health.py — 35 tests, all passing
setup.py/package.xml updated: sensor_msgs, diagnostic_msgs deps + new entry point.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-14 11:47:01 -04:00
36643dd652
feat: Pan/tilt gimbal servo driver for ST3215 bus servos (Issue #547 )
...
- servo_bus.c/h: half-duplex USART3 driver for Feetech ST3215 servos at
1 Mbps; blocking TX/RX with CRC checksum; read/write position, torque
enable, speed; deg<->raw conversion (center=2048, 4096 counts/360°)
- gimbal.c/h: gimbal_t controller; 50 Hz feedback poll alternating pan/tilt
at 25 Hz each; clamps to ±GIMBAL_PAN/TILT_LIMIT_DEG soft limits
- jlink.c: dispatch JLINK_CMD_GIMBAL_POS (0x0B, 6-byte payload int16+int16+
uint16); jlink_send_gimbal_state() for JLINK_TLM_GIMBAL_STATE (0x84)
- main.c: servo_bus_init() + gimbal_init() on boot; gimbal_tick() in main
loop; gimbal_updated flag handler; GIMBAL_STATE telemetry at 50 Hz
- config.h: SERVO_BUS_UART/PORT/PIN/BAUD, GIMBAL_PAN/TILT_ID, GIMBAL_TLM_HZ,
GIMBAL_PAN/TILT_LIMIT_DEG
- jlink.h: CMD_GIMBAL_POS, TLM_GIMBAL_STATE, jlink_tlm_gimbal_state_t (10 B),
gimbal_updated/pan_x10/tilt_x10/speed volatile fields in JLinkState
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-14 10:38:06 -04:00
59d164944d
Merge origin/sl-controls/issue-533-battery-adc — resolve jlink conflicts
...
Keep both Issue #531 (PID_RESULT telemetry) and Issue #533 (BATTERY
telemetry) additions in include/jlink.h and src/jlink.c.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-07 10:10:26 -05:00
cc0ffd1999
feat: Battery voltage ADC driver with DMA sampling (Issue #533 )
...
STM32F7 ADC driver for battery voltage/current monitoring using
DMA-based continuous sampling, IIR low-pass filter, voltage divider
calibration, and USART telemetry to Jetson. Integrates with power
management for low-battery sleep (Issue #467 ).
Implementation:
- include/battery_adc.h: New driver header with calibration struct and
public API (init, tick, get_voltage_mv, get_current_ma, calibrate,
publish, check_pm, is_low, is_critical)
- src/battery_adc.c: ADC3 continuous-scan DMA (DMA2_Stream0/Ch2), 4x
hardware oversampling of both Vbat (PC1/IN11) and Ibat (PC3/IN13),
IIR LPF (alpha=1/8, cutoff ~4 Hz at 100 Hz tick rate), calibration
with ±500 mV offset clamp, 3S/4S auto-detection, 1 Hz USART publish
- include/jlink.h + src/jlink.c: Add JLINK_TLM_BATTERY (0x82) telemetry
type and jlink_tlm_battery_t (10-byte packed struct), implement
jlink_send_battery_telemetry() using CRC16-XModem framing
- include/power_mgmt.h + src/power_mgmt.c: Add
power_mgmt_notify_battery() — triggers STOP-mode sleep when Vbat
sustains critical level (Issue #467 )
- test/test_battery_adc.c: 27 unit tests (27/27 passing): voltage
conversion, calibration offset/scale, IIR LPF convergence, SoC
estimation (3S/4S), low/critical flags, PM notification timing,
calibration reset, publish rate-limiting
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-07 10:01:02 -05:00
19a30a1c4f
feat: PID auto-tune for balance mode (Issue #531 )
...
Implement Ziegler-Nichols relay feedback auto-tuning with flash persistence:
Firmware (STM32F722):
- pid_flash.c/h: erase+write Kp/Ki/Kd to flash sector 7 (0x0807FFC0),
magic-validated; load on boot to restore saved tune
- jlink.h: add JLINK_CMD_PID_SAVE (0x0A) and JLINK_TLM_PID_RESULT (0x83)
with jlink_tlm_pid_result_t struct and pid_save_req flag in JLinkState
- jlink.c: dispatch JLINK_CMD_PID_SAVE -> pid_save_req; add
jlink_send_pid_result() to confirm flash write outcome over USART1
- main.c: load saved PID from flash after balance_init(); handle
pid_save_req in main loop (disarmed-only, erase stalls CPU ~1s)
Jetson ROS2 (saltybot_pid_autotune):
- pid_autotune_node.py: add Ki to Ziegler-Nichols formula (ZN PID:
Kp=0.6Ku, Ki=1.2Ku/Tu, Kd=0.075KuTu); add JLink serial client that
sends JLINK_CMD_PID_SET + JLINK_CMD_PID_SAVE after tuning completes
- autotune_config.yaml: add jlink_serial_port and jlink_baud_rate params
Trigger: ros2 service call /saltybot/autotune_pid std_srvs/srv/Trigger
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-07 09:56:19 -05:00
f446e5766e
feat(power): STOP-mode sleep/wake power manager — Issue #178
...
Adds STM32F7 STOP-mode power management with <10ms wake latency:
- power_mgmt.c: state machine (ACTIVE→SLEEP_PENDING→SLEEPING→WAKING),
30s idle timeout (PM_IDLE_TIMEOUT_MS), 3s LED fade before STOP,
gate SPI3/I2S3+SPI2+USART6+UART5 on sleep (clock-only, state preserved),
EXTI1(PA1/CRSF)+EXTI7(PB7/JLink)+EXTI4(PC4/IMU) wake sources,
PLL restore after STOP (PLLM=8/N=216/P=2 → 216MHz), uwTick save/restore
- Peripheral gating: I2S3, SPI2(OSD), USART6, UART5 disabled during STOP;
SPI1(IMU), UART4(CRSF), USART1(JLink), I2C1 remain active as wake sources
- Sleep LED: triangle-wave pulse (2s period) on LED1 during SLEEP_PENDING,
software PWM in main loop (1-bit, pm_pwm_phase vs brightness)
- IWDG: fed just before WFI; <10ms wake << 50ms WATCHDOG_TIMEOUT_MS
- JLink: JLINK_CMD_SLEEP=0x09, JLINK_TLM_POWER=0x81 (11-byte power frame
at 1Hz: power_state, est_total_ma, est_audio_ma, est_osd_ma, idle_ms)
- main.c: power_mgmt_init(), activity() on CRSF/JLink/armed, tick() when
disarmed, sleep_req handler, LED PWM, JLINK_TLM_POWER telemetry
- config.h: PM_* constants, PM_CURRENT_*_MA estimates, PM_TLM_HZ
- test_power_mgmt.py: 72 tests passing (state machine, LED, gating,
current estimates, JLink protocol, wake latency, hardware constants)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:53:02 -05:00
c3ada4a156
feat(audio): I2S3 audio amplifier driver — Issue #143
...
Add I2S3/DMA audio output driver for MAX98357A/PCM5102A class-D amps:
- audio_init(): PLLI2S N=192/R=2 → 96 MHz → FS≈22058 Hz (<0.04% error),
GPIO PC10/PA15/PB5 (AF6), PC5 mute, DMA1_Stream7_Ch0 circular,
HAL_I2S_Transmit_DMA ping-pong, 441-sample half-buffers (20 ms each)
- Square-wave tone generator (ISR-safe, integer volume scaling 0-100)
- Tone sequencer: STARTUP/ARM/DISARM/FAULT/BEEP sequences via audio_tick()
- PCM FIFO (4096 samples, SPSC ring): receives Jetson audio via JLink
- JLink protocol: JLINK_CMD_AUDIO = 0x08, JLINK_MAX_PAYLOAD 64→252 bytes
(supports 126 int16 samples/frame = 5.7 ms @22050 Hz)
- main.c: audio_init(), STARTUP tone on boot, ARM/FAULT tones, audio_tick()
- config.h: AUDIO_BCLK/LRCK/DOUT/MUTE pin defines + PLLI2S constants
- test_audio.py: 45 tests, all passing
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:34:35 -05:00
4beef8da03
feat(firmware): OTA DFU entry via JLink command and Python flash script (Issue #124 )
...
- Add ota.h / ota.c: ota_enter_dfu() (armed guard, writes BKP15R, resets),
ota_fw_crc32() using STM32F7 hardware CRC peripheral (CRC-32/MPEG-2, 512 KB)
- Add JLINK_CMD_DFU_ENTER (0x06) and dfu_req flag to jlink.h / jlink.c
- Handle dfu_req in main loop: calls ota_enter_dfu(is_armed) — no-op if armed
- Update usbd_cdc_if.c: move DFU magic from BKP0R to BKP15R (OTA_DFU_BKP_IDX)
resolving BKP register conflict with BNO055 calibration (BKP0R–6R, PR #150 )
- Add scripts/flash_firmware.py: CRC-32/MPEG-2 + ISO-HDLC verification,
dfu-util flash, host-side backup/rollback, --trigger-dfu JLink serial path
- Add test/test_ota.py: 42 tests passing (CRC-32/MPEG-2, CRC-16/XMODEM,
DFU_ENTER frame structure, BKP register safety, flash constants)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 09:56:18 -05:00
6f0ad8e92e
feat(firmware): Jetson binary serial protocol on USART1 (Issue #120 )
...
New jlink module replaces ASCII-over-USB-CDC jetson_cmd with a dedicated
hardware UART binary protocol at 921600 baud for reliable Jetson comms.
- include/jlink.h: JLinkState struct, jlink_tlm_status_t (20-byte packed),
command/telemetry IDs (0x01-0x07 cmd, 0x80 status), API declarations
- src/jlink.c: USART1 DMA2_Stream2_Channel4 circular RX (128 bytes),
IDLE interrupt, CRC16-XModem (poly 0x1021) frame parser state machine,
command dispatch (HEARTBEAT/DRIVE/ARM/DISARM/PID_SET/ESTOP),
jlink_send_telemetry() blocking TX (≈0.28 ms per frame)
- include/config.h: JLINK_BAUD=921600, JLINK_HB_TIMEOUT_MS=1000,
JLINK_TLM_HZ=50, FW_MAJOR/MINOR/PATCH version constants
- src/main.c: jlink_init(), jlink_process() in main loop, arm/disarm/
estop/PID flag handling, 50 Hz STATUS telemetry TX, jlink takes
priority over legacy jetson_cmd for speed/steer injection
- test/test_jlink_frames.py: 39 pytest tests (39/39 pass) — CRC16,
frame building, parser state machine, drive/PID/status encoding
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 09:22:34 -05:00
