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>
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>
Implement automatic mission logging with bag recorder:
- Auto-records to ~/.saltybot-data/bags/ with 30min rotation
- Records mission-critical topics: /scan, /cmd_vel, /odom, /tf, /camera/color/image_raw/compressed, /saltybot/diagnostics
- MCAP format (preferred) with fallback to sqlite3 with zstd compression
- Services: /saltybot/save_bag, /saltybot/start_recording, /saltybot/stop_recording
- FIFO 20GB disk limit with automatic cleanup of oldest bags
- Auto-starts on launch, auto-saves on graceful shutdown
Changes:
- Updated bag_recorder_node.py with new parameters and services
- Changed default bag_dir to ~/.saltybot-data/bags/
- Set max_storage_gb to 20 (FIFO limit)
- Changed storage_format to MCAP by default
- Added start/stop recording service callbacks
- Updated package.xml description for mission logging
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
- Integrate saltybot_docking package into full_stack.launch.py
- Auto-trigger docking when battery drops to 20% (configurable via battery_low_pct)
- Launch docking at t=7s (after sensors, before Nav2)
- Add /saltybot/docking_state publisher (std_msgs/String) for state monitoring
- Update docking_params.yaml:
- battery_low_pct: 15% → 20% per Issue #489
- Add references to Issue #475 for conservative FC+hoverboard speeds
- Docking behavior includes:
- ArUco marker or IR beacon detection for dock location
- Nav2-based approach to pre-dock pose (~1m away)
- Visual servoing final alignment with contact detection
- Auto-undocking on full charge (80%) or command
- Integration with power management for mission interruption/resumption
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
