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>
Add saltybot_depth_costmap — a Nav2 costmap2d plugin that converts
D435i depth images directly into obstacle markings on both local and
global costmaps.
Pipeline:
1. Subscribe to /camera/depth/image_rect_raw (16UC1 mm) + camera_info
2. Back-project depth pixels to 3D using pinhole camera intrinsics
3. Transform points to costmap global_frame via TF2
4. Apply configurable height filter (min_height..max_height above ground)
5. Mark obstacle cells as LETHAL_OBSTACLE
6. Inflate neighbours within inflation_radius as INSCRIBED_INFLATED_OBSTACLE
Parameters:
min_height: 0.05 m — floor clearance (ignores ground returns)
max_height: 0.80 m — ceiling cutoff (ignores lights/ceiling)
obstacle_range: 3.5 m — max marking distance from camera
clearing_range: 4.0 m — max distance processed at all
inflation_radius: 0.10 m — in-layer inflation (works before inflation_layer)
downsample_factor: 4 — process 1 of N rows+cols (~19k pts @ 640×480)
Integration (#478):
- Added depth_costmap_layer to local_costmap plugins list
- Added depth_costmap_layer to global_costmap plugins list
- Plugin registered via pluginlib (plugin.xml)
Files:
jetson/ros2_ws/src/saltybot_depth_costmap/
CMakeLists.txt, package.xml, plugin.xml
include/saltybot_depth_costmap/depth_costmap_layer.hpp
src/depth_costmap_layer.cpp
jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml (updated)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Adds TeleopWebUI component — a dedicated browser-based remote control
panel combining live video and joystick teleoperation in one view:
- Live camera stream (front/rear/left/right) via rosbridge CompressedImage
- Virtual joystick (canvas-based, touch + mouse, 10% deadzone)
- WASD / arrow-key keyboard fallback, Space for quick stop
- Speed presets: SLOW (20%), NORMAL (50%), FAST (100%)
- Latching E-stop button with pulsing visual indicator
- Real-time linear/angular velocity display
- Mobile-responsive: stacks vertically on small screens, side-by-side on lg+
- Added TELEOP tab group → Drive tab in App.jsx
Topics: /camera/<name>/image_raw/compressed (subscribe)
/cmd_vel geometry_msgs/Twist (publish)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Three bugs prevented mpu6000_is_calibrated() from returning true,
blocking arming and balance mode:
1. WHO_AM_I single-attempt: one SPI glitch returning 0x00 caused
icm42688_init() to return -128, skipping mpu6000_calibrate()
entirely. Fix: retry WHO_AM_I up to 3 times with 10ms gaps.
2. icm42688_read() rx[15] uninitialized: if HAL_SPI_TransmitReceive()
failed, garbage stack data was accumulated as gyro bias. Fix: zero-
init rx[15] so failed transfers produce zero data.
3. mpu6000_calibrate() raw uninitialized: UB if icm42688_read() is
a no-op (imu_type mismatch). Fix: zero-init raw each iteration.
Also add SCB_InvalidateDCache_by_Addr() on SPI rx buffers in rreg()
and icm42688_read() for DCache coherency. Currently a no-op (DCache
is not enabled), but required if SCB_EnableDCache() is added — stack
buffers in SRAM2 are in the cacheable memory region on STM32F7.
Fix misleading DCache comment in icm42688.c (claimed DCache was
disabled by main.c; actually SCB_EnableDCache() is never called).
Build: 59904 bytes Flash (+512), 17100 bytes RAM — SUCCESS
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
- esc_hoverboard.c: huart2 static in production; non-static only under
#ifdef DEBUG_MOTOR_TEST (needed by R command in jetson_uart.c)
- esc_hoverboard.c: UART5 diagnostic in hoverboard_backend_init() and
per-packet printf in hoverboard_backend_send() guarded by same flag
- esc_hoverboard.c: #include <stdio.h> also guarded (not needed in production)
- jetson_uart.c: R (baud sweep) and X (GPIO test) commands guarded by
#ifdef DEBUG_MOTOR_TEST — not compiled into production firmware
Production build: no debug output, static huart2, no R/X commands.
Debug build: define DEBUG_MOTOR_TEST to re-enable all diagnostics.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Add saltybot_motor_daemon ROS2 package — Python daemon that subscribes
to /cmd_vel and drives the FC via W<speed>,<steer>\n over /dev/ttyTHS1
at 921600 baud.
- motor_daemon_node.py: 50 Hz fixed-rate TX, 200ms safety watchdog,
Twist→ESC conversion (±1000 range), FC ack parsing (W:<s>,<st>),
periodic ? status query, /diagnostics publisher, auto-reconnect
- config/motor_daemon_params.yaml: all tunable params with comments
- launch/motor_daemon.launch.py: parameterised launch file
- test/test_motor_daemon.py: 25 unit tests (all passing)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
USART1 IDLE interrupt (DMA circular RX) was calling HAL_UART_IRQHandler
mid-frame during polling HAL_UART_Transmit, resetting gState and causing
leading nulls / truncated frames on the Jetson telemetry link at 921600 baud.
Fix: introduce jlink_tx_locked() which disables USART1_IRQn around every
blocking HAL_UART_Transmit call, preventing IRQHandler from corrupting
gState while the TX loop is running. A s_tx_busy flag drops any
re-entrant caller (ESC debug, future USART6/VESC paths).
Both jlink_send_telemetry (50 Hz) and jlink_send_power_telemetry (1 Hz)
now use jlink_tx_locked(). Also correct the stale config.h comment that
misidentified the Jetson link as USART6 (it moved to USART1 in Issue #120).
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Add saltybot_motor_daemon ROS2 package — Python daemon that subscribes
to /cmd_vel and drives the FC via W<speed>,<steer>\n over /dev/ttyTHS1
at 921600 baud.
- motor_daemon_node.py: 50 Hz fixed-rate TX, 200ms safety watchdog,
Twist→ESC conversion (±1000 range), FC ack parsing (W:<s>,<st>),
periodic ? status query, /diagnostics publisher, auto-reconnect
- config/motor_daemon_params.yaml: all tunable params with comments
- launch/motor_daemon.launch.py: parameterised launch file
- test/test_motor_daemon.py: 25 unit tests (all passing)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Root causes confirmed from code audit:
1. DCache coherency: USB OTG FS reads physical SRAM while CPU writes through
DCache. Fix: MPU Region 0 marks 512B aligned USB buffer struct non-cacheable
(TEX=1, C=0, B=0) before HAL_PCD_Init(). DCache stays enabled globally.
2. IWDG ordering: safety_init() (IWDG start) deferred after all peripheral inits
to avoid watchdog reset during mpu6000_calibrate() (~510ms blocking).
DMA conflicts, GPIO conflicts, clock tree, and interrupt priorities all ruled out
with evidence. Full findings documented in USB_CDC_BUG.md.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
