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
