mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-06 00:36:25 +08:00
Bolt lateral tests
This commit is contained in:
firestar5683
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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() {
|
||||
</select>
|
||||
</div>
|
||||
|
||||
<p class="testingGroundHint">
|
||||
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.
|
||||
</p>
|
||||
|
||||
${() => getSelectedSlot() ? html`
|
||||
<div class="testingGroundDetails">
|
||||
<h3>${getSelectedSlot().name}</h3>
|
||||
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user