Each anchor only runs DW1000Ranging during its 50ms time slot.
Anchor 0 responds in even slots, anchor 1 in odd slots.
Prevents RF collisions that caused wild range readings.
- MODE_LONGDATA_RANGE_ACCURACY → MODE_LONGDATA_RANGE_LOWPOWER (both anchor+tag)
- Add delay(1) in tag loop to prevent tight-loop when no anchors respond
- ACCURACY was unnecessary and kept DW1000 radio hot
Tag firmware (esp32/uwb_tag):
- SSD1306 128x64 OLED: shows distance, anchor ranges, RSSI bars, link status
- ESP-NOW broadcast: sends range/heartbeat/estop packets (20 bytes, peer-to-peer)
- Emergency stop: GPIO 0 (BOOT), active LOW, 10Hz TX while held, 3x clear on release
- Display updates at 5Hz, ranging still at 20Hz round-robin
- Added Adafruit SSD1306 + GFX lib_deps to platformio.ini
Anchor firmware (esp32/uwb_anchor):
- ESP-NOW receiver: captures tag packets via ISR ring buffer
- Forwards to Jetson serial as +ESPNOW: and +ESTOP: lines
- E-STOP packets get priority immediate output
- Zero impact on existing TWR ranging loop
For Makerfabs ESP32 UWB Pro with Display (DW3000 chip).
Tag firmware for Makerfabs ESP32 UWB Pro worn by person being tracked.
Initiates DS-TWR with each robot anchor in 20 Hz round-robin.
- DS-TWR initiator: Poll→(RESP received)→Final with timestamps Ra/Da/Db
- Anchor side computes authoritative range; tag computes local estimate
- Round-robin across NUM_ANCHORS anchors (default 2) at 20 Hz
- Streams +RANGE:<id>,<mm>,<rssi> over USB serial (bench debug)
- LED blink on each successful range
- TAG_ID, NUM_ANCHORS, RANGE_INTERVAL_MS configurable in platformio.ini
- Pin map: SCK=18 MISO=19 MOSI=23 CS=21 RST=27 IRQ=34
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
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>
