From 587ca8a98eec42bd4b5696ca5f0fff02fc9ad19f Mon Sep 17 00:00:00 2001 From: sl-uwb Date: Sat, 14 Mar 2026 15:03:53 -0400 Subject: [PATCH 1/3] feat: UWB anchor auto-calibration via inter-anchor ranging + MDS (Issue #602) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Anchor firmware (esp32/uwb_anchor/src/main.cpp): - Add peer_range_once(peer_id) — DS-TWR initiator role toward a peer anchor - Add AT+PEER_RANGE= command: triggers inter-anchor ranging and returns +PEER_RANGE:,,, (or ERR,TIMEOUT) ROS2 package saltybot_uwb_calibration_msgs: - CalibrateAnchors.srv: request (anchor_ids[], n_samples) → response (positions_x/y/z[], residual_rms_m, anchor_positions_json) ROS2 package saltybot_uwb_calibration: - mds_math.py: classical MDS (double-centred D², eigendecomposition), anchor_frame_align() to fix anchor-0 at origin / anchor-1 on +X - calibration_node.py: /saltybot/uwb/calibrate_anchors service — opens anchor serial ports, rounds-robin AT+PEER_RANGE= for all pairs, builds N×N distance matrix, runs MDS, returns JSON anchor positions - 12/12 unit tests passing (test/test_mds_math.py) - Supports ≥ 4 anchors; 5× averaged ranging per pair by default Co-Authored-By: Claude Sonnet 4.6 --- .../config/calibration_params.yaml | 17 ++ .../launch/uwb_calibration.launch.py | 32 +++ .../src/saltybot_uwb_calibration/package.xml | 29 ++ .../resource/saltybot_uwb_calibration | 0 .../saltybot_uwb_calibration/__init__.py | 0 .../calibration_node.py | 272 ++++++++++++++++++ .../saltybot_uwb_calibration/mds_math.py | 140 +++++++++ .../src/saltybot_uwb_calibration/setup.cfg | 4 + .../src/saltybot_uwb_calibration/setup.py | 29 ++ .../test/test_mds_math.py | 142 +++++++++ .../CMakeLists.txt | 12 + .../saltybot_uwb_calibration_msgs/package.xml | 23 ++ .../srv/CalibrateAnchors.srv | 29 ++ 13 files changed, 729 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/config/calibration_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/launch/uwb_calibration.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/resource/saltybot_uwb_calibration create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/calibration_node.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/mds_math.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration/test/test_mds_math.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/CMakeLists.txt create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/srv/CalibrateAnchors.srv diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/config/calibration_params.yaml b/jetson/ros2_ws/src/saltybot_uwb_calibration/config/calibration_params.yaml new file mode 100644 index 0000000..b80e88c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration/config/calibration_params.yaml @@ -0,0 +1,17 @@ +uwb_anchor_calibration: + ros__parameters: + # Serial ports for each anchor, indexed by anchor ID + # udev rules create stable symlinks in /dev/uwb-anchor + anchor_serials: + - /dev/uwb-anchor0 + - /dev/uwb-anchor1 + - /dev/uwb-anchor2 + - /dev/uwb-anchor3 + + baud_rate: 115200 + + # Seconds to wait for a +PEER_RANGE response per attempt + peer_range_timeout_s: 2.0 + + # Range measurements to average per anchor pair (higher = more accurate) + default_n_samples: 5 diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/launch/uwb_calibration.launch.py b/jetson/ros2_ws/src/saltybot_uwb_calibration/launch/uwb_calibration.launch.py new file mode 100644 index 0000000..c546d58 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration/launch/uwb_calibration.launch.py @@ -0,0 +1,32 @@ +""" +uwb_calibration.launch.py — Launch the UWB anchor auto-calibration service. + +Usage: + ros2 launch saltybot_uwb_calibration uwb_calibration.launch.py + +Then trigger from command line: + ros2 service call /saltybot/uwb/calibrate_anchors \\ + saltybot_uwb_calibration_msgs/srv/CalibrateAnchors \\ + "{anchor_ids: [], n_samples: 5}" +""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description() -> LaunchDescription: + pkg_dir = get_package_share_directory("saltybot_uwb_calibration") + params_file = os.path.join(pkg_dir, "config", "calibration_params.yaml") + + calibration_node = Node( + package="saltybot_uwb_calibration", + executable="uwb_calibration", + name="uwb_anchor_calibration", + parameters=[params_file], + output="screen", + emulate_tty=True, + ) + + return LaunchDescription([calibration_node]) diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/package.xml b/jetson/ros2_ws/src/saltybot_uwb_calibration/package.xml new file mode 100644 index 0000000..078b016 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration/package.xml @@ -0,0 +1,29 @@ + + + + saltybot_uwb_calibration + 0.1.0 + + UWB anchor auto-calibration for SaltyBot (Issue #602). + Commands anchors to range against each other via AT+PEER_RANGE=, + builds a pairwise distance matrix, and uses classical MDS to recover + 2-D anchor positions. Exposes /saltybot/uwb/calibrate_anchors service. + + sl-uwb + Apache-2.0 + + rclpy + saltybot_uwb_calibration_msgs + + python3-numpy + python3-serial + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/resource/saltybot_uwb_calibration b/jetson/ros2_ws/src/saltybot_uwb_calibration/resource/saltybot_uwb_calibration new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/__init__.py b/jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/calibration_node.py b/jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/calibration_node.py new file mode 100644 index 0000000..0d86671 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/calibration_node.py @@ -0,0 +1,272 @@ +""" +calibration_node.py — UWB anchor auto-calibration ROS2 service (Issue #602) + +Service: /saltybot/uwb/calibrate_anchors (CalibrateAnchors) + +Workflow +──────── + 1. For every ordered pair (i, j) with i < j, send AT+PEER_RANGE= over + anchor i's serial port, collect `n_samples` measurements, average them. + 2. Build the symmetric N×N distance matrix D. + 3. Run classical MDS (mds_math.mds_2d) to recover (x, y) positions. + 4. Align the frame: anchor-0 at origin, anchor-1 on +X axis. + 5. Return positions + residual RMS + JSON config string. + +Parameters (ROS2, see config/calibration_params.yaml) +────────────────────────────────────────────────────── + anchor_serials list of serial port paths, index = anchor ID + e.g. ["/dev/uwb-anchor0", "/dev/uwb-anchor1", ...] + baud_rate 115200 + peer_range_timeout_s 2.0 (per AT+PEER_RANGE= attempt) + default_n_samples 5 +""" + +from __future__ import annotations + +import json +import re +import time +from typing import Optional + +import numpy as np +import serial + +import rclpy +from rclpy.node import Node + +from saltybot_uwb_calibration_msgs.srv import CalibrateAnchors +from saltybot_uwb_calibration.mds_math import mds_2d, anchor_frame_align + + +_PEER_RANGE_RE = re.compile( + r"\+PEER_RANGE:(\d+),(\d+),(\d+),([-\d.]+)" +) +_PEER_ERR_RE = re.compile( + r"\+PEER_RANGE:ERR,(\d+),(\w+)" +) + + +class CalibrationNode(Node): + + def __init__(self) -> None: + super().__init__("uwb_anchor_calibration") + + self.declare_parameter("anchor_serials", [ + "/dev/uwb-anchor0", + "/dev/uwb-anchor1", + "/dev/uwb-anchor2", + "/dev/uwb-anchor3", + ]) + self.declare_parameter("baud_rate", 115200) + self.declare_parameter("peer_range_timeout_s", 2.0) + self.declare_parameter("default_n_samples", 5) + + self._serials: list[str] = ( + self.get_parameter("anchor_serials").value + ) + self._baud: int = self.get_parameter("baud_rate").value + self._timeout: float = self.get_parameter("peer_range_timeout_s").value + self._default_n: int = self.get_parameter("default_n_samples").value + + self._srv = self.create_service( + CalibrateAnchors, + "/saltybot/uwb/calibrate_anchors", + self._handle_calibrate, + ) + + self.get_logger().info( + f"UWB calibration service ready — " + f"{len(self._serials)} configured anchors" + ) + + # ── Service handler ──────────────────────────────────────────────────── + + def _handle_calibrate( + self, + request: CalibrateAnchors.Request, + response: CalibrateAnchors.Response, + ) -> CalibrateAnchors.Response: + + # Determine anchor IDs to calibrate + if request.anchor_ids: + ids = list(request.anchor_ids) + else: + ids = list(range(len(self._serials))) + + n_anchors = len(ids) + if n_anchors < 4: + response.success = False + response.message = ( + f"Need at least 4 anchors, got {n_anchors}" + ) + return response + + n_samples = int(request.n_samples) if request.n_samples > 0 \ + else self._default_n + + self.get_logger().info( + f"Starting calibration: anchors={ids}, n_samples={n_samples}" + ) + + # Open serial ports + ports: dict[int, serial.Serial] = {} + try: + for anchor_id in ids: + if anchor_id >= len(self._serials): + raise RuntimeError( + f"No serial port configured for anchor {anchor_id}" + ) + port_path = self._serials[anchor_id] + ports[anchor_id] = serial.Serial( + port_path, + baudrate=self._baud, + timeout=self._timeout, + ) + # Flush any stale data + ports[anchor_id].reset_input_buffer() + time.sleep(0.05) + self.get_logger().info( + f" Opened anchor {anchor_id} → {port_path}" + ) + except Exception as exc: + _close_all(ports) + response.success = False + response.message = f"Serial open failed: {exc}" + return response + + # Measure all unique pairs + D = np.zeros((n_anchors, n_anchors), dtype=float) + id_to_idx = {aid: idx for idx, aid in enumerate(ids)} + + try: + for i_idx, i_id in enumerate(ids): + for j_idx, j_id in enumerate(ids): + if j_idx <= i_idx: + continue + + dist_m = self._measure_pair( + ports[i_id], i_id, j_id, n_samples + ) + if dist_m is None: + raise RuntimeError( + f"No valid range between anchor {i_id} " + f"and anchor {j_id}" + ) + D[i_idx, j_idx] = dist_m + D[j_idx, i_idx] = dist_m + self.get_logger().info( + f" [{i_id}↔{j_id}] = {dist_m:.3f} m" + ) + + except Exception as exc: + _close_all(ports) + response.success = False + response.message = str(exc) + return response + + _close_all(ports) + + # MDS + try: + positions, residual = mds_2d(D) + positions = anchor_frame_align(positions) + except Exception as exc: + response.success = False + response.message = f"MDS failed: {exc}" + return response + + # Build JSON config + pos_dict = {} + for idx, aid in enumerate(ids): + pos_dict[str(aid)] = [ + round(float(positions[idx, 0]), 4), + round(float(positions[idx, 1]), 4), + 0.0, + ] + json_str = json.dumps(pos_dict, indent=2) + + self.get_logger().info( + f"Calibration complete — residual RMS {residual:.4f} m\n{json_str}" + ) + + # Fill response + response.success = True + response.message = ( + f"OK — residual RMS {residual*1000:.1f} mm" + ) + response.anchor_ids = [int(a) for a in ids] + response.positions_x = [float(positions[i, 0]) for i in range(n_anchors)] + response.positions_y = [float(positions[i, 1]) for i in range(n_anchors)] + response.positions_z = [0.0] * n_anchors + response.residual_rms_m = float(residual) + response.anchor_positions_json = json_str + return response + + # ── Per-pair measurement ─────────────────────────────────────────────── + + def _measure_pair( + self, + port: serial.Serial, + my_id: int, + peer_id: int, + n_samples: int, + ) -> Optional[float]: + """ + Send AT+PEER_RANGE= `n_samples` times over `port`. + Returns the mean range in metres, or None if all attempts fail. + """ + measurements: list[float] = [] + + for _ in range(n_samples): + cmd = f"AT+PEER_RANGE={peer_id}\r\n" + port.write(cmd.encode()) + port.flush() + + deadline = time.monotonic() + self._timeout + while time.monotonic() < deadline: + line = port.readline().decode(errors="replace").strip() + if not line: + continue + + m = _PEER_RANGE_RE.match(line) + if m: + # +PEER_RANGE:,,, + if int(m.group(1)) == my_id and int(m.group(2)) == peer_id: + mm = float(m.group(3)) + measurements.append(mm / 1000.0) # mm → m + break + + if _PEER_ERR_RE.match(line): + break # timeout from firmware — try next sample + + time.sleep(0.05) # inter-command gap + + if not measurements: + return None + return float(np.mean(measurements)) + + +# ── Helpers ──────────────────────────────────────────────────────────────── + +def _close_all(ports: dict) -> None: + for p in ports.values(): + try: + p.close() + except Exception: + pass + + +def main(args=None) -> None: + rclpy.init(args=args) + node = CalibrationNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/mds_math.py b/jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/mds_math.py new file mode 100644 index 0000000..6042d4c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/mds_math.py @@ -0,0 +1,140 @@ +""" +mds_math.py — Classical Multi-Dimensional Scaling for UWB anchor calibration + (Issue #602) + +Given an N×N symmetric distance matrix D (in metres), recover the 2-D +Cartesian positions of N anchors using classical (metric) MDS. + +Algorithm +───────── + 1. Compute the double-centred squared-distance matrix B = -½ H D² H + where H = I - (1/N) 11ᵀ (centering matrix). + 2. Eigen-decompose B = V Λ Vᵀ (symmetric). + 3. Keep the 2 largest positive eigenvalues (λ₁, λ₂). + 4. Positions X = V[:, :2] · diag(√λ₁, √λ₂). + +The recovered configuration is up to a global rigid transformation +(rotation, reflection, translation). The caller is responsible for +anchoring the coordinate frame (e.g. fix anchor-0 at origin, anchor-1 +on the positive-X axis). + +Usage +───── + from saltybot_uwb_calibration.mds_math import mds_2d, anchor_frame_align + + D = np.array([[0, d01, d02, d03], + [d01, 0, d12, d13], + ...]) + positions, residual = mds_2d(D) # (N,2) array, float RMS (m) + positions = anchor_frame_align(positions) # align frame: anchor-0 at origin +""" + +from __future__ import annotations + +import numpy as np + + +def mds_2d(D: np.ndarray) -> tuple[np.ndarray, float]: + """ + Recover 2-D anchor positions from pairwise distance matrix D. + + Parameters + ---------- + D : (N, N) symmetric ndarray of float + Pairwise distances in metres. Diagonal must be 0. + N must be >= 3 (4+ recommended for overdetermined system). + + Returns + ------- + positions : (N, 2) ndarray + Recovered x, y coordinates in metres. + residual_rms : float + RMS of (D_measured - D_recovered) in metres across all unique pairs. + + Raises + ------ + ValueError + If D is not square, not symmetric, or N < 3. + """ + D = np.asarray(D, dtype=float) + n = D.shape[0] + if D.ndim != 2 or D.shape[1] != n: + raise ValueError(f"D must be square, got shape {D.shape}") + if n < 3: + raise ValueError(f"Need at least 3 anchors, got {n}") + if not np.allclose(D, D.T, atol=1e-3): + raise ValueError("Distance matrix D is not symmetric") + + # ── Step 1: double-centre D² ────────────────────────────────────────── + D2 = D ** 2 + H = np.eye(n) - np.ones((n, n)) / n + B = -0.5 * H @ D2 @ H + + # ── Step 2: eigendecomposition ──────────────────────────────────────── + eigvals, eigvecs = np.linalg.eigh(B) # eigvals in ascending order + + # ── Step 3: keep top-2 positive eigenvalues ─────────────────────────── + idx = np.argsort(eigvals)[::-1] # descending + top2_idx = idx[:2] + lam = eigvals[top2_idx] + lam = np.maximum(lam, 0.0) # clamp numerical negatives + + # ── Step 4: recover positions ───────────────────────────────────────── + V = eigvecs[:, top2_idx] + positions = V * np.sqrt(lam) # (N, 2) + + # ── Residual RMS ────────────────────────────────────────────────────── + residual_rms = _residual(D, positions) + + return positions, residual_rms + + +def anchor_frame_align(positions: np.ndarray) -> np.ndarray: + """ + Align recovered positions so that: + - anchor-0 is at the origin (0, 0) + - anchor-1 lies on the positive-X axis (y = 0) + + This removes the MDS ambiguity (translation + rotation + reflection). + + Parameters + ---------- + positions : (N, 2) ndarray + + Returns + ------- + (N, 2) ndarray — aligned positions + """ + positions = np.array(positions, dtype=float) + if positions.shape[0] < 2: + return positions + + # Translate so anchor-0 is at origin + positions -= positions[0] + + # Rotate so anchor-1 is on positive-X axis + dx, dy = positions[1, 0], positions[1, 1] + angle = np.arctan2(dy, dx) + cos_a, sin_a = np.cos(-angle), np.sin(-angle) + R = np.array([[cos_a, -sin_a], + [sin_a, cos_a]]) + positions = (R @ positions.T).T + + # Ensure anchor-1 has positive X (handle reflection) + if positions[1, 0] < 0: + positions[:, 0] *= -1.0 + + return positions + + +def _residual(D_meas: np.ndarray, positions: np.ndarray) -> float: + """Compute RMS of (measured - recovered) distances over all unique pairs.""" + n = D_meas.shape[0] + errors = [] + for i in range(n): + for j in range(i + 1, n): + d_rec = float(np.linalg.norm(positions[i] - positions[j])) + errors.append(D_meas[i, j] - d_rec) + if not errors: + return 0.0 + return float(np.sqrt(np.mean(np.array(errors) ** 2))) diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/setup.cfg b/jetson/ros2_ws/src/saltybot_uwb_calibration/setup.cfg new file mode 100644 index 0000000..ad99724 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_uwb_calibration +[install] +install_scripts=$base/lib/saltybot_uwb_calibration diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/setup.py b/jetson/ros2_ws/src/saltybot_uwb_calibration/setup.py new file mode 100644 index 0000000..18862cb --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import setup + +package_name = "saltybot_uwb_calibration" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", + [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-uwb", + maintainer_email="sl-uwb@saltylab.local", + description="UWB anchor auto-calibration via MDS (Issue #602)", + license="Apache-2.0", + entry_points={ + "console_scripts": [ + "uwb_calibration = saltybot_uwb_calibration.calibration_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration/test/test_mds_math.py b/jetson/ros2_ws/src/saltybot_uwb_calibration/test/test_mds_math.py new file mode 100644 index 0000000..ec1d5f7 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration/test/test_mds_math.py @@ -0,0 +1,142 @@ +""" +Unit tests for saltybot_uwb_calibration.mds_math (Issue #602). +No ROS2 or hardware required — pure numpy. +""" + +import math +import sys +import os + +import numpy as np +import pytest + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) +from saltybot_uwb_calibration.mds_math import mds_2d, anchor_frame_align, _residual + + +def _dist_matrix(positions: np.ndarray) -> np.ndarray: + """Build exact pairwise distance matrix from ground-truth positions.""" + n = len(positions) + D = np.zeros((n, n)) + for i in range(n): + for j in range(n): + D[i, j] = np.linalg.norm(positions[i] - positions[j]) + return D + + +# ── Square configuration (4 anchors) ────────────────────────────────────── + +GT_SQUARE = np.array([ + [0.0, 0.0], + [4.0, 0.0], + [4.0, 3.0], + [0.0, 3.0], +]) + + +class TestMdsSquare: + def test_residual_near_zero(self): + D = _dist_matrix(GT_SQUARE) + positions, rms = mds_2d(D) + assert rms < 1e-6, f"Residual too large: {rms}" + + def test_correct_distances_preserved(self): + D = _dist_matrix(GT_SQUARE) + positions, _ = mds_2d(D) + positions = anchor_frame_align(positions) + D_rec = _dist_matrix(positions) + # All pairwise distances should match to < 1 mm + assert np.max(np.abs(D - D_rec)) < 1e-3 + + def test_anchor_0_at_origin(self): + D = _dist_matrix(GT_SQUARE) + positions, _ = mds_2d(D) + positions = anchor_frame_align(positions) + assert abs(positions[0, 0]) < 1e-9 + assert abs(positions[0, 1]) < 1e-9 + + def test_anchor_1_on_positive_x(self): + D = _dist_matrix(GT_SQUARE) + positions, _ = mds_2d(D) + positions = anchor_frame_align(positions) + assert positions[1, 0] > 0 + assert abs(positions[1, 1]) < 1e-9 + + +# ── Non-uniform configuration (5 anchors) ───────────────────────────────── + +GT_5 = np.array([ + [0.0, 0.0], + [5.0, 0.0], + [5.0, 4.0], + [2.5, 6.0], + [0.0, 4.0], +]) + + +class TestMds5Anchors: + def test_residual_near_zero(self): + D = _dist_matrix(GT_5) + positions, rms = mds_2d(D) + assert rms < 1e-6 + + def test_shape(self): + D = _dist_matrix(GT_5) + positions, _ = mds_2d(D) + assert positions.shape == (5, 2) + + +# ── Noisy distances (simulate UWB measurement noise) ────────────────────── + +class TestMdsNoisyInput: + def test_residual_acceptable_with_5cm_noise(self): + rng = np.random.default_rng(42) + D = _dist_matrix(GT_SQUARE) + noise = rng.normal(0, 0.05, D.shape) + noise = (noise + noise.T) / 2 # keep symmetric + np.fill_diagonal(noise, 0) + D_noisy = np.maximum(D + noise, 0) + positions, rms = mds_2d(D_noisy) + # With 5 cm noise the residual should be sub-10 cm + assert rms < 0.10, f"Residual {rms:.4f} m too large" + + +# ── Error handling ───────────────────────────────────────────────────────── + +class TestMdsErrors: + def test_non_square_raises(self): + with pytest.raises(ValueError): + mds_2d(np.ones((3, 4))) + + def test_too_few_anchors_raises(self): + with pytest.raises(ValueError): + mds_2d(np.array([[0.0, 1.0], [1.0, 0.0]])) + + def test_asymmetric_raises(self): + D = _dist_matrix(GT_SQUARE) + D[0, 1] += 0.5 # break symmetry + with pytest.raises(ValueError): + mds_2d(D) + + +# ── anchor_frame_align ───────────────────────────────────────────────────── + +class TestFrameAlign: + def test_translated_positions_aligned(self): + pts = GT_SQUARE.copy() + np.array([10.0, -5.0]) + aligned = anchor_frame_align(pts) + assert abs(aligned[0, 0]) < 1e-9 + assert abs(aligned[0, 1]) < 1e-9 + + def test_rotated_positions_aligned(self): + angle = math.pi / 3 + R = np.array([[math.cos(angle), -math.sin(angle)], + [math.sin(angle), math.cos(angle)]]) + pts = (R @ GT_SQUARE.T).T + aligned = anchor_frame_align(pts) + assert aligned[1, 0] > 0 + assert abs(aligned[1, 1]) < 1e-9 + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/CMakeLists.txt new file mode 100644 index 0000000..1799bbc --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project(saltybot_uwb_calibration_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/CalibrateAnchors.srv" +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/package.xml b/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/package.xml new file mode 100644 index 0000000..103dc9b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/package.xml @@ -0,0 +1,23 @@ + + + + saltybot_uwb_calibration_msgs + 0.1.0 + + Service definitions for UWB anchor auto-calibration (Issue #602). + Provides CalibrateAnchors.srv used by saltybot_uwb_calibration node. + + sl-uwb + Apache-2.0 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/srv/CalibrateAnchors.srv b/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/srv/CalibrateAnchors.srv new file mode 100644 index 0000000..4b004fd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/srv/CalibrateAnchors.srv @@ -0,0 +1,29 @@ +# CalibrateAnchors.srv — UWB anchor auto-calibration service (Issue #602) +# +# Request: list of anchor IDs to calibrate (minimum 4). +# If anchor_ids is empty, the node uses all discovered anchors. +# +# Each anchor is commanded via AT+PEER_RANGE= to range against every +# other anchor. The resulting N×N distance matrix is passed through classical +# MDS to recover 2-D positions. Results are written to anchor_positions_json. +# +# Response: success flag, per-anchor [x, y, z] positions, residual RMS error, +# and JSON string suitable for dropping into fusion_params.yaml. + +# ── Request ──────────────────────────────────────────────────────────────── +uint8[] anchor_ids # IDs of anchors to calibrate (empty = auto-discover) +uint8 n_samples # range measurements to average per pair (default 5) + +--- + +# ── Response ─────────────────────────────────────────────────────────────── +bool success +string message # human-readable status / error description + +uint8[] anchor_ids # same order as positions below +float64[] positions_x # metres, MDS-recovered X (2-D plane) +float64[] positions_y # metres, MDS-recovered Y +float64[] positions_z # metres, always 0.0 (flat-floor assumption) + +float64 residual_rms_m # RMS of |D_measured - D_recovered| across all pairs +string anchor_positions_json # JSON: {"0": [x,y,z], "1": [x,y,z], ...} -- 2.47.2 From 5f03e4cbeffbfe439215bc15de4cd9995016c944 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sat, 14 Mar 2026 15:04:58 -0400 Subject: [PATCH 2/3] feat: Tilt compensation for slopes (Issue #600) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds a slow-adapting terrain slope estimator (IIR tau=5s) that decouples the robot's balance offset from genuine ground incline. The balance controller subtracts the slope estimate from measured pitch so the PID balances around the slope surface rather than absolute vertical. - include/slope_estimator.h + src/slope_estimator.c: first-order IIR filter clamped to ±15°; JLINK_TLM_SLOPE (0x88) telemetry at 1 Hz - include/jlink.h + src/jlink.c: add JLINK_TLM_SLOPE (0x88), jlink_tlm_slope_t (4 bytes), jlink_send_slope_tlm() - include/balance.h + src/balance.c: integrate slope_estimator into balance_t; update, reset on tilt-fault and disarm - test/test_slope_estimator.c: 35 unit tests, all passing Co-Authored-By: Claude Sonnet 4.6 --- include/balance.h | 4 + include/jlink.h | 16 ++ include/slope_estimator.h | 101 +++++++++ src/balance.c | 14 +- src/jlink.c | 24 ++ src/slope_estimator.c | 82 +++++++ test/test_slope_estimator.c | 421 ++++++++++++++++++++++++++++++++++++ 7 files changed, 660 insertions(+), 2 deletions(-) create mode 100644 include/slope_estimator.h create mode 100644 src/slope_estimator.c create mode 100644 test/test_slope_estimator.c diff --git a/include/balance.h b/include/balance.h index 3f6a81e..073ea3d 100644 --- a/include/balance.h +++ b/include/balance.h @@ -3,6 +3,7 @@ #include #include "mpu6000.h" +#include "slope_estimator.h" /* * SaltyLab Balance Controller @@ -36,6 +37,9 @@ typedef struct { /* Safety */ float max_tilt; /* Cutoff angle (degrees) */ int16_t max_speed; /* Speed limit */ + + /* Slope compensation (Issue #600) */ + slope_estimator_t slope; } balance_t; void balance_init(balance_t *b); diff --git a/include/jlink.h b/include/jlink.h index ab8dedf..ef6cbfd 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -87,6 +87,7 @@ #define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */ #define JLINK_TLM_MOTOR_CURRENT 0x86u /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */ #define JLINK_TLM_FAULT_LOG 0x87u /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */ +#define JLINK_TLM_SLOPE 0x88u /* jlink_tlm_slope_t (4 bytes, Issue #600) */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ typedef struct __attribute__((packed)) { @@ -164,6 +165,14 @@ typedef struct __attribute__((packed)) { uint8_t _pad; /* reserved */ } jlink_tlm_motor_current_t; /* 8 bytes */ +/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */ +/* Published at SLOPE_TLM_HZ (1 Hz); reports slow-adapting terrain slope estimate. */ +typedef struct __attribute__((packed)) { + int16_t slope_x100; /* slope estimate degrees x100 (0.01° resolution) */ + uint8_t active; /* 1 = slope compensation enabled */ + uint8_t _pad; /* reserved */ +} jlink_tlm_slope_t; /* 4 bytes */ + /* ---- Telemetry FAULT_LOG payload (20 bytes, packed) Issue #565 ---- */ /* Sent on boot (if last fault != NONE) and in response to FAULT_LOG_GET. */ typedef struct __attribute__((packed)) { @@ -272,6 +281,13 @@ JLinkSchedSetBuf *jlink_get_sched_set(void); */ void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm); +/* + * jlink_send_slope_tlm(tlm) - transmit JLINK_TLM_SLOPE (0x88) frame + * (10 bytes total) to Jetson. Issue #600. + * Rate-limiting is handled by slope_estimator_send_tlm(); call from there only. + */ +void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm); + /* * jlink_send_fault_log(fl) - transmit JLINK_TLM_FAULT_LOG (0x87) frame * (26 bytes) on boot (if fault log non-empty) and in response to diff --git a/include/slope_estimator.h b/include/slope_estimator.h new file mode 100644 index 0000000..3141c86 --- /dev/null +++ b/include/slope_estimator.h @@ -0,0 +1,101 @@ +#ifndef SLOPE_ESTIMATOR_H +#define SLOPE_ESTIMATOR_H + +#include +#include + +/* + * slope_estimator — slow-adapting terrain slope estimator for Issue #600. + * + * On a slope the robot must lean slightly into the hill to stay balanced. + * The IMU pitch reading therefore includes both the robot's balance offset + * and the ground incline. This module decouples the two by tracking the + * slowly-changing DC component of the pitch signal with a first-order IIR + * low-pass filter (time constant SLOPE_TAU_S = 5 s). + * + * HOW IT WORKS: + * Every call to slope_estimator_update(pitch_deg, dt): + * + * alpha = dt / (SLOPE_TAU_S + dt) // ≈ 0.0002 at 1 kHz + * raw = slope * (1 - alpha) + pitch * alpha + * slope = clamp(raw, -SLOPE_MAX_DEG, +SLOPE_MAX_DEG) + * + * The IIR converges to the steady-state pitch in ~5 s. Fast tilt + * transients (balance corrections, steps, bumps) are attenuated by + * the long time constant and do not corrupt the estimate. + * + * INTEGRATION IN BALANCE PID: + * Subtract slope_estimate from the measured pitch before computing + * the PID error so the controller balances around the slope surface + * rather than absolute vertical: + * + * tilt_corrected = pitch_deg - slope_estimate_deg + * error = setpoint - tilt_corrected + * + * This is equivalent to continuously adjusting the balance setpoint + * to track the incline. + * + * SAFETY: + * - Estimate is clamped to ±SLOPE_MAX_DEG (15°) to prevent drift from + * extreme falls being mistaken for genuine slopes. + * - slope_estimator_reset() zeroes the state; call on disarm or after + * a tilt fault so re-arming starts fresh. + * + * TELEMETRY: + * JLINK_TLM_SLOPE (0x88) published at SLOPE_TLM_HZ (1 Hz): + * jlink_tlm_slope_t { int16 slope_x100, uint8 active, uint8 _pad } + */ + +/* ---- Configuration ---- */ +#define SLOPE_TAU_S 5.0f /* IIR time constant (seconds) */ +#define SLOPE_MAX_DEG 15.0f /* Maximum estimate magnitude (degrees) */ +#define SLOPE_TLM_HZ 1u /* JLINK_TLM_SLOPE publish rate (Hz) */ + +/* ---- State ---- */ +typedef struct { + float estimate_deg; /* current slope estimate (degrees, + = nose-up) */ + bool enabled; /* compensation on/off; off = estimate frozen */ + uint32_t last_tlm_ms; /* timestamp of last TLM transmission */ +} slope_estimator_t; + +/* ---- API ---- */ + +/* + * slope_estimator_init(se) — zero state, enable estimation. + * Call once during system init. + */ +void slope_estimator_init(slope_estimator_t *se); + +/* + * slope_estimator_reset(se) — zero estimate without changing enabled flag. + * Call on disarm or after BALANCE_TILT_FAULT to avoid stale state on rearm. + */ +void slope_estimator_reset(slope_estimator_t *se); + +/* + * slope_estimator_update(se, pitch_deg, dt) — advance the IIR filter. + * pitch_deg : current fused pitch angle from IMU (degrees) + * dt : loop interval (seconds) + * No-op if se->enabled == false or dt <= 0. + */ +void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt); + +/* + * slope_estimator_get_deg(se) — return current estimate (degrees). + * Returns 0 if disabled. + */ +float slope_estimator_get_deg(const slope_estimator_t *se); + +/* + * slope_estimator_set_enabled(se, en) — enable or disable compensation. + * Disabling freezes the estimate at its current value. + */ +void slope_estimator_set_enabled(slope_estimator_t *se, bool en); + +/* + * slope_estimator_send_tlm(se, now_ms) — transmit JLINK_TLM_SLOPE (0x88) + * frame to Jetson. Rate-limited to SLOPE_TLM_HZ; safe to call every tick. + */ +void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms); + +#endif /* SLOPE_ESTIMATOR_H */ diff --git a/src/balance.c b/src/balance.c index 4cb9ff7..d6382fb 100644 --- a/src/balance.c +++ b/src/balance.c @@ -1,4 +1,5 @@ #include "balance.h" +#include "slope_estimator.h" #include "config.h" #include @@ -19,6 +20,9 @@ void balance_init(balance_t *b) { /* Safety defaults from config */ b->max_tilt = MAX_TILT_DEG; b->max_speed = MAX_SPEED_LIMIT; + + /* Slope estimator */ + slope_estimator_init(&b->slope); } void balance_update(balance_t *b, const IMUData *imu, float dt) { @@ -28,12 +32,16 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) { b->pitch_deg = imu->pitch; b->pitch_rate = imu->pitch_rate; + /* Advance slope estimator (no-op when not armed) */ + slope_estimator_update(&b->slope, b->pitch_deg, dt); + /* Safety: tilt cutoff */ if (b->state == BALANCE_ARMED) { if (fabsf(b->pitch_deg) > b->max_tilt) { b->state = BALANCE_TILT_FAULT; b->motor_cmd = 0; b->integral = 0.0f; + slope_estimator_reset(&b->slope); return; } } @@ -45,8 +53,9 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) { return; } - /* PID */ - float error = b->setpoint - b->pitch_deg; + /* PID — subtract slope estimate so controller balances on incline */ + float tilt_corrected = b->pitch_deg - slope_estimator_get_deg(&b->slope); + float error = b->setpoint - tilt_corrected; /* Proportional */ float p_term = b->kp * error; @@ -85,4 +94,5 @@ void balance_disarm(balance_t *b) { b->state = BALANCE_DISARMED; b->motor_cmd = 0; b->integral = 0.0f; + slope_estimator_reset(&b->slope); } diff --git a/src/jlink.c b/src/jlink.c index fba8f08..bd6345f 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -541,6 +541,30 @@ void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm) jlink_tx_locked(frame, (uint16_t)(3u + plen + 3u)); } +/* ---- jlink_send_slope_tlm() -- Issue #600 ---- */ +void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm) +{ + /* + * Frame: [STX][LEN][0x88][4 bytes SLOPE][CRC_hi][CRC_lo][ETX] + * Total: 1+1+1+4+2+1 = 10 bytes + */ + static uint8_t frame[10]; + const uint8_t plen = (uint8_t)sizeof(jlink_tlm_slope_t); /* 4 */ + const uint8_t len = 1u + plen; /* CMD byte + payload */ + + frame[0] = JLINK_STX; + frame[1] = len; + frame[2] = JLINK_TLM_SLOPE; + memcpy(&frame[3], tlm, plen); + + uint16_t crc = crc16_xmodem(&frame[2], len); + frame[3 + plen] = (uint8_t)(crc >> 8); + frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); + frame[3 + plen + 2] = JLINK_ETX; + + jlink_tx_locked(frame, sizeof(frame)); +} + /* ---- jlink_send_fault_log() -- Issue #565 ---- */ void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl) { diff --git a/src/slope_estimator.c b/src/slope_estimator.c new file mode 100644 index 0000000..959869f --- /dev/null +++ b/src/slope_estimator.c @@ -0,0 +1,82 @@ +/* + * slope_estimator.c — slow-adapting terrain slope estimator (Issue #600). + * + * First-order IIR low-pass filter on fused IMU pitch with tau = SLOPE_TAU_S (5 s). + * The long time constant means fast balance corrections and transients are + * ignored; only sustained pitch bias (i.e., genuine slope) accumulates. + * + * Estimate is clamped to ±SLOPE_MAX_DEG (15°) per the safety requirement. + */ + +#include "slope_estimator.h" +#include "jlink.h" + +/* ---- slope_estimator_init() ---- */ +void slope_estimator_init(slope_estimator_t *se) +{ + se->estimate_deg = 0.0f; + se->enabled = true; + /* Initialize so first send_tlm call fires immediately */ + se->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (SLOPE_TLM_HZ > 0u ? SLOPE_TLM_HZ : 1u))); +} + +/* ---- slope_estimator_reset() ---- */ +void slope_estimator_reset(slope_estimator_t *se) +{ + se->estimate_deg = 0.0f; +} + +/* ---- slope_estimator_update() ---- */ +void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt) +{ + if (!se->enabled || dt <= 0.0f) return; + + /* + * First-order IIR: alpha = dt / (tau + dt) + * At 1 kHz (dt=0.001): alpha ≈ 0.0002 → time to ~63% ≈ 5 s + * At 100 Hz (dt=0.01): alpha ≈ 0.002 → time to ~63% ≈ 5 s (same tau) + */ + float alpha = dt / (SLOPE_TAU_S + dt); + float raw = se->estimate_deg * (1.0f - alpha) + pitch_deg * alpha; + + /* Clamp to ±SLOPE_MAX_DEG */ + if (raw > SLOPE_MAX_DEG) raw = SLOPE_MAX_DEG; + if (raw < -SLOPE_MAX_DEG) raw = -SLOPE_MAX_DEG; + + se->estimate_deg = raw; +} + +/* ---- slope_estimator_get_deg() ---- */ +float slope_estimator_get_deg(const slope_estimator_t *se) +{ + if (!se->enabled) return 0.0f; + return se->estimate_deg; +} + +/* ---- slope_estimator_set_enabled() ---- */ +void slope_estimator_set_enabled(slope_estimator_t *se, bool en) +{ + se->enabled = en; +} + +/* ---- slope_estimator_send_tlm() ---- */ +void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms) +{ + if (SLOPE_TLM_HZ == 0u) return; + + uint32_t interval_ms = 1000u / SLOPE_TLM_HZ; + if ((now_ms - se->last_tlm_ms) < interval_ms) return; + /* Cast away const for timestamp update -- only mutable field */ + ((slope_estimator_t *)se)->last_tlm_ms = now_ms; + + jlink_tlm_slope_t tlm; + /* Scale to ×100 for 0.01° resolution, clamped to int16 range */ + float scaled = se->estimate_deg * 100.0f; + if (scaled > 32767.0f) scaled = 32767.0f; + if (scaled < -32768.0f) scaled = -32768.0f; + tlm.slope_x100 = (int16_t)scaled; + tlm.active = se->enabled ? 1u : 0u; + tlm._pad = 0u; + + jlink_send_slope_tlm(&tlm); +} diff --git a/test/test_slope_estimator.c b/test/test_slope_estimator.c new file mode 100644 index 0000000..b181b66 --- /dev/null +++ b/test/test_slope_estimator.c @@ -0,0 +1,421 @@ +/* + * test_slope_estimator.c — Unit tests for slope estimator (Issue #600). + * + * Build (host, no hardware): + * gcc -I include -I test/stubs -DTEST_HOST -lm \ + * -o /tmp/test_slope src/slope_estimator.c test/test_slope_estimator.c + * + * All tests are self-contained; no HAL, no ADC, no UART required. + * slope_estimator.c calls jlink_send_slope_tlm() which is stubbed below. + */ + +/* ---- Stubs ---- */ + +/* Prevent jlink.h from pulling in HAL / pid_flash types */ +#define JLINK_H +#include +#include + +/* Minimal jlink_tlm_slope_t matching the real definition */ +typedef struct __attribute__((packed)) { + int16_t slope_x100; + uint8_t active; + uint8_t _pad; +} jlink_tlm_slope_t; + +/* Capture last transmitted tlm for inspection */ +static jlink_tlm_slope_t g_last_tlm; +static int g_tlm_count = 0; + +void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm) +{ + g_last_tlm = *tlm; + g_tlm_count++; +} + +/* ---- Include implementation directly ---- */ +#include "../src/slope_estimator.c" + +/* ---- Test framework ---- */ +#include +#include +#include + +static int g_pass = 0; +static int g_fail = 0; + +#define ASSERT(cond, msg) do { \ + if (cond) { g_pass++; } \ + else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \ +} while(0) + +#define ASSERT_NEAR(a, b, tol, msg) \ + ASSERT(fabsf((a)-(b)) <= (tol), msg) + +/* ---- Tests ---- */ + +static void test_init_zeroes_state(void) +{ + slope_estimator_t se; + /* dirty the memory first */ + memset(&se, 0xAB, sizeof(se)); + slope_estimator_init(&se); + ASSERT(se.estimate_deg == 0.0f, "init: estimate_deg == 0"); + ASSERT(se.enabled == true, "init: enabled == true"); +} + +static void test_get_when_disabled_returns_zero(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + /* Force a non-zero estimate by running a few ticks, then disable */ + slope_estimator_update(&se, 10.0f, 0.1f); + slope_estimator_set_enabled(&se, false); + ASSERT(slope_estimator_get_deg(&se) == 0.0f, + "get while disabled must return 0"); +} + +static void test_update_when_disabled_no_change(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + slope_estimator_set_enabled(&se, false); + slope_estimator_update(&se, 10.0f, 0.01f); + ASSERT(se.estimate_deg == 0.0f, + "update while disabled must not change estimate"); +} + +static void test_update_zero_dt_no_change(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + slope_estimator_update(&se, 5.0f, 0.0f); + ASSERT(se.estimate_deg == 0.0f, + "update with dt=0 must not change estimate"); +} + +static void test_update_negative_dt_no_change(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + slope_estimator_update(&se, 5.0f, -0.001f); + ASSERT(se.estimate_deg == 0.0f, + "update with dt<0 must not change estimate"); +} + +static void test_single_step_converges_toward_input(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + /* One update with dt=0.01s, pitch=10 deg */ + slope_estimator_update(&se, 10.0f, 0.01f); + /* alpha = 0.01 / (5.0 + 0.01) ≈ 0.002; new estimate ≈ 0 + 0.002*10 = 0.02 */ + ASSERT(se.estimate_deg > 0.0f, + "estimate should move toward positive pitch"); + ASSERT(se.estimate_deg < 10.0f, + "estimate should not jump to full pitch in one step"); +} + +static void test_iir_reaches_63pct_in_one_tau(void) +{ + /* Run estimator at 1 kHz for exactly SLOPE_TAU_S seconds */ + slope_estimator_t se; + slope_estimator_init(&se); + const float pitch = 10.0f; + const float dt = 0.001f; + const int steps = (int)(SLOPE_TAU_S / dt); /* 5000 steps */ + for (int i = 0; i < steps; i++) { + slope_estimator_update(&se, pitch, dt); + } + /* IIR should be at ≈63.2% of the step */ + float expected_lo = pitch * 0.60f; + float expected_hi = pitch * 0.66f; + ASSERT(se.estimate_deg >= expected_lo && se.estimate_deg <= expected_hi, + "IIR should reach ~63% of step at t=tau"); +} + +static void test_iir_reaches_63pct_at_100hz(void) +{ + /* Same tau, but at 100 Hz */ + slope_estimator_t se; + slope_estimator_init(&se); + const float pitch = 8.0f; + const float dt = 0.01f; + const int steps = (int)(SLOPE_TAU_S / dt); /* 500 steps */ + for (int i = 0; i < steps; i++) { + slope_estimator_update(&se, pitch, dt); + } + float expected_lo = pitch * 0.60f; + float expected_hi = pitch * 0.66f; + ASSERT(se.estimate_deg >= expected_lo && se.estimate_deg <= expected_hi, + "IIR 100 Hz: should reach ~63% at t=tau"); +} + +static void test_positive_clamp(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + /* Drive with a huge pitch for a long time — estimate must clamp at SLOPE_MAX_DEG */ + for (int i = 0; i < 100000; i++) { + slope_estimator_update(&se, 90.0f, 0.001f); + } + ASSERT_NEAR(se.estimate_deg, SLOPE_MAX_DEG, 0.001f, + "estimate must clamp at +SLOPE_MAX_DEG"); +} + +static void test_negative_clamp(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + for (int i = 0; i < 100000; i++) { + slope_estimator_update(&se, -90.0f, 0.001f); + } + ASSERT_NEAR(se.estimate_deg, -SLOPE_MAX_DEG, 0.001f, + "estimate must clamp at -SLOPE_MAX_DEG"); +} + +static void test_reset_zeroes_estimate(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + /* Build up some estimate */ + for (int i = 0; i < 1000; i++) { + slope_estimator_update(&se, 12.0f, 0.001f); + } + ASSERT(se.estimate_deg > 0.1f, "pre-reset: estimate should be non-zero"); + slope_estimator_reset(&se); + ASSERT(se.estimate_deg == 0.0f, "post-reset: estimate should be zero"); + /* enabled state must be preserved */ + ASSERT(se.enabled == true, "reset must not change enabled flag"); +} + +static void test_reset_preserves_disabled_state(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + slope_estimator_set_enabled(&se, false); + slope_estimator_reset(&se); + ASSERT(se.enabled == false, "reset must not re-enable disabled estimator"); +} + +static void test_balance_setpoint_compensation(void) +{ + /* + * Simulate: robot on 5-deg slope, pitch = 5 deg, slope estimate converges. + * After convergence, tilt_corrected = pitch - slope ≈ 0. + * Verify that the effective PID error (setpoint=0, tilt_corrected≈0) is near 0. + */ + slope_estimator_t se; + slope_estimator_init(&se); + const float slope_deg = 5.0f; + const float dt = 0.001f; + const int steps = (int)(3.0f * SLOPE_TAU_S / dt); /* 3 tau = ~95% converged */ + + for (int i = 0; i < steps; i++) { + /* Robot holds steady on slope — pitch stays constant */ + slope_estimator_update(&se, slope_deg, dt); + } + + float est = slope_estimator_get_deg(&se); + float tilt_corrected = slope_deg - est; + + /* After 3 tau, estimate should be within 5% of slope */ + ASSERT(est >= slope_deg * 0.90f, "3-tau estimate >= 90% of slope"); + /* tilt_corrected should be close to zero — minimal PID error */ + ASSERT(fabsf(tilt_corrected) < slope_deg * 0.10f, + "corrected tilt error < 10% of slope after 3 tau"); +} + +static void test_fast_tilt_not_tracked(void) +{ + /* + * Simulate: robot starts balanced (pitch≈0), then tips to 20 deg suddenly + * for 100 ms (balance intervention), then returns to 0. + * The slope estimate should not move significantly. + */ + slope_estimator_t se; + slope_estimator_init(&se); + const float dt = 0.001f; + + /* 2 s of stable balancing at pitch ≈ 0 */ + for (int i = 0; i < 2000; i++) { + slope_estimator_update(&se, 0.0f, dt); + } + float before = se.estimate_deg; + + /* 100 ms spike to 20 deg (fast tilt) */ + for (int i = 0; i < 100; i++) { + slope_estimator_update(&se, 20.0f, dt); + } + /* Return to 0 for another 100 ms */ + for (int i = 0; i < 100; i++) { + slope_estimator_update(&se, 0.0f, dt); + } + float after = se.estimate_deg; + + /* 200ms / 5000ms = 4% of tau — estimate should move < 1 deg */ + ASSERT(fabsf(after - before) < 1.0f, + "fast tilt transient should not significantly move slope estimate"); +} + +static void test_slope_change_tracks_slowly(void) +{ + /* Robot transitions from flat (0°) to 10° slope at t=0. + * After 1 tau, estimate should be ~63% of 10 = ~6.3 deg. */ + slope_estimator_t se; + slope_estimator_init(&se); + const float dt = 0.001f; + const int steps = (int)(SLOPE_TAU_S / dt); + + for (int i = 0; i < steps; i++) { + slope_estimator_update(&se, 10.0f, dt); + } + ASSERT(se.estimate_deg >= 6.0f && se.estimate_deg <= 6.5f, + "slope change: 1-tau estimate should be 6.0-6.5 deg"); +} + +static void test_symmetry_negative_slope(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + const float dt = 0.001f; + const int steps = (int)(SLOPE_TAU_S / dt); + + for (int i = 0; i < steps; i++) { + slope_estimator_update(&se, -8.0f, dt); + } + /* Should be ~63% of -8 */ + ASSERT(se.estimate_deg <= -4.0f && se.estimate_deg >= -5.5f, + "negative slope: estimate should converge symmetrically"); +} + +static void test_enable_disable_toggle(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + + /* Build estimate */ + for (int i = 0; i < 2000; i++) { + slope_estimator_update(&se, 10.0f, 0.001f); + } + float val = se.estimate_deg; + ASSERT(val > 0.1f, "estimate built before disable"); + + /* Disable: estimate frozen */ + slope_estimator_set_enabled(&se, false); + slope_estimator_update(&se, 10.0f, 0.001f); + ASSERT(se.estimate_deg == val, "estimate frozen while disabled"); + + /* get returns 0 when disabled */ + ASSERT(slope_estimator_get_deg(&se) == 0.0f, "get returns 0 while disabled"); + + /* Re-enable: estimate resumes */ + slope_estimator_set_enabled(&se, true); + slope_estimator_update(&se, 10.0f, 0.001f); + ASSERT(se.estimate_deg > val, "estimate resumes after re-enable"); +} + +static void test_tlm_rate_limiting(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + + g_tlm_count = 0; + + /* Call at 1000 Hz for 3 seconds → should send 3 TLMs (at 1 Hz rate limit) */ + for (uint32_t ms = 0; ms < 3000; ms++) { + slope_estimator_send_tlm(&se, ms); + } + /* First call at ms=0 sends, then 1000ms, 2000ms → 3 total */ + ASSERT(g_tlm_count == 3, "TLM rate-limited to SLOPE_TLM_HZ (1 Hz)"); +} + +static void test_tlm_payload_encoding(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + + /* Force a known estimate */ + se.estimate_deg = 7.5f; + se.enabled = true; + + g_tlm_count = 0; + g_last_tlm.slope_x100 = 0; + + /* Trigger a TLM by advancing time enough */ + slope_estimator_send_tlm(&se, 0u); + + ASSERT(g_tlm_count == 1, "TLM sent"); + ASSERT(g_last_tlm.slope_x100 == 750, "7.5 deg -> slope_x100 = 750"); + ASSERT(g_last_tlm.active == 1u, "active flag set when enabled"); +} + +static void test_tlm_disabled_active_flag(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + slope_estimator_set_enabled(&se, false); + se.estimate_deg = 3.0f; + + g_tlm_count = 0; + slope_estimator_send_tlm(&se, 0u); + + ASSERT(g_tlm_count == 1, "TLM sent when disabled"); + ASSERT(g_last_tlm.active == 0u, "active flag clear when disabled"); + /* slope_x100 reflects stored estimate but get() returns 0 */ + ASSERT(g_last_tlm.slope_x100 == 300, "slope_x100 encodes stored estimate"); +} + +static void test_tlm_clamped_to_int16(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + /* Manually set beyond int16 range for test */ + se.estimate_deg = 400.0f; /* 400 * 100 = 40000, within int16 */ + se.enabled = true; + g_tlm_count = 0; + slope_estimator_send_tlm(&se, 0u); + ASSERT(g_last_tlm.slope_x100 == 32767, "large estimate clamped to INT16_MAX"); +} + +static void test_multiple_resets_idempotent(void) +{ + slope_estimator_t se; + slope_estimator_init(&se); + slope_estimator_reset(&se); + slope_estimator_reset(&se); + ASSERT(se.estimate_deg == 0.0f, "multiple resets: still zero"); + ASSERT(se.enabled == true, "multiple resets: still enabled"); +} + +/* ---- main ---- */ +int main(void) +{ + printf("=== test_slope_estimator ===\n"); + + test_init_zeroes_state(); + test_get_when_disabled_returns_zero(); + test_update_when_disabled_no_change(); + test_update_zero_dt_no_change(); + test_update_negative_dt_no_change(); + test_single_step_converges_toward_input(); + test_iir_reaches_63pct_in_one_tau(); + test_iir_reaches_63pct_at_100hz(); + test_positive_clamp(); + test_negative_clamp(); + test_reset_zeroes_estimate(); + test_reset_preserves_disabled_state(); + test_balance_setpoint_compensation(); + test_fast_tilt_not_tracked(); + test_slope_change_tracks_slowly(); + test_symmetry_negative_slope(); + test_enable_disable_toggle(); + test_tlm_rate_limiting(); + test_tlm_payload_encoding(); + test_tlm_disabled_active_flag(); + test_tlm_clamped_to_int16(); + test_multiple_resets_idempotent(); + + printf("\nResults: %d passed, %d failed\n", g_pass, g_fail); + return g_fail ? 1 : 0; +} -- 2.47.2 From 82cc223fb80947f319e5895846a579c6e9229388 Mon Sep 17 00:00:00 2001 From: sl-uwb Date: Sat, 14 Mar 2026 15:06:13 -0400 Subject: [PATCH 3/3] feat: Add AT+PEER_RANGE= command for inter-anchor calibration (Issue #602) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - peer_range_once(): DS-TWR initiator role toward a peer anchor (POLL → RESP → FINAL, one-sided range estimate Ra - Da/2) - AT+PEER_RANGE=: returns +PEER_RANGE:,,, or +PEER_RANGE:ERR,,TIMEOUT Co-Authored-By: Claude Sonnet 4.6 --- esp32/uwb_anchor/src/main.cpp | 129 ++++++++++++++++++++++++++++++++++ 1 file changed, 129 insertions(+) diff --git a/esp32/uwb_anchor/src/main.cpp b/esp32/uwb_anchor/src/main.cpp index e022ee2..1f8c5df 100644 --- a/esp32/uwb_anchor/src/main.cpp +++ b/esp32/uwb_anchor/src/main.cpp @@ -27,6 +27,8 @@ * AT+RANGE_ADDR= → pair with specific tag (filter others) * AT+RANGE_ADDR= → clear pairing (accept all tags) * AT+ID? → returns +ID: + * AT+PEER_RANGE= → inter-anchor DS-TWR (for auto-calibration) + * → +PEER_RANGE:,,, * * Pin mapping — Makerfabs ESP32 UWB Pro * ────────────────────────────────────── @@ -188,6 +190,116 @@ static float rx_power_dbm(void) { return 10.0f * log10f((f * f) / (n * n)) - 121.74f; } +/* ── Peer-anchor ranging (initiator role, for auto-calibration) ─── */ + +/* Timeout waiting for peer's RESP during inter-anchor ranging */ +#define PEER_RX_RESP_TIMEOUT_US 3500 +/* Block up to this many ms waiting for the interrupt flags */ +#define PEER_WAIT_MS 20 + +/* Initiate a single DS-TWR exchange toward peer anchor `peer_id`. + * Returns range in mm (>=0) on success, or -1 on timeout/error. + * RSSI is stored in *rssi_out if non-null. + * + * Exchange: + * This anchor → peer POLL + * peer → This RESP (carries T_poll_rx, T_resp_tx) + * This anchor → peer FINAL (carries Ra, Da) + * This side computes its own range estimate from Ra/Da. + */ +static int32_t peer_range_once(uint8_t peer_id, float *rssi_out) { + /* ── Reset interrupt flags ── */ + g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false; + + /* ── Build POLL frame ── */ + uint8_t poll_buf[FRAME_HDR + FCS_LEN] = { + FTYPE_POLL, + (uint8_t)ANCHOR_ID, + peer_id, + }; + + dwt_writetxdata(sizeof(poll_buf), poll_buf, 0); + dwt_writetxfctrl(sizeof(poll_buf), 0, 1); + dwt_setrxtimeout(PEER_RX_RESP_TIMEOUT_US); + dwt_rxenable(DWT_START_RX_IMMEDIATE); + if (dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) + return -1; + + /* Wait for TX done */ + uint32_t t0 = millis(); + while (!g_tx_done && !g_rx_err && !g_rx_to) { + if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1; + } + if (g_rx_err || g_rx_to) return -1; + g_tx_done = false; + + /* Capture T_poll_tx */ + uint64_t T_poll_tx; + dwt_readtxtimestamp((uint8_t *)&T_poll_tx); + T_poll_tx &= DWT_TS_MASK; + + /* Wait for RESP */ + t0 = millis(); + while (!g_rx_ok && !g_rx_err && !g_rx_to) { + if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1; + } + if (!g_rx_ok) return -1; + + /* Validate RESP */ + if (g_rx_len < (uint32_t)(FRAME_HDR + RESP_PAYLOAD + FCS_LEN)) return -1; + if (g_rx_buf[0] != FTYPE_RESP) return -1; + if (g_rx_buf[1] != peer_id) return -1; + if (g_rx_buf[2] != (uint8_t)ANCHOR_ID) return -1; + + uint64_t T_resp_rx; + dwt_readrxtimestamp((uint8_t *)&T_resp_rx); + T_resp_rx &= DWT_TS_MASK; + + /* Extract peer timestamps from RESP payload */ + const uint8_t *pl = g_rx_buf + FRAME_HDR; + uint64_t T_poll_rx_peer = ts_read(pl); + uint64_t T_resp_tx_peer = ts_read(pl + 5); + + /* DS-TWR Ra, Da */ + uint64_t Ra = ts_diff(T_resp_rx, T_poll_tx); + uint64_t Da = ts_diff(T_resp_tx_peer, T_poll_rx_peer); + + g_rx_ok = g_rx_err = g_rx_to = false; + + /* ── Build FINAL frame ── */ + uint8_t final_buf[FRAME_HDR + FINAL_PAYLOAD + FCS_LEN]; + final_buf[0] = FTYPE_FINAL; + final_buf[1] = (uint8_t)ANCHOR_ID; + final_buf[2] = peer_id; + ts_write(final_buf + FRAME_HDR, Ra); + ts_write(final_buf + FRAME_HDR + 5, Da); + ts_write(final_buf + FRAME_HDR + 10, (uint64_t)0); /* Db placeholder */ + + dwt_setrxtimeout(0); + dwt_writetxdata(sizeof(final_buf), final_buf, 0); + dwt_writetxfctrl(sizeof(final_buf), 0, 1); + if (dwt_starttx(DWT_START_TX_IMMEDIATE) != DWT_SUCCESS) return -1; + + t0 = millis(); + while (!g_tx_done && !g_rx_err) { + if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1; + } + g_tx_done = false; + + /* Simplified one-sided range estimate: tof = Ra - Da/2 */ + double tof_s = ticks_to_s(Ra) - ticks_to_s(Da) / 2.0; + if (tof_s < 0.0) tof_s = 0.0; + int32_t range_mm = (int32_t)(tof_s * SPEED_OF_LIGHT * 1000.0); + + if (rssi_out) *rssi_out = rx_power_dbm(); + + /* Re-enable normal RX for tag ranging */ + dwt_setrxtimeout(0); + dwt_rxenable(DWT_START_RX_IMMEDIATE); + + return range_mm; +} + /* ── AT command handler ─────────────────────────────────────────── */ static char g_at_buf[64]; @@ -212,6 +324,23 @@ static void at_dispatch(const char *cmd) { g_paired_tag_id = (uint8_t)strtoul(v, nullptr, 0); Serial.printf("+OK:PAIRED=0x%02X\r\n", g_paired_tag_id); } + + } else if (strncmp(cmd, "AT+PEER_RANGE=", 14) == 0) { + /* Inter-anchor ranging for calibration. + * Usage: AT+PEER_RANGE= + * Response: +PEER_RANGE:,,, + * or: +PEER_RANGE:ERR,,TIMEOUT + */ + uint8_t peer_id = (uint8_t)strtoul(cmd + 14, nullptr, 0); + float rssi = 0.0f; + int32_t mm = peer_range_once(peer_id, &rssi); + if (mm >= 0) { + Serial.printf("+PEER_RANGE:%d,%d,%ld,%.1f\r\n", + ANCHOR_ID, peer_id, mm, (double)rssi); + } else { + Serial.printf("+PEER_RANGE:ERR,%d,TIMEOUT\r\n", peer_id); + } + } else { Serial.println("+ERR:UNKNOWN_CMD"); } -- 2.47.2