Anchor firmware for Makerfabs ESP32 UWB Pro (DW3000 chip). Two anchors
mount on SaltyBot (port/starboard), USB-connected to Jetson Orin.
- DS-TWR responder: Poll→Resp→Final with ±10cm accuracy
- Streams +RANGE:<id>,<mm>,<rssi_dbm> on Serial 115200
- AT command interface: AT+RANGE?, AT+RANGE_ADDR=, AT+ID?
- ANCHOR_ID 0/1 set at build time (env:anchor0 / env:anchor1)
- PlatformIO config for Makerfabs MaUWB_DW3000 library
- udev rules for /dev/uwb-anchor0 /dev/uwb-anchor1 USB symlinks
- Pin map: SCK=18 MISO=19 MOSI=23 CS=21 RST=27 IRQ=34
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Add saltybot_uwb_position — ROS2 Python package that reads JSON range
measurements from an ESP32 DW3000 UWB tag over USB serial, trilaterates
the robot's absolute position from 3+ fixed infrastructure anchors, and
publishes position + TF2 to the rest of the stack.
Serial protocol (one JSON line per frame):
Full frame: {"ts":…, "ranges": [{"id":0,"d_mm":1500,"rssi":-65}, …]}
Per-anchor: {"id":0, "d_mm":1500, "rssi":-65.0}
Accepts both "d_mm" and "range_mm" field names.
Trilateration (trilateration.py, numpy, no ROS deps):
Linear least-squares: linearise sphere equations around anchor 0,
solve (N-1)x2 (2D) or (N-1)x3 (3D) system via np.linalg.lstsq.
2D mode (default): robot_z fixed, needs >=3 anchors.
3D mode (solve_z=true): full 3D, needs >=4 anchors.
Outlier rejection:
After initial solve, compute per-anchor residual |r_meas - r_pred|.
Reject anchors with residual > outlier_threshold_m (0.4 m default).
Re-solve with inliers if >= min_anchors remain.
Track consecutive outlier strikes; flag in /status after N strikes.
Kalman filter (KalmanFilter3D, constant-velocity, 6-state, numpy):
Predict-only coasting when anchors drop below minimum.
Q=0.05, R=0.10 (tunable).
Topics:
/saltybot/uwb/pose PoseStamped 10 Hz Kalman-filtered position
/saltybot/uwb/range/<id> UwbRange on arrival, raw per-anchor ranges
/saltybot/uwb/status String/JSON 10 Hz state+residuals+flags
TF2: uwb_link -> map (identity rotation)
Anchor config: flat float arrays in YAML.
Default layout: 4-anchor 5x5m room at 2m height.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Adds a full gimbal control panel with live camera preview:
Standalone page (ui/gimbal_panel.html + .js + .css):
- Self-contained HTML page, no build step, served directly
- roslib.js via CDN, connects to rosbridge WebSocket
- 2-D canvas pan/tilt pad: click-drag + touch pointer capture
- Live camera stream (front/rear/left/right selector, base64 CompressedImage)
- FPS badge + angle overlay on video feed
- Preset positions: CENTER / LEFT / RIGHT / UP / DOWN
- Home button (0° / 0°)
- Person-tracking toggle → /gimbal/tracking_enabled
- Current angle display from /gimbal/state feedback
- WS URL persisted in localStorage
React component (GimbalPanel.jsx) + App.jsx integration:
- Same features in dashboard — TELEOP group → Gimbal tab
- Shares rosbridge connection from parent
- Mobile-responsive: stacks vertically on mobile, side-by-side on lg+
ROS topics:
PUB /gimbal/cmd geometry_msgs/Vector3
SUB /gimbal/state geometry_msgs/Vector3
PUB /gimbal/tracking_enabled std_msgs/Bool
SUB /camera/*/image_raw/compressed sensor_msgs/CompressedImage
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Add saltybot_head_tracking — ROS2 Python node for automatic person-
following using dual-axis PID control targeting the pan/tilt camera head.
Pipeline:
1. Subscribe to /saltybot/objects (DetectedObjectArray from YOLOv8n)
2. Filter for class_id==0 (person); select best target by score:
score = 0.6 * 1/(1+dist_m) + 0.4 * confidence
(falls back to confidence-only when distance_m==0 / unknown)
3. Compute pixel error of bbox centre from image centre
4. Apply dead-zone (10 px default) to suppress micro-jitter
5. Convert pixel error to angle error via camera FOV
6. Independent PID controllers for pan and tilt axes
7. Accumulate PID output into absolute angle setpoint
8. Publish geometry_msgs/Point to /saltybot/gimbal/cmd:
x = pan_angle_deg, y = tilt_angle_deg, z = confidence
State machine:
IDLE -> waiting for first detection
TRACKING -> active PID
LOST -> hold last angle for hold_duration_s (3 s)
CENTERING -> return to (0, 0) at 20 deg/s -> IDLE
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Add phone/voice_commander.py — Termux-based voice command listener for SaltyBot:
- Continuous wake word detection ('Hey Salty') via Whisper STT on short audio clips
- Command recording after wake word, transcribed with local Whisper (tiny/base/small)
- Parses go forward/back/left/right, stop, follow me, go home, look at me
- Publishes JSON to /saltybot/voice/cmd via ROS2 (rclpy) or rosbridge WebSocket
- TTS confirmation via termux-tts-speak; 'Yes?' prompt on wake word
- Fuzzy token-overlap fallback for wake word matching
- Flags: --host, --port, --model, --threshold, --record-sec, --no-tts, --debug
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
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>
- Add VoskSTT class to audio_utils.py: offline Vosk STT backend as
low-latency CPU alternative to Whisper for Jetson deployments
- Update audio_pipeline_node.py: stt_backend param ("whisper"/"vosk"),
Vosk loading with Whisper fallback, CPU auto-detection for Whisper,
dual-backend _process_utterance dispatch, STT/<backend> log prefix
- Update audio_pipeline_params.yaml: add stt_backend and vosk_model_path
- Add test/test_audio_pipeline.py: 40 unit tests covering EnergyVAD,
PCM conversion, AudioBuffer, UtteranceSegmenter, VoskSTT, JabraAudioDevice,
AudioMetrics, AudioState
- Integrate into full_stack.launch.py: audio_pipeline at t=5s with
enable_audio_pipeline and audio_stt_backend args
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
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>
