diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 375df1f5e..2e0c42068 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -10,12 +10,19 @@ from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority
from openpilot.common.swaglog import cloudlog
from opendbc.car.car_helpers import interfaces
+from opendbc.car.gm.values import CAR as GM_CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
-from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.controls.lib.latcontrol_torque import (
+ BOLT_2018_2021_STEER_RATIO_TEST_SCALE,
+ BOLT_2017_STEER_RATIO_TEST_SCALE,
+ LatControlTorque,
+ bolt_2018_2021_lateral_testing_ground_active,
+ bolt_2017_lateral_testing_ground_active,
+)
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
@@ -92,6 +99,10 @@ class Controls:
lp = self.sm['liveParameters']
x = max(lp.stiffnessFactor, 0.1)
sr = max(lp.steerRatio, 0.1)
+ if self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2017 and bolt_2017_lateral_testing_ground_active():
+ sr *= BOLT_2017_STEER_RATIO_TEST_SCALE
+ elif self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2018_2021 and bolt_2018_2021_lateral_testing_ground_active():
+ sr *= BOLT_2018_2021_STEER_RATIO_TEST_SCALE
self.VM.update_params(x, sr)
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
index 0598df6df..11ff7081b 100644
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/selfdrive/controls/lib/latcontrol_torque.py
@@ -10,6 +10,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.pid import PIDController
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.starpilot.common.testing_grounds import testing_ground
# At higher speeds (25+mph) we can assume:
# Lateral acceleration achieved by a specific car correlates to
@@ -57,12 +58,68 @@ BOLT_2017_CARS = (
)
BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS
+BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3
+BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045
+BOLT_2017_TORQUE_SCALE_BP = [0.0, 0.2, 0.5, 1.0, 1.5, 2.5]
+BOLT_2017_TORQUE_SCALE_LEFT = [1.0, 1.0, 1.05, 1.04, 1.03, 1.02]
+BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.04, 1.03, 1.01, 1.0]
+
+BOLT_2018_2021_LATERAL_TESTING_GROUND_ID = testing_ground.id_4
+BOLT_2018_2021_STEER_RATIO_TEST_SCALE = 1.01
+BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.10
+BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.065
+BOLT_2018_2021_TORQUE_RISE = 0.24
+BOLT_2018_2021_TORQUE_FALL = 1.8
+BOLT_2018_2021_JERK_TAPER_CUTOFF = 0.55
+BOLT_2018_2021_FRICTION_MULT = 1.11
+BOLT_2018_2021_FRICTION_THRESHOLD_BUMP = 0.018
+BOLT_2018_2021_FRICTION_THRESHOLD_SPEED = 8.0
+
def get_friction_threshold(v_ego: float) -> float:
# Keep the speed-scaled friction threshold behavior.
return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27]))
+def bolt_2017_lateral_testing_ground_active() -> bool:
+ return testing_ground.use(BOLT_2017_LATERAL_TESTING_GROUND_ID)
+
+
+def get_bolt_2017_torque_scale(desired_lateral_accel: float) -> float:
+ if desired_lateral_accel == 0.0:
+ return 1.0
+
+ scale_values = BOLT_2017_TORQUE_SCALE_LEFT if desired_lateral_accel > 0.0 else BOLT_2017_TORQUE_SCALE_RIGHT
+ return float(np.interp(abs(desired_lateral_accel), BOLT_2017_TORQUE_SCALE_BP, scale_values))
+
+
+def bolt_2018_2021_lateral_testing_ground_active() -> bool:
+ return testing_ground.use(BOLT_2018_2021_LATERAL_TESTING_GROUND_ID)
+
+
+def get_bolt_2018_2021_torque_scale(desired_lateral_accel: float) -> float:
+ if desired_lateral_accel == 0.0:
+ return 1.0
+
+ gain = BOLT_2018_2021_TORQUE_GAIN_LEFT if desired_lateral_accel > 0.0 else BOLT_2018_2021_TORQUE_GAIN_RIGHT
+ abs_lateral_accel = abs(desired_lateral_accel)
+ mid_corner_bump = (1.0 - math.exp(-abs_lateral_accel / BOLT_2018_2021_TORQUE_RISE)) * math.exp(-abs_lateral_accel / BOLT_2018_2021_TORQUE_FALL)
+ return 1.0 + gain * mid_corner_bump
+
+
+def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
+ base_scale = get_bolt_2018_2021_torque_scale(desired_lateral_accel)
+ extra_scale = max(base_scale - 1.0, 0.0)
+ jerk_taper = 1.0 / (1.0 + (abs(desired_lateral_jerk) / BOLT_2018_2021_JERK_TAPER_CUTOFF) ** 2)
+ return 1.0 + (extra_scale * jerk_taper)
+
+
+def get_bolt_2018_2021_friction_threshold(v_ego: float) -> float:
+ base_threshold = get_friction_threshold(v_ego)
+ low_speed_bump = BOLT_2018_2021_FRICTION_THRESHOLD_BUMP / (1.0 + (max(v_ego, 0.0) / BOLT_2018_2021_FRICTION_THRESHOLD_SPEED) ** 2)
+ return base_threshold + low_speed_bump
+
+
class LatControlTorque(LatControl):
def __init__(self, CP, CI, dt):
super().__init__(CP, CI, dt)
@@ -94,6 +151,7 @@ class LatControlTorque(LatControl):
self.torque_ff_scale_neg = 1.0
self.torque_deadzone_boost = float(getattr(self.torque_params, "kfDEPRECATED", 0.0))
self.torque_ki_mult = 1.0
+ self.bolt_2018_2021_test_active = self.is_bolt_2018_2021 and bolt_2018_2021_lateral_testing_ground_active()
if self.is_bolt:
kp_scale = getattr(self.torque_params, "kp", getattr(self.torque_params, "kpDEPRECATED", 1.0))
@@ -169,7 +227,14 @@ class LatControlTorque(LatControl):
ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL],
[self.torque_ff_scale_neg, 1.0, self.torque_ff_scale_pos])
ff *= ff_scale
- ff += get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, get_friction_threshold(CS.vEgo), self.torque_params)
+ friction_threshold = get_friction_threshold(CS.vEgo)
+ if self.bolt_2018_2021_test_active:
+ friction_threshold = get_bolt_2018_2021_friction_threshold(CS.vEgo)
+ effective_friction = self.torque_params.as_builder()
+ effective_friction.friction *= BOLT_2018_2021_FRICTION_MULT
+ else:
+ effective_friction = self.torque_params
+ ff += get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, effective_friction)
deadzone_boost_active = False
if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL:
boost_scale = np.interp(abs(gravity_adjusted_future_lateral_accel), [0.0, DEADZONE_BOOST_LAT_ACCEL], [1.0, 0.0])
@@ -182,6 +247,10 @@ class LatControlTorque(LatControl):
CS.vEgo < self.low_speed_reset_threshold or unwind_detected)
output_lataccel = self.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
+ if self.is_bolt_2017 and bolt_2017_lateral_testing_ground_active():
+ output_torque *= get_bolt_2017_torque_scale(setpoint)
+ elif self.bolt_2018_2021_test_active:
+ output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk)
pid_log.active = True
pid_log.p = float(self.pid.p)
diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py
index eb82c54fd..be5ff16e7 100644
--- a/selfdrive/controls/tests/test_latcontrol.py
+++ b/selfdrive/controls/tests/test_latcontrol.py
@@ -1,7 +1,7 @@
from parameterized import parameterized
from types import SimpleNamespace
-from cereal import car, log
+from cereal import car, custom, log
from opendbc.car.car_helpers import interfaces
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.toyota.values import CAR as TOYOTA
@@ -9,19 +9,49 @@ from opendbc.car.nissan.values import CAR as NISSAN
from opendbc.car.gm.values import CAR as GM
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
-from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from openpilot.selfdrive.controls.lib.latcontrol_torque import (
+ LatControlTorque,
+ get_friction_threshold,
+ get_bolt_2017_torque_scale,
+ get_bolt_2018_2021_dynamic_torque_scale,
+ get_bolt_2018_2021_friction_threshold,
+ get_bolt_2018_2021_torque_scale,
+)
class TestLatControl:
+ def test_bolt_2017_testing_ground_scale_curve(self):
+ assert get_bolt_2017_torque_scale(0.1) == 1.0
+ assert get_bolt_2017_torque_scale(-0.1) == 1.0
+ assert get_bolt_2017_torque_scale(0.5) > get_bolt_2017_torque_scale(-0.5)
+ assert 1.0 < get_bolt_2017_torque_scale(1.2) < get_bolt_2017_torque_scale(0.5)
+ assert get_bolt_2017_torque_scale(-2.5) == 1.0
+
+ def test_bolt_2018_2021_testing_ground_scale_curve(self):
+ assert get_bolt_2018_2021_torque_scale(0.0) == 1.0
+ assert get_bolt_2018_2021_torque_scale(0.2) > get_bolt_2018_2021_torque_scale(0.08)
+ assert get_bolt_2018_2021_torque_scale(0.4) > get_bolt_2018_2021_torque_scale(-0.4)
+ assert get_bolt_2018_2021_torque_scale(2.0) < get_bolt_2018_2021_torque_scale(0.8)
+ assert get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.8) < get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.1)
+
+ def test_bolt_2018_2021_friction_threshold_curve(self):
+ low = get_bolt_2018_2021_friction_threshold(2.0)
+ mid = get_bolt_2018_2021_friction_threshold(10.0)
+ high = get_bolt_2018_2021_friction_threshold(30.0)
+ assert low > get_friction_threshold(2.0)
+ assert mid > get_friction_threshold(10.0)
+ assert high > get_friction_threshold(30.0)
+ assert (low - get_friction_threshold(2.0)) > (mid - get_friction_threshold(10.0)) > (high - get_friction_threshold(30.0))
+
@parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
(NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_ACC_2022_2023, LatControlTorque)])
def test_saturation(self, car_name, controller):
CarInterface = interfaces[car_name]
CP = CarInterface.get_non_essential_params(car_name)
- CI = CarInterface(CP)
+ CI = CarInterface(CP, custom.StarPilotCarParams.new_message())
VM = VehicleModel(CP)
controller = controller(CP.as_reader(), CI, DT_CTRL)
diff --git a/starpilot/common/testing_grounds.py b/starpilot/common/testing_grounds.py
index 8bc6cea13..bb3539985 100644
--- a/starpilot/common/testing_grounds.py
+++ b/starpilot/common/testing_grounds.py
@@ -46,17 +46,17 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = (
},
{
"id": TESTING_GROUND_3,
- "name": "Unused",
- "description": "",
- "aLabel": "A",
- "bLabel": "B",
+ "name": "Bolt 2017 Lat Tune",
+ "description": "2017 Bolt manual lateral A/B sandbox for steer-ratio and torque-curve testing.",
+ "aLabel": "A - Installed tune",
+ "bLabel": "B - 2017 lateral test",
},
{
"id": TESTING_GROUND_4,
- "name": "Unused",
- "description": "",
- "aLabel": "A",
- "bLabel": "B",
+ "name": "Bolt 18-21 Lat Tune",
+ "description": "Bolt 2018-2021 lateral torque A/B sandbox.",
+ "aLabel": "A - Installed tune",
+ "bLabel": "B - Bolt lateral test",
},
{
"id": TESTING_GROUND_5,
diff --git a/starpilot/system/the_pond/assets/components/tools/testing_ground.js b/starpilot/system/the_pond/assets/components/tools/testing_ground.js
index a6fd23586..46e9ba329 100644
--- a/starpilot/system/the_pond/assets/components/tools/testing_ground.js
+++ b/starpilot/system/the_pond/assets/components/tools/testing_ground.js
@@ -96,11 +96,11 @@ function isModeActive(mode) {
function getSelectedMode() {
const selectedSlot = getSelectedSlot()
- if (!selectedSlot) return "A"
+ if (!selectedSlot) return "Not active"
if (String(state.data?.activeSlot || "").trim() === String(state.selectedSlot || "").trim()) {
return String(state.data?.activeVariant || "").trim().toUpperCase() || getDefaultMode(selectedSlot)
}
- return getDefaultMode(selectedSlot)
+ return "Not active"
}
function modeButtonClass(mode) {
@@ -188,19 +188,8 @@ async function selectMode(mode) {
async function selectSlot(slotValue) {
const normalizedSlot = String(slotValue || "").trim()
- state.selectedSlot = normalizedSlot
if (!normalizedSlot || state.busy) return
-
- const activeSlot = String(state.data?.activeSlot || "").trim()
- if (normalizedSlot === activeSlot) return
-
- const success = await applySelection(normalizedSlot, "A", false)
- if (!success) {
- state.selectedSlot = activeSlot
- if (state.error) {
- showSnackbar(state.error, "error")
- }
- }
+ state.selectedSlot = normalizedSlot
}
function initialize() {
@@ -253,6 +242,10 @@ export function TestingGround() {
+
+ Only one Testing Ground can be active at a time. Switching slots only changes what you're viewing, and the active test stays enabled until you explicitly choose another mode.
+
+
${() => getSelectedSlot() ? html`
${getSelectedSlot().name}
diff --git a/tools/tuning/analyze_bolt_lateral.py b/tools/tuning/analyze_bolt_lateral.py
new file mode 100644
index 000000000..7bcff9930
--- /dev/null
+++ b/tools/tuning/analyze_bolt_lateral.py
@@ -0,0 +1,209 @@
+#!/usr/bin/env python3
+import argparse
+import math
+from dataclasses import dataclass
+
+import numpy as np
+
+from openpilot.tools.lib.logreader import LogReader, ReadMode
+from openpilot.selfdrive.locationd.torqued import TorqueEstimator
+from opendbc.car.gm.interface import NON_LINEAR_TORQUE_PARAMS
+
+
+@dataclass
+class ControlSample:
+ v_ego: float
+ steering_pressed: bool
+ lat_active: bool
+ saturated: bool
+ actual_la: float
+ desired_la: float
+ desired_jerk: float
+ p_term: float
+ i_term: float
+ f_term: float
+ torque_cmd: float
+
+
+def siglin_torque(lat_accel: float, params: dict[str, list[float]]) -> float:
+ side = "left" if lat_accel >= 0.0 else "right"
+ a, b, c, d = params[side]
+ sig_input = a * lat_accel
+ sig = math.copysign((1.0 / (1.0 + math.exp(-abs(sig_input))) - 0.5), sig_input)
+ return (sig * b) + (lat_accel * c) + d
+
+
+def summarize_control_samples(samples: list[ControlSample]) -> None:
+ if not samples:
+ print("No lateral torque samples found.")
+ return
+
+ v = np.array([s.v_ego for s in samples])
+ steering_pressed = np.array([s.steering_pressed for s in samples], dtype=bool)
+ lat_active = np.array([s.lat_active for s in samples], dtype=bool)
+ saturated = np.array([s.saturated for s in samples], dtype=bool)
+ actual = np.array([s.actual_la for s in samples])
+ desired = np.array([s.desired_la for s in samples])
+ jerk = np.array([s.desired_jerk for s in samples])
+ p_term = np.array([s.p_term for s in samples])
+ i_term = np.array([s.i_term for s in samples])
+ f_term = np.array([s.f_term for s in samples])
+ torque_cmd = np.array([s.torque_cmd for s in samples])
+
+ base = lat_active & (~steering_pressed) & (v > 8.0)
+ masks = (
+ ("all", base),
+ ("all_non_sat", base & (~saturated)),
+ ("left", base & (~saturated) & (desired >= 0.1)),
+ ("right", base & (~saturated) & (desired <= -0.1)),
+ ("center", base & (~saturated) & (np.abs(desired) < 0.1)),
+ ("steady_left", base & (~saturated) & (desired >= 0.1) & (np.abs(jerk) < 0.2)),
+ ("steady_right", base & (~saturated) & (desired <= -0.1) & (np.abs(jerk) < 0.2)),
+ )
+
+ print("\nControlsState tracking:")
+ for name, mask in masks:
+ if not np.any(mask):
+ continue
+ print(
+ f" {name:12s} n={int(mask.sum()):5d} "
+ f"mae={np.mean(np.abs(desired[mask] - actual[mask])):.4f} "
+ f"bias={np.mean(actual[mask] - desired[mask]):+.4f} "
+ f"|p|={np.mean(np.abs(p_term[mask])):.4f} "
+ f"|i|={np.mean(np.abs(i_term[mask])):.4f} "
+ f"|f|={np.mean(np.abs(f_term[mask])):.4f} "
+ f"torque={np.mean(torque_cmd[mask]):+.4f}"
+ )
+
+
+def summarize_torque_points(car_fingerprint: str, points: np.ndarray) -> None:
+ if points.size == 0:
+ print("No torque-estimator points found.")
+ return
+
+ params = NON_LINEAR_TORQUE_PARAMS.get(car_fingerprint)
+ if params is None:
+ print(f"No siglin torque params configured for {car_fingerprint}.")
+ return
+
+ steer = points[:, 0]
+ lat = points[:, 1]
+ pred = np.array([siglin_torque(x, params) for x in lat])
+ err = pred - steer
+
+ print("\nTorque map residuals:")
+ print(f" all n={points.shape[0]:5d} mae={np.mean(np.abs(err)):.4f} bias={np.mean(err):+.4f}")
+ for name, mask in (("left", lat >= 0.0), ("right", lat < 0.0), ("small", np.abs(lat) < 0.3), ("mid", (np.abs(lat) >= 0.3) & (np.abs(lat) < 0.8))):
+ if np.any(mask):
+ print(f" {name:12s} n={int(mask.sum()):5d} mae={np.mean(np.abs(err[mask])):.4f} bias={np.mean(err[mask]):+.4f}")
+
+ print("\nLinearized correction against current siglin:")
+ for name, mask in (("all", np.ones_like(lat, dtype=bool)), ("left", lat >= 0.0), ("right", lat < 0.0)):
+ x = np.column_stack([pred[mask], np.ones(mask.sum())])
+ y = steer[mask]
+ scale, offset = np.linalg.lstsq(x, y, rcond=None)[0]
+ fit = scale * pred[mask] + offset
+ print(
+ f" {name:12s} scale={scale:.4f} offset={offset:+.4f} "
+ f"mae_fit={np.mean(np.abs(fit - y)):.4f}"
+ )
+
+
+def main() -> None:
+ parser = argparse.ArgumentParser(description="Analyze a Bolt route for lateral tuning opportunities.")
+ parser.add_argument("route", help="Route name, e.g. dongle/route")
+ parser.add_argument("--mode", choices=("auto", "qlog", "rlog"), default="auto")
+ args = parser.parse_args()
+
+ mode_map = {
+ "auto": ReadMode.AUTO,
+ "qlog": ReadMode.QLOG,
+ "rlog": ReadMode.RLOG,
+ }
+ log_reader = LogReader(args.route, default_mode=mode_map[args.mode], sort_by_time=True)
+
+ car_params = None
+ live_torque_snapshots = []
+ torque_estimator = None
+ latest = {}
+ control_samples: list[ControlSample] = []
+
+ for msg in log_reader:
+ which = msg.which()
+ if which == "carParams" and car_params is None:
+ car_params = msg.carParams
+ torque_estimator = TorqueEstimator(car_params, track_all_points=True)
+ continue
+
+ if car_params is None:
+ continue
+
+ if which in ("carState", "carControl"):
+ latest[which] = getattr(msg, which)
+ elif which == "controlsState" and "carState" in latest and "carControl" in latest:
+ lateral_state = msg.controlsState.lateralControlState
+ if lateral_state.which() == "torqueState":
+ torque_state = lateral_state.torqueState
+ control_samples.append(ControlSample(
+ v_ego=latest["carState"].vEgo,
+ steering_pressed=latest["carState"].steeringPressed,
+ lat_active=latest["carControl"].latActive,
+ saturated=torque_state.saturated,
+ actual_la=torque_state.actualLateralAccel,
+ desired_la=torque_state.desiredLateralAccel,
+ desired_jerk=torque_state.desiredLateralJerk,
+ p_term=torque_state.p,
+ i_term=torque_state.i,
+ f_term=torque_state.f,
+ torque_cmd=latest["carControl"].actuators.torque,
+ ))
+ elif which == "liveTorqueParameters" and len(live_torque_snapshots) < 8:
+ live_torque_snapshots.append(msg.liveTorqueParameters)
+
+ if torque_estimator is not None and which in ("carControl", "carOutput", "carState", "liveCalibration", "livePose", "liveDelay"):
+ torque_estimator.handle_log(msg.logMonoTime / 1e9, which, getattr(msg, which))
+
+ if car_params is None:
+ raise RuntimeError("No carParams found in route.")
+
+ torque_tune = car_params.lateralTuning.torque
+ print(f"carFingerprint={car_params.carFingerprint}")
+ print(f"steerRatio={car_params.steerRatio:.4f} steerActuatorDelay={car_params.steerActuatorDelay:.4f}")
+ print(
+ "torqueTune="
+ f"latAccelFactor={torque_tune.latAccelFactor:.4f} "
+ f"friction={torque_tune.friction:.4f} "
+ f"latAccelOffset={torque_tune.latAccelOffset:.4f} "
+ f"kp={getattr(torque_tune, 'kpDEPRECATED', 0.0):.4f} "
+ f"ki={getattr(torque_tune, 'kiDEPRECATED', 0.0):.4f} "
+ f"kd={getattr(torque_tune, 'kdDEPRECATED', 0.0):.4f} "
+ f"kf={getattr(torque_tune, 'kfDEPRECATED', 0.0):.4f}"
+ )
+
+ if live_torque_snapshots:
+ last = live_torque_snapshots[-1]
+ print(
+ "liveTorqueFiltered="
+ f"latAccelFactor={last.latAccelFactorFiltered:.4f} "
+ f"latAccelOffset={last.latAccelOffsetFiltered:.4f} "
+ f"friction={last.frictionCoefficientFiltered:.4f} "
+ f"useParams={last.useParams} liveValid={last.liveValid}"
+ )
+
+ summarize_control_samples(control_samples)
+
+ points = np.array(torque_estimator.all_torque_points) if torque_estimator is not None else np.empty((0, 2))
+ if torque_estimator is not None and torque_estimator.filtered_points.is_calculable():
+ slope, offset, friction = torque_estimator.estimate_params()
+ print(
+ "\nTorqueEstimator fit:"
+ f" latAccelFactor={slope:.4f}"
+ f" latAccelOffset={offset:.4f}"
+ f" friction={friction:.4f}"
+ f" bucket_points={len(torque_estimator.filtered_points)}"
+ )
+ summarize_torque_points(car_params.carFingerprint, points)
+
+
+if __name__ == "__main__":
+ main()