mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-11-14 08:23:59 +08:00
Code Issues Packages Projects Releases Wiki Activity

Lateral

Browse Source 
Integrate torque feedback, lane-change smoothing and low-speed Turn Assist through dedicated lateral owners.

Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com>

Co-authored-by: pharmacomaniac <blittle65@gmail.com>
This commit is contained in:
firestar5683
2026-09-25 14:53:19 -05:00
parent 30f9a629d8
commit bc3ac6ad72
94 changed files with 16280 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera.py
+196
View File
@@ -0,0 +1,196 @@
import unittest
from opendbc.can import CANPacker
from opendbc.car import Bus, gen_empty_fingerprint, structs
from opendbc.car.gm.interface import CarInterface
from opendbc.car.gm.values import DBC, ORDINARY_CAMERA_CAR, ORDINARY_CAMERA_ALPHA_CAR, is_ordinary_camera_profile
def params(identity, *, alpha=False, release=False, camera=True, be=True):
fp = gen_empty_fingerprint()
fp[0].update({0xF1: 6, 0xC9: 8, 0x1C4: 8, 0x184: 8, 0x34A: 5})
if be:
fp[0][0xBE] = 6
if not camera:
fp[0][0x1E1] = 7
if camera:
fp[2].update({0x320: 6, 0x180: 4, 0x370: 6})
return CarInterface.get_params(identity, fp, [], alpha, release, False)
def feed(ci, packer, now, *, cruise=True, gas=False, brake=False, fcw=0, counter=0):
from opendbc.car.gm.tests.test_cc_gateway_stock import pt_frames
frames = pt_frames(packer, gas=gas, counter=counter, acc_cruise=2 if cruise else 0)
frames = [frame for frame in frames if frame[0] != 0xC9]
frames.append(packer.make_can_msg("EBCMBrakePedalPosition", 0, {"BrakePedalPosition": 40 if brake else 0}))
frames.append(packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1, 'BrakePressed': int(brake)}))
frames += [packer.make_can_msg('ASCMLKASteeringCmd', 2, {'RollingCounter': counter}),
packer.make_can_msg('AEBCmd', 2, {}),
packer.make_can_msg('ASCMActiveCruiseControlStatus', 2,
{'ACCCruiseState': 2, 'ACCSpeedSetpoint': 80, 'FCWAlert': fcw})]
return ci.update([(now, frames)]), frames
class TestOrdinaryCamera(unittest.TestCase):
def test_final_cp_and_default_policy_modes(self):
from opendbc.car.gm.camera import policy_for
from opendbc.car.gm.feature_capabilities import longitudinal_supported, display_supported
from opendbc.car.gm.lateral import lane_centering_supported
from opendbc.car.gm.aol import qualified_gm
from openpilot.starpilot.lateral.controller_selection import policy_for as lateral_policy_for
self.assertEqual(len(ORDINARY_CAMERA_CAR), 6)
for identity in ORDINARY_CAMERA_CAR:
for release in (False, True):
for alpha in (False, True):
cp = params(identity, alpha=alpha, release=release)
long = alpha and not release and identity in ORDINARY_CAMERA_ALPHA_CAR
self.assertEqual(cp.openpilotLongitudinalControl, long)
self.assertEqual(cp.pcmCruise, not long)
self.assertEqual(cp.safetyConfigs[0].safetyParam, 0xC170 if long else 0xC171)
self.assertFalse(cp.dashcamOnly)
self.assertTrue(is_ordinary_camera_profile(cp, longitudinal=long))
self.assertEqual(policy_for(cp) is not None, long)
self.assertEqual(longitudinal_supported(cp), long)
self.assertTrue(display_supported(cp))
self.assertTrue(lane_centering_supported(cp))
self.assertTrue(qualified_gm(cp))
self.assertEqual(lateral_policy_for(cp), 'ordinary_camera')
absent = params(identity, alpha=alpha, release=release, camera=False)
self.assertFalse(absent.dashcamOnly)
self.assertTrue(is_ordinary_camera_profile(absent, longitudinal=long))
self.assertEqual(absent.safetyConfigs[0].safetyParam, 0xC173 if long else 0xC172)
def test_actual_required_sources_and_alt_status_ownership(self):
for identity in ORDINARY_CAMERA_CAR:
cp = params(identity)
ci = CarInterface(cp)
packer = CANPacker(DBC[identity][Bus.pt])
for tick in range(30):
out, _ = feed(ci, packer, 1_000_000_000 + tick * 10_000_000, fcw=2, counter=tick % 4)
self.assertTrue(out.canValid)
self.assertTrue(out.cruiseState.enabled)
self.assertFalse(out.cruiseState.nonAdaptive)
self.assertAlmostEqual(out.cruiseState.speed, 80 / 3.6, places=5)
self.assertEqual(ci.CS.stock_fcw_alert, 2)
def test_packed_commands_gas_lateral_and_stock_cancel_route(self):
for identity in ORDINARY_CAMERA_CAR:
for alpha in (False, True):
cp = params(identity, alpha=alpha)
ci = CarInterface(cp)
packer = CANPacker(DBC[identity][Bus.pt])
emitted_cancel = False
for tick in range(30):
now = 1_000_000_000 + tick * 40_000_000
out, _ = feed(ci, packer, now, gas=tick >= 12, counter=tick % 4)
self.assertTrue(out.canValid)
cc = structs.CarControl(enabled=True, latActive=True, longActive=cp.openpilotLongitudinalControl and tick < 12)
cc.actuators.torque = .1
cc.actuators.accel = 1.
cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.pid
cc.cruiseControl.cancel = not cp.openpilotLongitudinalControl
ci.CC.frame = tick * 4
_, messages = ci.apply(cc.as_reader(), now)
self.assertFalse(any(message[0] in (0x200, 0xBD, 0x1F5, 0x3D1, 0xA1, 0x306, 0x308, 0x310) for message in messages))
if cp.openpilotLongitudinalControl:
self.assertEqual([message[0] for message in messages if message[0] in (0x2CD, 0x2CB, 0x315, 0x370)],
[0x2CD, 0x2CB, 0x315, 0x370])
self.assertTrue(all(message[2] == 0 for message in messages if message[0] in (0x2CD, 0x2CB, 0x315, 0x370)))
if tick >= 12:
self.assertEqual((ci.CC.apply_gas, ci.CC.apply_brake), (-500., 0))
else:
self.assertFalse(any(message[0] in (0x2CD, 0x2CB, 0x315, 0x370) for message in messages))
for message in messages:
if message[0] == 0x1E1:
emitted_cancel = True
self.assertEqual(message[2], 2)
if tick >= 12:
self.assertNotEqual(ci.CC.apply_torque_last, 0)
if not cp.openpilotLongitudinalControl:
self.assertTrue(emitted_cancel)
def test_expired_camera_and_main_sources_zero_actual_steering(self):
for alpha in (False, True):
identity = next(iter(ORDINARY_CAMERA_ALPHA_CAR))
for omitted in ('camera', 'main'):
cp = params(identity, alpha=alpha)
ci = CarInterface(cp)
packer = CANPacker(DBC[identity][Bus.pt])
for tick in range(30):
now = 1_000_000_000 + tick * 10_000_000
out, messages = feed(ci, packer, now, counter=tick % 4)
self.assertTrue(out.canValid)
cc = structs.CarControl(enabled=True, latActive=True)
cc.actuators.torque = .1
ci.CC.frame = 120
ci.apply(cc.as_reader(), now)
self.assertNotEqual(ci.CC.apply_torque_last, 0)
retained = [message for message in messages if message[2] == 0] if omitted == 'camera' else [
message for message in messages if message[0] != 0xC9]
for tick in range(1, 80):
stamp = now + tick * 20_000_000
ci.update([(stamp, retained)])
ci.CC.frame += 10
_, emitted = ci.apply(cc.as_reader(), stamp)
self.assertEqual(ci.CC.apply_torque_last, 0)
self.assertTrue(any(message[0] == 0x180 for message in emitted))
def test_f1_analog_source_with_and_without_be_keeps_c9_brake_owner(self):
from opendbc.car.gm.values import GMFlags
for identity in ORDINARY_CAMERA_CAR:
for be in (False, True):
for alpha in (False, True):
cp = params(identity, alpha=alpha, be=be)
self.assertEqual(bool(cp.flags & GMFlags.NO_ACCELERATOR_POS_MSG), not be)
ci = CarInterface(cp)
packer = CANPacker(DBC[identity][Bus.pt])
for tick in range(30):
now = 1_000_000_000 + tick * 10_000_000
_, messages = feed(ci, packer, now, counter=tick % 4)
messages = [message for message in messages if message[0] not in (0xC9, 0xF1) and (be or message[0] != 0xBE)]
messages += [packer.make_can_msg('EBCMBrakePedalPosition', 0, {'BrakePedalPosition': 80}),
packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1, 'BrakePressed': 0})]
out = ci.update([(now + 1, messages)])
self.assertTrue(out.canValid)
self.assertFalse(out.brakePressed)
messages = [message for message in messages if message[0] not in (0xC9, 0xF1)]
messages += [packer.make_can_msg('EBCMBrakePedalPosition', 0, {'BrakePedalPosition': 0}),
packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1, 'BrakePressed': 1})]
out = ci.update([(now + 10_000_000, messages)])
self.assertTrue(out.brakePressed)
def test_actual_card_preserves_final_camera_owner(self):
import os
from unittest.mock import patch
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.car.card import Car
from opendbc.car.gm.radar_interface import RadarInterface
for identity in ORDINARY_CAMERA_CAR:
with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1'}):
saved = Params()
saved.put_bool('OpenpilotEnabledToggle', True, block=True)
saved.put_bool('AlwaysOnLateral', True, block=True)
cp = params(identity, alpha=True, be=False)
card = Car(CI=CarInterface(cp), RI=RadarInterface(cp))
long = identity in ORDINARY_CAMERA_ALPHA_CAR
self.assertEqual(card.CP.openpilotLongitudinalControl, long)
self.assertEqual(card.CP.pcmCruise, not long)
self.assertEqual(card.CP.safetyConfigs[0].safetyParam, 0xC170 if long else 0xC171)
self.assertTrue(is_ordinary_camera_profile(card.CP, longitudinal=long))
def test_declared_f1_source_recipe_matches_selected_analog_owner(self):
from unittest.mock import patch
from opendbc.car.gm import carstate
real_parser = carstate.CANParser
for be in (False, True):
recipes = {}
def make_parser(dbc, messages, bus, recipes=recipes):
recipes[bus] = list(messages)
return real_parser(dbc, messages, bus)
cp = params(next(iter(ORDINARY_CAMERA_ALPHA_CAR)), alpha=True, be=be)
with patch.object(carstate, 'CANParser', side_effect=make_parser):
CarInterface(cp)
self.assertEqual([frequency for name, frequency in recipes[0] if name == 'EBCMBrakePedalPosition'],
[10 if be else 100])
self.assertEqual(any(name == 'ECMAcceleratorPos' for name, _ in recipes[0]), be)
opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera_removed.py
+117
View File
@@ -0,0 +1,117 @@
"""Observed PT source contract for ordinary GM camera-absent layouts."""
import unittest
import os
from unittest.mock import patch
from opendbc.can import CANPacker
from opendbc.car import Bus, gen_empty_fingerprint, structs
from opendbc.car.gm.interface import CarInterface
from opendbc.car.gm.values import DBC, GMFlags, ORDINARY_CAMERA_CAR, ORDINARY_CAMERA_ALPHA_CAR, is_ordinary_camera_profile
def params(identity, *, alpha=False, release=False, analog=True):
fp = gen_empty_fingerprint()
fp[0].update({0xF1: 6, 0xC9: 8, 0x1C4: 8, 0x184: 8, 0x34A: 5, 0x1E1: 7})
if analog:
fp[0][0xBE] = 6
return CarInterface.get_params(identity, fp, [], alpha, release, False)
def feed(ci, packer, now, *, cruise=True, main=True, gas=False, brake=False, counter=0):
from opendbc.car.gm.tests.test_cc_gateway_stock import pt_frames
frames = pt_frames(packer, main=main, gas=gas, brake=brake, counter=counter, acc_cruise=2 if cruise else 0)
if ci.CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG:
frames = [message for message in frames if message[0] != 0xBE]
frames.append(packer.make_can_msg('EBCMBrakePedalPosition', 0, {'BrakePedalPosition': 40 if brake else 0}))
frames.append(packer.make_can_msg('ASCMLKASteeringCmd', 128, {'RollingCounter': counter}))
return ci.update([(now, frames)]), frames
class TestOrdinaryCameraRemoved(unittest.TestCase):
def test_final_profile_shared_owners_and_absent_camera_state(self):
from opendbc.car.gm.aol import qualified_gm
from opendbc.car.gm.lateral import lane_centering_supported
from opendbc.car.gm.feature_capabilities import longitudinal_supported
from openpilot.starpilot.lateral.controller_selection import policy_for
for identity in ORDINARY_CAMERA_CAR:
for analog in (False, True):
for release in (False, True):
for alpha in (False, True):
cp = params(identity, alpha=alpha, release=release, analog=analog)
long = alpha and not release and identity in ORDINARY_CAMERA_ALPHA_CAR
self.assertTrue(is_ordinary_camera_profile(cp, longitudinal=long))
self.assertTrue(cp.flags & GMFlags.NO_CAMERA)
self.assertEqual(cp.safetyConfigs[0].safetyParam, 0xC173 if long else 0xC172)
self.assertEqual(cp.openpilotLongitudinalControl, long)
self.assertTrue(qualified_gm(cp))
self.assertTrue(lane_centering_supported(cp))
self.assertEqual(longitudinal_supported(cp), long)
self.assertIsNotNone(policy_for(cp))
ci = CarInterface(cp)
packer = CANPacker(DBC[identity][Bus.pt])
for tick in range(12):
out, _ = feed(ci, packer, 1_000_000_000 + tick * 10_000_000, counter=tick % 4)
self.assertTrue(out.canValid)
self.assertTrue(out.cruiseState.enabled)
self.assertEqual(out.cruiseState.speed, 0.)
self.assertFalse(out.stockAeb or out.stockFcw or out.cruiseState.nonAdaptive)
def test_actual_card_and_malformed_or_missing_startup_sources(self):
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.car.card import Car
from opendbc.car.gm.radar_interface import RadarInterface
from opendbc.car.gm.values import is_ordinary_camera_removed
self.assertFalse(is_ordinary_camera_removed(None))
for identity in ORDINARY_CAMERA_CAR:
with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1'}):
saved = Params()
saved.put_bool('OpenpilotEnabledToggle', True, block=True)
cp = params(identity, alpha=True)
card = Car(CI=CarInterface(cp), RI=RadarInterface(cp))
self.assertTrue(is_ordinary_camera_removed(card.CP))
self.assertEqual(card.CP.safetyConfigs[0].safetyParam, cp.safetyConfigs[0].safetyParam)
for missing in (0xF1, 0xC9, 0x1C4, 0x184, 0x34A, 0x1E1):
fp = gen_empty_fingerprint()
fp[0].update({0xF1: 6, 0xC9: 8, 0x1C4: 8, 0x184: 8, 0x34A: 5, 0x1E1: 7})
fp[0].pop(missing)
denied = CarInterface.get_params(identity, fp, [], True, False, False)
self.assertTrue(denied.dashcamOnly)
fp[0][missing] = 7
fp[2][0x320] = 5
self.assertTrue(CarInterface.get_params(identity, fp, [], True, False, False).dashcamOnly)
def test_actual_controller_routes_and_source_withdrawal(self):
for identity in ORDINARY_CAMERA_CAR:
for alpha in (False, True):
cp = params(identity, alpha=alpha)
ci = CarInterface(cp)
packer = CANPacker(DBC[identity][Bus.pt])
seen_cancel = False
for tick in range(112):
now = 1_000_000_000 + tick * 10_000_000
out, _ = feed(ci, packer, now, gas=tick >= 50, counter=tick % 4)
self.assertTrue(out.canValid)
cc = structs.CarControl(enabled=True, latActive=True, longActive=cp.openpilotLongitudinalControl)
cc.actuators.torque = .1
cc.actuators.accel = 1.
cc.cruiseControl.cancel = True
_, emitted = ci.apply(cc.as_reader(), now)
ids = [m[0] for m in emitted]
self.assertNotIn(0x2CD, ids)
self.assertFalse(any(i in ids for i in (0x200, 0xBD, 0x1F5, 0x3D1)))
self.assertEqual(any(i in ids for i in (0x409, 0x40A)), cp.openpilotLongitudinalControl and tick % 100 == 0)
if 0x409 in ids:
self.assertLess(ids.index(0x409), ids.index(0x184))
for address, payload, bus in emitted:
if address == 0x1E1:
seen_cancel = True
self.assertEqual(bus, 2)
self.assertEqual(payload[4] & 3, tick % 4)
if not cp.openpilotLongitudinalControl:
self.assertFalse(any(i in ids for i in (0x2CB, 0x315, 0x370, 0x409, 0x40A)))
self.assertEqual(seen_cancel, not cp.openpilotLongitudinalControl)
ci.CS.ordinary_removed_sources = (0,) * 6
ci.CC.frame = 112
applied, _ = ci.apply(cc.as_reader(), now + 10_000_000)
self.assertEqual(applied.torqueOutputCan, 0)
opendbc_repo/opendbc/car/gm/tests/test_ordinary_sdgm.py
+94
View File
@@ -0,0 +1,94 @@
import unittest
from opendbc.car.gm.tests.test_ascm_intercept import params
from opendbc.car.gm.values import CAR, ORDINARY_SDGM_CAR, SDGM_CANCEL_PT_CAR, CarControllerParams, is_ordinary_sdgm_profile
from opendbc.car.gm.longitudinal import sdgm_policy_for
from opendbc.car.gm.ordinary import demands
class TestOrdinarySdgm(unittest.TestCase):
def test_exact_final_configuration(self):
self.assertEqual(len(ORDINARY_SDGM_CAR), 7)
for candidate in ORDINARY_SDGM_CAR:
for release in (False, True):
for alpha in (False, True):
for sascm in (False, True):
for c9 in (False, True):
cp = params(candidate, sascm=sascm, accelerator=not c9, alpha=alpha, release=release)
long = alpha and sascm and not release
expected = 0x1001 | (2 if long else 0) | (0x400 if c9 else 0)
if not long and candidate in SDGM_CANCEL_PT_CAR:
expected |= 0x2000
self.assertEqual(cp.safetyConfigs[0].safetyParam, expected)
self.assertTrue(is_ordinary_sdgm_profile(cp, longitudinal=long))
self.assertEqual(sdgm_policy_for(cp) is not None, long)
if long:
limits = CarControllerParams(cp)
self.assertEqual((limits.MAX_GAS, limits.MAX_ACC_REGEN, limits.INACTIVE_REGEN), (2698, -540, -500))
def test_blazer_and_xt4_final_overrides(self):
cp = params(CAR.CHEVROLET_BLAZER, sascm=True, alpha=True)
self.assertAlmostEqual(cp.longitudinalActuatorDelay, .7, places=6)
self.assertAlmostEqual(cp.stopAccel, -.30, places=6)
for actual, expected in zip(sdgm_policy_for(cp).kp[1], (.09, .075, .055, .040), strict=True):
self.assertAlmostEqual(actual, expected, places=6)
xt4 = params(CAR.CADILLAC_XT4)
self.assertAlmostEqual(xt4.minSteerSpeed, 30 * .44704, places=6)
self.assertEqual(xt4.safetyConfigs[0].safetyParam, 0x3001)
def test_camera_endpoints_and_braking_inactive(self):
cp = params(CAR.CHEVROLET_TRAVERSE, sascm=True, alpha=True)
self.assertEqual(demands(-4, 0, (), cp, min_gas=-540, max_gas=2698, inactive_gas=-500, brake_threshold=0), (-500, 400))
self.assertEqual(demands(2, 100, (), cp, min_gas=-540, max_gas=2698, inactive_gas=-500, brake_threshold=0), (2698, 0))
def test_actual_packed_required_sources_and_selected_brake(self):
from opendbc.can import CANPacker
from opendbc.car import Bus
from opendbc.car.gm.carstate import CarState
from opendbc.car.gm.values import DBC
for candidate in ORDINARY_SDGM_CAR:
for c9 in (False, True):
cp = params(candidate, sascm=True, accelerator=not c9, alpha=True)
state = CarState(cp)
parsers = state.get_can_parsers(cp)
packer = CANPacker(DBC[candidate][Bus.pt])
required = ('PSCMStatus', 'ESPStatus', 'EBCMWheelSpdFront', 'EBCMWheelSpdRear',
'EBCMFrictionBrakeStatus', 'PSCMSteeringAngle', 'ECMPRDNL2', 'AcceleratorPedal2',
'ECMEngineStatus', 'BCMTurnSignals', 'BCMDoorBeltStatus', 'BCMGeneralPlatformStatus', 'ASCMSteeringButton')
if not c9:
required += ('ECMAcceleratorPos',)
for index in range(30):
now = 1_000_000_000 + index * 10_000_000
frames = [packer.make_can_msg(name, 0, {'BrakePressed': 1} if name == 'ECMEngineStatus' and c9 else
{'BrakePedalPos': 10} if name == 'ECMAcceleratorPos' else {}) for name in required]
parsers[Bus.pt].update([(now, frames)])
parsers[Bus.cam].update([(now, [packer.make_can_msg('ASCMLKASteeringCmd', 2, {}),
packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'FCWAlert': 2})])])
result = state.update(parsers)
self.assertTrue(parsers[Bus.pt].can_valid)
self.assertTrue(parsers[Bus.cam].can_valid)
self.assertTrue(result.brakePressed)
self.assertEqual(state.stock_fcw_alert, 2)
def test_shared_feature_lane_and_aol_finalized_owners(self):
from opendbc.car.gm.feature_capabilities import longitudinal_supported
from opendbc.car.gm.lateral import lane_centering_supported
from opendbc.car.gm.aol import qualified_gm
from openpilot.starpilot.lateral.controller_selection import policy_for
for candidate in ORDINARY_SDGM_CAR:
for release in (False, True):
for alpha in (False, True):
cp = params(candidate, sascm=True, alpha=alpha, release=release)
self.assertEqual(longitudinal_supported(cp), alpha and not release)
self.assertTrue(lane_centering_supported(cp))
self.assertTrue(qualified_gm(cp))
self.assertEqual(policy_for(cp), 'ordinary_sdgm')
cp.safetyConfigs[0].safetyParam |= 4
self.assertFalse(longitudinal_supported(cp))
self.assertFalse(qualified_gm(cp))
def test_scoped_geometry_preserves_cc_neighbor(self):
from opendbc.car.gm.values import CAR
for candidate in (CAR.CADILLAC_XT5, CAR.CHEVROLET_BLAZER, CAR.BUICK_BABYENCLAVE):
self.assertEqual(candidate.config.specs.tireStiffnessFactor, 1.0)
self.assertEqual(CAR.CADILLAC_XT5_CC.config.specs.tireStiffnessFactor, 1.0)
openpilot/selfdrive/controls/lib/latcontrol_torque.py
+13 -1
View File
@@ -8,6 +8,8 @@ from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.common.pid import PIDController
from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, policy_for
from openpilot.starpilot.lateral.torque_extension import create_extension
# At higher speeds (25+mph) we can assume:
# Lateral acceleration achieved by a specific car correlates to
@@ -33,8 +35,12 @@ LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
VERSION = 1
class LatControlTorque(LatControl):
def __init__(self, CP, CI, dt):
def __init__(self, CP, CI, dt, *, controller_mode: ControllerMode | None = None, turn_assist: bool = False):
super().__init__(CP, CI, dt)
self.controller_mode = default_selection(CP).mode if controller_mode is None else ControllerMode(controller_mode)
self.controller_policy = policy_for(CP)
if self.controller_mode == ControllerMode.STARPILOT and self.controller_policy is None:
raise ValueError('No StarPilot torque policy for CarParams')
self.torque_params = CP.lateralTuning.torque.as_builder()
self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
@@ -45,8 +51,11 @@ class LatControlTorque(LatControl):
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.starpilot_extension = create_extension(self, CP, self.controller_mode, self.controller_policy, turn_assist=turn_assist)
def update_torque_parameters(self, latAccelFactor, latAccelOffset, friction):
if self.starpilot_extension is not None:
latAccelFactor, latAccelOffset, friction = self.starpilot_extension.transform_torque_parameters(latAccelFactor, latAccelOffset, friction)
self.torque_params.latAccelFactor = latAccelFactor
self.torque_params.latAccelOffset = latAccelOffset
self.torque_params.friction = friction
@@ -57,6 +66,9 @@ class LatControlTorque(LatControl):
self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
if self.starpilot_extension is not None:
return self.starpilot_extension.update(active, CS, VM, params, steer_limited_by_safety,
desired_curvature, curvature_limited, lat_delay)
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = VERSION
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
openpilot/selfdrive/locationd/lagd.py
+3 -1
View File
@@ -398,6 +398,8 @@ def main():
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['deviceMotion'].frequency)
from openpilot.starpilot.lateral.gm_geometry_runtime import GeometryPublicationOwner
geometry_owner = GeometryPublicationOwner(params, CP)
if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
lag, valid_blocks = initial_lag_params
lag_learner.reset(lag, valid_blocks)
@@ -415,7 +417,7 @@ def main():
if sm.frame % 5 == 0:
lag_learner.update_estimate()
lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG)
lag_msg_dat = lag_msg.to_bytes()
lag_msg_dat = geometry_owner.delay(lag_msg).to_bytes()
pm.send('lateralDelay', lag_msg_dat)
if sm.frame % 1200 == 0: # cache every 60 seconds
openpilot/selfdrive/locationd/paramsd.py
+3 -1
View File
@@ -258,6 +258,8 @@ def main():
steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG)
learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)
from openpilot.starpilot.lateral.gm_geometry_runtime import GeometryPublicationOwner
geometry_owner = GeometryPublicationOwner(params, CP)
while True:
sm.update()
@@ -270,7 +272,7 @@ def main():
if sm.updated['deviceMotion']:
msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
msg_dat = msg.to_bytes()
msg_dat = geometry_owner.parameters(msg).to_bytes()
if sm.frame % 1200 == 0: # once a minute
put_cache(params, "LiveParametersV2", msg)
openpilot/starpilot/lateral/LICENSE
+8
View File
@@ -0,0 +1,8 @@
Copyright (c) 2018, Comma.ai, Inc.
Copyright (c) 2026, firestar5683 and StarPilot contributors
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
openpilot/starpilot/lateral/README.md
+79
View File
@@ -0,0 +1,79 @@
# Lateral control
The lane-centering contribution, torque controller and torque parameter source
have separate owners. None grants steering permission: controlsd, the vehicle
controller and Panda retain their existing authority and limits. See
[TORQUE.md](TORQUE.md) for controller selection, learned parameters and manual
factor/friction preferences.
## Lane centering
`LaneCenteringController.update` returns a bounded curvature contribution and
an unclipped candidate. The host applies it after selecting the model or
maneuver curvature and before the native curvature limiter. Longitudinal-only
operation cannot authorize a lateral contribution.
Both inner lane lines must have probability at least 0.6 and standard deviation
at most 0.3 m, with lane width between 2.6 and 4.8 m. Offset is limited to
0.3 m and 1.1 m center-to-line clearance. Lookahead follows speed within 8–35 m;
the center-error deadband is 0.08 m. End-to-end authority controls how strongly
a confident model path reduces the contribution between 0.15 and 0.50 m of
center error. Raw curvature is capped at 0.004 and scaled by 0.30.
Acquisition uses a 0.4 s exponential filter. Signal pause and confidence loss
release the contribution over 0.2 s; driver override resets it immediately.
Signal pause takes precedence over the lane-change reset when both apply.
Malformed inputs and nonfinite settings are rejected. The host supplies elapsed
time, model freshness and explicit clock discontinuities. The filter advances
on each valid control tick even when the same model sample is held.
`LaneCenteringHost` reads saved preferences at 1 Hz, including when a drive
starts with Lane Centering off, so Favorites can enable it during the drive.
Normal startup currently admits the tagged Ioniq 6 longitudinal configuration;
other vehicles are not yet admitted. `LANE_CENTERING_REPLAY_RUNTIME=1` provides
the separate offline replay path. Runtime checks require valid CAN, effective
lateral authority, enabled preferences, and fresh carState, selfdriveState, modelV2 and
vehicleParameters within 2.5 service periods. Missing evidence removes the
contribution without changing the saved preferences.
Tests cover geometry, filtering, overrides, curvature-source priority, native
limiting and unchanged longitudinal commands. They do not establish vehicle
qualification. This module derives from the lane-centering work introduced by
jc01rho in `9f1066ce8380a2602beaf5d43fdb38ee6ec2dcbe`; the applicable MIT notice
is retained in the repository LICENSE.
## Automatic lane changes
The ordinary lane-change path requires a driver nudge. The saved Auto option
adds a delay of 0–5 s and minimum adjacent-lane width of 0–15 ft (stored in
meters), defaulting to 1 s and 0 ft. The saved option takes effect at modeld
startup on vehicles with configured steering control. Active lateral authority
is required; cruise control and Always On Lateral preferences do not enable it.
DesireHelper owns the transition, blindspot check and signal latch. Auto uses
same-frame model evidence, current calibration, fresh carState/carControl,
engaged lateral control and a recognized vehicle. Camera EOF age uses
CLOCK_BOOTTIME; message and calibration timestamps use CLOCK_MONOTONIC.
Adjacent width is the median distance between the inner and outer lane lines.
A nearer road edge returns zero width. A saved minimum of zero skips the width
requirement. Malformed geometry is rejected.
The typed status in Event Data @126 supplies pre-change alert wording only when
its model frame, camera EOF, direction and lifetime match. Missing or stale
status produces neutral text and cannot affect engagement checks.
## Vehicle-specific torque policies
The Ioniq 6 policy retains separate 2023 and observed-firmware 2025 shaping for
curvature history, low-speed control, asymmetric feedforward, friction and
stop/pull-away behavior. The 2025 selection requires both firmware markers;
missing evidence selects the standard variant. The policy applies its factor
multiplier once per parameter update. Vehicle initialization uses factor 3.0
and friction 0.09, with the original startup PID limits; subsequent learned or
manual updates use the parameter-update contract described in TORQUE.md.
Reference fixtures cover both Ioniq variants through stops, pull-away, highway,
reversal and driver override, with independent non-Ioniq regression cases.
Other explicit policies and the standard controller remain separate; tuning
one platform must not change another platform's controller or parameter source.
openpilot/starpilot/lateral/TORQUE.md
+69
View File
@@ -0,0 +1,69 @@
# Torque controllers and parameter sources
Controller selection and torque parameter selection are separate. `LatControlTorque` uses the standard controller unless the vehicle has an explicit policy, currently Ioniq 6, Genesis G70 2020, Corolla TSS2, the five manual Bolt identities or the five Volt identities. The `AdvancedLateralTune` preference does **not** select a different controller; it enables manual factor/friction adjustments through `TorqueHost`.
## Controller choice
Supported vehicles can save either the standard torque controller or their StarPilot vehicle policy. `LateralControllerSelection` is a versioned JSON document with separate choices by vehicle identity. Missing or invalid settings preserve the existing controller: the vehicle policy for the registered supported platforms, and the standard controller elsewhere. An unsupported vehicle cannot select another vehicle's policy.
`controlsd` resolves the choice once from CarParams before constructing the controller. Later preference changes take effect on the next drive; they cannot change the running controller or carry its integrator into another algorithm. Selecting standard skips vehicle-policy construction, including policy-specific gains and factor multipliers. It retains the vehicle's CarParams, delay estimation and separately configured manual torque values. Automatic learning is a separate preference. It therefore selects a control algorithm, not OEM steering or a complete upstream vehicle tune.
Galaxy and native settings share the preference owner. The owner checks the current vehicle and displayed document before saving, preserves other vehicles' choices, and reports a saved selection rather than claiming that it is already active.
**Automatic Steering Learning** saves an On or Off choice for the exact vehicle brand and fingerprint in `SteeringLearningPreferences`. Controller changes preserve this choice. Vehicles without a saved entry use the valid legacy `ForceAutoTuneOff` preference, or default to On when it is absent; no migration rewrites that legacy value. The UI shows the saved choice separately from controller compatibility. On does not enable incompatible scalar learning or claim an automatic result is applied. Prep My Vehicle for Tuning deliberately saves Off for the prepared vehicle and journals the prior choice for exact restoration.
The runtime uses the same exact-platform learning policy. The running StarPilot controller, including a default or invalid-selection fallback to an established policy, cannot consume learned parameters through either the direct controls path or `TorqueHost`. The `torqued` process samples this policy at startup and, when disabled, skips old caches, sample collection, fitting and cache writes; existing cached data is retained. Standard with a saved learning-off preference also disables learning, even when manual adjustments are off. Unknown platforms keep their established learning behavior. Saved controller and learning changes are intent for the next process startup, not evidence of the currently running controller. Offline `TorqueEstimator` analysis retains estimation by default; the running process explicitly supplies its learning policy.
## Runtime admission
### Preparing a support log
**Prep My Vehicle for Tuning** is a parked On/Off toggle for registered steering-policy vehicles. On saves a durable, vehicle-bound snapshot before selecting StarPilot steering, turning learning/manual adjustments/Turn Assist Off and selecting default lateral lane preferences. Off restores the exact saved bytes, including absent keys, after a restart or interrupted write. Later user edits are preserved and require review rather than being overwritten. Manual torque documents, calibration and longitudinal preferences remain saved. Record a route, then open a tuning request on the StarPilot Discord: https://firestar.link/discord.
The journal is written before preference changes, and every replacement rechecks the vehicle and parked authority. Interrupted preparation is shown as incomplete; switching Off recovers the original snapshot. Invalid controller/lane documents or later user edits block changes without overwriting them.
Start a new drive after preparation and use the ordinary route log. The controls startup event records its actual selected controller/policy, learning decision, initial vehicle source, torque values and PID limits. The usual route data contains vehicle/model/calibration and control behavior; this setup does not claim that a saved request proves a running baseline or vehicle qualification. The preparation journal records saved preference state, not proof of a qualified running tune.
`torque_runtime.py` admits normal Ioniq 6 sessions when manual adjustments are enabled and valid, independently of longitudinal ownership. `TORQUE_REPLAY_RUNTIME=1` additionally permits the exact Toyota/Lexus torque platforms in `torque_supported.py` for offline replay. Admission requires the expected brand, torque steering and lateral tuning, finite valid CarParams values, and a non-dashcam configuration. The five manual Bolt identities also support an explicit, vehicle-bound friction profile; their requirements are described below. Other vehicles keep the standard parameter path and cannot inherit these manual settings.
The source selector does not grant steering, longitudinal or independent-axis permission. Existing controls and vehicle safety checks continue to own actuation.
## Parameter precedence
The source starts with `CarParams.lateralTuning.torque`. When the startup learning policy permits it, fresh, valid version-1 `lateralTorqueParameters` may replace those values within the car-relative limits in `TorqueHost._learned`. Messages must pass the service checks and be no older than two publication periods. `ForceAutoTuneOff` ignores learned values without disabling valid manual overrides.
Manual factor and friction are independent. A manual factor uses the vehicle's original offset; an unchanged field may still use its learned value when learning is allowed, or its vehicle value otherwise. Missing settings do not create overrides. Malformed values or a saved profile requiring review fall back to the vehicle values while preserving the saved data.
`torque_settings.py` stores manual choices by vehicle identity and the factor/offset/friction basis. A changed vehicle basis requires review before reusing its custom values. For the existing Ioniq/Toyota parameter hosts, legacy `SteerLatAccel` and `SteerFriction` are used only when no profile document exists. Bolt does not import these device-wide values. Matching `SteerLatAccelStock` and `SteerFrictionStock` markers identify stock tracking rather than a manual override; these optional keys remain persistent. Legacy comparisons retain two-decimal rounding and are resolved before checking custom-value bounds.
## Application and reset
`TorqueTuning` is the typed boundary between source selection and the controller. `TorqueHost` refreshes saved settings at 1 Hz, checks source freshness every control frame, and rate-limits changes while active. A missing learner falls back to valid manual values or the vehicle tune. Clock discontinuities, invalid settings or loss of lateral permission reset the optional tune to vehicle values. Invalid CAN suppresses lateral output and clears the optional controller's integrator.
In admitted sessions, the torque learner skips old cached CarParams and learned values without deleting them. Other vehicles retain the standard cache policy. No learned cache is automatically translated into a manual profile.
A vehicle policy may convert a selected raw factor before application. The Ioniq 6 policy applies its factor multiplier once; callers must not pre-apply it. `TorqueHost.apply` also avoids redundant initial updates because a vehicle policy's startup PID limits can differ from its later parameter-update limits. Tests under `tests/` cover source selection, vehicle isolation, transitions and driver-override behavior.
## Manual Bolt controller
The manual Bolt ACC 2022–2023, ACC Pedal 2022–2023, CC 2022–2023, CC 2018–2021 and CC 2017 identities default to the StarPilot controller. Their generation policy owns asymmetric torque conversion, feedforward, friction, center shaping and request history. It adjusts the live VehicleModel steering ratio for CC 2017 and CC 2018–2021. Saved STANDARD selection preserves the upstream controller and ratio. Ordinary Bolt EUV keeps its standard controller.
The default Bolt policy uses its established constant proportional gain and generation calibration. Manual friction is available with either controller, independently of automatic learning. The current torque estimator does not authorize learned parameters for GM; this feature preserves that producer policy. Starting the optional parameter host requires manual adjustments On and a valid custom-friction profile for the exact vehicle and current calibration. Enabling it takes effect on the next drive. An absent profile leaves the existing parameter path unchanged; old device-wide tuning values are retained but never activated automatically.
Once started, the host follows the refresh and transition rules above. Returning friction to its source, or turning manual adjustments Off, removes the manual override and uses the admitted learned or vehicle values according to the running learning policy. Invalid settings or inactive lateral control restore vehicle values. The Bolt editor uses the existing calibration-relative friction bounds. It does not expose lateral-acceleration factor editing: the StarPilot Bolt conversion uses a fixed nonlinear curve, and historical factor selections also affected offset selection. Profiles containing that unsupported choice require an explicit reset for this vehicle; other vehicle profiles are preserved.
Manual steering ratio/delay and FLM trial overrides are separate from these ordinary manual preferences. Steering and longitudinal admission remain with the current control and safety owners.
Vehicle-specific torque behavior is selected once in `torque_extension.py`. The upstream controller calls its generic update and parameter hooks; Controls calls its generic vehicle-model ratio hook. Standard selection constructs no extension and retains the upstream numerical path.
## Manual Bolt steering response
The same five manual Bolt identities support a typed custom proportional gain with either selected controller. The editor calls this Steering response: higher values respond more strongly and lower values more gently. Custom values must be finite and within 0.3–0.9. This is independent of friction and automatic learning. A gain-only profile does not start `TorqueHost`, alter torque-parameter sources, or change the estimator cache policy. No raw `SteerKP` value is imported.
A custom gain is bound to the selected controller, its initialized proportional-gain table and the vehicle's current torque calibration. The StarPilot source gain is 0.6; Standard retains its upstream speed table. A changed binding pauses only the gain until explicitly reviewed, preserving friction and other vehicle profiles. Version-1 documents remain readable with no gain override; an explicit gain save adds the version-2 fields.
Starting the gain owner requires manual adjustments On and a qualified custom gain at startup, so enabling it takes effect on the next drive. Once started, saved values refresh at 1 Hz. Returning to Selected controller, turning manual adjustments Off, or invalidating the saved gain restores the captured source table. Changing gain never resets the integrator or request history. This typed refresh contract rejects out-of-range values rather than reproducing the old device-wide knob's clamping and cached reload timing. Steering permission and output limits remain with the existing controllers and safety owners.
## Volt controller
Admitted Volt, Volt ASCM, Volt Camera, Volt CC and Volt 2019 configurations use their StarPilot torque policy by default. It uses the common Volt torque calibration, asymmetric fixed conversion, proportional gain 0.6 and integral gain 0.35. A saved Standard selection preserves the standard torque algorithm and its parameter policy, using the restored vehicle geometry and calibration. The vehicle model uses the selected configuration’s geometry without a policy ratio multiplier. This controller selection does not enable manual torque settings or expand automatic learning.
openpilot/starpilot/lateral/__init__.py
+1
View File
@@ -0,0 +1 @@
"""Standalone lateral feature contracts. Nothing in this package actuates a car."""
openpilot/starpilot/lateral/adjacent_lane_evidence.py
+57
View File
@@ -0,0 +1,57 @@
"""Same-frame adjacent width for automatic lane changes."""
import bisect
import math
from statistics import median
from openpilot.selfdrive.modeld.constants import ModelConstants
def _points(line):
xs, ys = list(line.x), list(line.y)
if (len(xs) != ModelConstants.IDX_N or len(ys) != ModelConstants.IDX_N or
not all(math.isfinite(value) for value in xs + ys) or
any(b <= a for a, b in zip(xs, xs[1:], strict=False))):
return None
return xs, ys
def _at(points, x):
xs, ys = points
if x <= xs[0]:
return ys[0]
if x >= xs[-1]:
return ys[-1]
index = bisect.bisect_left(xs, x)
ratio = (x - xs[index - 1]) / (xs[index] - xs[index - 1])
return ys[index - 1] + ratio * (ys[index] - ys[index - 1])
def adjacent_lane_available(model, direction: int, minimum_width_m: float) -> bool:
"""Compare median adjacent width with the saved minimum."""
try:
if not math.isfinite(minimum_width_m) or not 0 <= minimum_width_m <= 4.572 or direction not in (-1, 1):
return False
inner, outer, edge = ((1, 0, 0) if direction == -1 else (2, 3, 1))
if (len(model.laneLines) != 4 or len(model.roadEdges) != 2 or len(model.laneLineProbs) != 4 or
len(model.laneLineStds) != 4 or len(model.roadEdgeStds) != 2):
return False
if (not all(math.isfinite(value) and 0 <= value <= 1 for value in model.laneLineProbs) or
not all(math.isfinite(value) and value >= 0 for value in list(model.laneLineStds) + list(model.roadEdgeStds))):
return False
inner_points = _points(model.laneLines[inner])
outer_points = _points(model.laneLines[outer])
edge_points = _points(model.roadEdges[edge])
if inner_points is None or outer_points is None or edge_points is None:
return False
xs, ys = inner_points
lane_distances = [abs(y - _at(outer_points, x)) for x, y in zip(xs, ys, strict=True)]
edge_distances = [abs(y - _at(edge_points, x)) for x, y in zip(xs, ys, strict=True)]
if not all(math.isfinite(value) for value in lane_distances + edge_distances):
return False
width = median(lane_distances)
if median(edge_distances) < width:
width = 0.0
return width >= minimum_width_m
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
return False
openpilot/starpilot/lateral/ascm_policy.py
+24
View File
@@ -0,0 +1,24 @@
"""Ordinary ASCM torque selection; factory PID identity stays on PID."""
import math
from opendbc.car.structs import CarParams
from opendbc.car.gm.values import is_ordinary_ascm_profile
from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy
def supported_cp(cp):
if not (is_ordinary_ascm_profile(cp, longitudinal=False) or is_ordinary_ascm_profile(cp, longitudinal=True)):
return False
if cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != "torque":
return False
tune = cp.lateralTuning.torque
return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and
math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0)
class AscmTorquePolicy(GMOrdinaryTorquePolicy):
def __init__(self, parent, cp):
if not supported_cp(cp):
raise ValueError("Unsupported ordinary ASCM torque profile")
super().__init__(parent, cp)
openpilot/starpilot/lateral/auto_lane_change.py
+117
View File
@@ -0,0 +1,117 @@
"""Auto Lane Change qualification from one model frame."""
import math
import time
from dataclasses import replace
from opendbc.car import structs
from openpilot.cereal import log
from openpilot.starpilot.lateral.adjacent_lane_evidence import adjacent_lane_available
from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy
MODEL_MAX_AGE_NS = 150_000_000 # camera EOF uses CLOCK_BOOTTIME
CAR_MAX_AGE_NS = 100_000_000
CONTROL_MAX_AGE_NS = 100_000_000
CALIBRATION_MAX_AGE_NS = 750_000_000
CLOCK_PAIR_MAX_SKEW_NS = 1_000_000
class ClockEpochGuard:
"""After a suspend offset jump, require new inputs in the resumed epoch."""
def __init__(self, now_mono_ns: int, now_boot_ns: int, sample_skew_ns: int = 0):
self.offset_ns = now_boot_ns - now_mono_ns
self.sample_skew_ns = sample_skew_ns
self.barrier_mono_ns = now_mono_ns if sample_skew_ns > CLOCK_PAIR_MAX_SKEW_NS else 0
def ready(self, sm, now_mono_ns: int, now_boot_ns: int, sample_skew_ns: int = 0) -> bool:
if sample_skew_ns < 0 or sample_skew_ns > CLOCK_PAIR_MAX_SKEW_NS:
return False
offset = now_boot_ns - now_mono_ns
if abs(offset - self.offset_ns) > max(CLOCK_PAIR_MAX_SKEW_NS, sample_skew_ns + self.sample_skew_ns):
self.offset_ns = offset
self.sample_skew_ns = sample_skew_ns
self.barrier_mono_ns = now_mono_ns
return False
self.sample_skew_ns = sample_skew_ns
if self.barrier_mono_ns:
try:
services = ("carState", "carControl", "extrinsicsCalibration")
if any(int(sm.logMonoTime[name]) <= self.barrier_mono_ns or
int(sm.recv_time[name] * 1e9) <= self.barrier_mono_ns for name in services):
return False
except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
return False
self.barrier_mono_ns = 0
return True
def steering_capable(cp) -> bool:
try:
return bool(cp is not None and cp.carFingerprint and not (cp.notCar or cp.passive or cp.dashcamOnly) and
cp.steerControlType in (structs.CarParams.SteerControlType.torque, structs.CarParams.SteerControlType.angle,
structs.CarParams.SteerControlType.curvature) and
cp.safetyConfigs and not any(config.safetyModel == structs.CarParams.SafetyModel.allOutput for config in cp.safetyConfigs) and
any(config.safetyModel not in
(structs.CarParams.SafetyModel.silent, structs.CarParams.SafetyModel.noOutput,
structs.CarParams.SafetyModel.allOutput) for config in cp.safetyConfigs))
except (AttributeError, TypeError, ValueError):
return False
def session_policy(saved: LaneChangePolicy, vehicle_capable: bool) -> LaneChangePolicy:
"""Latch the saved request only for a configured steering owner."""
return saved if vehicle_capable else replace(saved, auto_lane_change=False)
def boottime_ns() -> int:
return time.clock_gettime_ns(getattr(time, "CLOCK_BOOTTIME", time.CLOCK_MONOTONIC))
def paired_clocks_ns() -> tuple[int, int, int]:
mono_before = time.monotonic_ns()
boot = boottime_ns()
mono_after = time.monotonic_ns()
return (mono_before + mono_after) // 2, boot, mono_after - mono_before
def _fresh_submaster(sm, service: str, now_mono_ns: int, limit_ns: int) -> bool:
try:
observed = int(sm.logMonoTime[service])
return (sm.seen[service] and sm.alive[service] and sm.valid[service] and
0 < observed <= now_mono_ns and now_mono_ns - observed <= limit_ns - CLOCK_PAIR_MAX_SKEW_NS)
except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
return False
def current_calibration(sm, now_mono_ns: int) -> bool:
if not _fresh_submaster(sm, "extrinsicsCalibration", now_mono_ns, CALIBRATION_MAX_AGE_NS):
return False
try:
calibration = sm["extrinsicsCalibration"]
return (calibration.calStatus == log.ExtrinsicsCalibration.Status.calibrated and
len(calibration.rpyCalib) == 3 and all(math.isfinite(float(v)) for v in calibration.rpyCalib))
except (AttributeError, TypeError, ValueError, OverflowError):
return False
def auto_evidence(sm, model, direction: int, minimum_width_m: float, *, now_mono_ns: int,
now_boot_ns: int, model_valid: bool, vehicle_capable: bool) -> bool:
"""Qualify only the just-filled model, with each clock in its own domain."""
try:
if not model_valid or not vehicle_capable or int(model.frameAge) > 1:
return False
eof = int(model.timestampEof)
if not 0 < eof <= now_boot_ns or now_boot_ns - eof > MODEL_MAX_AGE_NS:
return False
if not (_fresh_submaster(sm, "carState", now_mono_ns, CAR_MAX_AGE_NS) and
_fresh_submaster(sm, "carControl", now_mono_ns, CONTROL_MAX_AGE_NS) and
current_calibration(sm, now_mono_ns)):
return False
control = sm["carControl"]
car_state = sm["carState"]
return (bool(control.latActive and car_state.canValid and not car_state.canTimeout) and
adjacent_lane_available(model, direction, minimum_width_m))
except (AttributeError, TypeError, ValueError, OverflowError):
return False
openpilot/starpilot/lateral/bolt_policy.py
+175
View File
@@ -0,0 +1,175 @@
"""Vehicle-owned default torque controller for the five manual Bolt identities."""
from collections import deque
import math
import numpy as np
from opendbc.car import structs
from opendbc.car.gm.values import CAR
from opendbc.car.lateral import get_friction
from openpilot.cereal import log
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.pid import PIDController
from openpilot.starpilot.lateral import bolt_shaping as shaping
from openpilot.starpilot.lateral.torque_shaping import (
FF_ROLL_OFFSET_FADE_BP,
FF_ROLL_OFFSET_FADE_V,
JERK_GAIN,
KI,
LAT_ACCEL_REQUEST_BUFFER_SECONDS,
LOW_SPEED_X,
LOW_SPEED_Y,
LP_FILTER_CUTOFF_HZ,
MAX_LAT_JERK_UP,
MIN_SPEED,
UNWIND_D_DES_THRESHOLD,
UNWIND_LAT_ACCEL_NEAR_ZERO,
center_chatter_friction_jerk_deadzone,
)
BOLT_GENERATIONS = {
CAR.CHEVROLET_BOLT_CC_2017: 2017,
CAR.CHEVROLET_BOLT_CC_2018_2021: 2018,
CAR.CHEVROLET_BOLT_CC_2022_2023: 2022,
CAR.CHEVROLET_BOLT_ACC_2022_2023: 2022,
CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL: 2022,
}
def supported_cp(cp):
return (
cp.brand == 'gm'
and cp.carFingerprint in BOLT_GENERATIONS
and not cp.notCar
and not cp.passive
and not cp.dashcamOnly
and cp.steerControlType == structs.CarParams.SteerControlType.torque
and cp.lateralTuning.which() == 'torque'
and math.isfinite(cp.lateralTuning.torque.latAccelFactor)
and cp.lateralTuning.torque.latAccelFactor > 0
and math.isfinite(cp.lateralTuning.torque.latAccelOffset)
and math.isfinite(cp.lateralTuning.torque.friction)
and cp.lateralTuning.torque.friction >= 0
)
class BoltTorquePolicy:
def __init__(self, parent, cp):
self.parent = parent
if not supported_cp(cp):
raise ValueError('Unsupported Bolt torque profile')
self.generation = BOLT_GENERATIONS[cp.carFingerprint]
self.dt = parent.dt
# Preserve the source calibration's Float32 parameter representation.
self.ff_positive = float(np.float32(1.03))
self.ff_negative = float(np.float32(float(np.float32(1.07)) * (0.9 if self.generation == 2017 else 1.07)))
self.ki_multiplier = float(np.float32(float(np.float32(0.93)) * (0.9 if self.generation == 2017 else 0.93)))
self.center_boost = float(np.float32(0.026)) if self.generation == 2017 else float(np.float32(float(np.float32(0.02)) * 0.75))
coefficients = {
2017: ((2.15, 1.0, 0.129, 0.0), (2.15, 1.0, 0.145, 0.0)),
2018: ((1.8, 1.1, 0.27, 0.0), (2.0, 1.0, 0.205, 0.0)),
2022: ((2.6531724862969748, 1.1, 0.1919764879840985, 0.0), (2.7031724862969748, 1.0, 0.1469764879840985, 0.0)),
}[self.generation]
lateral_values = np.arange(-5.0, 5.0, 0.01)
torque_values = []
for value in lateral_values:
a, b, c, d = coefficients[0 if value >= 0 else 1]
sig_input = a * value
sigmoid = np.sign(sig_input) * (1 / (1 + math.exp(-abs(sig_input))) - 0.5)
torque_values.append(float(sigmoid * b + value * c + d))
parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral_values, torque_values)
parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque_values, lateral_values)
parent.pid = PIDController(0.6, KI * self.ki_multiplier, rate=1 / self.dt)
parent.update_limits()
self.buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.curvature_buffer = deque([0.0] * self.buffer_len, maxlen=self.buffer_len)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.measurement_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.low_speed_reset_threshold = max(cp.minSteerSpeed, 0.3)
self.previous_measurement = 0.0
self.previous_setpoint = 0.0
self.previous_output = 0.0
self.previous_pressed = False
def steer_ratio_scale(self, speed):
if self.generation == 2017:
return 1.0 + (1.045 - 1.0) * shaping._bolt_2017_high_speed_factor(speed)
return 1.01 if self.generation == 2018 else 1.0
def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay):
parent = self.parent
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = 2
measurement = -vm.calc_curvature(math.radians(cs.steeringAngleDeg - params.angleOffsetDeg), cs.vEgo, params.roll) * cs.vEgo**2
future = curvature * cs.vEgo**2
if not active:
parent.pid.reset()
self.previous_output = 0.0
self.curvature_buffer.append(curvature)
self.previous_measurement = measurement
self.measurement_filter.x = self.jerk_filter.x = 0.0
self.previous_setpoint = future
self.previous_pressed = cs.steeringPressed
pid_log.active = False
return 0.0, 0.0, pid_log
if self.previous_pressed and not cs.steeringPressed:
parent.pid.i *= 0.8
fade = np.interp(cs.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V)
roll = params.roll * ACCELERATION_DUE_TO_GRAVITY * fade
deadzone = abs(vm.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), cs.vEgo, 0.0)) * cs.vEgo**2
delay_frames = int(np.clip(delay / self.dt, 1, self.buffer_len))
expected = self.curvature_buffer[-delay_frames] * cs.vEgo**2
self.curvature_buffer.append(curvature)
raw_jerk = np.clip((future - expected) / max(delay, self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
jerk = np.clip(self.jerk_filter.update(raw_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
setpoint = expected + jerk * delay
unwind = (setpoint - self.previous_setpoint) / self.dt < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO
self.previous_setpoint = setpoint
rate = np.clip(self.measurement_filter.update((measurement - self.previous_measurement) / self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
self.previous_measurement = measurement
lsf = (np.interp(cs.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(cs.vEgo, MIN_SPEED)) ** 2
kp = np.interp(cs.vEgo, parent.pid._k_p[0], parent.pid._k_p[1])
error = (setpoint - measurement) * (1 + lsf / max(kp, 1e-3))
gravity_adjusted = future - roll
ff = gravity_adjusted - parent.torque_params.latAccelOffset * fade
ff *= np.interp(ff, [-0.05, 0.0, 0.05], [self.ff_negative, 1.0, self.ff_positive])
threshold = shaping.get_gm_base_friction_threshold(cs.vEgo)
friction_scale = 1.0
if self.generation == 2022:
ff *= shaping.get_bolt_2022_2023_ff_scale(setpoint, jerk, cs.vEgo)
threshold = shaping.get_bolt_2022_2023_friction_threshold(cs.vEgo, setpoint, jerk)
friction_scale = shaping.get_bolt_2022_2023_friction_scale(cs.vEgo, setpoint, jerk)
elif self.generation == 2018:
threshold = shaping.get_bolt_2018_2021_friction_threshold(cs.vEgo, setpoint, jerk)
friction_scale = shaping.get_bolt_2018_2021_friction_scale(cs.vEgo, setpoint, jerk)
jerk_deadzone = center_chatter_friction_jerk_deadzone(cs.vEgo, setpoint, 0.0)
friction_jerk = math.copysign(max(abs(jerk) - jerk_deadzone, 0.0), jerk)
ff += friction_scale * get_friction(error + JERK_GAIN * friction_jerk, deadzone, threshold, parent.torque_params)
if abs(gravity_adjusted) < 0.15:
ff += np.sign(gravity_adjusted) * self.center_boost * np.interp(abs(gravity_adjusted), [0.0, 0.15], [1.0, 0.0])
if cs.vEgo < self.low_speed_reset_threshold:
parent.pid.reset()
freeze = safety_limited or cs.steeringPressed or cs.vEgo < self.low_speed_reset_threshold or unwind
pid_log.error = float(error)
output = parent.torque_from_lateral_accel(
parent.pid.update(pid_log.error, error_rate=-rate, speed=cs.vEgo, feedforward=ff, freeze_integrator=freeze), parent.torque_params
)
if self.generation == 2022:
output *= shaping.get_bolt_2022_2023_center_output_scale(setpoint, cs.vEgo)
limit = shaping.get_bolt_2022_2023_low_speed_center_output_limit(setpoint, cs.vEgo)
output = float(np.clip(output, -limit, limit))
output = shaping.get_bolt_2022_2023_low_speed_center_output(output, self.previous_output, setpoint, cs.vEgo)
elif self.generation == 2017:
output *= shaping.get_bolt_2017_torque_scale(setpoint, jerk, cs.vEgo)
else:
output *= shaping.get_bolt_2018_2021_dynamic_torque_scale(setpoint, jerk, cs.vEgo)
self.previous_output = float(output)
self.previous_pressed = cs.steeringPressed
pid_log.active = True
pid_log.p, pid_log.i, pid_log.d, pid_log.f = map(float, (parent.pid.p, parent.pid.i, parent.pid.d, parent.pid.f))
pid_log.output = float(-output)
pid_log.actualLateralAccel, pid_log.desiredLateralAccel, pid_log.desiredLateralJerk = map(float, (measurement, setpoint, jerk))
pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output) < 1e-3, cs, safety_limited, curvature_limited))
return -float(output), 0.0, pid_log
openpilot/starpilot/lateral/bolt_shaping.py
+368
View File
@@ -0,0 +1,368 @@
'Default generation-specific Bolt torque shaping.'
import math
import numpy as np
from opendbc.car.common.conversions import Conversions as CV
from openpilot.starpilot.lateral.torque_shaping import sigmoid
BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS
BOLT_2017_STEER_RATIO_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS
BOLT_2017_CENTER_TAPER_LAT = 0.1
BOLT_2017_CENTER_TAPER_WIDTH = 0.03
BOLT_2017_CENTER_TAPER_GAIN = 0.055
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.065, 1.06, 1.055, 1.045]
BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.035, 1.02, 0.995, 0.985]
BOLT_2017_TRANSITION_SPEED = 10.0
BOLT_2017_PHASE_SCALE = 0.12
BOLT_2017_TURN_IN_BOOST_LEFT = 0.28
BOLT_2017_TURN_IN_BOOST_RIGHT = 0.18
BOLT_2017_UNWIND_TAPER_LEFT = 0.08
BOLT_2017_UNWIND_TAPER_RIGHT = 0.28
BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.09
BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.05
BOLT_2018_2021_TORQUE_ONSET = 0.18
BOLT_2018_2021_TORQUE_ONSET_WIDTH = 0.08
BOLT_2018_2021_TORQUE_CUTOFF = 1.05
BOLT_2018_2021_TORQUE_CUTOFF_WIDTH = 0.24
BOLT_2018_2021_JERK_TAPER_CUTOFF = 0.42
BOLT_2018_2021_CENTER_TAPER_LAT = 0.12
BOLT_2018_2021_CENTER_TAPER_WIDTH = 0.04
BOLT_2018_2021_CENTER_TAPER_GAIN = 0.35
BOLT_2018_2021_TRANSITION_SPEED = 8.5
BOLT_2018_2021_PHASE_SCALE = 0.1
BOLT_2018_2021_TURN_IN_BOOST_LEFT = 0.22
BOLT_2018_2021_TURN_IN_BOOST_RIGHT = 0.12
BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT = 0.8
BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT = 1.04
BOLT_2018_2021_FRICTION_MULT = 1.01
BOLT_2018_2021_FRICTION_LAT_RISE = 0.24
BOLT_2018_2021_FRICTION_JERK_RISE = 0.28
BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16
BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.16
BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT = 0.15
BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.25
BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT = 0.08
BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT = 0.08
BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT = 0.17
BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT = 0.27
BOLT_2022_2023_FF_GAIN_LEFT = 0.11
BOLT_2022_2023_FF_GAIN_RIGHT = 0.06
BOLT_2022_2023_FF_ONSET = 0.12
BOLT_2022_2023_FF_ONSET_WIDTH = 0.07
BOLT_2022_2023_FF_CUTOFF = 1.35
BOLT_2022_2023_FF_CUTOFF_WIDTH = 0.28
BOLT_2022_2023_TRANSITION_SPEED = 9.0
BOLT_2022_2023_PHASE_SCALE = 0.12
BOLT_2022_2023_TURN_IN_BOOST_LEFT = 0.18
BOLT_2022_2023_TURN_IN_BOOST_RIGHT = 0.13
BOLT_2022_2023_UNWIND_TAPER_LEFT = 0.38
BOLT_2022_2023_UNWIND_TAPER_RIGHT = 0.4
BOLT_2022_2023_FRICTION_MULT = 1.09
BOLT_2022_2023_FRICTION_LAT_RISE = 0.22
BOLT_2022_2023_FRICTION_JERK_RISE = 0.26
BOLT_2022_2023_CENTER_TAPER_MAX = 0.11
BOLT_2022_2023_CENTER_TAPER_LAT = 0.18
BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH = 0.03
BOLT_2022_2023_CENTER_TAPER_SPEED = 25.0
BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH = 2.5
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_MAX = 0.12
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT = 0.14
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.04
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED = 4.0
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.5
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR = 2.0
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR_WIDTH = 0.7
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 16.5
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LIMIT = 0.38
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT = 0.17
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT_WIDTH = 0.04
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED = 2.5
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH = 0.7
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX = 8.2
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH = 0.6
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SCALE_MIN = 0.62
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_ALPHA_MIN = 0.28
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.08
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT = 0.18
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED = 6.7
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 1.5
BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16
BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.12
BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.26
BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.28
BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT = 0.1
BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT = 0.07
BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.27
BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.28
def get_gm_base_friction_threshold(v_ego: float) -> float:
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_high_speed_factor(v_ego: float) -> float:
return sigmoid((max(v_ego, 0.0) - BOLT_2017_STEER_RATIO_ONSET_SPEED) / BOLT_2017_STEER_RATIO_ONSET_WIDTH)
def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
center_window = sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH)
return 1.0 - BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window
def _bolt_2017_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2017_TRANSITION_SPEED) ** 2)
def _bolt_2017_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh(desired_lateral_accel * desired_lateral_jerk / BOLT_2017_PHASE_SCALE)
def _bolt_2017_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_bolt_2017_base_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 get_bolt_2017_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float = 0.0, v_ego: float = 30.0) -> float:
base_scale = get_bolt_2017_base_torque_scale(desired_lateral_accel)
scale = base_scale
if base_scale > 1.0 and desired_lateral_jerk != 0.0:
low_speed_factor = _bolt_2017_low_speed_factor(v_ego)
phase = _bolt_2017_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
turn_in_boost = 1.0 + _bolt_2017_side_value(desired_lateral_accel, BOLT_2017_TURN_IN_BOOST_LEFT, BOLT_2017_TURN_IN_BOOST_RIGHT) * turn_in_weight * (
0.35 + 0.65 * low_speed_factor
)
unwind_taper = 1.0 - _bolt_2017_side_value(desired_lateral_accel, BOLT_2017_UNWIND_TAPER_LEFT, BOLT_2017_UNWIND_TAPER_RIGHT) * unwind_weight * (
0.45 + 0.55 * low_speed_factor
)
scale = 1.0 + (base_scale - 1.0) * turn_in_boost * max(unwind_taper, 0.0)
return scale * get_bolt_2017_center_taper_scale(desired_lateral_accel, v_ego)
def _bolt_2018_2021_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2018_2021_TRANSITION_SPEED) ** 2)
def _bolt_2018_2021_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh(desired_lateral_accel * desired_lateral_jerk / BOLT_2018_2021_PHASE_SCALE)
def _bolt_2018_2021_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _bolt_2018_2021_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2018_2021_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2018_2021_FRICTION_JERK_RISE)
return _bolt_2018_2021_low_speed_factor(v_ego) * lat_factor * jerk_factor
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)
onset = sigmoid((abs_lateral_accel - BOLT_2018_2021_TORQUE_ONSET) / BOLT_2018_2021_TORQUE_ONSET_WIDTH)
cutoff = sigmoid((BOLT_2018_2021_TORQUE_CUTOFF - abs_lateral_accel) / BOLT_2018_2021_TORQUE_CUTOFF_WIDTH)
return 1.0 + gain * onset * cutoff
def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
base_scale = get_bolt_2018_2021_torque_scale(desired_lateral_accel)
extra_scale = max(base_scale - 1.0, 0.0)
abs_lateral_accel = abs(desired_lateral_accel)
low_speed_factor = _bolt_2018_2021_low_speed_factor(v_ego)
high_speed_factor = 1.0 - low_speed_factor
center_window = sigmoid((BOLT_2018_2021_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2018_2021_CENTER_TAPER_WIDTH)
center_taper = 1.0 - BOLT_2018_2021_CENTER_TAPER_GAIN * high_speed_factor * center_window
phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
jerk_taper = 1.0 / (1.0 + (abs(desired_lateral_jerk) / BOLT_2018_2021_JERK_TAPER_CUTOFF) ** 2)
turn_in_boost = (
1.0
+ _bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_BOOST_LEFT, BOLT_2018_2021_TURN_IN_BOOST_RIGHT)
* turn_in_weight
* low_speed_factor
)
unwind_weight = max(-phase, 0.0)
unwind_taper = 1.0 - _bolt_2018_2021_side_value(
desired_lateral_accel, BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT, BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT
) * unwind_weight * (0.55 + 0.45 * low_speed_factor)
return 1.0 + extra_scale * center_taper * jerk_taper * turn_in_boost * max(unwind_taper, 0.0)
def get_bolt_2018_2021_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
threshold_scale = (
1.0
- _bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT)
* transition_envelope
* turn_in_weight
)
threshold_scale += (
_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT)
* transition_envelope
* unwind_weight
)
return base_threshold * min(max(threshold_scale, 0.82), 1.12)
def get_bolt_2018_2021_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = BOLT_2018_2021_FRICTION_MULT
friction_scale += (
_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT)
* transition_envelope
* turn_in_weight
)
friction_scale -= (
_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT)
* transition_envelope
* unwind_weight
)
return min(max(friction_scale, 0.88), 1.1)
def _bolt_2022_2023_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2022_2023_TRANSITION_SPEED) ** 2)
def _bolt_2022_2023_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh(desired_lateral_accel * desired_lateral_jerk / BOLT_2022_2023_PHASE_SCALE)
def _bolt_2022_2023_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _bolt_2022_2023_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2022_2023_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2022_2023_FRICTION_JERK_RISE)
return _bolt_2022_2023_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_FF_GAIN_LEFT, BOLT_2022_2023_FF_GAIN_RIGHT)
abs_lateral_accel = abs(desired_lateral_accel)
onset = sigmoid((abs_lateral_accel - BOLT_2022_2023_FF_ONSET) / BOLT_2022_2023_FF_ONSET_WIDTH)
cutoff = sigmoid((BOLT_2022_2023_FF_CUTOFF - abs_lateral_accel) / BOLT_2022_2023_FF_CUTOFF_WIDTH)
extra_scale = gain * onset * cutoff
speed_weight = sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) / BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH)
center_weight = sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH)
center_taper = 1.0 - BOLT_2022_2023_CENTER_TAPER_MAX * speed_weight * center_weight
low_speed_factor = _bolt_2022_2023_low_speed_factor(v_ego)
transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
turn_in_boost = (
1.0
+ _bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_BOOST_LEFT, BOLT_2022_2023_TURN_IN_BOOST_RIGHT)
* turn_in_weight
* low_speed_factor
)
unwind_envelope = (0.25 + 0.75 * low_speed_factor) * (1.0 + 0.45 * transition_envelope)
unwind_taper = (
1.0
- _bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_TAPER_LEFT, BOLT_2022_2023_UNWIND_TAPER_RIGHT) * unwind_weight * unwind_envelope
)
return 1.0 + extra_scale * center_taper * turn_in_boost * max(unwind_taper, 0.0)
def get_bolt_2022_2023_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
highway_speed_weight = sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) / BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH)
highway_center_weight = sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH)
low_speed_onset = sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED) / BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_cutoff = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH)
low_speed_center_weight = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
low_speed_floor = sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR) / BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR_WIDTH)
highway_reduction = BOLT_2022_2023_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight
low_speed_reduction = BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_MAX * low_speed_onset * low_speed_cutoff * low_speed_center_weight * low_speed_floor
return 1.0 - min(highway_reduction + low_speed_reduction, 0.95)
def get_bolt_2022_2023_low_speed_center_output_limit(desired_lateral_accel: float, v_ego: float) -> float:
"""Limit small-signal output while the Bolt is in its low-speed chatter band."""
speed_onset = sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH)
speed_cutoff = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX - v_ego) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH)
center_weight = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT_WIDTH)
speed_weight = speed_onset * speed_cutoff
reduction = (1.0 - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LIMIT) * speed_weight * center_weight
return 1.0 - reduction
def get_bolt_2022_2023_low_speed_center_output(output_torque: float, prev_output_torque: float, desired_lateral_accel: float, v_ego: float) -> float:
"""Damp low-speed center reversals without reducing real turn authority."""
speed_weight = sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH) * sigmoid(
(BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX - v_ego) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH
)
center_weight = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT_WIDTH)
envelope = speed_weight * center_weight
output_scale = 1.0 - (1.0 - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SCALE_MIN) * envelope
output_alpha = 1.0 - (1.0 - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_ALPHA_MIN) * envelope
limited_output = output_torque * output_scale
return float(prev_output_torque + output_alpha * (limited_output - prev_output_torque))
def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
center_weight = sigmoid((BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH)
low_speed_weight = sigmoid((BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED - v_ego) / BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH)
base_threshold += BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP * center_weight * low_speed_weight
transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
threshold_scale = (
1.0
- _bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT)
* transition_envelope
* turn_in_weight
)
threshold_scale += (
_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT)
* transition_envelope
* unwind_weight
)
return base_threshold * min(max(threshold_scale, 0.84), 1.14)
def get_bolt_2022_2023_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = BOLT_2022_2023_FRICTION_MULT
friction_scale += (
_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT)
* transition_envelope
* turn_in_weight
)
friction_scale -= (
_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT)
* transition_envelope
* unwind_weight
)
return min(max(friction_scale, 0.92), 1.22)
openpilot/starpilot/lateral/camera_policy.py
+51
View File
@@ -0,0 +1,51 @@
"""Default lateral law for exact GM camera and Silverado interceptor configurations."""
import math
import numpy as np
from opendbc.car.gm.values import CAR, is_ordinary_camera_profile, is_silverado_cc_pedal_profile
from opendbc.car.structs import CarParams
from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy
def supported_cp(cp):
if not (is_ordinary_camera_profile(cp, longitudinal=cp.openpilotLongitudinalControl) or is_silverado_cc_pedal_profile(cp)):
return False
if cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != 'torque':
return False
tune = cp.lateralTuning.torque
return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and
math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0)
def sigmoid(value):
z = math.exp(-value) if value >= 0 else math.exp(value)
return 1 / (1 + z) if value >= 0 else z / (1 + z)
class CameraTorquePolicy(GMOrdinaryTorquePolicy):
def __init__(self, parent, cp):
if not supported_cp(cp):
raise ValueError('Unsupported GM camera or Silverado interceptor torque profile')
self.silverado = cp.carFingerprint in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC)
coefficients = ((3.8, .81, .24, .0465122) if self.silverado else
(3.8060, .8282, .1702, 0.) if cp.carFingerprint == CAR.CHEVROLET_TRAX else None)
if coefficients is not None:
a, b, c, d = coefficients
lateral = np.arange(-5., 5., .01)
torque = []
for value in lateral:
x = a * value
curve = np.sign(x) * (1 / (1 + math.exp(-abs(x))) - .5)
torque.append(float(curve * b + value * c + d))
parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral, torque)
parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque, lateral)
super().__init__(parent, cp)
def taper(self, setpoint, speed):
return 1 - .22 * sigmoid((speed - 12.) / 2.5) * sigmoid((.18 - abs(setpoint)) / .05) if self.silverado else 1.
def feedforward(self, value, setpoint, jerk, speed):
return value * self.taper(setpoint, speed)
def output(self, value, setpoint, speed):
return value * self.taper(setpoint, speed)
openpilot/starpilot/lateral/controller_selection.py
+252
View File
@@ -0,0 +1,252 @@
"""Startup-only selection between the standard torque controller and exact vehicle policies."""
from __future__ import annotations
from dataclasses import dataclass
from enum import StrEnum
import json
from pathlib import Path
from opendbc.car import structs
DOCUMENT_KEY = 'LateralControllerSelection'
DOCUMENT_VERSION = 1
LEARNING_OFF_KEY = 'ForceAutoTuneOff'
MAX_DOCUMENT_BYTES = 4096
KNOWN_POLICIES = {'HYUNDAI_IONIQ_6': 'hyundai', 'GENESIS_G70_2020': 'hyundai', 'GENESIS_GV70_ELECTRIFIED_1ST_GEN': 'hyundai', 'TOYOTA_COROLLA_TSS2': 'toyota'}
KNOWN_POLICIES.update({'HYUNDAI_SONATA_HYBRID': 'hyundai', 'TOYOTA_RAV4_TSS2': 'toyota'})
KNOWN_POLICIES.update(dict.fromkeys(('HYUNDAI_PALISADE', 'HYUNDAI_PALISADE_2023'), 'hyundai'))
KNOWN_POLICIES.update(dict.fromkeys((
'CHEVROLET_BOLT_CC_2017', 'CHEVROLET_BOLT_CC_2018_2021', 'CHEVROLET_BOLT_CC_2022_2023',
'CHEVROLET_BOLT_ACC_2022_2023', 'CHEVROLET_BOLT_ACC_2022_2023_PEDAL',
), 'gm'))
KNOWN_POLICIES.update(dict.fromkeys((
'CHEVROLET_VOLT', 'CHEVROLET_VOLT_ASCM', 'CHEVROLET_VOLT_CAMERA', 'CHEVROLET_VOLT_CC', 'CHEVROLET_VOLT_2019',
), 'gm'))
KNOWN_POLICIES.update(dict.fromkeys((
'CHEVROLET_MALIBU_ASCM', 'BUICK_LACROSSE_ASCM', 'BUICK_LACROSSE_ASCM_19US',
'CADILLAC_ESCALADE_ASCM', 'CADILLAC_ESCALADE_ESV_2019_ASCM', 'CHEVROLET_SUBURBAN_ASCM',
), 'gm'))
KNOWN_POLICIES.update(dict.fromkeys((
'CADILLAC_XT4', 'CHEVROLET_TRAVERSE', 'CADILLAC_XT5', 'CADILLAC_XT6',
'BUICK_BABYENCLAVE', 'CHEVROLET_BLAZER', 'CHEVROLET_MALIBU_SDGM',
), 'gm'))
KNOWN_POLICIES.update(dict.fromkeys((
'CADILLAC_CT6_CC', 'CADILLAC_XT4_CC', 'CADILLAC_XT5_CC', 'CHEVROLET_EQUINOX_CC',
'CHEVROLET_MALIBU_CC', 'CHEVROLET_MALIBU_HYBRID_CC', 'CHEVROLET_SUBURBAN_CC', 'CHEVROLET_TRAILBLAZER_CC', 'GMC_YUKON_CC',
), 'gm'))
KNOWN_POLICIES.update(dict.fromkeys((
'CHEVROLET_SILVERADO', 'CHEVROLET_EQUINOX', 'CHEVROLET_TRAILBLAZER', 'CHEVROLET_TRAX',
'GMC_YUKON', 'CHEVROLET_SUBURBAN_CAMERA', 'CHEVROLET_SILVERADO_CC', 'CHEVROLET_SUBURBAN',
), 'gm'))
KNOWN_POLICIES.update(dict.fromkeys((
'HYUNDAI_IONIQ_5',
'HYUNDAI_IONIQ_EV_LTD', 'HYUNDAI_IONIQ_EV_2020',
'HYUNDAI_SONATA',
'HYUNDAI_ELANTRA_2022_NON_SCC', 'HYUNDAI_ELANTRA_HEV_2022_NON_SCC',
'KIA_XCEED_PHEV',
'KIA_NIRO_EV', 'KIA_NIRO_PHEV_2022',
'KIA_STINGER_2022',
'KIA_FORTE', 'KIA_FORTE_2019_NON_SCC', 'KIA_FORTE_2021_NON_SCC',
'KIA_EV6',
'KIA_CARNIVAL_2025', 'KIA_CARNIVAL_HEV_4TH_GEN',
'HYUNDAI_TUCSON_4TH_GEN',
'HYUNDAI_KONA_NON_SCC',
'HYUNDAI_KONA_EV_2022',
), 'hyundai'))
class ControllerMode(StrEnum):
STANDARD = 'standard'
STARPILOT = 'starpilot'
@dataclass(frozen=True)
class ControllerSelection:
mode: ControllerMode
policy: str | None
source: str
def policy_for(CP) -> str | None:
"""Return only an already-implemented, exact torque policy identity."""
if CP.notCar:
return None
from openpilot.starpilot.lateral.hkg_vehicle_policy import policy_for as hkg_vehicle_policy
profile = hkg_vehicle_policy(CP)
if profile is not None:
return profile
from openpilot.starpilot.lateral.corolla_tss2_policy import supported_cp as corolla_supported
from openpilot.starpilot.lateral.genesis_g70_policy import supported_cp as g70_supported
from openpilot.starpilot.lateral.genesis_gv70_policy import supported_cp as gv70_supported
from openpilot.starpilot.lateral.bolt_policy import supported_cp as bolt_supported
from openpilot.starpilot.lateral.volt_policy import supported_cp as volt_supported
from openpilot.starpilot.lateral.ascm_policy import supported_cp as ascm_supported
from openpilot.starpilot.lateral.sdgm_policy import supported_cp as sdgm_supported
from openpilot.starpilot.lateral.ordinary_cc_policy import supported_cp as cc_supported
from openpilot.starpilot.lateral.camera_policy import supported_cp as camera_supported
from openpilot.starpilot.lateral.suburban_policy import supported_cp as suburban_supported
from opendbc.car.gm.values import is_silverado_cc_pedal_profile
if suburban_supported(CP):
return 'suburban'
if camera_supported(CP):
return 'silverado_cc' if is_silverado_cc_pedal_profile(CP) else 'ordinary_camera'
if cc_supported(CP):
return 'ordinary_cc'
if sdgm_supported(CP):
return 'ordinary_sdgm'
if ascm_supported(CP):
return 'ordinary_ascm'
if volt_supported(CP):
return 'volt'
if bolt_supported(CP):
return 'bolt'
if corolla_supported(CP):
return 'corolla_tss2'
from openpilot.starpilot.lateral.rav4_tss2_policy import supported_cp as rav4_supported
if rav4_supported(CP):
return 'rav4_tss2'
from openpilot.starpilot.lateral.palisade_policy import supported_cp as palisade_supported
from openpilot.starpilot.lateral.sonata_hybrid_policy import supported_cp as sonata_hybrid_supported
if sonata_hybrid_supported(CP):
return 'sonata_hybrid'
if palisade_supported(CP):
return 'palisade'
if gv70_supported(CP):
return 'genesis_gv70_electrified'
if g70_supported(CP):
return 'genesis_g70_2020'
if (str(CP.carFingerprint) == 'HYUNDAI_IONIQ_6' and str(CP.brand) == 'hyundai' and
CP.steerControlType == structs.CarParams.SteerControlType.torque and
CP.lateralTuning.which() == 'torque' and not CP.dashcamOnly and not CP.passive):
return 'ioniq6'
return None
def turn_assist_supported(CP) -> bool:
return bool(CP.steerControlType == structs.CarParams.SteerControlType.torque and
CP.lateralTuning.which() == 'torque' and not CP.notCar and not CP.passive and not CP.dashcamOnly and
any(config.safetyModel not in (structs.CarParams.SafetyModel.noOutput,
structs.CarParams.SafetyModel.silent, structs.CarParams.SafetyModel.allOutput)
for config in CP.safetyConfigs))
def model_turn_assist_supported(CP) -> bool:
return turn_assist_supported(CP) and policy_for(CP) == 'ioniq6'
def default_selection(CP) -> ControllerSelection:
policy = policy_for(CP)
from openpilot.starpilot.lateral.hkg_vehicle_policy import PROFILES
forte_non_scc = policy == 'kia_forte' and str(CP.carFingerprint) in ('KIA_FORTE_2019_NON_SCC', 'KIA_FORTE_2021_NON_SCC')
mode = ControllerMode.STARPILOT if policy and (policy not in PROFILES or forte_non_scc) else ControllerMode.STANDARD
return ControllerSelection(mode, policy, 'default')
def _unique_pairs(pairs):
result = {}
for key, value in pairs:
if key in result:
raise ValueError('Duplicate controller selection field')
result[key] = value
return result
def parse_document(raw: bytes) -> dict:
"""Reject unknown, ambiguous or partial documents before any saved edit."""
if len(raw) > MAX_DOCUMENT_BYTES:
raise ValueError('Oversize controller selection')
document = json.loads(raw.decode('utf-8'), object_pairs_hook=_unique_pairs)
if (not isinstance(document, dict) or set(document) != {'version', 'vehicles'} or
type(document['version']) is not int or document['version'] != DOCUMENT_VERSION):
raise ValueError('Invalid controller selection version')
vehicles = document['vehicles']
if not isinstance(vehicles, dict) or len(vehicles) > len(KNOWN_POLICIES):
raise ValueError('Invalid controller selection vehicles')
for fingerprint, choice in vehicles.items():
if (fingerprint not in KNOWN_POLICIES or not isinstance(choice, dict) or set(choice) != {'brand', 'mode'} or
choice['brand'] != KNOWN_POLICIES[fingerprint] or type(choice['mode']) is not str or
choice['mode'] not in (ControllerMode.STANDARD, ControllerMode.STARPILOT)):
raise ValueError('Invalid controller selection choice')
return document
def replace_mode(raw: bytes | None, CP, mode: ControllerMode) -> bytes:
"""Change only one exact platform's saved choice, preserving other entries."""
if policy_for(CP) is None:
raise ValueError('No exact torque policy for CarParams')
mode = ControllerMode(mode)
document = {'version': DOCUMENT_VERSION, 'vehicles': {}} if raw is None else parse_document(raw)
vehicles = dict(document['vehicles'])
vehicles[str(CP.carFingerprint)] = {'brand': str(CP.brand), 'mode': mode.value}
encoded = json.dumps({'version': DOCUMENT_VERSION, 'vehicles': vehicles},
sort_keys=True, separators=(',', ':'), allow_nan=False).encode()
if len(encoded) > MAX_DOCUMENT_BYTES:
raise ValueError('Oversize controller selection')
return encoded
def selection_from_bytes(CP, raw: bytes | None) -> ControllerSelection:
default = default_selection(CP)
if raw is None or default.policy is None:
return default
try:
choice = parse_document(raw)['vehicles'].get(str(CP.carFingerprint))
if choice is None or choice['brand'] != str(CP.brand):
return default
return ControllerSelection(ControllerMode(choice['mode']), default.policy, 'saved')
except (UnicodeError, ValueError, TypeError, OverflowError):
return ControllerSelection(default.mode, default.policy, 'invalid')
def read_selection(params, CP) -> ControllerSelection:
"""Read once at controls startup; a later settings write cannot change this controller."""
if policy_for(CP) is None:
return default_selection(CP)
try:
with Path(params.get_param_path(DOCUMENT_KEY)).open('rb') as source:
return selection_from_bytes(CP, source.read(MAX_DOCUMENT_BYTES + 1))
except FileNotFoundError:
return default_selection(CP)
except OSError:
default = default_selection(CP)
return ControllerSelection(default.mode, default.policy, 'invalid')
def controller_uses_native_learning(selection: ControllerSelection) -> bool:
# RAV4 adds only two shaping stages to the modern native controller.
return selection.mode == ControllerMode.STANDARD or selection.policy == "rav4_tss2"
def learning_allowed(params, CP, *, selection: ControllerSelection | None = None) -> bool:
"""Latch the exact controller's learning policy at each process startup.
An unrelated vehicle must not inherit another platform's learning preference.
Callers that already chose their controller supply that same startup selection.
"""
from openpilot.starpilot.lateral.learning_preferences import read_preference
preference = read_preference(params, CP)
if not preference.valid or not preference.enabled:
return False
if policy_for(CP) is None:
return True
selected = read_selection(params, CP) if selection is None else selection
from openpilot.starpilot.lateral.gm_geometry_runtime import read_geometry
geometry = read_geometry(params, CP)
if geometry.force_off:
return False
if geometry.force_auto:
return selected.source != 'invalid'
return controller_uses_native_learning(selected) and (selected.policy != "rav4_tss2" or selected.source != "invalid")
openpilot/starpilot/lateral/corolla_tss2_policy.py
+194
View File
@@ -0,0 +1,194 @@
"""Default Corolla TSS2 torque shaping from StarPilot ba901b5f (no FLM overrides)."""
import math
from collections import deque
import numpy as np
from openpilot.cereal import log
from opendbc.car import structs
from opendbc.car.lateral import get_friction
from opendbc.car.toyota.values import CAR
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.pid import PIDController
from openpilot.starpilot.lateral.torque_shaping import (
INTERP_SPEEDS,
KP_INTERP,
KI,
LAT_ACCEL_REQUEST_BUFFER_SECONDS,
LP_FILTER_CUTOFF_HZ,
MAX_LAT_JERK_UP,
LOW_SPEED_X,
LOW_SPEED_Y,
MIN_SPEED,
JERK_GAIN,
FF_ROLL_OFFSET_FADE_BP,
FF_ROLL_OFFSET_FADE_V,
UNWIND_D_DES_THRESHOLD,
UNWIND_LAT_ACCEL_NEAR_ZERO,
center_chatter_friction_jerk_deadzone,
sigmoid as _sigmoid,
get_standard_friction_threshold,
)
def supported_cp(CP):
return (
CP.brand == "toyota"
and CP.carFingerprint == CAR.TOYOTA_COROLLA_TSS2
and CP.steerControlType == structs.CarParams.SteerControlType.torque
and CP.lateralTuning.which() == "torque"
and not CP.dashcamOnly
and not CP.passive
)
TOYOTA_COROLLA_TSS2_PHASE_SCALE = 0.12
TOYOTA_COROLLA_TSS2_TURN_IN_FF_BOOST = 0.035
TOYOTA_COROLLA_TSS2_UNWIND_FF_REDUCTION = 0.06
TOYOTA_COROLLA_TSS2_CURVE_LAT_ONSET = 0.24
TOYOTA_COROLLA_TSS2_CURVE_LAT_WIDTH = 0.1
TOYOTA_COROLLA_TSS2_SPEED_ONSET = 4.0
TOYOTA_COROLLA_TSS2_SPEED_ONSET_WIDTH = 1.5
TOYOTA_COROLLA_TSS2_SPEED_CUTOFF = 24.0
TOYOTA_COROLLA_TSS2_SPEED_CUTOFF_WIDTH = 3.0
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_MAX = 0.3
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT = 0.18
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_GAIN = 0.12
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_ONSET = 12.0
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.0
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF = 25.0
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF_WIDTH = 3.0
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT = 0.24
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.1
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK = 0.25
TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK_WIDTH = 0.1
def get_toyota_corolla_tss2_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
"""Add a small, transition-only turn-in correction for Corolla TSS2 torque EPS."""
if desired_lateral_accel == 0.0:
return 1.0
phase = math.tanh(desired_lateral_accel * desired_lateral_jerk / TOYOTA_COROLLA_TSS2_PHASE_SCALE)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
curve_weight = _sigmoid((abs(desired_lateral_accel) - TOYOTA_COROLLA_TSS2_CURVE_LAT_ONSET) / TOYOTA_COROLLA_TSS2_CURVE_LAT_WIDTH)
speed_weight = _sigmoid((v_ego - TOYOTA_COROLLA_TSS2_SPEED_ONSET) / TOYOTA_COROLLA_TSS2_SPEED_ONSET_WIDTH) * _sigmoid(
(TOYOTA_COROLLA_TSS2_SPEED_CUTOFF - v_ego) / TOYOTA_COROLLA_TSS2_SPEED_CUTOFF_WIDTH
)
boost = TOYOTA_COROLLA_TSS2_TURN_IN_FF_BOOST
unwind_reduction = TOYOTA_COROLLA_TSS2_UNWIND_FF_REDUCTION
return 1.0 + curve_weight * speed_weight * (boost * turn_in_weight - unwind_reduction * unwind_weight)
def get_toyota_corolla_tss2_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
"""Reduce center-only friction chasing on highway-sized model corrections."""
speed_weight = _sigmoid(
(v_ego - TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_ONSET) / TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH
) * _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF - v_ego) / TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF_WIDTH)
center_weight = _sigmoid(
(TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT_WIDTH
)
calm_weight = _sigmoid(
(TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK - abs(desired_lateral_jerk)) / TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK_WIDTH
)
gain = TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_GAIN
return get_standard_friction_threshold(v_ego) * (1.0 + gain * speed_weight * center_weight * calm_weight)
def get_toyota_corolla_tss2_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
"""Taper only near-center crawl-speed torque during manual handoff."""
center_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH)
low_speed_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED - v_ego) / TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
reduction = TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_MAX * center_weight * low_speed_weight
return max(1.0 - reduction, 0.65)
class CorollaTSS2TorquePolicy:
def __init__(self, parent, CP):
if not supported_cp(CP):
raise ValueError("Corolla TSS2 policy does not match CarParams")
self.parent = parent
self.dt = parent.dt
parent.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1 / self.dt)
parent.update_limits()
self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.curvature_request_buffer = deque([0.0] * self.request_buffer_len, maxlen=self.request_buffer_len)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.previous_measurement = 0.0
self.prev_desired_lateral_accel = 0.0
self.prev_steering_pressed = False
self.low_speed_reset_threshold = max(CP.minSteerSpeed, 0.3)
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
parent = self.parent
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = 2
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
measurement = measured_curvature * CS.vEgo**2
future_desired_lateral_accel = desired_curvature * CS.vEgo**2
if not active:
parent.pid.reset()
self.curvature_request_buffer.append(desired_curvature)
self.previous_measurement = measurement
self.measurement_rate_filter.x = 0.0
self.jerk_filter.x = 0.0
self.prev_desired_lateral_accel = future_desired_lateral_accel
self.prev_steering_pressed = CS.steeringPressed
pid_log.active = False
return 0.0, 0.0, pid_log
if self.prev_steering_pressed and not CS.steeringPressed:
parent.pid.i *= 0.8
roll_offset_fade = float(np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V))
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade
curvature_deadzone = abs(VM.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), CS.vEgo, 0.0))
lateral_accel_deadzone = curvature_deadzone * CS.vEgo**2
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len))
expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo**2
self.curvature_request_buffer.append(desired_curvature)
raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt)
raw_lateral_jerk = float(np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
desired_lateral_jerk = float(np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay
desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt
unwind_detected = desired_lateral_accel_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO
self.prev_desired_lateral_accel = setpoint
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
measurement_rate = float(np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
self.previous_measurement = measurement
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
current_kp = np.interp(CS.vEgo, parent.pid._k_p[0], parent.pid._k_p[1])
error = setpoint - measurement
error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3))
pid_log.error = float(error_with_lsf)
ff = gravity_adjusted_future_lateral_accel - parent.torque_params.latAccelOffset * roll_offset_fade
ff *= get_toyota_corolla_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
friction_threshold = get_toyota_corolla_tss2_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
friction_jerk_deadzone = center_chatter_friction_jerk_deadzone(CS.vEgo, setpoint, 0.0)
friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), desired_lateral_jerk)
ff += get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, parent.torque_params)
if CS.vEgo < self.low_speed_reset_threshold:
parent.pid.reset()
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold or unwind_detected
output_lataccel = parent.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
output_torque = parent.torque_from_lateral_accel(output_lataccel, parent.torque_params)
output_torque *= get_toyota_corolla_tss2_center_output_scale(setpoint, CS.vEgo)
pid_log.active = True
pid_log.p = float(parent.pid.p)
pid_log.i = float(parent.pid.i)
pid_log.d = float(parent.pid.d)
pid_log.f = float(parent.pid.f)
pid_log.output = float(-output_torque)
pid_log.actualLateralAccel = float(measurement)
pid_log.desiredLateralAccel = float(setpoint)
pid_log.desiredLateralJerk = float(desired_lateral_jerk)
pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
self.prev_steering_pressed = CS.steeringPressed
return -output_torque, 0.0, pid_log
openpilot/starpilot/lateral/gain_runtime.py
+68
View File
@@ -0,0 +1,68 @@
"""Startup-selected manual gain ownership; torque parameter learning stays independent."""
from __future__ import annotations
import time
from openpilot.starpilot.lateral.torque_runtime import gm_manual_supported_cp as supported_cp
from openpilot.starpilot.lateral.torque_supported import BOLT_VEHICLES
from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, MAX_DOCUMENT_BYTES, GainBasis, parse_document
from openpilot.starpilot.saved_source import read_saved
REFRESH_NS = 1_000_000_000
def gain_basis(cp, controller) -> GainBasis:
tune = cp.lateralTuning.torque
return GainBasis(str(controller.controller_mode),
(tuple(controller.pid._k_p[0]), tuple(controller.pid._k_p[1])),
(tune.latAccelFactor, tune.latAccelOffset, tune.friction))
def _selected(params, cp, basis: GainBasis) -> float | None:
try:
if str(cp.carFingerprint) in BOLT_VEHICLES:
advanced, readable = read_saved(params, "AdvancedLateralTune", 64)
if not readable or advanced != b"1":
return None
raw, readable = read_saved(params, DOCUMENT_KEY, MAX_DOCUMENT_BYTES)
if not readable or raw is None:
return None
profile = parse_document(raw).get(str(cp.carFingerprint))
if profile is None or profile.gain_basis != basis or profile.proportional_gain.mode != "custom":
return None
return profile.proportional_gain.custom_value
except (OSError, ValueError, TypeError, UnicodeError, OverflowError):
return None
class GenericGainOwner:
def __init__(self, params, cp, controller, basis: GainBasis, value: float):
self.params = params
self.cp = cp
self.pid = controller.pid
self.basis = basis
self.last_refresh_ns: int | None = None
self.current_table = basis.source_table
self._apply(value)
def _apply(self, value: float | None) -> None:
table = self.basis.source_table if value is None else ((0.0,), (value,))
if table != self.current_table:
self.pid._k_p = [list(row) for row in table]
self.current_table = table
def refresh(self, *, now_ns: int | None = None) -> None:
now_ns = time.monotonic_ns() if now_ns is None else now_ns
if self.last_refresh_ns is not None and 0 <= now_ns - self.last_refresh_ns < REFRESH_NS:
return
self.last_refresh_ns = now_ns
self._apply(_selected(self.params, self.cp, self.basis))
def create_gain_owner(params, cp, controller) -> GenericGainOwner | None:
if not supported_cp(cp):
return None
basis = gain_basis(cp, controller)
value = _selected(params, cp, basis)
return None if value is None else GenericGainOwner(params, cp, controller, basis, value)
openpilot/starpilot/lateral/genesis_g70_policy.py
+436
View File
@@ -0,0 +1,436 @@
"""Default Genesis G70 2020 torque shaping from StarPilot ba901b5f (no FLM overrides)."""
import math
from collections import deque
import numpy as np
from openpilot.starpilot.lateral.torque_extension import apply_turn_assist
from openpilot.cereal import log
from opendbc.car import structs
from opendbc.car.lateral import get_friction
from opendbc.car.hyundai.values import CAR
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY, CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.pid import PIDController
from openpilot.starpilot.lateral.torque_shaping import (
INTERP_SPEEDS,
KP_INTERP,
KI,
LAT_ACCEL_REQUEST_BUFFER_SECONDS,
LP_FILTER_CUTOFF_HZ,
MAX_LAT_JERK_UP,
LOW_SPEED_X,
LOW_SPEED_Y,
MIN_SPEED,
JERK_GAIN,
FF_ROLL_OFFSET_FADE_BP,
FF_ROLL_OFFSET_FADE_V,
UNWIND_D_DES_THRESHOLD,
UNWIND_LAT_ACCEL_NEAR_ZERO,
center_chatter_friction_jerk_deadzone,
sigmoid as _sigmoid,
get_standard_friction_threshold,
)
def supported_cp(CP):
return (
CP.brand == "hyundai"
and CP.carFingerprint == CAR.GENESIS_G70_2020
and CP.steerControlType == structs.CarParams.SteerControlType.torque
and CP.lateralTuning.which() == "torque"
and not CP.dashcamOnly
and not CP.passive
)
GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10
GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION = 0.50
GENESIS_G70_CURVE_TURN_IN_SPEED_BP = [20.0, 25.0]
GENESIS_G70_CURVE_TURN_IN_LAT_BP = [0.35, 0.70]
GENESIS_G70_FRICTION_THRESHOLD_SPEED_BP = [10.0, 20.0]
GENESIS_G70_FRICTION_THRESHOLD_SPEED_V = [1.0, 2.0]
GENESIS_G70_FRICTION_SPEED_ONSET = 10.0
GENESIS_G70_FRICTION_SPEED_ONSET_WIDTH = 3.0
GENESIS_G70_FRICTION_SPEED_CUTOFF = 35.0
GENESIS_G70_FRICTION_SPEED_CUTOFF_WIDTH = 6.0
GENESIS_G70_FRICTION_CENTER_LAT = 0.28
GENESIS_G70_FRICTION_CENTER_LAT_WIDTH = 0.10
GENESIS_G70_FRICTION_CALM_JERK = 0.35
GENESIS_G70_FRICTION_CALM_JERK_WIDTH = 0.10
GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.39
GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.26
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED = 35.0 * CV.MPH_TO_MS
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT = 0.35
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.15
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF = 1.75
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF_WIDTH = 0.30
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK = 0.20
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH = 0.12
GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.30
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0
GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0
GENESIS_G70_LOW_SPEED_CENTER_TAPER_MAX = 0.06
GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT = 0.14
GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.05
GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.5
GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.2
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_MAX = 0.24
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED = 6.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED_WIDTH = 1.5
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR = 7.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR_WIDTH = 3.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL = 8.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL_WIDTH = 4.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_BLEND = 0.50
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_REDUCTION = 0.85
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.34
GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.13
GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.17
GENESIS_G70_UNWIND_FF_JERK = 0.08
GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.11
GENESIS_G70_UNWIND_FF_SPEED = 18.0
GENESIS_G70_UNWIND_FF_SPEED_WIDTH = 3.0
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.22
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10
GENESIS_G70_OUTPUT_SMOOTHING_SPEED = 40.0 * CV.MPH_TO_MS
GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS
GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT = 0.42
GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.14
GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.18
GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10
GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC = 0.03
GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT = 0.08
GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45
GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0
GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0
def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_standard_friction_threshold(v_ego)
base_threshold *= np.interp(v_ego, GENESIS_G70_FRICTION_THRESHOLD_SPEED_BP, GENESIS_G70_FRICTION_THRESHOLD_SPEED_V)
speed_onset = _sigmoid((v_ego - GENESIS_G70_FRICTION_SPEED_ONSET) / GENESIS_G70_FRICTION_SPEED_ONSET_WIDTH)
speed_cutoff = _sigmoid((GENESIS_G70_FRICTION_SPEED_CUTOFF - v_ego) / GENESIS_G70_FRICTION_SPEED_CUTOFF_WIDTH)
center_weight = _sigmoid((GENESIS_G70_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_FRICTION_CENTER_LAT_WIDTH)
calm_jerk_weight = _sigmoid((GENESIS_G70_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) /
GENESIS_G70_FRICTION_CALM_JERK_WIDTH)
gain = (GENESIS_G70_FRICTION_THRESHOLD_GAIN * speed_onset * speed_cutoff *
center_weight * calm_jerk_weight)
return base_threshold * (1.0 + gain)
def get_genesis_g70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float,
desired_lateral_jerk: float = 0.0,
measured_lateral_accel: float = 0.0) -> float:
speed_weight = _sigmoid((v_ego - GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED) /
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_G70_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH)
deadzone = GENESIS_G70_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight
if desired_lateral_accel * desired_lateral_jerk > 0.0:
turn_in_weight = (np.interp(v_ego, GENESIS_G70_CURVE_TURN_IN_SPEED_BP, [0.0, 1.0]) *
np.interp(abs(desired_lateral_accel), GENESIS_G70_CURVE_TURN_IN_LAT_BP, [0.0, 1.0]))
deadzone += (GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION * turn_in_weight *
max(abs(desired_lateral_jerk) - deadzone, 0.0))
overshoot = max(abs(measured_lateral_accel) - abs(desired_lateral_accel), 0.0)
if (desired_lateral_accel * desired_lateral_jerk < 0.0 and
desired_lateral_accel * measured_lateral_accel > 0.0 and overshoot > 0.0):
curve_speed_weight = _sigmoid(
(max(v_ego, 0.0) - GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED) /
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH
)
curve_onset_weight = _sigmoid(
(abs(desired_lateral_accel) - GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT) /
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH
)
curve_cutoff_weight = _sigmoid(
(GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF - abs(desired_lateral_accel)) /
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF_WIDTH
)
jerk_weight = _sigmoid(
(abs(desired_lateral_jerk) - GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK) /
GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH
)
overshoot_weight = _sigmoid((overshoot - 0.08) / 0.10)
boundary_weight = get_genesis_g70_overshoot_blend(desired_lateral_accel, measured_lateral_accel)
boundary_weight *= min(abs(desired_lateral_jerk) / 0.15, 1.0)
deadzone += (GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX * curve_speed_weight *
curve_onset_weight * curve_cutoff_weight * jerk_weight * overshoot_weight * boundary_weight)
return deadzone
def get_genesis_g70_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((v_ego - GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED) /
GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_G70_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH)
reduction = GENESIS_G70_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight
low_speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_center_weight = _sigmoid((GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
reduction += GENESIS_G70_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight
return 1.0 - reduction
def get_genesis_g70_low_speed_angle_damping(desired_angle_deg: float, actual_angle_deg: float,
current_output_torque: float, v_ego: float) -> float:
angle_error = desired_angle_deg - actual_angle_deg
speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED - max(v_ego, 0.0)) /
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED_WIDTH)
error_weight = _sigmoid((abs(angle_error) - GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR) /
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR_WIDTH)
actual_angle_weight = _sigmoid((abs(actual_angle_deg) - GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL) /
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL_WIDTH)
damping_torque = math.copysign(
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_MAX * speed_weight * error_weight * actual_angle_weight,
-angle_error,
)
if abs(damping_torque) < 1e-4:
return current_output_torque
if current_output_torque * damping_torque >= 0.0:
damping_torque *= GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_BLEND
return float(np.clip(current_output_torque + damping_torque, -1.0, 1.0))
def get_genesis_g70_low_speed_output_limit(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) /
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH)
return max(0.05, 1.0 - GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_REDUCTION * speed_weight * center_weight)
def get_genesis_g70_angle_output_scale(steering_angle_deg: float, output_torque: float) -> float:
"""Ease G70 torque as it approaches the EPS high-angle protection threshold."""
if steering_angle_deg == 0.0 or output_torque * steering_angle_deg <= 0.0:
return 1.0
angle_weight = _sigmoid((abs(steering_angle_deg) - GENESIS_G70_ANGLE_OUTPUT_TAPER_START) /
GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH)
return 1.0 - ((1.0 - GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN) * angle_weight)
def get_genesis_g70_overshoot_blend(setpoint: float, measured_lateral_accel: float) -> float:
if setpoint * measured_lateral_accel <= 0.0:
return 0.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
return float(np.interp(abs(setpoint), [0.10, 0.35], [0.0, 1.0]) * min(overshoot / 0.15, 1.0))
def get_genesis_g70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
overshoot_weight = _sigmoid((overshoot - GENESIS_G70_UNWIND_FF_OVERSHOOT) /
GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_UNWIND_FF_JERK) /
GENESIS_G70_UNWIND_FF_JERK_WIDTH)
speed_weight = _sigmoid((v_ego - GENESIS_G70_UNWIND_FF_SPEED) /
GENESIS_G70_UNWIND_FF_SPEED_WIDTH)
boundary_weight = get_genesis_g70_overshoot_blend(setpoint, measured_lateral_accel)
boundary_weight *= min(abs(desired_lateral_jerk) / 0.15, 1.0)
return 1.0 - GENESIS_G70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight * boundary_weight
def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
if (setpoint == 0.0 or setpoint * measured_lateral_accel <= 0.0 or
abs(measured_lateral_accel) <= abs(setpoint)):
return 1.0
tracking_error = abs(measured_lateral_accel - setpoint)
speed_weight = _sigmoid((v_ego - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED) /
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH)
error_weight = _sigmoid((tracking_error - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR) /
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK) /
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH)
unwind_jerk = -math.copysign(1.0, setpoint) * desired_lateral_jerk
phase_weight = float(np.interp(unwind_jerk, [0.0, 0.15], [0.45, 1.0]))
reduction = (GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight *
(0.35 + (0.65 * jerk_weight)) * phase_weight *
get_genesis_g70_overshoot_blend(setpoint, measured_lateral_accel))
return 1.0 - reduction
def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: float,
desired_lateral_accel: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float, dt: float) -> float:
speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_G70_OUTPUT_SMOOTHING_SPEED) /
GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH)
response_time = (GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC * (1.0 - center_weight) +
GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC * center_weight)
measured_overshoot = (desired_lateral_accel * measured_lateral_accel > 0.0 and
abs(measured_lateral_accel) > abs(desired_lateral_accel) + GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT)
reducing_output = (prev_output_torque * output_torque <= 0.0 or
abs(output_torque) < abs(prev_output_torque))
if reducing_output or (desired_lateral_accel * desired_lateral_jerk < 0.0 and measured_overshoot):
response_time = GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC
output_alpha = dt / (response_time + dt)
smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque)
return float(output_torque + speed_weight * (smoothed_output - output_torque))
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX = 0.09
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP = [50.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS]
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP = [0.15, 0.35]
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP = [0.20, 0.50]
def get_genesis_g70_center_measurement_damping_gain(v_ego: float, desired_lateral_accel: float,
measured_lateral_accel: float, desired_lateral_jerk: float) -> float:
speed_weight = np.interp(v_ego, GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP, [0.0, 1.0])
center_weight = np.interp(max(abs(desired_lateral_accel), abs(measured_lateral_accel)),
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP, [1.0, 0.0])
jerk_weight = np.interp(abs(desired_lateral_jerk), GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP, [1.0, 0.0])
return float(GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX * speed_weight * center_weight * jerk_weight)
class GenesisG70TorquePolicy:
def __init__(self, parent, CP):
if not supported_cp(CP):
raise ValueError("Genesis G70 2020 policy does not match CarParams")
self.parent = parent
self.dt = parent.dt
parent.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1 / self.dt)
parent.update_limits()
self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.curvature_request_buffer = deque([0.0] * self.request_buffer_len, maxlen=self.request_buffer_len)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.previous_measurement = 0.0
self.prev_desired_lateral_accel = 0.0
self.prev_steering_pressed = False
self.prev_output_torque = 0.0
self.low_speed_reset_threshold = max(CP.minSteerSpeed, 0.3)
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
parent = self.parent
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = 2
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
measurement = measured_curvature * CS.vEgo**2
future_desired_lateral_accel = desired_curvature * CS.vEgo**2
if not active:
parent.pid.reset()
self.prev_output_torque = 0.0
self.curvature_request_buffer.append(desired_curvature)
self.previous_measurement = measurement
self.measurement_rate_filter.x = 0.0
self.jerk_filter.x = 0.0
self.prev_desired_lateral_accel = future_desired_lateral_accel
self.prev_steering_pressed = CS.steeringPressed
pid_log.active = False
return 0.0, 0.0, pid_log
if self.prev_steering_pressed and not CS.steeringPressed:
parent.pid.i *= 0.8
roll_offset_fade = float(np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V))
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade
curvature_deadzone = abs(VM.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), CS.vEgo, 0.0))
lateral_accel_deadzone = curvature_deadzone * CS.vEgo**2
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len))
expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo**2
self.curvature_request_buffer.append(desired_curvature)
raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt)
raw_lateral_jerk = float(np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
desired_lateral_jerk = float(np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay
desired_lateral_accel_magnitude_rate = (abs(setpoint) - abs(self.prev_desired_lateral_accel)) / self.dt
unwind_detected = desired_lateral_accel_magnitude_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO
self.prev_desired_lateral_accel = setpoint
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
measurement_rate = float(np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
self.previous_measurement = measurement
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
current_kp = np.interp(CS.vEgo, parent.pid._k_p[0], parent.pid._k_p[1])
error = setpoint - measurement
error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3))
pid_log.error = float(error_with_lsf)
ff = gravity_adjusted_future_lateral_accel - parent.torque_params.latAccelOffset * roll_offset_fade
ff *= get_genesis_g70_unwind_ff_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
friction_threshold = get_genesis_g70_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
if not CS.steeringPressed and not self.prev_steering_pressed:
friction_threshold += get_genesis_g70_traffic_friction_increment(
CS.vEgo, setpoint, desired_lateral_jerk, desired_curvature, measured_curvature)
vehicle_deadzone = get_genesis_g70_friction_jerk_deadzone(CS.vEgo, setpoint, desired_lateral_jerk, measurement)
friction_jerk_deadzone = center_chatter_friction_jerk_deadzone(CS.vEgo, setpoint, vehicle_deadzone)
friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), desired_lateral_jerk)
ff += get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, parent.torque_params)
if CS.vEgo < self.low_speed_reset_threshold:
parent.pid.reset()
damping_gain = 0.0 if CS.steeringPressed else get_genesis_g70_center_measurement_damping_gain(
CS.vEgo, setpoint, measurement, desired_lateral_jerk,
)
parent.pid._k_d = [[0.0], [damping_gain]]
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold or unwind_detected
output_lataccel = parent.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
output_torque = parent.torque_from_lateral_accel(output_lataccel, parent.torque_params)
output_torque = apply_turn_assist(parent, CS, VM, params, desired_curvature, output_torque)
if not CS.steeringPressed and CS.vEgo < GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED + 2.0:
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
actual_angle = CS.steeringAngleDeg - params.angleOffsetDeg
output_torque = get_genesis_g70_low_speed_angle_damping(desired_angle, actual_angle, output_torque, CS.vEgo)
output_torque *= get_genesis_g70_center_output_scale(setpoint, CS.vEgo)
output_torque *= get_genesis_g70_high_speed_error_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
output_torque *= get_genesis_g70_angle_output_scale(CS.steeringAngleDeg, -output_torque)
output_limit = get_genesis_g70_low_speed_output_limit(setpoint, CS.vEgo)
output_torque = float(np.clip(output_torque, -output_limit, output_limit))
if not CS.steeringPressed:
output_torque = get_genesis_g70_stabilized_output(output_torque, self.prev_output_torque, setpoint, measurement,
desired_lateral_jerk, CS.vEgo, self.dt)
pid_log.active = True
pid_log.p = float(parent.pid.p)
pid_log.i = float(parent.pid.i)
pid_log.d = float(parent.pid.d)
pid_log.f = float(parent.pid.f)
pid_log.output = float(-output_torque)
pid_log.actualLateralAccel = float(measurement)
pid_log.desiredLateralAccel = float(setpoint)
pid_log.desiredLateralJerk = float(desired_lateral_jerk)
pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
self.prev_steering_pressed = CS.steeringPressed
self.prev_output_torque = float(output_torque)
return -output_torque, 0.0, pid_log
GENESIS_G70_TRAFFIC_FRICTION_CURVATURE_BP = [0.0035, 0.0080]
GENESIS_G70_TRAFFIC_FRICTION_INCREMENT = 0.90
GENESIS_G70_TRAFFIC_FRICTION_JERK_BP = [0.15, 0.50]
GENESIS_G70_TRAFFIC_FRICTION_LAT_BP = [0.12, 0.30]
GENESIS_G70_TRAFFIC_FRICTION_SPEED_BP = [6.0, 8.0]
def get_genesis_g70_traffic_friction_increment(v_ego: float, desired_lateral_accel: float,
desired_lateral_jerk: float, future_curvature: float,
measured_curvature: float) -> float:
speed = max(v_ego, 0.0)
speed_weight = np.interp(speed, GENESIS_G70_TRAFFIC_FRICTION_SPEED_BP, [1.0, 0.0])
request_accel = max(abs(desired_lateral_accel), abs(future_curvature) * speed ** 2)
accel_weight = np.interp(request_accel, GENESIS_G70_TRAFFIC_FRICTION_LAT_BP, [1.0, 0.0])
curvature = max(abs(future_curvature), abs(measured_curvature), abs(desired_lateral_accel) / max(speed ** 2, 1.0))
curve_weight = np.interp(curvature, GENESIS_G70_TRAFFIC_FRICTION_CURVATURE_BP, [1.0, 0.0])
calm_weight = np.interp(abs(desired_lateral_jerk), GENESIS_G70_TRAFFIC_FRICTION_JERK_BP, [1.0, 0.0])
return float(GENESIS_G70_TRAFFIC_FRICTION_INCREMENT * speed_weight * accel_weight * curve_weight * calm_weight)
openpilot/starpilot/lateral/genesis_gv70_policy.py
+159
View File
@@ -0,0 +1,159 @@
"""First-generation Electrified GV70 torque shaping."""
import math
from collections import deque
import numpy as np
from openpilot.cereal import log
from opendbc.car import structs
from opendbc.car.lateral import get_friction
from opendbc.car.hyundai.values import CAR
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.pid import PIDController
from openpilot.starpilot.lateral.torque_shaping import (
INTERP_SPEEDS,
KP_INTERP,
KI,
LAT_ACCEL_REQUEST_BUFFER_SECONDS,
LP_FILTER_CUTOFF_HZ,
MAX_LAT_JERK_UP,
LOW_SPEED_X,
LOW_SPEED_Y,
MIN_SPEED,
JERK_GAIN,
FF_ROLL_OFFSET_FADE_BP,
FF_ROLL_OFFSET_FADE_V,
UNWIND_D_DES_THRESHOLD,
UNWIND_LAT_ACCEL_NEAR_ZERO,
center_chatter_friction_jerk_deadzone,
)
from openpilot.starpilot.lateral.gv70_shaping import (
get_genesis_gv70_friction_threshold,
get_genesis_gv70_friction_jerk_deadzone,
get_genesis_gv70_center_output_scale,
get_genesis_gv70_unwind_ff_scale,
get_genesis_gv70_high_speed_error_scale,
get_genesis_gv70_reversal_output_scale,
get_genesis_gv70_low_speed_center_overshoot_scale,
get_genesis_gv70_stabilized_output,
GENESIS_GV70_MEASUREMENT_DAMPING_SPEED_BP, GENESIS_GV70_MEASUREMENT_DAMPING_V,
)
def supported_cp(CP):
return (
CP.brand == "hyundai"
and CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
and CP.steerControlType == structs.CarParams.SteerControlType.torque
and CP.lateralTuning.which() == "torque"
and not CP.dashcamOnly
and not CP.passive
and not CP.notCar
)
class GenesisGV70TorquePolicy:
def __init__(self, parent, CP):
if not supported_cp(CP):
raise ValueError("Electrified GV70 policy does not match CarParams")
self.parent = parent
self.dt = parent.dt
parent.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI,
[GENESIS_GV70_MEASUREMENT_DAMPING_SPEED_BP, GENESIS_GV70_MEASUREMENT_DAMPING_V],
rate=1 / self.dt)
parent.update_limits()
self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.curvature_request_buffer = deque([0.0] * self.request_buffer_len, maxlen=self.request_buffer_len)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.previous_measurement = 0.0
self.prev_desired_lateral_accel = 0.0
self.prev_steering_pressed = False
self.prev_output_torque = 0.0
self.previous_feedforward = None
self.low_speed_reset_threshold = max(CP.minSteerSpeed, 0.3)
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
parent = self.parent
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = 2
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
measurement = measured_curvature * CS.vEgo**2
future_desired_lateral_accel = desired_curvature * CS.vEgo**2
if not active:
parent.pid.reset()
self.prev_output_torque = 0.0
self.previous_feedforward = None
self.curvature_request_buffer.append(desired_curvature)
self.previous_measurement = measurement
self.measurement_rate_filter.x = 0.0
self.jerk_filter.x = 0.0
self.prev_desired_lateral_accel = future_desired_lateral_accel
self.prev_steering_pressed = CS.steeringPressed
pid_log.active = False
return 0.0, 0.0, pid_log
if self.prev_steering_pressed and not CS.steeringPressed:
parent.pid.i *= 0.8
roll_offset_fade = float(np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V))
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade
curvature_deadzone = abs(VM.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), CS.vEgo, 0.0))
lateral_accel_deadzone = curvature_deadzone * CS.vEgo**2
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len))
expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo**2
self.curvature_request_buffer.append(desired_curvature)
raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt)
raw_lateral_jerk = float(np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
desired_lateral_jerk = float(np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay
desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt
unwind_detected = desired_lateral_accel_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO
self.prev_desired_lateral_accel = setpoint
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
measurement_rate = float(np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
self.previous_measurement = measurement
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
current_kp = np.interp(CS.vEgo, parent.pid._k_p[0], parent.pid._k_p[1])
error = setpoint - measurement
error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3))
pid_log.error = float(error_with_lsf)
ff = gravity_adjusted_future_lateral_accel - parent.torque_params.latAccelOffset * roll_offset_fade
ff *= get_genesis_gv70_unwind_ff_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
friction_threshold = get_genesis_gv70_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
vehicle_deadzone = get_genesis_gv70_friction_jerk_deadzone(CS.vEgo, setpoint)
friction_jerk_deadzone = center_chatter_friction_jerk_deadzone(CS.vEgo, setpoint, vehicle_deadzone)
friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), desired_lateral_jerk)
ff += get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, parent.torque_params)
if CS.vEgo < self.low_speed_reset_threshold:
parent.pid.reset()
# Filter feedforward before PID so feedback remains immediate.
ff *= get_genesis_gv70_center_output_scale(setpoint, CS.vEgo)
ff *= get_genesis_gv70_low_speed_center_overshoot_scale(setpoint, measurement, CS.vEgo)
ff *= get_genesis_gv70_high_speed_error_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
ff *= get_genesis_gv70_reversal_output_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
if not CS.steeringPressed and not self.prev_steering_pressed and self.previous_feedforward is not None:
ff = get_genesis_gv70_stabilized_output(ff, self.previous_feedforward, setpoint,
desired_lateral_jerk, CS.vEgo, self.dt)
self.previous_feedforward = ff
error_rate = 0.0 if CS.steeringPressed else -measurement_rate
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold or unwind_detected
output_lataccel = parent.pid.update(pid_log.error, error_rate=error_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
output_torque = parent.torque_from_lateral_accel(output_lataccel, parent.torque_params)
pid_log.active = True
pid_log.p = float(parent.pid.p)
pid_log.i = float(parent.pid.i)
pid_log.d = float(parent.pid.d)
pid_log.f = float(parent.pid.f)
pid_log.output = float(-output_torque)
pid_log.actualLateralAccel = float(measurement)
pid_log.desiredLateralAccel = float(setpoint)
pid_log.desiredLateralJerk = float(desired_lateral_jerk)
pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
self.prev_steering_pressed = CS.steeringPressed
self.prev_output_torque = float(output_torque)
return -output_torque, 0.0, pid_log
openpilot/starpilot/lateral/gm_geometry_runtime.py
+83
View File
@@ -0,0 +1,83 @@
"""Canonical exact-GM geometry projection and explicit learner intent."""
from dataclasses import dataclass
import time
from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, MAX_DOCUMENT_BYTES, GeometryProfile, parse_document
from openpilot.starpilot.saved_source import read_saved
@dataclass(frozen=True)
class GeometrySelection:
profile: GeometryProfile | None = None
force_off: bool = False
@property
def force_auto(self):
return self.profile is not None and self.profile.learning == 'force_auto' and not self.force_off
def geometry_basis(CP):
return float(CP.steerRatio), float(CP.steerActuatorDelay) + .2
def read_geometry(params, CP):
from openpilot.starpilot.lateral.torque_runtime import gm_manual_supported_cp
if not gm_manual_supported_cp(CP):
return GeometrySelection()
try:
raw, readable = read_saved(params, DOCUMENT_KEY, MAX_DOCUMENT_BYTES)
if not readable or raw is None:
return GeometrySelection()
profile = parse_document(raw).get(str(CP.carFingerprint))
tune = CP.lateralTuning.torque
if (profile is None or profile.geometry is None or
profile.basis != (tune.latAccelFactor, tune.latAccelOffset, tune.friction) or
profile.geometry.basis != geometry_basis(CP)):
return GeometrySelection()
from openpilot.starpilot.lateral.learning_preferences import read_preference
preference = read_preference(params, CP)
if not preference.valid:
return GeometrySelection()
return GeometrySelection(profile.geometry, not preference.enabled or profile.geometry.learning == 'force_off')
except (OSError, ValueError, TypeError, UnicodeError, OverflowError):
return GeometrySelection()
class GeometryPublicationOwner:
"""Project a copy only; estimator messages/state/cache remain upstream-owned."""
def __init__(self, params, CP):
self.params, self.CP = params, CP
self.last_read_ns = None
self.selected = GeometrySelection()
def selection(self):
now = time.monotonic_ns()
if self.last_read_ns is None or not 0 <= now - self.last_read_ns < 1_000_000_000:
self.selected = read_geometry(self.params, self.CP)
self.last_read_ns = now
return self.selected
def parameters(self, original):
selection = self.selection()
profile = selection.profile
if profile is None:
return original
ratio = profile.ratio.custom_value if profile.ratio.mode == 'custom' and not selection.force_auto else None
if selection.force_off and ratio is None:
ratio = profile.basis[0]
if ratio is None and not selection.force_off:
return original
message = original.as_reader().as_builder()
if ratio is not None:
message.vehicleParameters.steerRatio = ratio
if selection.force_off:
message.vehicleParameters.stiffnessFactor = 1.
return message
def delay(self, original):
profile = self.selection().profile
if profile is None or profile.automatic_delay:
return original
message = original.as_reader().as_builder()
message.lateralDelay.lateralDelay = profile.full_delay.custom_value if profile.full_delay.mode == 'custom' else profile.basis[1]
return message
openpilot/starpilot/lateral/gm_ordinary_policy.py
+105
View File
@@ -0,0 +1,105 @@
"""Shared ordinary GM torque law, without vehicle callback selection."""
from collections import deque
import math
import numpy as np
from opendbc.car.lateral import get_friction
from openpilot.cereal import log
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.pid import PIDController
from openpilot.starpilot.lateral import bolt_shaping as shaping
from openpilot.starpilot.lateral.torque_shaping import (
FF_ROLL_OFFSET_FADE_BP,
FF_ROLL_OFFSET_FADE_V,
JERK_GAIN,
KI,
LAT_ACCEL_REQUEST_BUFFER_SECONDS,
LOW_SPEED_X,
LOW_SPEED_Y,
LP_FILTER_CUTOFF_HZ,
MAX_LAT_JERK_UP,
MIN_SPEED,
UNWIND_D_DES_THRESHOLD,
UNWIND_LAT_ACCEL_NEAR_ZERO,
center_chatter_friction_jerk_deadzone,
)
class GMOrdinaryTorquePolicy:
def __init__(self, parent, cp):
self.parent = parent
self.dt = parent.dt
parent.pid = PIDController(0.6, KI, rate=1 / self.dt)
parent.update_limits()
self.buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.curvature_buffer = deque([0.0] * self.buffer_len, maxlen=self.buffer_len)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.measurement_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.low_speed_reset_threshold = max(cp.minSteerSpeed, 0.3)
self.previous_measurement = 0.0
self.previous_setpoint = 0.0
self.previous_pressed = False
def feedforward(self, value, setpoint, jerk, speed):
return value
def output(self, value, setpoint, speed):
return value
def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay):
parent = self.parent
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = 2
measurement = -vm.calc_curvature(math.radians(cs.steeringAngleDeg - params.angleOffsetDeg), cs.vEgo, params.roll) * cs.vEgo**2
future = curvature * cs.vEgo**2
if not active:
parent.pid.reset()
self.curvature_buffer.append(curvature)
self.previous_measurement = measurement
self.measurement_filter.x = self.jerk_filter.x = 0.0
self.previous_setpoint = future
self.previous_pressed = cs.steeringPressed
pid_log.active = False
return 0.0, 0.0, pid_log
if self.previous_pressed and not cs.steeringPressed:
parent.pid.i *= 0.8
fade = np.interp(cs.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V)
roll = params.roll * ACCELERATION_DUE_TO_GRAVITY * fade
deadzone = abs(vm.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), cs.vEgo, 0.0)) * cs.vEgo**2
delay_frames = int(np.clip(delay / self.dt, 1, self.buffer_len))
expected = self.curvature_buffer[-delay_frames] * cs.vEgo**2
self.curvature_buffer.append(curvature)
raw_jerk = np.clip((future - expected) / max(delay, self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
jerk = np.clip(self.jerk_filter.update(raw_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
setpoint = expected + jerk * delay
unwind = (setpoint - self.previous_setpoint) / self.dt < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO
self.previous_setpoint = setpoint
rate = np.clip(self.measurement_filter.update((measurement - self.previous_measurement) / self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
self.previous_measurement = measurement
lsf = (np.interp(cs.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(cs.vEgo, MIN_SPEED)) ** 2
kp = np.interp(cs.vEgo, parent.pid._k_p[0], parent.pid._k_p[1])
error = (setpoint - measurement) * (1 + lsf / max(kp, 1e-3))
gravity_adjusted = future - roll
ff = gravity_adjusted - parent.torque_params.latAccelOffset * fade
ff = self.feedforward(ff, setpoint, jerk, cs.vEgo)
threshold = shaping.get_gm_base_friction_threshold(cs.vEgo)
jerk_deadzone = center_chatter_friction_jerk_deadzone(cs.vEgo, setpoint, 0.0)
friction_jerk = math.copysign(max(abs(jerk) - jerk_deadzone, 0.0), jerk)
ff += get_friction(error + JERK_GAIN * friction_jerk, deadzone, threshold, parent.torque_params)
if cs.vEgo < self.low_speed_reset_threshold:
parent.pid.reset()
freeze = safety_limited or cs.steeringPressed or cs.vEgo < self.low_speed_reset_threshold or unwind
pid_log.error = float(error)
output = parent.torque_from_lateral_accel(
parent.pid.update(pid_log.error, error_rate=-rate, speed=cs.vEgo, feedforward=ff, freeze_integrator=freeze), parent.torque_params
)
output = self.output(output, setpoint, cs.vEgo)
self.previous_pressed = cs.steeringPressed
pid_log.active = True
pid_log.p, pid_log.i, pid_log.d, pid_log.f = map(float, (parent.pid.p, parent.pid.i, parent.pid.d, parent.pid.f))
pid_log.output = float(-output)
pid_log.actualLateralAccel, pid_log.desiredLateralAccel, pid_log.desiredLateralJerk = map(float, (measurement, setpoint, jerk))
pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output) < 1e-3, cs, safety_limited, curvature_limited))
return -float(output), 0.0, pid_log
openpilot/starpilot/lateral/gv70_shaping.py
+272
View File
@@ -0,0 +1,272 @@
"""Exact default first-generation Electrified GV70 shaping from Dom385428f6.
Only default source formulas; learned torque calibration remains caller-owned.
See the repository's retained StarPilot attribution and licenses.
"""
import numpy as np
from openpilot.common.constants import CV
from openpilot.starpilot.lateral.torque_shaping import sigmoid as _sigmoid
def get_hkg_canfd_base_friction_threshold(v_ego):
# Dom's default GM schedule tops out at0.27, below HKG's0.39 floor.
return 0.39
GENESIS_GV70_FRICTION_THRESHOLD_GAIN = 0.12
GENESIS_GV70_FRICTION_SPEED_ONSET = 8.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_SPEED_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_SPEED_CUTOFF = 60.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_SPEED_CUTOFF_WIDTH = 10.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_CENTER_LAT = 0.28
GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.12
GENESIS_GV70_FRICTION_CALM_JERK = 0.35
GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.10
GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX = 0.55
GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT = 0.30
GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED = 12.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5 * CV.MPH_TO_MS
GENESIS_GV70_CENTER_OUTPUT_TAPER_MAX = 0.20
GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT = 0.30
GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED = 22.0 * CV.MPH_TO_MS
GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0 * CV.MPH_TO_MS
GENESIS_GV70_UNWIND_FF_REDUCTION_MAX = 0.35
GENESIS_GV70_UNWIND_FF_OVERSHOOT = 0.15
GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH = 0.18
GENESIS_GV70_UNWIND_FF_JERK = 0.10
GENESIS_GV70_UNWIND_FF_JERK_WIDTH = 0.10
GENESIS_GV70_UNWIND_FF_SPEED = 10.0 * CV.MPH_TO_MS
GENESIS_GV70_UNWIND_FF_SPEED_WIDTH = 4.0 * CV.MPH_TO_MS
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.20
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_MAX = 0.28
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_SPEED = 25.0 * CV.MPH_TO_MS
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_SPEED_WIDTH = 5.0 * CV.MPH_TO_MS
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_ERROR = 0.30
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_ERROR_WIDTH = 0.16
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_JERK = 0.20
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_JERK_WIDTH = 0.10
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MAX = 0.28
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED = 18.0 * CV.MPH_TO_MS
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_WIDTH = 3.5 * CV.MPH_TO_MS
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_CUTOFF = 34.0 * CV.MPH_TO_MS
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_CUTOFF_WIDTH = 4.5 * CV.MPH_TO_MS
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT = 0.22
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT_WIDTH = 0.08
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MIN = 0.06
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT = 0.12
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT_WIDTH = 0.10
GENESIS_GV70_OUTPUT_SMOOTHING_SPEED = 38.0 * CV.MPH_TO_MS
GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT = 0.60
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.16
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC = 0.42
GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.14
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.12
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08
GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55
GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.065
GENESIS_GV70_MEASUREMENT_DAMPING_SPEED_BP = [10.0 * CV.MPH_TO_MS, 35.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS]
GENESIS_GV70_MEASUREMENT_DAMPING_V = [0.0, 0.08, 0.12]
def get_genesis_gv70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_hkg_canfd_base_friction_threshold(v_ego)
speed_onset = _sigmoid((v_ego - GENESIS_GV70_FRICTION_SPEED_ONSET) / GENESIS_GV70_FRICTION_SPEED_ONSET_WIDTH)
speed_cutoff = _sigmoid((GENESIS_GV70_FRICTION_SPEED_CUTOFF - v_ego) / GENESIS_GV70_FRICTION_SPEED_CUTOFF_WIDTH)
center_weight = _sigmoid((GENESIS_GV70_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /
GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH)
calm_jerk_weight = _sigmoid((GENESIS_GV70_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) /
GENESIS_GV70_FRICTION_CALM_JERK_WIDTH)
gain = (GENESIS_GV70_FRICTION_THRESHOLD_GAIN * speed_onset * speed_cutoff *
center_weight * calm_jerk_weight)
return base_threshold * (1.0 + gain)
def get_genesis_gv70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float:
"""Suppress small jerk-driven friction flips around the GV70 lane center."""
speed_weight = _sigmoid((v_ego - GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED) /
GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT_WIDTH)
return GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight
def get_genesis_gv70_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
"""Dampen high-speed center corrections without reducing turn authority."""
speed_weight = _sigmoid((v_ego - GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED) /
GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - (GENESIS_GV70_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight)
def get_genesis_gv70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Remove old-turn feedforward when the GV70 has already over-rotated."""
if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
overshoot_weight = _sigmoid((overshoot - GENESIS_GV70_UNWIND_FF_OVERSHOOT) /
GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_UNWIND_FF_JERK) /
GENESIS_GV70_UNWIND_FF_JERK_WIDTH)
speed_weight = _sigmoid((v_ego - GENESIS_GV70_UNWIND_FF_SPEED) /
GENESIS_GV70_UNWIND_FF_SPEED_WIDTH)
tracking_weight = _genesis_gv70_curve_damping_weight(setpoint, measured_lateral_accel)
return 1.0 - GENESIS_GV70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight * tracking_weight
def get_genesis_gv70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
tracking_error = abs(measured_lateral_accel - setpoint)
if tracking_error <= 0.0:
return 1.0
speed_weight = _sigmoid((v_ego - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED) /
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH)
error_weight = _sigmoid((tracking_error - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR) /
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK) /
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH)
phase_weight = 1.0 if setpoint * desired_lateral_jerk < 0.0 else 0.45
reduction = (GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight *
(0.35 + (0.65 * jerk_weight)) * phase_weight)
return 1.0 - reduction * _genesis_gv70_curve_damping_weight(setpoint, measured_lateral_accel)
def get_genesis_gv70_reversal_output_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
commanded_unwind = setpoint * desired_lateral_jerk < 0.0
measured_reversal = setpoint * measured_lateral_accel < 0.0
if not commanded_unwind and not measured_reversal:
return 1.0
tracking_error = abs(measured_lateral_accel - setpoint)
speed_weight = _sigmoid((v_ego - GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_SPEED) /
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_SPEED_WIDTH)
error_weight = _sigmoid((tracking_error - GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_ERROR) /
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_ERROR_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_JERK) /
GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_JERK_WIDTH)
reduction = (GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_MAX * speed_weight * error_weight * jerk_weight)
return 1.0 - reduction * _genesis_gv70_curve_damping_weight(setpoint, measured_lateral_accel)
def get_genesis_gv70_low_speed_center_overshoot_scale(setpoint: float, measured_lateral_accel: float,
v_ego: float) -> float:
if abs(setpoint) > 0.08 and setpoint * measured_lateral_accel < 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot < GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MIN:
return 1.0
overshoot_weight = _sigmoid((overshoot - GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT) /
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT_WIDTH)
center_weight = _sigmoid((GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT - abs(setpoint)) /
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT_WIDTH)
speed_weight = _sigmoid((v_ego - GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED) /
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_WIDTH)
speed_cutoff = _sigmoid((GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_CUTOFF - v_ego) /
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_CUTOFF_WIDTH)
return 1.0 - (GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MAX * overshoot_weight * center_weight *
speed_weight * speed_cutoff)
def get_genesis_gv70_stabilized_output(output_torque: float, prev_output_torque: float,
desired_lateral_accel: float, desired_lateral_jerk: float,
v_ego: float, dt: float) -> float:
speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_GV70_OUTPUT_SMOOTHING_SPEED) /
GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT - abs(desired_lateral_accel)) /
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH)
curve_weight = 1.0 - center_weight
response_time = (GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC * curve_weight +
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC * center_weight)
unwind_phase = -desired_lateral_accel * desired_lateral_jerk
unwind_weight = _sigmoid((unwind_phase - GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE) /
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH)
response_time += GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC * curve_weight * unwind_weight
changing_direction = (abs(desired_lateral_accel) >= GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT and
prev_output_torque * desired_lateral_accel <= 0.0)
if changing_direction:
response_time = min(response_time, GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC)
output_alpha = dt / (max(response_time, 0.0) + dt)
smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque)
return float(output_torque + speed_weight * (smoothed_output - output_torque))
GENESIS_GV70_CURVE_DAMPING_LAT_BP = [0.25, 0.55]
GENESIS_GV70_CURVE_DAMPING_OVERSHOOT_BP = [0.0, 0.20]
def _genesis_gv70_curve_damping_weight(setpoint: float, measured_lateral_accel: float) -> float:
curve_weight = float(np.interp(abs(setpoint), GENESIS_GV70_CURVE_DAMPING_LAT_BP, [0.0, 1.0]))
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) if setpoint * measured_lateral_accel > 0.0 else 0.0
overshoot_weight = float(np.interp(overshoot, GENESIS_GV70_CURVE_DAMPING_OVERSHOOT_BP, [0.0, 1.0]))
return 1.0 - curve_weight * (1.0 - overshoot_weight)
openpilot/starpilot/lateral/hkg_torque_policy.py
+129
View File
@@ -0,0 +1,129 @@
from collections import deque
import math
import numpy as np
from openpilot.starpilot.lateral.torque_extension import apply_turn_assist
from openpilot.cereal import log
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.pid import PIDController
from opendbc.car import structs
from openpilot.starpilot.lateral.torque_shaping import (
FF_ROLL_OFFSET_FADE_BP,
FF_ROLL_OFFSET_FADE_V,
JERK_GAIN,
KI,
INTERP_SPEEDS,
KP_INTERP,
LAT_ACCEL_REQUEST_BUFFER_SECONDS,
LOW_SPEED_X,
LOW_SPEED_Y,
LP_FILTER_CUTOFF_HZ,
MAX_LAT_JERK_UP,
MIN_SPEED,
UNWIND_D_DES_THRESHOLD,
UNWIND_LAT_ACCEL_NEAR_ZERO,
center_chatter_friction_jerk_deadzone,
)
def supported_torque_cp(cp, vehicles):
if (cp.brand != 'hyundai' or cp.carFingerprint not in vehicles or cp.notCar or cp.passive or cp.dashcamOnly or
cp.steerControlType != structs.CarParams.SteerControlType.torque or cp.lateralTuning.which() != 'torque'):
return False
tune = cp.lateralTuning.torque
return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and
math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0)
class HKGShapedTorquePolicy:
FACTOR_MULT = 1.0
def __init__(self, parent, cp):
if not self.supported_cp(cp):
raise ValueError("Unsupported HKG torque profile")
self.parent = parent
self.dt = parent.dt
parent.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1 / self.dt)
parent.update_limits()
parent.torque_params.latAccelFactor *= self.FACTOR_MULT
self.buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.curvature_buffer = deque([0.0] * self.buffer_len, maxlen=self.buffer_len)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.measurement_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.low_speed_reset_threshold = max(cp.minSteerSpeed, 0.3)
self.previous_measurement = 0.0
self.previous_setpoint = 0.0
self.previous_pressed = False
def feedforward_context(self, value, setpoint, jerk, measurement, speed):
return self.feedforward(value, setpoint, jerk, speed)
def friction_context(self, setpoint, jerk, measurement, speed):
return self.friction(setpoint, jerk, speed)
def jerk_deadzone(self, setpoint, jerk, measurement, speed):
return 0.0
def pid_error_context(self, error):
return error
def output_context(self, value, setpoint, jerk, measurement, speed):
return self.output(value, setpoint, speed)
def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay):
parent = self.parent
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = 2
measurement = -vm.calc_curvature(math.radians(cs.steeringAngleDeg - params.angleOffsetDeg), cs.vEgo, params.roll) * cs.vEgo**2
future = curvature * cs.vEgo**2
if not active:
parent.pid.reset()
self.curvature_buffer.append(curvature)
self.previous_measurement = measurement
self.measurement_filter.x = self.jerk_filter.x = 0.0
self.previous_setpoint = future
self.previous_pressed = cs.steeringPressed
pid_log.active = False
return 0.0, 0.0, pid_log
if self.previous_pressed and not cs.steeringPressed:
parent.pid.i *= 0.8
fade = np.interp(cs.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V)
roll = params.roll * ACCELERATION_DUE_TO_GRAVITY * fade
deadzone = abs(vm.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), cs.vEgo, 0.0)) * cs.vEgo**2
delay_frames = int(np.clip(delay / self.dt, 1, self.buffer_len))
expected = self.curvature_buffer[-delay_frames] * cs.vEgo**2
self.curvature_buffer.append(curvature)
raw_jerk = np.clip((future - expected) / max(delay, self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
jerk = np.clip(self.jerk_filter.update(raw_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
setpoint = expected + jerk * delay
unwind = (setpoint - self.previous_setpoint) / self.dt < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO
self.previous_setpoint = setpoint
rate = np.clip(self.measurement_filter.update((measurement - self.previous_measurement) / self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
self.previous_measurement = measurement
lsf = (np.interp(cs.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(cs.vEgo, MIN_SPEED)) ** 2
kp = np.interp(cs.vEgo, parent.pid._k_p[0], parent.pid._k_p[1])
error = (setpoint - measurement) * (1 + lsf / max(kp, 1e-3))
gravity_adjusted = future - roll
ff = gravity_adjusted - parent.torque_params.latAccelOffset * fade
ff = self.feedforward_context(ff, setpoint, jerk, measurement, cs.vEgo)
threshold, friction_scale = self.friction_context(setpoint, jerk, measurement, cs.vEgo)
jerk_deadzone = center_chatter_friction_jerk_deadzone(cs.vEgo, setpoint, self.jerk_deadzone(setpoint, jerk, measurement, cs.vEgo))
friction_jerk = math.copysign(max(abs(jerk) - jerk_deadzone, 0.0), jerk)
ff += friction_scale * parent.friction(error + JERK_GAIN * friction_jerk, deadzone, threshold, cs, setpoint)
if cs.vEgo < self.low_speed_reset_threshold:
parent.pid.reset()
freeze = safety_limited or cs.steeringPressed or cs.vEgo < self.low_speed_reset_threshold or unwind
pid_log.error = float(self.pid_error_context(error))
output = parent.torque_from_lateral_accel(
parent.pid.update(pid_log.error, error_rate=-rate, speed=cs.vEgo, feedforward=ff, freeze_integrator=freeze), parent.torque_params
)
output = apply_turn_assist(parent, cs, vm, params, curvature, output)
output = self.output_context(output, setpoint, jerk, measurement, cs.vEgo)
self.previous_pressed = cs.steeringPressed
pid_log.active = True
pid_log.p, pid_log.i, pid_log.d, pid_log.f = map(float, (parent.pid.p, parent.pid.i, parent.pid.d, parent.pid.f))
pid_log.output = float(-output)
pid_log.actualLateralAccel, pid_log.desiredLateralAccel, pid_log.desiredLateralJerk = map(float, (measurement, setpoint, jerk))
pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output) < 1e-3, cs, safety_limited, curvature_limited))
return -float(output), 0.0, pid_log
openpilot/starpilot/lateral/hkg_vehicle_policy.py
+178
View File
@@ -0,0 +1,178 @@
import numpy as np
from opendbc.car.hyundai.values import CAR, HyundaiFlags
from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping
from openpilot.starpilot.lateral.hkg_torque_policy import HKGShapedTorquePolicy, supported_torque_cp
PROFILES = {
'ioniq_5': (frozenset((CAR.HYUNDAI_IONIQ_5,)), shaping.IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT),
'ioniq_ev_old': (
frozenset(
(
CAR.HYUNDAI_IONIQ_EV_LTD,
CAR.HYUNDAI_IONIQ_EV_2020,
)
),
shaping.IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT,
),
'sonata': (frozenset((CAR.HYUNDAI_SONATA,)), 1),
'elantra_non_scc': (
frozenset(
(
CAR.HYUNDAI_ELANTRA_2022_NON_SCC,
CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC,
)
),
1,
),
'kia_xceed': (frozenset((CAR.KIA_XCEED_PHEV,)), 1),
'kia_niro_ev': (frozenset((CAR.KIA_NIRO_EV,)), 1),
'kia_niro_phev_2022': (frozenset((CAR.KIA_NIRO_PHEV_2022,)), 1),
'kia_stinger_2022': (frozenset((CAR.KIA_STINGER_2022,)), 1),
'kia_forte': (
frozenset(
(
CAR.KIA_FORTE,
CAR.KIA_FORTE_2019_NON_SCC,
CAR.KIA_FORTE_2021_NON_SCC,
)
),
shaping.KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT,
),
'kia_ev6': (frozenset((CAR.KIA_EV6,)), 1),
'kia_carnival': (
frozenset(
(
CAR.KIA_CARNIVAL_2025,
CAR.KIA_CARNIVAL_HEV_4TH_GEN,
)
),
1,
),
'tucson_4th_gen': (frozenset((CAR.HYUNDAI_TUCSON_4TH_GEN,)), 1),
'kona_non_scc': (frozenset((CAR.HYUNDAI_KONA_NON_SCC,)), 1),
'kona_ev_2022': (frozenset((CAR.HYUNDAI_KONA_EV_2022,)), 1),
}
def policy_for(cp):
return next((key for key, (cars, _) in PROFILES.items() if supported_torque_cp(cp, cars)), None)
class HKGVehicleTorquePolicy(HKGShapedTorquePolicy):
def __init__(self, parent, cp):
self.profile = policy_for(cp)
if self.profile is None:
raise ValueError("Unsupported HKG vehicle torque profile")
self.FACTOR_MULT = PROFILES[self.profile][1]
self.canfd = bool(cp.flags & HyundaiFlags.CANFD)
self.carnival_hev = cp.carFingerprint == CAR.KIA_CARNIVAL_HEV_4TH_GEN
self.requested_curvature = 0.0
self.future_lateral_accel = 0.0
self.steering_pressed = False
self.hev_center_weight = 0.0
super().__init__(parent, cp)
supported_cp = staticmethod(lambda cp: policy_for(cp) is not None)
def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay):
self.requested_curvature = curvature
self.future_lateral_accel = curvature * cs.vEgo ** 2
self.steering_pressed = bool(cs.steeringPressed)
return super().update(active, cs, vm, params, safety_limited, curvature, curvature_limited, delay)
def feedforward_context(self, value, setpoint, jerk, measurement, speed):
if self.profile == 'ioniq_5':
return value * shaping.get_ioniq_5_ff_scale(setpoint, jerk, speed) * shaping.get_ioniq_5_center_taper_scale(setpoint, speed)
if self.profile == 'ioniq_ev_old':
return value * shaping.get_ioniq_ev_old_ff_scale(setpoint, jerk, speed) * shaping.get_ioniq_ev_old_center_taper_scale(setpoint, speed)
if self.profile == 'sonata':
return value * shaping.get_sonata_ff_scale(setpoint, jerk, speed) * shaping.get_sonata_center_taper_scale(setpoint, speed)
if self.profile == 'elantra_non_scc':
return value * shaping.get_elantra_non_scc_ff_scale(setpoint, jerk, speed)
if self.profile == 'kia_xceed':
return value * shaping.get_kia_xceed_ff_scale(setpoint, jerk, speed) * shaping.get_kia_xceed_center_taper_scale(setpoint, speed)
if self.profile == 'kia_forte':
return value * shaping.get_kia_forte_ff_scale(setpoint, jerk, speed) * shaping.get_kia_forte_center_taper_scale(setpoint, speed)
if self.profile == 'kia_ev6':
return value * shaping.get_kia_ev6_ff_scale(setpoint, jerk, speed) * shaping.get_kia_ev6_center_taper_scale(setpoint, speed)
if self.profile == 'kia_carnival':
return value * shaping.get_kia_carnival_unwind_ff_scale(setpoint, measurement, jerk, speed)
return value
def friction_context(self, setpoint, jerk, measurement, speed):
base = shaping.get_hkg_canfd_base_friction_threshold(speed) if self.canfd else shaping.get_standard_friction_threshold(speed)
if self.profile == 'ioniq_5':
return shaping.get_ioniq_5_friction_threshold(speed, setpoint, jerk), 1 + (
shaping.get_ioniq_5_friction_scale(speed, setpoint, jerk) - 1
) * shaping.get_ioniq_5_center_taper_scale(setpoint, speed)
if self.profile == 'kia_niro_ev' and not self.steering_pressed:
return shaping.get_kia_niro_ev_friction_threshold(speed, setpoint, jerk, self.future_lateral_accel), 1
if self.profile == 'kia_niro_phev_2022':
return shaping.get_kia_niro_phev_2022_friction_threshold(speed, setpoint, jerk), 1
if self.profile == 'kia_stinger_2022':
return shaping.get_kia_stinger_2022_friction_threshold(
speed, setpoint, jerk, self.future_lateral_accel, self.steering_pressed or self.previous_pressed), 1
if self.profile == 'kia_forte':
return shaping.get_kia_forte_friction_threshold(speed, setpoint, jerk), 1
if self.profile == 'kia_ev6':
return shaping.get_kia_ev6_friction_threshold(speed, setpoint, jerk), 1 + (
shaping.get_kia_ev6_friction_scale(speed, setpoint, jerk) - 1
) * shaping.get_kia_ev6_center_taper_scale(setpoint, speed)
if self.profile == 'kia_carnival':
self.hev_center_weight = (shaping.get_kia_carnival_hev_center_weight(
speed, setpoint, measurement, jerk, self.future_lateral_accel)
if self.carnival_hev and not self.steering_pressed and not self.previous_pressed else 0.0)
threshold = shaping.get_kia_carnival_friction_threshold(speed, setpoint, jerk)
return (threshold + shaping.KIA_CARNIVAL_HEV_CENTER_FRICTION_INCREMENT * self.hev_center_weight,
shaping.get_kia_carnival_friction_center_fade_scale(setpoint, speed))
if self.profile == 'tucson_4th_gen':
return shaping.get_tucson_4th_gen_friction_threshold(speed, setpoint, jerk), 1
if self.profile == 'kona_non_scc':
return shaping.get_kona_non_scc_friction_threshold(speed, setpoint, jerk), 1
if self.profile == 'kona_ev_2022':
return shaping.get_kona_ev_2022_friction_threshold(speed, setpoint), 1
return (base, 1.0)
def pid_error_context(self, error):
return error * (1.0 - shaping.KIA_CARNIVAL_HEV_CENTER_ERROR_REDUCTION * self.hev_center_weight)
def jerk_deadzone(self, setpoint, jerk, measurement, speed):
if self.profile == 'ioniq_5':
return shaping.get_ioniq_5_friction_jerk_deadzone(speed, setpoint)
if self.profile == 'kia_carnival':
return shaping.get_kia_carnival_friction_jerk_deadzone(speed, setpoint, jerk)
return 0.0
def output_context(self, value, setpoint, jerk, measurement, speed):
if self.profile == 'ioniq_5':
limit = shaping.get_ioniq_5_low_speed_output_limit(
setpoint, jerk, speed, self.requested_curvature if not self.steering_pressed and not self.previous_pressed else 0.0, value)
return float(np.clip(value, -limit, limit))
if self.profile == 'kia_niro_phev_2022':
return value * shaping.get_kia_niro_phev_2022_center_taper_scale(setpoint, speed)
if self.profile == 'kia_stinger_2022':
return value * shaping.get_kia_stinger_2022_center_taper_scale(setpoint, speed)
if self.profile == 'kia_ev6':
return value * shaping.get_kia_ev6_low_speed_center_taper_scale(setpoint, speed) * shaping.get_kia_ev6_center_output_scale(setpoint, speed)
if self.profile == 'kia_carnival':
return (
value
* shaping.get_kia_carnival_center_taper_scale(setpoint, speed)
* shaping.get_kia_carnival_unwind_output_scale(
setpoint, measurement, jerk, speed,
output_torque=value if self.carnival_hev and not self.steering_pressed and not self.previous_pressed else None)
* shaping.get_kia_carnival_highway_transition_output_scale(setpoint, jerk, speed)
)
if self.profile == 'tucson_4th_gen':
return value * shaping.get_tucson_4th_gen_center_taper_scale(setpoint, speed)
if self.profile == 'kona_non_scc':
value *= shaping.get_kona_non_scc_center_taper_scale(setpoint, speed)
if value * setpoint > 0 or setpoint * jerk < 0:
value *= shaping.get_kona_non_scc_highway_transition_output_scale(setpoint, jerk, speed)
return value
if self.profile == 'kona_ev_2022':
return value * shaping.get_kona_ev_2022_center_output_scale(setpoint, speed)
return value
openpilot/starpilot/lateral/hkg_vehicle_shaping.py
+1354
View File
File diff suppressed because it is too large Load Diff
openpilot/starpilot/lateral/ioniq6_policy.py
+897
View File
@@ -0,0 +1,897 @@
"""Ioniq 6 torque shaping; native control retains output ownership and limits."""
from __future__ import annotations
import math
import numpy as np
from opendbc.car import structs
from opendbc.car.hyundai.values import CAR
from openpilot.common.constants import CV
from openpilot.starpilot.lateral.torque_shaping import (
KP_INTERP,
KI,
LAT_ACCEL_REQUEST_BUFFER_SECONDS,
LP_FILTER_CUTOFF_HZ,
MAX_LAT_JERK_UP,
LOW_SPEED_X,
LOW_SPEED_Y,
MIN_SPEED,
JERK_GAIN,
FF_ROLL_OFFSET_FADE_BP,
FF_ROLL_OFFSET_FADE_V,
UNWIND_D_DES_THRESHOLD,
UNWIND_LAT_ACCEL_NEAR_ZERO,
center_chatter_friction_jerk_deadzone,
sigmoid as _sigmoid,
)
HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39
IONIQ_6_CARS = (CAR.HYUNDAI_IONIQ_6,)
def get_hkg_canfd_base_friction_threshold(v_ego: float) -> float:
base = 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]))
return max(base, HKG_CANFD_BASE_FRICTION_THRESHOLD)
def is_ioniq_6_2025_model(CP) -> bool:
if getattr(CP, "carFingerprint", None) not in IONIQ_6_CARS:
return False
try:
versions = [fw.fwVersion.decode("ascii", errors="ignore") if isinstance(fw.fwVersion, bytes) else str(fw.fwVersion)
for fw in CP.carFw]
except (AttributeError, TypeError, ValueError):
return False
return any("230915" in version for version in versions) and any("240206" in version for version in versions)
IONIQ_6_FF_GAIN_LEFT = 0.045
IONIQ_6_FF_GAIN_RIGHT = 0.015
IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22
IONIQ_6_BASE_FRICTION_THRESHOLD = HKG_CANFD_BASE_FRICTION_THRESHOLD
IONIQ_6_FF_ONSET = 0.10
IONIQ_6_FF_ONSET_WIDTH = 0.04
IONIQ_6_FF_CUTOFF = 0.48
IONIQ_6_FF_CUTOFF_WIDTH = 0.12
IONIQ_6_TRANSITION_SPEED = 10.0
IONIQ_6_PHASE_SCALE = 0.10
IONIQ_6_TURN_IN_BOOST_LEFT = 1.64
IONIQ_6_TURN_IN_BOOST_RIGHT = 2.10
IONIQ_6_UNWIND_TAPER_LEFT = 3.18
IONIQ_6_UNWIND_TAPER_RIGHT = 8.20
IONIQ_6_FRICTION_MULT = 0.928
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.78
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.42
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.90
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 10.20
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.44
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.94
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.55
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 9.10
IONIQ_6_CENTER_TAPER_MAX = 0.082
IONIQ_6_CENTER_TAPER_LAT = 0.24
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025
IONIQ_6_CENTER_TAPER_SPEED = 18.0
IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.046
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.10
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.035
IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5
IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8
IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX = 0.10
IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14
IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04
IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5
IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.18
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 1.05
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.22
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.24
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.14
IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088
IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN = 8.5
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX = 16.5
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.5
IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.19
IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.11
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.10
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 2.10
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.20
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 1.00
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.30
IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE = 0.45
IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC = 0.4
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.98
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06
IONIQ_6_UNWIND_HIGH_SPEED_SPEED = 23.2
IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH = 1.7
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.18
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.24
IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.3
IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 1.0
IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.06
IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.035
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.46
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 3.25
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.45
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 1.9
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.20
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 5.5
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.4
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START = 0.66
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH = 0.12
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR = 0.26
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.35, 0.65, 1.0]
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.88, 0.08]
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE = 0.30
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED = 3.35
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH = 0.50
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR = 1.6
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH = 0.95
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE = 10.5
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH = 4.0
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND = 0.52
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035
IONIQ_6_CURVY_SPEED_MIN = 7.2
IONIQ_6_CURVY_SPEED_MAX = 21.5
IONIQ_6_CURVY_SPEED_MIN_WIDTH = 1.1
IONIQ_6_CURVY_SPEED_MAX_WIDTH = 1.8
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT = 0.26
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT = 0.30
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT = 0.22
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT = 0.28
IONIQ_6_CURVY_UNWIND_LAT_START = 0.45
IONIQ_6_CURVY_UNWIND_LAT_END = 3.6
IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH = 0.14
IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH = 0.55
IONIQ_6_CURVY_RIGHT_UNWIND_JERK_ONSET = 0.40
IONIQ_6_CURVY_RIGHT_UNWIND_JERK_WIDTH = 0.22
IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN = 11.5
IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX = 20.5
IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH = 1.2
IONIQ_6_CURVY_TURN_IN_TRIM_LEFT = 0.08
IONIQ_6_CURVY_TURN_IN_TRIM_RIGHT = 0.09
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START = 1.0
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END = 2.5
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH = 0.18
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH = 0.30
IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX = 0.24
IONIQ_6_2023_UNWIND_FF_OVERSHOOT = 0.15
IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH = 0.18
IONIQ_6_2023_UNWIND_FF_JERK = 0.10
IONIQ_6_2023_UNWIND_FF_JERK_WIDTH = 0.10
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET = 8.0
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET_WIDTH = 2.5
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF = 23.5
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF_WIDTH = 2.0
IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS
IONIQ_6_FRICTION_JERK_DEADZONE = 0.30
IONIQ_6_FRICTION_CENTER_FADE_MAX = 0.50
IONIQ_6_FRICTION_CENTER_FADE_LAT = 0.15
IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH = 0.06
IONIQ_6_FRICTION_CENTER_FADE_SPEED = 18.0
IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH = 2.5
IONIQ_6_2025_FRICTION_SCALE_MULT = 0.80
IONIQ_6_2025_FRICTION_JERK_DEADZONE = 0.45
IONIQ_6_2025_CENTER_OUTPUT_TAPER_MAX = 0.32
IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT = 0.35
IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED = 22.0
IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_2025_LOW_SPEED_CENTER_ERROR_SCALE = 0.68
IONIQ_6_2025_LOW_SPEED_CENTER_FRICTION_SCALE = 0.72
IONIQ_6_2025_LOW_SPEED_CENTER_SPEED = 5.0
IONIQ_6_2025_LOW_SPEED_CENTER_SPEED_WIDTH = 1.3
IONIQ_6_2025_LOW_SPEED_CENTER_LAT = 0.22
IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH = 0.10
IONIQ_6_2025_LOW_SPEED_CENTER_JERK = 0.30
IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH = 0.13
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE = 0.22
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF = 0.50
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_RELIEF = 0.20
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.5
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.90
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.18
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.03
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.11
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.40
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.55
def _ioniq_6_sigmoid(x: float) -> float:
return _sigmoid(x)
def _ioniq_6_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_6_TRANSITION_SPEED) ** 2)
def _ioniq_6_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_PHASE_SCALE)
def _ioniq_6_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _ioniq_6_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / IONIQ_6_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / IONIQ_6_FRICTION_JERK_RISE)
return _ioniq_6_low_speed_factor(v_ego) * lat_factor * jerk_factor
def _ioniq_6_curvy_speed_weight(v_ego: float) -> float:
curvy_speed_min = IONIQ_6_CURVY_SPEED_MIN
curvy_speed_max = IONIQ_6_CURVY_SPEED_MAX
onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - curvy_speed_min) / IONIQ_6_CURVY_SPEED_MIN_WIDTH)
cutoff = _ioniq_6_sigmoid((curvy_speed_max - max(v_ego, 0.0)) / IONIQ_6_CURVY_SPEED_MAX_WIDTH)
return onset * cutoff
def _ioniq_6_curvy_turn_in_trim_speed_weight(v_ego: float) -> float:
curvy_turn_in_speed_min = IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN
curvy_turn_in_speed_max = IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX
onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - curvy_turn_in_speed_min) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH)
cutoff = _ioniq_6_sigmoid((curvy_turn_in_speed_max - max(v_ego, 0.0)) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH)
return onset * cutoff
def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float,
directional_taper_scale: float | None = None) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _ioniq_6_side_value(
desired_lateral_accel,
IONIQ_6_FF_GAIN_LEFT,
IONIQ_6_FF_GAIN_RIGHT,
)
abs_lateral_accel = abs(desired_lateral_accel)
onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_FF_ONSET) / IONIQ_6_FF_ONSET_WIDTH)
cutoff = _ioniq_6_sigmoid((IONIQ_6_FF_CUTOFF - abs_lateral_accel) / IONIQ_6_FF_CUTOFF_WIDTH)
extra_scale = gain * onset * cutoff
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _ioniq_6_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_ioniq_6_side_value(
desired_lateral_accel,
IONIQ_6_TURN_IN_BOOST_LEFT,
IONIQ_6_TURN_IN_BOOST_RIGHT,
) *
turn_in_weight * low_speed_factor)
unwind_taper = 1.0 - (_ioniq_6_side_value(
desired_lateral_accel,
IONIQ_6_UNWIND_TAPER_LEFT,
IONIQ_6_UNWIND_TAPER_RIGHT,
) *
unwind_weight * (0.30 + 0.70 * low_speed_factor))
crawl_turn_in_scale = 0.0
if desired_lateral_accel * desired_lateral_jerk > 0.0:
crawl_speed_weight = _ioniq_6_sigmoid((IONIQ_6_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) /
IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH)
crawl_lat_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CRAWL_TURN_IN_FF_LAT) /
IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH)
crawl_turn_in_scale = _ioniq_6_side_value(
desired_lateral_accel,
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT,
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT,
) * crawl_speed_weight * crawl_lat_weight
high_speed_right_turn_in_scale = 0.0
if desired_lateral_accel < 0.0 and desired_lateral_accel * desired_lateral_jerk > 0.0:
high_speed_weight = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED) /
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH)
high_speed_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START) /
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH)
high_speed_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END - abs_lateral_accel) /
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH)
high_speed_right_turn_in_scale = IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST * high_speed_weight * high_speed_lat_onset * high_speed_lat_cutoff
if directional_taper_scale is None:
directional_taper_scale = get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale +
(extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * directional_taper_scale
def get_ioniq_6_2023_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Trim residual curve feedforward when the 2023 car is already over-rotated."""
if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
overshoot_weight = _ioniq_6_sigmoid((overshoot - IONIQ_6_2023_UNWIND_FF_OVERSHOOT) /
IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_2023_UNWIND_FF_JERK) /
IONIQ_6_2023_UNWIND_FF_JERK_WIDTH)
speed_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_2023_UNWIND_FF_SPEED_ONSET) /
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET_WIDTH)
speed_cutoff = _ioniq_6_sigmoid((IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF - v_ego) /
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF_WIDTH)
reduction = (IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight *
speed_onset * speed_cutoff)
return 1.0 - reduction
def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = max(get_hkg_canfd_base_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD)
transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
unwind_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_UNWIND_HIGH_SPEED_SPEED) / IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH)
threshold_scale = 1.0 - (_ioniq_6_side_value(
desired_lateral_accel,
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT,
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT,
) *
transition_envelope * turn_in_weight)
threshold_scale += (_ioniq_6_side_value(
desired_lateral_accel,
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT,
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT,
) *
transition_envelope * unwind_weight * unwind_speed_weight)
return base_threshold * min(max(threshold_scale, 0.82), 1.18)
def get_ioniq_6_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
unwind_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_UNWIND_HIGH_SPEED_SPEED) / IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH)
friction_scale = IONIQ_6_FRICTION_MULT
friction_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT, IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT, IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight * unwind_speed_weight)
return min(max(friction_scale, 0.82), 1.08)
def get_ioniq_6_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_FRICTION_CENTER_FADE_SPEED) / IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_FRICTION_CENTER_FADE_LAT - abs(desired_lateral_accel)) / IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH)
return 1.0 - IONIQ_6_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight
def get_ioniq_6_2025_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED) /
IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - IONIQ_6_2025_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight
def get_ioniq_6_2025_low_speed_output_limit(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Limit small-signal torque at crawl speed while leaving real turn commands open."""
speed_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) /
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH)
calm_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) /
IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH)
center_weight *= calm_weight
limit = (IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE +
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF * (1.0 - center_weight) +
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_RELIEF * (1.0 - speed_weight))
return float(np.clip(limit, IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE, 1.0))
def _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_SPEED - max(v_ego, 0.0)) /
IONIQ_6_2025_LOW_SPEED_CENTER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH)
calm_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) /
IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH)
return speed_weight * center_weight * calm_weight
def get_ioniq_6_2025_low_speed_center_error_scale(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
envelope = _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel, desired_lateral_jerk, v_ego)
return 1.0 - (1.0 - IONIQ_6_2025_LOW_SPEED_CENTER_ERROR_SCALE) * envelope
def get_ioniq_6_2025_low_speed_center_friction_scale(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
envelope = _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel, desired_lateral_jerk, v_ego)
return 1.0 - (1.0 - IONIQ_6_2025_LOW_SPEED_CENTER_FRICTION_SCALE) * envelope
def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH)
high_speed_reduction = IONIQ_6_CENTER_TAPER_MAX * speed_weight * center_weight
highway_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED) / IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH)
highway_center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH)
highway_center_reduction = (IONIQ_6_HIGHWAY_CENTER_TAPER_MAX *
highway_speed_weight * highway_center_weight)
low_mid_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH)
low_mid_cutoff = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX - v_ego) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH)
low_mid_speed_weight = low_mid_onset * low_mid_cutoff
low_mid_center_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH)
low_mid_reduction = IONIQ_6_LOW_MID_CENTER_TAPER_MAX * low_mid_speed_weight * low_mid_center_weight
return 1.0 - min(high_speed_reduction + highway_center_reduction + low_mid_reduction, 0.12)
def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float | None = None) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH)
cutoff = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LAT_END - abs_lateral_accel) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH)
band_weight = onset * cutoff
heavy_band_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH)
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE)
unwind_weight = max(-phase, 0.0) * _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET) /
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH)
low_speed_relief_weight = 0.0
curvy_turn_in_trim_weight = 0.0
if v_ego is not None:
low_speed_weight = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED - max(v_ego, 0.0)) /
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH)
tight_turn_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT) /
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH)
low_speed_relief_weight = IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF * low_speed_weight * tight_turn_weight * (1.0 - unwind_weight)
turn_in_weight = max(phase, 0.0)
curvy_turn_in_speed_weight = _ioniq_6_curvy_turn_in_trim_speed_weight(v_ego)
curvy_turn_in_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START) /
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH)
curvy_turn_in_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END - abs_lateral_accel) /
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH)
curvy_turn_in_trim_weight = curvy_turn_in_speed_weight * curvy_turn_in_lat_onset * curvy_turn_in_lat_cutoff * turn_in_weight
base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT)
unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT)
heavy_base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT)
heavy_unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT)
base_reduction *= 1.0 - low_speed_relief_weight
heavy_base_reduction *= 1.0 - low_speed_relief_weight
reduction = band_weight * (base_reduction + unwind_reduction * unwind_weight)
reduction += heavy_band_weight * (heavy_base_reduction + heavy_unwind_reduction * unwind_weight)
reduction += (_ioniq_6_side_value(desired_lateral_accel,
IONIQ_6_CURVY_TURN_IN_TRIM_LEFT,
IONIQ_6_CURVY_TURN_IN_TRIM_RIGHT) *
curvy_turn_in_trim_weight)
curvy_unwind_weight = 0.0
curvy_unwind_floor_relief = 0.0
if v_ego is not None:
curvy_unwind_phase_weight = unwind_weight
if desired_lateral_accel < 0.0:
curvy_unwind_phase_weight = max(-phase, 0.0) * _ioniq_6_sigmoid(
(abs(desired_lateral_jerk) - IONIQ_6_CURVY_RIGHT_UNWIND_JERK_ONSET) / IONIQ_6_CURVY_RIGHT_UNWIND_JERK_WIDTH)
curvy_unwind_speed_weight = _ioniq_6_curvy_speed_weight(v_ego)
curvy_unwind_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CURVY_UNWIND_LAT_START) /
IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH)
curvy_unwind_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_UNWIND_LAT_END - abs_lateral_accel) /
IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH)
curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * curvy_unwind_phase_weight
curvy_unwind_floor_relief = (_ioniq_6_side_value(desired_lateral_accel,
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT,
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT) *
curvy_unwind_weight)
reduction += (_ioniq_6_side_value(desired_lateral_accel,
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT,
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT) *
curvy_unwind_weight)
floor = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT)
floor -= _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT) * unwind_weight
floor -= curvy_unwind_floor_relief
return max(1.0 - reduction, floor)
def get_ioniq_6_highway_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH)
reduction = IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def get_ioniq_6_highway_transition_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH)
jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK) /
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH)
reduction = IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX * speed_weight * center_weight * jerk_weight
return 1.0 - reduction
def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_angle_deg: float,
current_output_torque: float, v_ego: float) -> float:
angle_error = desired_angle_deg - actual_angle_deg
if desired_angle_deg * angle_error > 0.0:
speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED - max(v_ego, 0.0)) /
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH)
error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR) /
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH)
# Sigmoid tails remain nonzero at zero error. Taper only inside the
# existing transition width so sign reversals cannot introduce a torque step.
error_weight *= min(abs(angle_error) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH, 1.0)
desired_angle_weight = _ioniq_6_sigmoid((abs(desired_angle_deg) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE) /
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH)
tracking_ratio = abs(actual_angle_deg) / max(abs(desired_angle_deg), 1e-3)
tracking_taper = _ioniq_6_sigmoid((tracking_ratio - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START) /
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH)
tracking_scale = max(1.0 - tracking_taper, IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR)
assist_torque = math.copysign(
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE *
speed_weight * error_weight * desired_angle_weight * tracking_scale,
-angle_error,
)
if abs(assist_torque) < 1e-4:
return current_output_torque
if current_output_torque * assist_torque >= 0.0:
add_scale = float(np.interp(abs(current_output_torque),
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP,
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V))
return float(np.clip(current_output_torque + (assist_torque * add_scale), -1.0, 1.0))
return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0))
speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED - max(v_ego, 0.0)) /
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH)
error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR) /
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH)
error_weight *= min(abs(angle_error) / IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH, 1.0)
actual_angle_weight = _ioniq_6_sigmoid((abs(actual_angle_deg) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE) /
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH)
assist_torque = math.copysign(IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE * speed_weight * error_weight * actual_angle_weight, -angle_error)
if abs(assist_torque) < 1e-4:
return current_output_torque
if current_output_torque * assist_torque >= 0.0:
assist_torque *= IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND
return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0))
# Buffer curvature and scale at the current speed to avoid pull-away unwind
# from lateral acceleration recorded at old speeds.
from collections import deque
from openpilot.cereal import log
from opendbc.car.lateral import get_friction
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.pid import PIDController
class Ioniq6TorquePolicy:
"""Per-car controller state; the native LatControlTorque still owns output."""
FACTOR_MULT = IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT
def __init__(self, parent, CP, *, surface=None, turn_assist: bool = False):
# Validate the replay-only surface before changing the parent's PID, factor or limits.
surface_math = None
if surface is not None:
from openpilot.starpilot.flm import torque_surface as surface_math
expected_variant = "firmware_2025" if is_ioniq_6_2025_model(CP) else "standard"
if (str(CP.carFingerprint) != "HYUNDAI_IONIQ_6" or
not isinstance(surface, surface_math.Ioniq6Surface) or surface.variant != expected_variant):
raise ValueError("Ioniq 6 surface does not match CarParams")
self.turn_assist = bool(turn_assist)
minimum = float(CP.minSteerSpeed)
self.turn_assist_min_speed = max(0.044704, minimum) if math.isfinite(minimum) else math.inf
self.surface = surface
self.surface_math = surface_math
self.parent = parent
self.dt = parent.dt
self.is_2025 = is_ioniq_6_2025_model(CP)
self.vehicle_factor = float(CP.lateralTuning.torque.latAccelFactor)
# Startup limits use raw CP calibration before the 1.22 factor; using compensated
# parameters would widen them before the first live torque update.
startup_params = CP.lateralTuning.torque.as_builder()
startup_pos_limit = parent.lateral_accel_from_torque(parent.steer_max, startup_params)
startup_neg_limit = parent.lateral_accel_from_torque(-parent.steer_max, startup_params)
parent.pid = PIDController([[0.0], [KP_INTERP[-1]]], KI, rate=1 / self.dt)
parent.pid.set_limits(startup_pos_limit, startup_neg_limit)
parent.torque_params.latAccelFactor = self.vehicle_factor * IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT
self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.curvature_request_buffer = deque([0.0] * self.request_buffer_len, maxlen=self.request_buffer_len)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.directional_taper_filter = FirstOrderFilter(1.0, IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC, self.dt)
self.previous_measurement = 0.0
self.prev_desired_lateral_accel = 0.0
self.prev_steering_pressed = False
self.low_speed_reset_threshold = min(max(CP.minSteerSpeed, 0.3), IONIQ_6_LOW_SPEED_PID_RESET_SPEED)
def turn_assist_active(self, CS) -> bool:
return bool(self.turn_assist and math.isfinite(CS.vEgo) and
CS.vEgo >= self.turn_assist_min_speed and not CS.standstill and
CS.gearShifter == structs.CarState.GearShifter.drive and
not CS.steeringPressed and not CS.steerFaultTemporary and not CS.steerFaultPermanent)
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
parent = self.parent
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = 2
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
measurement = measured_curvature * CS.vEgo ** 2
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
if not active:
parent.pid.reset()
self.curvature_request_buffer.append(desired_curvature)
self.previous_measurement = measurement
self.measurement_rate_filter.x = 0.0
self.jerk_filter.x = 0.0
self.directional_taper_filter.x = 1.0
self.prev_desired_lateral_accel = future_desired_lateral_accel
self.prev_steering_pressed = CS.steeringPressed
pid_log.active = False
return 0.0, 0.0, pid_log
if self.prev_steering_pressed and not CS.steeringPressed:
parent.pid.i *= 0.8
roll_offset_fade = float(np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V))
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade
effective_deadzone_deg = parent.steering_angle_deadzone_deg
if self.surface is not None:
effective_deadzone_deg += self.surface_math.center_deadband(self.surface, CS.vEgo)
curvature_deadzone = abs(VM.calc_curvature(math.radians(effective_deadzone_deg), CS.vEgo, 0.0))
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len))
expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo ** 2
self.curvature_request_buffer.append(desired_curvature)
raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt)
raw_lateral_jerk = float(np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
desired_lateral_jerk = float(np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay
desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt
unwind_detected = desired_lateral_accel_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO
self.prev_desired_lateral_accel = setpoint
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
measurement_rate = float(np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP))
self.previous_measurement = measurement
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
current_kp = np.interp(CS.vEgo, parent.pid._k_p[0], parent.pid._k_p[1])
error = setpoint - measurement
error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3))
if self.is_2025:
error_with_lsf *= get_ioniq_6_2025_low_speed_center_error_scale(setpoint, desired_lateral_jerk, CS.vEgo)
pid_log.error = float(error_with_lsf)
ff = gravity_adjusted_future_lateral_accel - parent.torque_params.latAccelOffset * roll_offset_fade
if self.surface is None:
center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo)
directional_target = get_ioniq_6_directional_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
else:
center_taper = self.surface_math.center_taper(self.surface, CS.vEgo, setpoint)
directional_target = self.surface_math.directional_taper_target(self.surface, CS.vEgo, setpoint, desired_lateral_jerk)
directional_taper = self.directional_taper_filter.update(directional_target)
if self.surface is None:
ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo, directional_taper_scale=directional_taper) * center_taper
else:
ff *= self.surface_math.feedforward_scale(self.surface, CS.vEgo, setpoint, desired_lateral_jerk, directional_taper) * center_taper
if not self.is_2025:
ff *= get_ioniq_6_2023_unwind_ff_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
if self.surface is None:
friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(center_taper, 1e-3)
else:
friction_threshold = self.surface_math.friction_threshold(self.surface, CS.vEgo, setpoint, desired_lateral_jerk) / max(center_taper, 1e-3)
friction_scale = get_ioniq_6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = 1.0 + ((friction_scale - 1.0) * center_taper)
friction_scale *= get_ioniq_6_friction_center_fade_scale(setpoint, CS.vEgo)
if self.is_2025:
friction_scale *= IONIQ_6_2025_FRICTION_SCALE_MULT
friction_scale *= get_ioniq_6_2025_low_speed_center_friction_scale(setpoint, desired_lateral_jerk, CS.vEgo)
vehicle_jerk_deadzone = IONIQ_6_2025_FRICTION_JERK_DEADZONE if self.is_2025 else IONIQ_6_FRICTION_JERK_DEADZONE
friction_jerk_deadzone = center_chatter_friction_jerk_deadzone(CS.vEgo, setpoint, vehicle_jerk_deadzone)
friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), desired_lateral_jerk)
ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone,
friction_threshold, parent.torque_params)
if CS.vEgo < self.low_speed_reset_threshold:
parent.pid.reset()
freeze_integrator = (steer_limited_by_safety or CS.steeringPressed or
CS.vEgo < self.low_speed_reset_threshold or unwind_detected)
output_lataccel = parent.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo,
feedforward=ff, freeze_integrator=freeze_integrator)
output_torque = parent.torque_from_lateral_accel(output_lataccel, parent.torque_params)
if self.turn_assist_active(CS):
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
actual_angle = CS.steeringAngleDeg - params.angleOffsetDeg
if math.isfinite(desired_angle) and math.isfinite(actual_angle) and math.isfinite(output_torque):
if self.surface is None:
output_torque = get_ioniq_6_low_speed_angle_assist_torque(desired_angle, actual_angle, output_torque, CS.vEgo)
else:
output_torque = self.surface_math.low_speed_output(self.surface, self.surface_math.SurfaceInput(
CS.vEgo, setpoint, desired_lateral_jerk, desired_angle, actual_angle, output_torque, False, directional_taper))
output_torque *= get_ioniq_6_highway_output_taper_scale(setpoint, CS.vEgo)
output_torque *= get_ioniq_6_highway_transition_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
if self.is_2025:
output_torque *= get_ioniq_6_2025_center_output_scale(setpoint, CS.vEgo)
output_limit = get_ioniq_6_2025_low_speed_output_limit(setpoint, desired_lateral_jerk, CS.vEgo)
output_torque = float(np.clip(output_torque, -output_limit, output_limit))
pid_log.active = True
pid_log.p = float(parent.pid.p)
pid_log.i = float(parent.pid.i)
pid_log.d = float(parent.pid.d)
pid_log.f = float(parent.pid.f)
pid_log.output = float(-output_torque)
pid_log.actualLateralAccel = float(measurement)
pid_log.desiredLateralAccel = float(setpoint)
pid_log.desiredLateralJerk = float(desired_lateral_jerk)
pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output_torque) < 1e-3,
CS, steer_limited_by_safety, curvature_limited))
self.prev_steering_pressed = CS.steeringPressed
return -output_torque, 0.0, pid_log
openpilot/starpilot/lateral/lane_centering.py
+275
View File
@@ -0,0 +1,275 @@
"""Bounded lane-centering contribution for lateral control.
The caller owns model freshness, axis authority, and final curvature limiting.
This module neither reads settings nor sends commands.
"""
from dataclasses import dataclass
from enum import StrEnum
import math
from numbers import Real
from typing import Any
import capnp
import numpy as np
from openpilot.cereal import log
STRENGTH_BASELINE = 1.5
STRENGTH_MAX = 2 * STRENGTH_BASELINE
MIN_SPEED = 5.0
MIN_LANE_PROB = 0.6
MAX_LANE_STD = 0.3
MIN_LANE_WIDTH = 2.6
MAX_LANE_WIDTH = 4.8
MAX_OFFSET = 0.3
MIN_CENTER_TO_LINE = 1.1
MAX_CORRECTION = 0.004 * 0.30
CENTER_ERROR_DEADBAND = 0.08
E2E_MAX_PATH_STD = 0.35
E2E_BREAK_IN_START = 0.15
E2E_BREAK_IN_FULL = 0.50
ACQUIRE_TAU = 0.4
RELEASE_TAU = 0.2
class ControlMode(StrEnum):
OFF = "off"
LATERAL_ONLY = "lateral_only"
LONGITUDINAL_ONLY = "longitudinal_only"
COMBINED = "combined"
@property
def lateral_authority(self) -> bool:
return self in (ControlMode.LATERAL_ONLY, ControlMode.COMBINED)
@dataclass(frozen=True)
class LaneCenteringSettings:
enabled: bool = False
offset_m: float = 0.0
e2e_authority: float = 1.0
pause_on_signal: bool = True
strength: float = STRENGTH_BASELINE
@dataclass(frozen=True)
class LaneCenteringRequest:
"""Optional host configuration; vehicle/model facts come from the host itself."""
mode: ControlMode
settings: LaneCenteringSettings
elapsed_seconds: float
time_discontinuity: bool = False
@dataclass(frozen=True)
class LaneCenteringInput:
model: Any
model_sample_id: int
model_valid: bool
base_curvature: float
speed_mps: float
mode: ControlMode
lateral_active: bool
settings: LaneCenteringSettings
elapsed_seconds: float
turn_signal_active: bool = False
driver_override: bool = False
time_discontinuity: bool = False
@dataclass(frozen=True)
class LaneCenteringResult:
# None means the caller's proposed curvature was invalid and must be rejected.
candidate_curvature: float | None
correction: float
direction: int
reason: str
def _finite_number(value: Any) -> float | None:
if isinstance(value, bool) or not isinstance(value, Real):
return None
try:
result = float(value)
except (TypeError, ValueError, OverflowError):
return None
return result if math.isfinite(result) else None
def _path(model_path: Any) -> tuple[np.ndarray, np.ndarray] | None:
try:
return _path_xy(model_path.x, model_path.y)
except (AttributeError, TypeError, ValueError, OverflowError):
return None
def _path_xy(xs: Any, ys: Any) -> tuple[np.ndarray, np.ndarray] | None:
try:
x = np.asarray(xs)
y = np.asarray(ys)
except (TypeError, ValueError, OverflowError):
return None
if x.ndim != 1 or y.ndim != 1 or len(x) < 2 or len(x) != len(y):
return None
if x.dtype.kind not in "fiu" or y.dtype.kind not in "fiu":
return None
x, y = x.astype(float), y.astype(float)
if not np.isfinite(x).all() or not np.isfinite(y).all() or not np.all(np.diff(x) > 0):
return None
return x, y
def _at(path: tuple[np.ndarray, np.ndarray], distance: float) -> float | None:
x, y = path
if not x[0] <= distance <= x[-1]:
return None
return float(np.interp(distance, x, y))
def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: float, *, _paths=None) -> float | None:
"""Return a finite raw curvature correction, or None for unqualified model data."""
speed = _finite_number(speed_mps)
offset = _finite_number(offset_m)
authority = _finite_number(e2e_authority)
if speed is None or offset is None or authority is None or speed < MIN_SPEED:
return None
try:
lines = model.laneLines
probs = model.laneLineProbs
stds = model.laneLineStds
if len(lines) < 3 or len(probs) < 3 or len(stds) < 3:
return None
for i in (1, 2):
prob = _finite_number(probs[i])
std = _finite_number(stds[i])
if prob is None or std is None or not MIN_LANE_PROB <= prob <= 1.0 or not 0.0 <= std <= MAX_LANE_STD:
return None
left_path, right_path, model_path = (_path(lines[1]), _path(lines[2]), _path(model.position)) if _paths is None else _paths[:3]
if left_path is None or right_path is None or model_path is None:
return None
lookahead = float(np.clip(speed, 8.0, 35.0))
left, right, model_y = _at(left_path, lookahead), _at(right_path, lookahead), _at(model_path, lookahead)
if left is None or right is None or model_y is None:
return None
width = right - left
if not MIN_LANE_WIDTH <= width <= MAX_LANE_WIDTH:
return None
safe_offset = min(MAX_OFFSET, max(0.0, width * 0.5 - MIN_CENTER_TO_LINE))
target_y = (left + right) * 0.5 + float(np.clip(offset, -safe_offset, safe_offset))
error = target_y - model_y
error_abs = abs(error)
error = math.copysign(max(0.0, error_abs - CENTER_ERROR_DEADBAND), error)
# Reduce lane authority only when path variance establishes confidence.
try:
std_path = _path_xy(model.position.x, model.position.yStd) if _paths is None else _paths[3]
except (AttributeError, TypeError, ValueError, OverflowError):
std_path = None
if std_path is not None:
path_std = _at(std_path, lookahead)
if path_std is not None and 0.0 <= path_std <= E2E_MAX_PATH_STD:
break_in = float(np.clip((error_abs - E2E_BREAK_IN_START) /
(E2E_BREAK_IN_FULL - E2E_BREAK_IN_START), 0.0, 1.0))
error *= 1.0 - float(np.clip(authority, 0.0, 1.0)) * break_in
correction = 2.0 * error / lookahead ** 2
return correction if math.isfinite(correction) else None
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
return None
def _reader_geometry_key(model):
if not isinstance(model, capnp.lib.capnp._DynamicStructReader):
return None
try:
return (tuple(model.laneLineProbs), tuple(model.laneLineStds),
*((tuple(line.x), tuple(line.y)) for line in (model.laneLines[1], model.laneLines[2])),
tuple(model.position.x), tuple(model.position.y), tuple(model.position.yStd))
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
return None
class LaneCenteringController:
def __init__(self) -> None:
self._correction = 0.0
self._geometry_cache = None
def reset(self) -> None:
self._correction = 0.0
self._geometry_cache = None
def _result(self, base: float, reason: str) -> LaneCenteringResult:
correction = self._correction
return LaneCenteringResult(base + correction, correction, (correction > 0) - (correction < 0), reason)
def _fade(self, base: float, dt: float, reason: str) -> LaneCenteringResult:
self._correction *= math.exp(-dt / RELEASE_TAU)
if abs(self._correction) < 1e-12:
self._correction = 0.0
return self._result(base, reason)
def update(self, observation: LaneCenteringInput) -> LaneCenteringResult:
if not isinstance(observation, LaneCenteringInput):
self.reset()
return LaneCenteringResult(None, 0.0, 0, "invalid_input")
base = _finite_number(observation.base_curvature)
speed = _finite_number(observation.speed_mps)
dt = _finite_number(observation.elapsed_seconds)
settings = observation.settings
offset = _finite_number(settings.offset_m) if isinstance(settings, LaneCenteringSettings) else None
authority = _finite_number(settings.e2e_authority) if isinstance(settings, LaneCenteringSettings) else None
strength = _finite_number(settings.strength) if isinstance(settings, LaneCenteringSettings) else None
if (base is None or speed is None or dt is None or dt <= 0 or
offset is None or authority is None or strength is None or not 0.5 <= strength <= STRENGTH_MAX or type(observation.model_sample_id) is not int or
observation.model_sample_id < 0 or not isinstance(observation.mode, ControlMode) or
type(observation.model_valid) is not bool or type(observation.lateral_active) is not bool or
type(observation.turn_signal_active) is not bool or type(observation.driver_override) is not bool or
type(observation.time_discontinuity) is not bool or
type(settings.enabled) is not bool or type(settings.pause_on_signal) is not bool):
self.reset()
return LaneCenteringResult(base, 0.0, 0, "invalid_input")
if observation.time_discontinuity:
self.reset()
return self._result(base, "time_discontinuity")
if not observation.mode.lateral_authority or not settings.enabled or not observation.lateral_active or \
not observation.model_valid or speed < MIN_SPEED:
self.reset()
return self._result(base, "inactive")
if observation.driver_override:
self.reset()
return self._result(base, "driver_override")
if settings.pause_on_signal and observation.turn_signal_active:
return self._fade(base, dt, "signal_release")
try:
if observation.model.meta.laneChangeState != log.LaneChangeState.off:
self.reset()
return self._result(base, "lane_change")
except (AttributeError, TypeError, ValueError):
self.reset()
return self._result(base, "invalid_model")
# Filtering runs per control tick, including repeated model samples.
geometry = _reader_geometry_key(observation.model)
cache_key = (observation.model_sample_id, geometry)
cached = self._geometry_cache
paths = None
if geometry is not None:
if cached is not None and cached[0] is observation.model and cached[1] == cache_key:
paths = cached[2]
else:
paths = (_path_xy(*geometry[2]), _path_xy(*geometry[3]),
_path_xy(geometry[4], geometry[5]), _path_xy(geometry[4], geometry[6]))
self._geometry_cache = (observation.model, cache_key, paths)
else:
self._geometry_cache = None
raw = raw_correction(observation.model, speed, offset, authority, _paths=paths)
if raw is None or not math.isfinite(raw):
return self._fade(base, dt, "unqualified_model")
target = float(np.clip(raw, -0.004, 0.004)) * 0.30
if strength != 1.0:
target = float(np.clip(target * strength, -MAX_CORRECTION, MAX_CORRECTION))
self._correction += (target - self._correction) * (1.0 - math.exp(-dt / ACQUIRE_TAU))
self._correction = float(np.clip(self._correction, -MAX_CORRECTION, MAX_CORRECTION))
return self._result(base, "qualified")
openpilot/starpilot/lateral/lane_change_preferences.py
+204
View File
@@ -0,0 +1,204 @@
"""Optional, session-scoped preferences for driver-nudged lane changes."""
from dataclasses import dataclass, replace
import json
import math
import os
import stat
from typing import Any
from openpilot.common.constants import CV
KEY = "LaneChangePreferences"
MAX_BYTES = 512
MAX_SPEED_MPS = 100 * CV.MPH_TO_MS
MINIMUM_SPEED_MPS = 5 * CV.MPH_TO_MS
DEFAULT_SPEED_MPS = MINIMUM_SPEED_MPS
@dataclass(frozen=True)
class LaneChangePolicy:
enabled: bool = True
minimum_speed_mps: float = DEFAULT_SPEED_MPS
one_per_signal: bool = True
auto_lane_change: bool = False
auto_delay_s: float = 1.0
minimum_lane_width_m: float = 0.0
close_gap: bool = False
close_gap_seconds: float = 0.75
duration_s: float = 3.0 + 25.0 / 9.0
@dataclass(frozen=True)
class SavedLaneChange:
raw: bytes | None
policy: LaneChangePolicy = LaneChangePolicy()
valid: bool = True
readable: bool = True
def _unique_object(pairs: list[tuple[str, object]]) -> dict[str, object]:
value: dict[str, object] = {}
for key, item in pairs:
if key in value:
raise ValueError("duplicate lane-change field")
value[key] = item
return value
def decode(raw: bytes) -> LaneChangePolicy | None:
if len(raw) > MAX_BYTES:
return None
try:
data = json.loads(raw, object_pairs_hook=_unique_object,
parse_constant=lambda _value: (_ for _ in ()).throw(ValueError("nonfinite")))
except (UnicodeDecodeError, ValueError, TypeError, RecursionError, OverflowError):
return None
if not isinstance(data, dict):
return None
version = data.get("version")
fields = {"version", "enabled", "minimumSpeedMps", "onePerSignal"}
if version in (2, 3, 4):
fields |= {"autoLaneChange", "autoDelayS", "minimumLaneWidthM"}
if version in (3, 4):
fields |= {"closeGap", "closeGapSeconds"}
if version == 4:
fields |= {"durationS"}
if set(data) != fields:
return None
enabled = data["enabled"]
speed, once = data["minimumSpeedMps"], data["onePerSignal"]
if (type(version) is not int or version not in (1, 2, 3, 4) or type(enabled) is not bool or type(once) is not bool or
type(speed) not in (int, float)):
return None
try:
finite_speed = float(speed)
except (ValueError, OverflowError):
return None
if not math.isfinite(finite_speed) or not 0 <= finite_speed <= MAX_SPEED_MPS:
return None
if version == 1:
return LaneChangePolicy(enabled, finite_speed, once)
auto, delay, width = data["autoLaneChange"], data["autoDelayS"], data["minimumLaneWidthM"]
if type(auto) is not bool or type(delay) not in (int, float) or type(width) not in (int, float):
return None
try:
delay, width = float(delay), float(width)
except (ValueError, OverflowError):
return None
if not (math.isfinite(delay) and math.isfinite(width) and 0 <= delay <= 5 and 0 <= width <= 15 * 0.3048):
return None
if version == 2:
return LaneChangePolicy(enabled, finite_speed, once, auto, delay, width)
close_gap, seconds = data["closeGap"], data["closeGapSeconds"]
if type(close_gap) is not bool or type(seconds) not in (int, float):
return None
try:
seconds = float(seconds)
except (ValueError, OverflowError):
return None
if not math.isfinite(seconds) or not 0.75 <= seconds <= 1.0:
return None
duration = data.get("durationS", LaneChangePolicy().duration_s)
if type(duration) not in (int, float) or not math.isfinite(duration) or not 3.0 <= duration <= 8.0:
return None
return LaneChangePolicy(enabled, finite_speed, once, auto, delay, width, close_gap, seconds, float(duration))
def to_value(policy: LaneChangePolicy) -> dict[str, int | bool | float]:
if (type(policy.enabled) is not bool or type(policy.one_per_signal) is not bool or
type(policy.minimum_speed_mps) not in (int, float)):
raise ValueError("invalid lane-change policy")
try:
finite_speed = float(policy.minimum_speed_mps)
except (ValueError, OverflowError) as error:
raise ValueError("invalid lane-change speed") from error
if not math.isfinite(finite_speed) or not 0 <= finite_speed <= MAX_SPEED_MPS:
raise ValueError("invalid lane-change speed")
if type(policy.auto_lane_change) is not bool or type(policy.auto_delay_s) not in (int, float) or type(policy.minimum_lane_width_m) not in (int, float):
raise ValueError("invalid automatic lane-change policy")
try:
delay, width = float(policy.auto_delay_s), float(policy.minimum_lane_width_m)
except (ValueError, OverflowError) as error:
raise ValueError("invalid automatic lane-change bounds") from error
if not (math.isfinite(delay) and math.isfinite(width) and 0 <= delay <= 5 and 0 <= width <= 15 * 0.3048):
raise ValueError("invalid automatic lane-change bounds")
if type(policy.close_gap) is not bool or type(policy.close_gap_seconds) not in (int, float):
raise ValueError("invalid lane-change close gap")
try:
seconds = float(policy.close_gap_seconds)
except (ValueError, OverflowError) as error:
raise ValueError("invalid lane-change close gap") from error
if not math.isfinite(seconds) or not 0.75 <= seconds <= 1.0:
raise ValueError("invalid lane-change close gap")
if type(policy.duration_s) not in (int, float) or not math.isfinite(policy.duration_s) or not 3.0 <= policy.duration_s <= 8.0:
raise ValueError("invalid lane-change duration")
return {"version": 4, "enabled": policy.enabled, "minimumSpeedMps": finite_speed,
"onePerSignal": policy.one_per_signal, "autoLaneChange": policy.auto_lane_change,
"autoDelayS": delay, "minimumLaneWidthM": width,
"closeGap": policy.close_gap, "closeGapSeconds": seconds, "durationS": policy.duration_s}
def read_saved(params: Any) -> SavedLaneChange:
try:
fd = os.open(params.get_param_path(KEY), os.O_RDONLY | os.O_NONBLOCK | os.O_NOFOLLOW)
except FileNotFoundError:
return SavedLaneChange(None)
except (AttributeError, OSError, TypeError, ValueError):
return SavedLaneChange(None, valid=False, readable=False)
try:
if not stat.S_ISREG(os.fstat(fd).st_mode):
return SavedLaneChange(None, valid=False, readable=False)
raw = os.read(fd, MAX_BYTES + 1)
except OSError:
return SavedLaneChange(None, valid=False, readable=False)
finally:
os.close(fd)
if len(raw) > MAX_BYTES:
return SavedLaneChange(raw, valid=False, readable=False)
policy = decode(raw)
return SavedLaneChange(raw, policy if policy is not None else LaneChangePolicy(), valid=policy is not None)
def effective(saved: SavedLaneChange) -> LaneChangePolicy:
policy = saved.policy if saved.readable and saved.valid else LaneChangePolicy()
return replace(policy, minimum_speed_mps=max(MINIMUM_SPEED_MPS, policy.minimum_speed_mps))
class LiveLaneChangePolicy:
"""One canonical document, bounded refresh, safe per-maneuver adoption.
Disabling assistance is immediate on the next successful refresh. Enabling
it, or changing maneuver parameters, cannot turn a held signal into a newly
authorized maneuver or alter an already committed maneuver's pace.
"""
REFRESH_NS = 1_000_000_000
def __init__(self, params, *, now_ns=0):
self.params = params
saved = read_saved(params)
self.requested = effective(saved) if saved.readable and saved.valid else self._disabled()
self.applied = self.requested
self.last_read_ns = now_ns if type(now_ns) is int and now_ns > 0 else 0
@staticmethod
def _disabled():
return replace(LaneChangePolicy(), enabled=False, auto_lane_change=False)
def sample(self, now_ns, *, signaling=False, maneuver_active=False):
if type(now_ns) is not int or now_ns <= 0 or now_ns < self.last_read_ns:
self.last_read_ns = 0
self.requested = self.applied = self._disabled()
return self.applied
if not self.last_read_ns or now_ns - self.last_read_ns >= self.REFRESH_NS:
saved = read_saved(self.params)
self.requested = effective(saved) if saved.readable and saved.valid else self._disabled()
self.last_read_ns = now_ns
if not self.requested.enabled:
self.applied = replace(self.applied, enabled=False, auto_lane_change=False)
elif not self.requested.auto_lane_change and self.applied.auto_lane_change:
self.applied = replace(self.applied, auto_lane_change=False)
if not signaling and not maneuver_active:
self.applied = self.requested
return self.applied
openpilot/starpilot/lateral/lane_change_smoothing.py
+65
View File
@@ -0,0 +1,65 @@
"""Jerk-only lane-change shaping with a gradual return to native limits."""
import math
from openpilot.cereal import log
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_JERK, MIN_SPEED
class LaneChangeSmoother:
def __init__(self):
self.reset()
def reset(self):
self.release = 0.0
self.entry_sign = 0.0
self.previous_factor = 1.0
self.intersection_maneuver = False
def factor(self, *, active, lane_change_state, speed, minimum_speed, duration, previous, desired, turn_assist=False):
if (not active or not all(math.isfinite(value) for value in (speed, minimum_speed, duration, previous, desired)) or
not 3.0 < duration <= 8.0):
self.reset()
return 1.0
if not turn_assist:
self.intersection_maneuver = False
maneuver = lane_change_state in (log.LaneChangeState.laneChangeStarting, log.LaneChangeState.laneChangeFinishing)
# A model lane-change state can outlive an intersection turn while the car
# accelerates across the highway lane-change floor. Finish that exemption
# before admitting pacing for a new maneuver.
intersection = bool(turn_assist and maneuver and
(speed < LANE_CHANGE_SPEED_MIN or self.intersection_maneuver))
if turn_assist and (speed < LANE_CHANGE_SPEED_MIN or self.intersection_maneuver):
self.reset()
self.intersection_maneuver = intersection
return 1.0
base = min(1., math.pi ** 3 * 3.5 / duration ** 3 * 1.3 / MAX_LATERAL_JERK)
if maneuver and speed >= minimum_speed:
self.release = 2.0
if self.entry_sign == 0.0 and abs(desired - previous) > 2e-4:
self.entry_sign = math.copysign(1., desired - previous)
else:
self.release = max(0., self.release - DT_CTRL)
if self.release <= 0.:
self.entry_sign = 0.
factor = 1.0
if self.release > 0.:
release = 1. - self.release / 2.
factor = base + (1. - base) * release
step = desired - previous
if self.entry_sign and abs(step) > 5e-5 and math.copysign(1., step) == -self.entry_sign:
pursuit = (max(abs(step) - 5e-5, 0.) / .2) * max(speed, 1.) ** 2 / MAX_LATERAL_JERK
factor = max(factor, min(.6 + .4 * release, factor + pursuit))
if factor > self.previous_factor:
factor = self.previous_factor + (1. - math.exp(-DT_CTRL / .2)) * (factor - self.previous_factor)
self.previous_factor = factor
return max(0., min(1., factor))
def limit_rate(speed, previous, desired, factor):
if factor >= 1.:
return desired
step = MAX_LATERAL_JERK / max(speed, MIN_SPEED) ** 2 * DT_CTRL * factor
return max(previous - step, min(previous + step, desired))
openpilot/starpilot/lateral/lane_change_status_wire.py
+132
View File
@@ -0,0 +1,132 @@
"""Bounded typed status for optional lane-change alert wording.
This status is presentation evidence, never engagement authority.
"""
from dataclasses import dataclass
from enum import IntEnum
import struct
import capnp
from openpilot.cereal import custom
SERVICE = "laneChangeAssistWire"
KIND = 3
VERSION = 1
MAX_BYTES = 512
MAX_WORDS = (MAX_BYTES - 8) // 8
class Phase(IntEnum):
MANUAL_REQUIRED = 0
WAITING_FOR_DELAY = 1
LANE_UNAVAILABLE = 2
BLINDSPOT_BLOCKED = 3
class Direction(IntEnum):
NONE = 0
LEFT = 1
RIGHT = 2
@dataclass(frozen=True)
class LaneChangeStatus:
producer_session_id: str
sequence: int
model_frame_id: int
model_timestamp_eof_ns: int
observed_mono_time_ns: int
valid_until_mono_time_ns: int
phase: Phase
direction: Direction
auto_configured: bool
engaged: bool
def _valid(value: LaneChangeStatus) -> bool:
return (type(value.producer_session_id) is str and 1 <= len(value.producer_session_id.encode("utf-8")) <= 96 and
0 <= value.sequence < 2**64 and 0 <= value.model_frame_id < 2**32 and
0 < value.model_timestamp_eof_ns < 2**64 and
0 < value.observed_mono_time_ns <= value.valid_until_mono_time_ns < 2**64 and
type(value.phase) is Phase and type(value.direction) is Direction and
type(value.auto_configured) is bool and type(value.engaged) is bool)
def encode(value: LaneChangeStatus) -> bytes:
if not _valid(value):
raise ValueError("invalid lane-change status")
msg = custom.AolAxisState.LaneChangeStatusWire.new_message(
kind=KIND, version=VERSION, producerSessionId=value.producer_session_id,
sequence=value.sequence, modelFrameId=value.model_frame_id,
modelTimestampEofNs=value.model_timestamp_eof_ns,
observedMonoTimeNs=value.observed_mono_time_ns, validUntilMonoTimeNs=value.valid_until_mono_time_ns,
phase=int(value.phase), direction=int(value.direction),
autoConfigured=value.auto_configured, engaged=value.engaged)
raw = msg.to_bytes()
if len(raw) > MAX_BYTES:
raise ValueError("lane-change status exceeds wire bound")
return raw
def encode_optional(value: LaneChangeStatus) -> bytes | None:
"""Bad optional camera metadata must not interrupt model publication."""
try:
return encode(value)
except (ValueError, OverflowError, RuntimeError, capnp.KjException):
return None
def decode(raw_value: bytes) -> LaneChangeStatus | None:
if not isinstance(raw_value, (bytes, bytearray, memoryview)):
return None
raw = bytes(raw_value)
if not 16 <= len(raw) <= MAX_BYTES or len(raw) % 8:
return None
try:
segments, words = struct.unpack_from("<II", raw)
if segments != 0 or words > MAX_WORDS or len(raw) != 8 + 8 * words:
return None
with custom.AolAxisState.LaneChangeStatusWire.from_bytes(raw, traversal_limit_in_words=MAX_WORDS, nesting_limit=4) as msg:
if msg.kind != KIND or msg.version != VERSION:
return None
result = LaneChangeStatus(str(msg.producerSessionId), int(msg.sequence), int(msg.modelFrameId),
int(msg.modelTimestampEofNs), int(msg.observedMonoTimeNs),
int(msg.validUntilMonoTimeNs), Phase(int(msg.phase.raw)), Direction(int(msg.direction.raw)),
bool(msg.autoConfigured), bool(msg.engaged))
return result if _valid(result) else None
except (ValueError, OverflowError, RuntimeError, UnicodeDecodeError, capnp.KjException):
return None
def fresh_for_model(status: LaneChangeStatus | None, model, now_mono_ns: int, recv_mono_ns: int,
last_sequence: int = -1, last_session: str = "", message_mono_ns: int | None = None) -> bool:
if status is None or (status.phase != Phase.MANUAL_REQUIRED and not status.engaged) or not 0 <= now_mono_ns - status.observed_mono_time_ns <= 100_000_000:
return False
if not 0 <= now_mono_ns - recv_mono_ns <= 100_000_000 or now_mono_ns > status.valid_until_mono_time_ns:
return False
if message_mono_ns is not None and not 0 <= now_mono_ns - message_mono_ns <= 100_000_000:
return False
if status.producer_session_id == last_session and status.sequence < last_sequence:
return False
try:
return (int(model.frameId) == status.model_frame_id and
int(model.timestampEof) == status.model_timestamp_eof_ns and
((status.direction == Direction.LEFT and str(model.meta.laneChangeDirection) == "left") or
(status.direction == Direction.RIGHT and str(model.meta.laneChangeDirection) == "right")))
except (AttributeError, TypeError, ValueError):
return False
def alert_wording(status: LaneChangeStatus | None, model, now_mono_ns: int, recv_mono_ns: int,
last_sequence: int = -1, last_session: str = "", message_mono_ns: int | None = None) -> tuple[str, str] | None:
"""None preserves stock manual copy only when a fresh status proves that path."""
if not fresh_for_model(status, model, now_mono_ns, recv_mono_ns, last_sequence, last_session, message_mono_ns):
return "Lane Change Pending", "Check surroundings"
assert status is not None
return {Phase.MANUAL_REQUIRED: None,
Phase.WAITING_FOR_DELAY: ("Automatic Lane Change Pending", "Check surroundings; steer to confirm now"),
Phase.LANE_UNAVAILABLE: ("Automatic Change Unavailable", "Steer to confirm when safe"),
Phase.BLINDSPOT_BLOCKED: ("Car Detected in Blindspot", "")}[status.phase]
openpilot/starpilot/lateral/lane_feedback.py
+50
View File
@@ -0,0 +1,50 @@
"""Display the lane contribution that survives the host curvature limiter."""
import math
from openpilot.cereal import messaging
from openpilot.starpilot.lateral.lane_centering import MAX_CORRECTION
SERVICE = 'starpilotLateralState'
MAX_AGE_NS = 250_000_000
MAX_PAIR_SKEW_NS = 100_000_000
DIRECTION_EPSILON = 1e-6
BLUE = (0, 176, 220)
def feedback_message(result, applied, lateral_active, *, model_mono_time, car_control_mono_time, valid):
event = messaging.new_message(SERVICE, valid=valid)
state = event.starpilotLateralState.laneCentering
state.version = 1
state.modelMonoTime = model_mono_time
state.carControlMonoTime = car_control_mono_time
state.lateralActive = lateral_active
state.requestedCorrection = result.correction if result is not None else 0.0
state.appliedCorrection = applied if lateral_active else 0.0
state.reason = result.reason if result is not None else 'inactive'
return event
def direction(sm, now_ns: int, after_frame: int) -> int:
try:
clocks = {}
for service in (SERVICE, 'carControl', 'modelV2'):
stamp = int(sm.logMonoTime[service])
if (not sm.alive[service] or not sm.valid[service] or sm.recv_frame[service] <= after_frame or
not 0 < stamp <= now_ns or now_ns - stamp > MAX_AGE_NS):
return 0
clocks[service] = stamp
state = sm[SERVICE].laneCentering
requested, applied = float(state.requestedCorrection), float(state.appliedCorrection)
if (state.version != 1 or not state.lateralActive or not sm['carControl'].latActive or
not 0 < state.modelMonoTime <= now_ns or now_ns - state.modelMonoTime > MAX_AGE_NS or
not 0 < state.carControlMonoTime <= clocks[SERVICE] or
abs(state.carControlMonoTime - clocks['carControl']) > MAX_PAIR_SKEW_NS or
abs(state.modelMonoTime - clocks['modelV2']) > MAX_PAIR_SKEW_NS or
not math.isfinite(requested) or not math.isfinite(applied) or
abs(requested) > MAX_CORRECTION + 1e-8 or abs(applied) > abs(requested) + 1e-8 or
requested * applied < 0):
return 0
return (applied > DIRECTION_EPSILON) - (applied < -DIRECTION_EPSILON)
except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
return 0
openpilot/starpilot/lateral/lane_runtime.py
+85
View File
@@ -0,0 +1,85 @@
"""Saved lane-centering preferences and fresh host authority, opt-in at startup."""
import math
import os
from pathlib import Path
from openpilot.cereal.services import SERVICE_LIST
from openpilot.common.realtime import DT_CTRL
from openpilot.common.params import UnknownKeyName
from openpilot.starpilot.lateral.lane_centering import ControlMode, LaneCenteringRequest, LaneCenteringSettings, STRENGTH_BASELINE, STRENGTH_MAX
from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_long_eligible
def read_settings(params) -> LaneCenteringSettings:
def raw(key, default):
try:
return Path(params.get_param_path(key)).read_bytes()
except FileNotFoundError:
return default
def boolean(key, default):
value = raw(key, default)
if value not in (b'0', b'1'):
raise ValueError('Invalid lane preference')
return value == b'1'
try:
enabled = boolean('LaneCentering', b'0')
pause = boolean('LaneCenteringPauseOnSignal', b'1')
offset = float(raw('LaneCenterOffset', b'0.0'))
authority = float(raw('LaneCenteringE2EAuthority', b'1.0'))
try:
strength = float(raw('LaneCenteringStrength', str(STRENGTH_BASELINE).encode()))
except UnknownKeyName:
strength = STRENGTH_BASELINE
if not math.isfinite(strength) or not 0.5 <= strength <= STRENGTH_MAX:
raise ValueError('Invalid lane strength')
if not math.isfinite(offset) or not -0.3 <= offset <= 0.3 or not math.isfinite(authority) or not 0 <= authority <= 1:
raise ValueError('Invalid lane tuning')
return LaneCenteringSettings(enabled, offset, authority, pause, strength)
except (OSError, ValueError):
return LaneCenteringSettings()
def runtime_supported(CP) -> bool:
"""Keep the preference reader available when a supported drive starts Off."""
from openpilot.starpilot.lateral.bolt_policy import supported_cp as bolt_supported_cp
from opendbc.car.gm.lateral import lane_centering_supported as gm_supported_cp
return (os.getenv('LANE_CENTERING_REPLAY_RUNTIME') == '1' or
ioniq6_long_eligible(CP) or (CP is not None and bolt_supported_cp(CP)) or gm_supported_cp(CP))
class LaneCenteringHost:
REFRESH_NS = 1_000_000_000
MAX_TICK_NS = 50_000_000
REQUIRED = ('carState', 'selfdriveState', 'modelV2', 'vehicleParameters')
def __init__(self, params):
self.params = params
self.settings = LaneCenteringSettings()
self.last_refresh_ns: int | None = None
self.last_tick_ns: int | None = None
def sample(self, sm, *, now_ns: int, lateral_active: bool, longitudinal_active: bool) -> LaneCenteringRequest | None:
previous = self.last_tick_ns
self.last_tick_ns = now_ns
discontinuity = previous is not None and not 0 < now_ns - previous <= self.MAX_TICK_NS
if now_ns <= 0:
self.settings = LaneCenteringSettings()
self.last_refresh_ns = None
return None
if self.last_refresh_ns is None or now_ns < self.last_refresh_ns or now_ns - self.last_refresh_ns >= self.REFRESH_NS:
self.settings = read_settings(self.params)
self.last_refresh_ns = now_ns
if not self.settings.enabled or not sm['carState'].canValid or sm['carState'].canTimeout:
return None
if not sm.all_checks(list(self.REQUIRED)) or any(
int(sm.logMonoTime[name]) <= 0 or not 0 <= now_ns - int(sm.logMonoTime[name]) <= int(2.5e9 / SERVICE_LIST[name].frequency)
for name in self.REQUIRED
):
return None
mode = (ControlMode.COMBINED if longitudinal_active else ControlMode.LATERAL_ONLY) if lateral_active else (
ControlMode.LONGITUDINAL_ONLY if longitudinal_active else ControlMode.OFF)
dt = DT_CTRL if previous is None or discontinuity else (now_ns - previous) / 1e9
return LaneCenteringRequest(mode, self.settings, dt, discontinuity)
openpilot/starpilot/lateral/learning_preferences.py
+88
View File
@@ -0,0 +1,88 @@
"""Vehicle-scoped user choice, independent of learning compatibility and results."""
from dataclasses import dataclass
import json
from openpilot.starpilot.saved_source import read_saved
KEY = 'SteeringLearningPreferences'
MAX_BYTES = 4096
LEGACY_KEY = 'ForceAutoTuneOff'
@dataclass(frozen=True)
class LearningPreference:
enabled: bool
valid: bool
source: str
raw: bytes | None
def _identity(CP):
brand, fingerprint = str(CP.brand), str(CP.carFingerprint)
if not brand or not fingerprint or len(brand) > 64 or len(fingerprint) > 256:
raise ValueError('Invalid vehicle identity')
return brand, fingerprint
def _unique(pairs):
result = {}
for key, value in pairs:
if key in result:
raise ValueError('Duplicate field')
result[key] = value
return result
def _decode(raw):
if raw is None:
return {'version': 1, 'vehicles': {}}
if not raw or len(raw) > MAX_BYTES:
raise ValueError('Invalid preference size')
try:
document = json.loads(raw, object_pairs_hook=_unique)
except RecursionError as error:
raise ValueError('Preference nesting exceeds parser limits') from error
if (not isinstance(document, dict) or set(document) != {'version', 'vehicles'} or
type(document['version']) is not int or document['version'] != 1 or
not isinstance(document['vehicles'], dict)):
raise ValueError('Invalid preference document')
count = 0
for brand, vehicles in document['vehicles'].items():
if not brand or len(brand) > 64 or not isinstance(vehicles, dict):
raise ValueError('Invalid preference brand')
for fingerprint, enabled in vehicles.items():
if not fingerprint or len(fingerprint) > 256 or type(enabled) is not bool:
raise ValueError('Invalid vehicle preference')
count += 1
if count > 64:
raise ValueError('Too many vehicle preferences')
return document
def read_preference(params, CP, *, legacy_snapshot=None) -> LearningPreference:
raw, readable = read_saved(params, KEY, MAX_BYTES)
try:
if not readable:
raise ValueError('Unreadable preferences')
brand, fingerprint = _identity(CP)
vehicles = _decode(raw)['vehicles']
if fingerprint in vehicles.get(brand, {}):
return LearningPreference(vehicles[brand][fingerprint], True, 'scope', raw)
legacy, readable = read_saved(params, LEGACY_KEY, 1) if legacy_snapshot is None else legacy_snapshot
if not readable or legacy not in (None, b'0', b'1'):
raise ValueError('Invalid legacy preference')
return LearningPreference(legacy != b'1', True, 'default' if legacy is None else 'legacy', raw)
except (AttributeError, TypeError, ValueError, UnicodeError):
return LearningPreference(False, False, 'invalid', raw)
def replace_preference(raw: bytes | None, CP, enabled: bool) -> bytes:
if type(enabled) is not bool:
raise ValueError('Invalid preference')
brand, fingerprint = _identity(CP)
document = _decode(raw)
document['vehicles'].setdefault(brand, {})[fingerprint] = enabled
encoded = json.dumps(document, sort_keys=True, separators=(',', ':'), allow_nan=False).encode()
_decode(encoded)
return encoded
openpilot/starpilot/lateral/low_speed_advisory.py
+29
View File
@@ -0,0 +1,29 @@
"""The first below-minimum-steering-speed episode in an onroad session."""
import math
class LowSpeedAdvisory:
def __init__(self):
self.drive_id = 0
self.has_been_above = False
self.shown = False
self.showing = False
def update(self, speed: float, minimum: float, *, drive_id: int = 0) -> bool:
# Unknown/transient device state does not restart the advisory lifecycle.
if drive_id > 0 and drive_id != self.drive_id:
self.drive_id = drive_id
self.has_been_above = False
self.shown = False
self.showing = False
if not math.isfinite(speed) or not math.isfinite(minimum) or minimum <= 0:
return False
under = speed < minimum
if not under:
self.has_been_above = True
self.showing = False
elif self.has_been_above and not self.shown:
self.shown = True
self.showing = True
return self.showing and under
openpilot/starpilot/lateral/model_turn_assist.py
+259
View File
@@ -0,0 +1,259 @@
"""Rolling model turn encouragement and curvature handoff for Ioniq 6."""
import math
from openpilot.common.realtime import DT_CTRL
from openpilot.common.constants import CV
CURVATURE_HOLD_HARD_SPEED = 4.5 * CV.MPH_TO_MS
CURVATURE_HOLD_RELEASE_SPEED = 10.0 * CV.MPH_TO_MS
CURVATURE_HOLD_PLAN_SOURCE_SPEED = 2.0 * CV.MPH_TO_MS
CURVATURE_HOLD_RATCHET_RATE = 0.04
CURVATURE_HOLD_HANDOFF_FRAC = 0.75
CURVATURE_HOLD_HANDOFF_TIME = 0.3
CURVATURE_HOLD_DECAY_TAU = 2.0
CURVATURE_HOLD_SWEPT_EXIT = 0.9
CURVATURE_HOLD_EXIT_DECAY_TAU = 0.5
CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0
CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR = 4.0
CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR = 7.0
CURVATURE_HOLD_PLAN_SCALE = 0.85
CURVATURE_HOLD_PLAN_CAP = 0.12
CURVATURE_HOLD_ONSET_HEADING = 10.0
CURVATURE_HOLD_ONSET_NEAR = 1.5
CURVATURE_HOLD_ONSET_FAR_GATE = 5.0
CURVATURE_HOLD_ONSET_FAR = 100.0
CURVATURE_HOLD_REACH_MIN = 7.0
CURVATURE_HOLD_REACH_FULL = 12.0
CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01
CURVATURE_HOLD_CONFIRM_MIN = 0.003
CURVATURE_HOLD_CONFIRM_SWEPT = 0.6
TWITCH_GUARD_MAX_SPEED = 4.0
TWITCH_GUARD_FADE_SPEED = 3.0
TWITCH_GUARD_DURATION = 1.5
TWITCH_GUARD_PLAN_RATIO = 4.0
TWITCH_GUARD_FLOOR = 0.002
TWITCH_GUARD_STRAIGHT_LO = 0.005
TWITCH_GUARD_STRAIGHT_HI = 0.014
TWITCH_GUARD_MIN_REACH = 12.0
def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
px, py = (0.0, 0.0)
for x, y in zip(xs, ys, strict=False):
try:
x, y = (float(x), float(y))
except (TypeError, ValueError, OverflowError):
return 0.0
if not (math.isfinite(x) and math.isfinite(y)):
return 0.0
px, py = (x, y)
if math.hypot(x, y) >= lookahead:
break
d2 = px * px + py * py
if d2 < 1.0:
return 0.0
return 2.0 * py / d2
def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float:
xs, ys = (model_v2.position.x, model_v2.position.y)
near = _plan_circle_curvature(xs, ys, d_near)
far = _plan_circle_curvature(xs, ys, d_far)
if near * far <= 0.0:
return 0.0
return near if abs(near) < abs(far) else far
def get_plan_spatial_curvature(model_v2) -> float:
return _plan_dual_probe(model_v2, CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR)
def get_plan_turn_onset_dist(model_v2) -> float:
xs, ys = (model_v2.position.x, model_v2.position.y)
n = min(len(xs), len(ys))
for i in range(2, n):
dx = xs[i] - xs[i - 1]
dy = ys[i] - ys[i - 1]
if abs(dx) < 0.001 and abs(dy) < 0.001:
continue
if abs(math.degrees(math.atan2(dy, dx))) > CURVATURE_HOLD_ONSET_HEADING:
return math.hypot(xs[i], ys[i])
return CURVATURE_HOLD_ONSET_FAR
def get_plan_reach(model_v2) -> float:
try:
xs = model_v2.position.x
return float(xs[-1]) if len(xs) else 0.0
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
return 0.0
def _plan_positions_are_finite(model_v2) -> bool:
try:
xs, ys = (model_v2.position.x, model_v2.position.y)
return len(xs) == len(ys) and all((math.isfinite(float(x)) and math.isfinite(float(y)) for x, y in zip(xs, ys, strict=True)))
except (AttributeError, TypeError, ValueError, OverflowError):
return False
def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
if not (math.isfinite(curvature) and math.isfinite(v_ego)):
return curvature
if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0:
return curvature
if not _plan_positions_are_finite(model_v2):
return curvature
reach = get_plan_reach(model_v2)
if not math.isfinite(reach) or reach < TWITCH_GUARD_MIN_REACH:
return curvature
plan = abs(_plan_circle_curvature(model_v2.position.x, model_v2.position.y, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR))
if not math.isfinite(plan):
return curvature
straightness = (plan - TWITCH_GUARD_STRAIGHT_LO) / (TWITCH_GUARD_STRAIGHT_HI - TWITCH_GUARD_STRAIGHT_LO)
limit = max(TWITCH_GUARD_PLAN_RATIO * plan * min(max(straightness, 0.0), 1.0), TWITCH_GUARD_FLOOR)
if abs(curvature) <= limit:
return curvature
fade = (TWITCH_GUARD_MAX_SPEED - v_ego) / (TWITCH_GUARD_MAX_SPEED - TWITCH_GUARD_FADE_SPEED)
fade = min(max(fade, 0.0), 1.0)
return curvature + (math.copysign(limit, curvature) - curvature) * fade
def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float:
if not (math.isfinite(remaining) and math.isfinite(v_ego)):
return 0.0
if standstill or abs(v_ego) <= 0.3:
return TWITCH_GUARD_DURATION
return max(remaining - DT_CTRL, 0.0)
TURN_LEAD_T = 1.3
TURN_LEAD_MIN_M = 4.0
TURN_LEAD_MAX_M = 14.0
TURN_LEAD_MIN_SPEED = 3.0
TURN_LEAD_FULL_SPEED = 4.0
TURN_LEAD_MAX_SPEED = 7.0
TURN_LEAD_SCALE = 0.85
TURN_LEAD_CAP = 0.12
TURN_LEAD_ENGAGED_FRAC = 0.5
TURN_LEAD_MODEL_OPPOSE = 0.003
TURN_LEAD_STOP_MARGIN = 1.5
TURN_LEAD_DECEL_GATE = -0.5
class ModelTurnAssist:
def __init__(self):
self.reset()
def reset(self):
self.turn_hold_curvature = 0.0
self.turn_hold_standstill_t = 0.0
self.turn_hold_swept = 0.0
self.turn_hold_handoff_t = 0.0
self.turn_blinker_swept = 0.0
self.twitch_guard_remaining = 0.0
self.curvature = 0.0
self.turn_hold_done = False
def update(self, CS, model_v2, CC, new_desired_curvature):
blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
if CC.latActive and self.twitch_guard_remaining > 0.0 and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0):
new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo)
if blinker_dir == 0.0:
self.turn_blinker_swept = 0.0
else:
self.turn_blinker_swept += max(CS.vEgo * self.curvature * blinker_dir, 0.0) * DT_CTRL
if CS.vEgo >= CURVATURE_HOLD_RELEASE_SPEED:
self.turn_hold_curvature = 0.0
self.turn_hold_standstill_t = 0.0
self.turn_hold_swept = 0.0
self.turn_hold_handoff_t = 0.0
self.turn_hold_done = False
else:
if self.turn_hold_curvature == 0.0:
self.turn_hold_swept = 0.0
else:
self.turn_hold_swept += max(CS.vEgo * self.curvature * math.copysign(1.0, self.turn_hold_curvature), 0.0) * DT_CTRL
turn_exiting = self.turn_hold_swept > CURVATURE_HOLD_SWEPT_EXIT
if (CS.vEgo > CURVATURE_HOLD_HARD_SPEED or turn_exiting) and CC.latActive and (self.turn_hold_curvature != 0.0):
hold_dir = math.copysign(1.0, self.turn_hold_curvature)
model_mag = max(new_desired_curvature * hold_dir, 0.0)
if model_mag < abs(self.turn_hold_curvature):
decay_tau = CURVATURE_HOLD_EXIT_DECAY_TAU if turn_exiting else CURVATURE_HOLD_DECAY_TAU
decayed = abs(self.turn_hold_curvature) + (model_mag - abs(self.turn_hold_curvature)) * (DT_CTRL / decay_tau)
self.turn_hold_curvature = math.copysign(decayed, self.turn_hold_curvature)
if CS.vEgo < 0.5:
self.turn_hold_standstill_t += DT_CTRL
if self.turn_hold_standstill_t > CURVATURE_HOLD_STANDSTILL_TIMEOUT:
self.turn_hold_curvature = 0.0
self.turn_hold_done = False
else:
self.turn_hold_standstill_t = 0.0
if CC.latActive and self.turn_hold_curvature != 0.0 and (
new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) < -CURVATURE_HOLD_OPPOSITE_RELEASE):
self.turn_hold_curvature = 0.0
self.turn_hold_done = True
if CC.latActive and self.turn_hold_curvature != 0.0 and (
new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) >= CURVATURE_HOLD_HANDOFF_FRAC * abs(self.turn_hold_curvature)):
self.turn_hold_handoff_t += DT_CTRL
if self.turn_hold_handoff_t > CURVATURE_HOLD_HANDOFF_TIME:
self.turn_hold_curvature = 0.0
self.turn_hold_done = True
else:
self.turn_hold_handoff_t = 0.0
if blinker_dir == 0.0:
self.turn_hold_done = False
elif (CC.latActive and CS.steeringPressed and (CS.steeringTorque * blinker_dir < 0.0) and
(self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN) and (self.turn_blinker_swept < CURVATURE_HOLD_CONFIRM_SWEPT)):
self.turn_hold_done = False
if blinker_dir != 0.0 and (not self.turn_hold_done):
turn_candidate = new_desired_curvature if CC.latActive else 0.0
if CC.latActive and CS.vEgo < CURVATURE_HOLD_PLAN_SOURCE_SPEED:
plan_curvature = get_plan_spatial_curvature(model_v2) * CURVATURE_HOLD_PLAN_SCALE
plan_curvature = max(min(plan_curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP)
onset = get_plan_turn_onset_dist(model_v2)
onset_w = min(max((CURVATURE_HOLD_ONSET_FAR_GATE - onset) / (CURVATURE_HOLD_ONSET_FAR_GATE - CURVATURE_HOLD_ONSET_NEAR), 0.0), 1.0)
reach = get_plan_reach(model_v2)
reach_w = min(max((reach - CURVATURE_HOLD_REACH_MIN) / (CURVATURE_HOLD_REACH_FULL - CURVATURE_HOLD_REACH_MIN), 0.0), 1.0)
plan_curvature *= onset_w * reach_w
if plan_curvature * blinker_dir > turn_candidate * blinker_dir:
turn_candidate = plan_curvature
driver_confirmed = False
if CC.latActive and CS.steeringPressed and (CS.steeringTorque * blinker_dir < 0.0) and (self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN):
wound_curvature = max(min(self.curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP)
if wound_curvature * blinker_dir > turn_candidate * blinker_dir:
turn_candidate = wound_curvature
driver_confirmed = True
if turn_candidate * blinker_dir > abs(self.turn_hold_curvature):
new_mag = turn_candidate * blinker_dir
if CS.vEgo > CURVATURE_HOLD_PLAN_SOURCE_SPEED and (not driver_confirmed):
new_mag = min(new_mag, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL)
self.turn_hold_curvature = math.copysign(new_mag, turn_candidate)
elif self.turn_hold_curvature * blinker_dir < 0.0:
self.turn_hold_curvature = 0.0
if CC.latActive and self.turn_hold_curvature != 0.0:
hold_dir = math.copysign(1.0, self.turn_hold_curvature)
if new_desired_curvature * hold_dir < abs(self.turn_hold_curvature):
new_desired_curvature = self.turn_hold_curvature
if (CC.latActive and (blinker_dir != 0.0) and (model_v2.meta.laneChangeState == 0) and
(TURN_LEAD_MIN_SPEED <= CS.vEgo < TURN_LEAD_MAX_SPEED) and (new_desired_curvature * blinker_dir > -TURN_LEAD_MODEL_OPPOSE)):
d_near = max(min(TURN_LEAD_T * CS.vEgo, TURN_LEAD_MAX_M), TURN_LEAD_MIN_M)
stopping_short = CS.aEgo < TURN_LEAD_DECEL_GATE and CS.vEgo ** 2 / (2.0 * -CS.aEgo) < TURN_LEAD_STOP_MARGIN * d_near
lead_curvature = 0.0 if stopping_short else _plan_dual_probe(model_v2, d_near, d_near + 3.0) * TURN_LEAD_SCALE
lead_curvature = max(min(lead_curvature, TURN_LEAD_CAP), -TURN_LEAD_CAP)
if lead_curvature * blinker_dir > 0.0:
speed_w = min(max((CS.vEgo - TURN_LEAD_MIN_SPEED) / (TURN_LEAD_FULL_SPEED - TURN_LEAD_MIN_SPEED), 0.0), 1.0)
engaged_ratio = abs(self.curvature) / abs(lead_curvature)
engage_w = min(max((1.0 - engaged_ratio) / (1.0 - TURN_LEAD_ENGAGED_FRAC), 0.0), 1.0)
lead_curvature *= speed_w * engage_w
if lead_curvature * blinker_dir > max(new_desired_curvature * blinker_dir, 0.0):
new_desired_curvature = lead_curvature
if CS.vEgo < CURVATURE_HOLD_RELEASE_SPEED and (not self.turn_hold_done) and (lead_curvature * blinker_dir > abs(self.turn_hold_curvature)):
held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL)
self.turn_hold_curvature = math.copysign(held_mag, lead_curvature)
return new_desired_curvature
def bind_turn_assist(controller):
"""Bind policy ownership once while observing later policy replacements."""
if hasattr(controller, "starpilot_extension"):
return lambda: bool(getattr(getattr(getattr(controller, "starpilot_extension", None), "policy", None), "turn_assist", False))
return lambda: bool(getattr(getattr(controller, "ioniq6_policy", None), "turn_assist", False))
openpilot/starpilot/lateral/ordinary_cc_policy.py
+36
View File
@@ -0,0 +1,36 @@
"""Default torque law for exact conventional-cruise GM owners."""
import math
import numpy as np
from opendbc.car.gm.values import CAR, is_ordinary_cc_profile, is_conventional_cc_pedal_profile, malibu_hybrid_profile
from opendbc.car.structs import CarParams
from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy
def supported_cp(cp):
if (not (is_ordinary_cc_profile(cp) or is_conventional_cc_pedal_profile(cp) or malibu_hybrid_profile(cp) is not None) or
cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != 'torque'):
return False
tune = cp.lateralTuning.torque
return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and
math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0)
class OrdinaryCcTorquePolicy(GMOrdinaryTorquePolicy):
def __init__(self, parent, cp):
if not supported_cp(cp):
raise ValueError('Unsupported conventional-cruise GM torque profile')
self.yukon = cp.carFingerprint == CAR.GMC_YUKON_CC
super().__init__(parent, cp)
def feedforward(self, value, setpoint, jerk, speed):
if not self.yukon:
return value
phase = math.tanh((setpoint * jerk) / .14)
speed_weight = float(np.interp(speed, [12.0, 30.0], [0.0, 1.0]))
x = (abs(setpoint) - .35) / .18
z = math.exp(-x) if x >= 0.0 else math.exp(x)
lat_weight = 1.0 / (1.0 + z) if x >= 0.0 else z / (1.0 + z)
scale = 1.0 + ((.08 * max(phase, 0.0) - .12 * max(-phase, 0.0)) * speed_weight * lat_weight)
return value * scale
openpilot/starpilot/lateral/palisade_policy.py
+26
View File
@@ -0,0 +1,26 @@
from opendbc.car.hyundai.values import CAR
from openpilot.starpilot.lateral import palisade_shaping as shaping
from openpilot.starpilot.lateral.hkg_torque_policy import HKGShapedTorquePolicy, supported_torque_cp
PALISADE_CARS = frozenset((CAR.HYUNDAI_PALISADE, CAR.HYUNDAI_PALISADE_2023,))
def supported_cp(cp):
return supported_torque_cp(cp, PALISADE_CARS)
class PalisadeTorquePolicy(HKGShapedTorquePolicy):
FACTOR_MULT = shaping.PALISADE_BASE_LAT_ACCEL_FACTOR_MULT
supported_cp = staticmethod(supported_cp)
def feedforward(self, value, setpoint, jerk, speed):
return value * shaping.get_palisade_ff_scale(setpoint, jerk, speed) * shaping.get_palisade_center_taper_scale(setpoint, speed)
def friction(self, setpoint, jerk, speed):
taper = shaping.get_palisade_center_taper_scale(setpoint, speed)
return (shaping.get_palisade_friction_threshold(speed, setpoint, jerk),
1.0 + (shaping.get_palisade_friction_scale(speed, setpoint, jerk) - 1.0) * taper)
def output(self, value, setpoint, speed):
return value * shaping.get_palisade_center_output_scale(setpoint, speed)
openpilot/starpilot/lateral/palisade_shaping.py
+154
View File
@@ -0,0 +1,154 @@
import math
from openpilot.starpilot.lateral.bolt_shaping import get_gm_base_friction_threshold
PALISADE_BASE_LAT_ACCEL_FACTOR_MULT = 0.98
PALISADE_FF_GAIN_LEFT = 0.14
PALISADE_FF_GAIN_RIGHT = 0.12
PALISADE_FF_ONSET = 0.08
PALISADE_FF_ONSET_WIDTH = 0.04
PALISADE_FF_CUTOFF = 1.25
PALISADE_FF_CUTOFF_WIDTH = 0.36
PALISADE_TRANSITION_SPEED = 9.0
PALISADE_PHASE_SCALE = 0.11
PALISADE_TURN_IN_BOOST_LEFT = 0.44
PALISADE_TURN_IN_BOOST_RIGHT = 0.34
PALISADE_UNWIND_TAPER_LEFT = 0.18
PALISADE_UNWIND_TAPER_RIGHT = 0.30
PALISADE_FRICTION_MULT = 1.02
PALISADE_FRICTION_LAT_RISE = 0.20
PALISADE_FRICTION_JERK_RISE = 0.24
PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18
PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14
PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT = 0.14
PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.22
PALISADE_TURN_IN_FRICTION_BOOST_LEFT = 0.08
PALISADE_TURN_IN_FRICTION_BOOST_RIGHT = 0.06
PALISADE_UNWIND_FRICTION_REDUCTION_LEFT = 0.12
PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT = 0.20
PALISADE_CENTER_TAPER_MAX = 0.12
PALISADE_CENTER_TAPER_LAT = 0.28
PALISADE_CENTER_TAPER_LAT_WIDTH = 0.055
PALISADE_CENTER_TAPER_SPEED = 12.0
PALISADE_CENTER_TAPER_SPEED_WIDTH = 2.5
PALISADE_CENTER_OUTPUT_TAPER_MAX = 0.18
PALISADE_CENTER_OUTPUT_TAPER_LAT = 0.28
PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.055
PALISADE_CENTER_OUTPUT_TAPER_SPEED = 15.0
PALISADE_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0
def _sigmoid(x: float) -> float:
if x >= 0.0:
z = math.exp(-x)
return 1.0 / (1.0 + z)
z = math.exp(x)
return z / (1.0 + z)
def _palisade_sigmoid(x: float) -> float:
return _sigmoid(x)
def _palisade_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / PALISADE_TRANSITION_SPEED) ** 2)
def _palisade_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PALISADE_PHASE_SCALE)
def _palisade_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _palisade_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PALISADE_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PALISADE_FRICTION_JERK_RISE)
return _palisade_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_palisade_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _palisade_side_value(desired_lateral_accel, PALISADE_FF_GAIN_LEFT, PALISADE_FF_GAIN_RIGHT)
abs_lateral_accel = abs(desired_lateral_accel)
onset = _palisade_sigmoid((abs_lateral_accel - PALISADE_FF_ONSET) / PALISADE_FF_ONSET_WIDTH)
cutoff = _palisade_sigmoid((PALISADE_FF_CUTOFF - abs_lateral_accel) / PALISADE_FF_CUTOFF_WIDTH)
extra_scale = gain * onset * cutoff
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _palisade_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_BOOST_LEFT, PALISADE_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_TAPER_LEFT, PALISADE_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))
def get_palisade_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
threshold_scale = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT, PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT) *
transition_envelope * turn_in_weight)
threshold_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT, PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT) *
transition_envelope * unwind_weight)
return base_threshold * min(max(threshold_scale, 0.84), 1.14)
def get_palisade_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = PALISADE_FRICTION_MULT
friction_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_FRICTION_BOOST_LEFT, PALISADE_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_FRICTION_REDUCTION_LEFT, PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight)
return min(max(friction_scale, 0.92), 1.12)
def get_palisade_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _palisade_sigmoid((v_ego - PALISADE_CENTER_TAPER_SPEED) / PALISADE_CENTER_TAPER_SPEED_WIDTH)
center_weight = _palisade_sigmoid((PALISADE_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
PALISADE_CENTER_TAPER_LAT_WIDTH)
return 1.0 - (PALISADE_CENTER_TAPER_MAX * speed_weight * center_weight)
def get_palisade_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
"""Reduce high-speed center corrections without reducing normal turn authority."""
speed_weight = _palisade_sigmoid((v_ego - PALISADE_CENTER_OUTPUT_TAPER_SPEED) /
PALISADE_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _palisade_sigmoid((PALISADE_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - (PALISADE_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight)
openpilot/starpilot/lateral/pause.py
+90
View File
@@ -0,0 +1,90 @@
"""Temporary steering output pauses, independent of driver authorization."""
from dataclasses import dataclass
import math
from openpilot.common.constants import CV
from openpilot.starpilot.saved_source import read_saved
KEYS = ('PauseLateralSpeed', 'PauseLateralOnSignal', 'LateralResumeDelay')
@dataclass(frozen=True)
class PauseSettings:
speed_mps: float = 0.0
signal_only: bool = False
delay_s: float = 0.0
def read_settings(params) -> PauseSettings | None:
values = {key: read_saved(params, key, 128) for key in (*KEYS, 'IsMetric')}
if not all(readable for _, readable in values.values()):
return None
try:
speed_raw = values[KEYS[0]][0]
speed = float(b'0' if speed_raw is None else speed_raw)
signal = values[KEYS[1]][0]
delay_raw = values[KEYS[2]][0]
delay = float(b'0' if delay_raw is None else delay_raw)
metric = values['IsMetric'][0]
if (signal not in (None, b'0', b'1') or metric not in (None, b'0', b'1') or
not math.isfinite(speed) or not 0 <= speed <= 100 or not math.isfinite(delay) or not 0 <= delay <= 5):
return None
return PauseSettings(speed * (CV.KPH_TO_MS if metric == b'1' else CV.MPH_TO_MS), signal == b'1', delay)
except (ValueError, OverflowError):
return None
class LateralPause:
def __init__(self, settings: PauseSettings | None, params=None):
self.settings = settings or PauseSettings()
self.params = params
self._refresh_ns = 0
self._drive_id = 0
self.reset()
def reset(self) -> None:
self._blinker = False
self._minimum = math.inf
self._release_ns = 0
self._last_ns = 0
def refresh(self, now_ns: int) -> None:
if self.params is not None and (now_ns < self._refresh_ns or now_ns - self._refresh_ns >= 1_000_000_000):
settings = read_settings(self.params)
if settings is not None:
if settings != self.settings:
self.reset()
self.settings = settings
self._refresh_ns = now_ns
def allowed(self, cs, *, now_ns: int, source_ns: int, drive_id: int | None = None) -> bool:
self.refresh(now_ns)
if self.settings.speed_mps == 0:
self.reset()
return True
if now_ns < self._last_ns:
self.reset()
return False
self._last_ns = now_ns
if drive_id is not None:
if drive_id <= 0:
return False
if drive_id != self._drive_id:
self.reset()
self._drive_id = drive_id
self._last_ns = now_ns
if (not cs.canValid or cs.canTimeout or not math.isfinite(cs.vEgo) or
not 0 < source_ns <= now_ns or now_ns - source_ns > 250_000_000):
return False
signal = bool(cs.leftBlinker or cs.rightBlinker)
if signal:
self._minimum = min(self._minimum, cs.vEgo) if self._blinker else cs.vEgo
self._release_ns = 0
elif self._blinker:
self._release_ns = now_ns
self._blinker = signal
delayed = (self.settings.signal_only and self._release_ns > 0 and
self._minimum < self.settings.speed_mps / 2 and
now_ns - self._release_ns < self.settings.delay_s * 1e9)
return bool((cs.vEgo >= self.settings.speed_mps or self.settings.signal_only and not signal) and not delayed)
openpilot/starpilot/lateral/sdgm_policy.py
+34
View File
@@ -0,0 +1,34 @@
"""Ordinary SDGM torque selection; factory PID identity stays on PID."""
import math
import numpy as np
from opendbc.car.structs import CarParams
from opendbc.car.gm.values import CAR, is_ordinary_sdgm_profile
from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy
def supported_cp(cp):
if not (is_ordinary_sdgm_profile(cp, longitudinal=False) or is_ordinary_sdgm_profile(cp, longitudinal=True)):
return False
if cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != "torque":
return False
tune = cp.lateralTuning.torque
return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and
math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0)
class SdgmTorquePolicy(GMOrdinaryTorquePolicy):
def __init__(self, parent, cp):
if not supported_cp(cp):
raise ValueError("Unsupported ordinary SDGM torque profile")
if cp.carFingerprint == CAR.CADILLAC_XT4:
lateral_values = np.arange(-5.0, 5.0, 0.01)
torque_values = []
for value in lateral_values:
sig_input = 2.4 * value
sigmoid = np.sign(sig_input) * (1 / (1 + math.exp(-abs(sig_input))) - 0.5)
torque_values.append(float(sigmoid * .95 + value * .28))
parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral_values, torque_values)
parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque_values, lateral_values)
super().__init__(parent, cp)
openpilot/starpilot/lateral/sonata_hybrid_policy.py
+24
View File
@@ -0,0 +1,24 @@
from opendbc.car.hyundai.values import CAR
from openpilot.starpilot.lateral import sonata_hybrid_shaping as shaping
from openpilot.starpilot.lateral.hkg_torque_policy import HKGShapedTorquePolicy, supported_torque_cp
SONATA_HYBRID_CARS = frozenset((CAR.HYUNDAI_SONATA_HYBRID,))
def supported_cp(cp):
return supported_torque_cp(cp, SONATA_HYBRID_CARS)
class SonataHybridTorquePolicy(HKGShapedTorquePolicy):
FACTOR_MULT = shaping.SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT
supported_cp = staticmethod(supported_cp)
def feedforward(self, value, setpoint, jerk, speed):
return value * shaping.get_sonata_hybrid_ff_scale(setpoint, jerk, speed) * shaping.get_sonata_hybrid_center_taper_scale(setpoint, speed)
def friction(self, setpoint, jerk, speed):
return shaping.get_sonata_hybrid_friction_threshold(speed, setpoint), 1.0
def output(self, value, setpoint, speed):
return value * shaping.get_sonata_hybrid_center_taper_scale(setpoint, speed) * shaping.get_sonata_hybrid_center_output_scale(setpoint, speed)
openpilot/starpilot/lateral/sonata_hybrid_shaping.py
+142
View File
@@ -0,0 +1,142 @@
import math
import numpy as np
from openpilot.starpilot.lateral.torque_shaping import get_standard_friction_threshold
SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05
SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09
SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.26
SONATA_HYBRID_FF_ONSET = 0.18
SONATA_HYBRID_FF_ONSET_WIDTH = 0.08
SONATA_HYBRID_FF_CUTOFF = 1.35
SONATA_HYBRID_FF_CUTOFF_WIDTH = 0.40
SONATA_HYBRID_TRANSITION_SPEED = 8.0
SONATA_HYBRID_PHASE_SCALE = 0.12
SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12
SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.02
SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18
SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10
SONATA_HYBRID_CENTER_TAPER_MAX = 0.10
SONATA_HYBRID_CENTER_TAPER_LAT = 0.16
SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025
SONATA_HYBRID_CENTER_TAPER_SPEED = 22.0
SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH = 2.5
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX = 0.14
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT = 0.10
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.5
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0
SONATA_HYBRID_CENTER_OUTPUT_TAPER_MAX = 0.14
SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT = 0.18
SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05
SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED = 12.5
SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5
SONATA_HYBRID_CHATTER_THRESHOLD_SPEED_BP = [0.0, 4.5, 7.5, 11.0, 20.0]
SONATA_HYBRID_CHATTER_THRESHOLD_BUMP = [0.02, 0.04, 0.04, 0.02, 0.0]
SONATA_HYBRID_CHATTER_THRESHOLD_CENTER = 0.20
SONATA_HYBRID_CHATTER_THRESHOLD_CENTER_WIDTH = 0.05
def _sigmoid(x: float) -> float:
if x >= 0.0:
z = math.exp(-x)
return 1.0 / (1.0 + z)
z = math.exp(x)
return z / (1.0 + z)
def _sonata_hybrid_sigmoid(x: float) -> float:
return _sigmoid(x)
def _sonata_hybrid_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_HYBRID_TRANSITION_SPEED) ** 2)
def _sonata_hybrid_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_HYBRID_PHASE_SCALE)
def _sonata_hybrid_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_sonata_hybrid_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _sonata_hybrid_sigmoid((abs_lateral_accel - SONATA_HYBRID_FF_ONSET) / SONATA_HYBRID_FF_ONSET_WIDTH)
cutoff = _sonata_hybrid_sigmoid((SONATA_HYBRID_FF_CUTOFF - abs_lateral_accel) / SONATA_HYBRID_FF_CUTOFF_WIDTH)
base_reduction = _sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_FF_REDUCTION_LEFT,
SONATA_HYBRID_FF_REDUCTION_RIGHT) * onset * cutoff
phase = _sonata_hybrid_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _sonata_hybrid_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_TURN_IN_BOOST_LEFT,
SONATA_HYBRID_TURN_IN_BOOST_RIGHT) *
turn_in_weight * low_speed_factor)
unwind_taper = 1.0 - (_sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_UNWIND_TAPER_LEFT,
SONATA_HYBRID_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
def get_sonata_hybrid_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sonata_hybrid_sigmoid((v_ego - SONATA_HYBRID_CENTER_TAPER_SPEED) / SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH)
reduction = SONATA_HYBRID_CENTER_TAPER_MAX * speed_weight * center_weight
low_speed_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
reduction += SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight
return 1.0 - reduction
def get_sonata_hybrid_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sonata_hybrid_sigmoid((v_ego - SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED) /
SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - SONATA_HYBRID_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight
def get_sonata_hybrid_friction_threshold(v_ego: float, desired_lateral_accel: float) -> float:
base_threshold = get_standard_friction_threshold(v_ego)
speed_bump = np.interp(max(v_ego, 0.0), SONATA_HYBRID_CHATTER_THRESHOLD_SPEED_BP,
SONATA_HYBRID_CHATTER_THRESHOLD_BUMP)
center_weight = _sonata_hybrid_sigmoid(
(SONATA_HYBRID_CHATTER_THRESHOLD_CENTER - abs(desired_lateral_accel)) /
SONATA_HYBRID_CHATTER_THRESHOLD_CENTER_WIDTH
)
return float(base_threshold + speed_bump * center_weight)
openpilot/starpilot/lateral/suburban_policy.py
+24
View File
@@ -0,0 +1,24 @@
"""Ordinary Suburban torque selection."""
import math
from opendbc.car.structs import CarParams
from opendbc.car.gm.suburban import supported_cp as supported_profile
from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy
def supported_cp(cp):
if not supported_profile(cp):
return False
if cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != "torque":
return False
tune = cp.lateralTuning.torque
return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and
math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0)
class SuburbanTorquePolicy(GMOrdinaryTorquePolicy):
def __init__(self, parent, cp):
if not supported_cp(cp):
raise ValueError("Unsupported ordinary Suburban torque profile")
super().__init__(parent, cp)
openpilot/starpilot/lateral/tests/__init__.py
View File
openpilot/starpilot/lateral/tests/fixtures/adjacent_lane_geometry.json
Vendored
+39
View File
@@ -0,0 +1,39 @@
{
"model_valid": true,
"model": {
"frameId": 6144,
"frameAge": 0,
"timestampEof": 401368071578,
"laneLineProbs": [0.8565725088119507, 0.983020007610321, 0.05520869046449661, 0.00023782203788869083],
"laneLineStds": [0.23406687378883362, 0.04350994899868965, 0.2947346866130829, 1.1171478033065796],
"roadEdgeStds": [1.0554050207138062, 0.177374467253685],
"laneLines": [
{
"x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0],
"y": [-4.328125, -4.32421875, -4.3203125, -4.33203125, -4.328125, -4.3359375, -4.3359375, -4.3359375, -4.32421875, -4.32421875, -4.328125, -4.34375, -4.3515625, -4.35546875, -4.3359375, -4.32421875, -4.3125, -4.3125, -4.29296875, -4.265625, -4.2421875, -4.21484375, -4.1953125, -4.203125, -4.2109375, -4.203125, -4.2109375, -4.1875, -4.1640625, -4.10546875, -4.0390625, -3.953125, -3.8359375]
},
{
"x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0],
"y": [-1.2509765625, -1.25390625, -1.251953125, -1.25390625, -1.255859375, -1.2578125, -1.2509765625, -1.2529296875, -1.255859375, -1.2568359375, -1.259765625, -1.2587890625, -1.2529296875, -1.244140625, -1.2373046875, -1.2255859375, -1.21484375, -1.201171875, -1.185546875, -1.1669921875, -1.15234375, -1.138671875, -1.12890625, -1.123046875, -1.1142578125, -1.1044921875, -1.083984375, -1.0625, -1.021484375, -0.95263671875, -0.87548828125, -0.783203125, -0.66748046875]
},
{
"x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0],
"y": [1.974609375, 1.9736328125, 1.9765625, 1.9765625, 1.97265625, 1.9765625, 1.970703125, 1.970703125, 1.966796875, 1.970703125, 1.97265625, 1.9716796875, 1.9775390625, 1.9853515625, 1.984375, 2.0, 2.017578125, 2.03125, 2.04296875, 2.0546875, 2.072265625, 2.078125, 2.08984375, 2.109375, 2.125, 2.138671875, 2.16796875, 2.203125, 2.2578125, 2.326171875, 2.421875, 2.533203125, 2.669921875]
},
{
"x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0],
"y": [4.14453125, 4.14453125, 4.16015625, 4.16015625, 4.16796875, 4.171875, 4.17578125, 4.1796875, 4.1796875, 4.19140625, 4.1875, 4.203125, 4.1953125, 4.203125, 4.19921875, 4.22265625, 4.23828125, 4.26171875, 4.265625, 4.2734375, 4.2578125, 4.2578125, 4.2578125, 4.25390625, 4.2578125, 4.265625, 4.2734375, 4.3125, 4.3359375, 4.390625, 4.4609375, 4.5703125, 4.6640625]
}
],
"roadEdges": [
{
"x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0],
"y": [-8.1875, -8.203125, -8.171875, -8.203125, -8.203125, -8.1875, -8.1953125, -8.2109375, -8.2109375, -8.21875, -8.2265625, -8.2265625, -8.25, -8.2578125, -8.2890625, -8.296875, -8.28125, -8.2734375, -8.2578125, -8.2265625, -8.171875, -8.1484375, -8.125, -8.1328125, -8.109375, -8.109375, -8.1015625, -8.078125, -8.0546875, -7.99609375, -7.91015625, -7.80859375, -7.70703125]
},
{
"x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0],
"y": [2.23828125, 2.2421875, 2.2421875, 2.24609375, 2.2421875, 2.251953125, 2.2421875, 2.2578125, 2.263671875, 2.2734375, 2.291015625, 2.318359375, 2.375, 2.4296875, 2.5078125, 2.580078125, 2.669921875, 2.744140625, 2.83203125, 2.876953125, 2.919921875, 2.955078125, 2.98046875, 3.021484375, 3.048828125, 3.107421875, 3.15234375, 3.224609375, 3.30078125, 3.400390625, 3.50390625, 3.623046875, 3.751953125]
}
]
}
}
openpilot/starpilot/lateral/tests/fixtures/corolla_tss2_ba901b5f.json
Vendored
+1
View File
File diff suppressed because one or more lines are too long
openpilot/starpilot/lateral/tests/fixtures/genesis_g70_ba901b5f.json
Vendored
+1
View File
File diff suppressed because one or more lines are too long
openpilot/starpilot/lateral/tests/fixtures/gv70_385428f6.json
Vendored
+3198
View File
File diff suppressed because it is too large Load Diff
openpilot/starpilot/lateral/tests/fixtures/oct2_turns.json
Vendored
+1
View File
File diff suppressed because one or more lines are too long
openpilot/starpilot/lateral/tests/test_auto_lane_change.py
+214
View File
@@ -0,0 +1,214 @@
"""Live Auto evidence and optional typed status contract."""
from types import SimpleNamespace
import time
import unittest
from openpilot.cereal import log
import openpilot.cereal.messaging as messaging
from openpilot.starpilot.lateral.auto_lane_change import ClockEpochGuard, auto_evidence, session_policy
from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy, decode as decode_policy
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.starpilot.lateral.lane_change_status_wire import (
Direction, LaneChangeStatus, Phase, alert_wording, decode, encode, encode_optional, fresh_for_model,
)
def line(y):
return SimpleNamespace(x=ModelConstants.X_IDXS, y=[y] * ModelConstants.IDX_N)
def model():
return SimpleNamespace(frameId=77, frameAge=0, timestampEof=10_000_000_000,
laneLines=[line(-5.5), line(-1.8), line(1.8), line(5.5)],
laneLineProbs=[0.95] * 4, laneLineStds=[0.1] * 4,
roadEdges=[line(-6.2), line(6.2)], roadEdgeStds=[0.1] * 2,
meta=SimpleNamespace(laneChangeDirection="left"))
def submaster(now):
services = ("carState", "carControl", "extrinsicsCalibration")
class FakeSubMaster(SimpleNamespace):
def __getitem__(self, key):
return self.values[key]
return FakeSubMaster(seen=dict.fromkeys(services, True), alive=dict.fromkeys(services, True),
valid=dict.fromkeys(services, True), logMonoTime=dict.fromkeys(services, now - 10_000_000),
recv_time=dict.fromkeys(services, (now - 10_000_000) / 1e9),
values={"carState": SimpleNamespace(canValid=True, canTimeout=False),
"carControl": SimpleNamespace(enabled=True, latActive=True),
"extrinsicsCalibration": SimpleNamespace(calStatus=log.ExtrinsicsCalibration.Status.calibrated,
rpyCalib=[0., 0., 0.])})
class TestAutoLaneChange(unittest.TestCase):
def test_four_startup_modes(self):
old = decode_policy(b'{"version":1,"enabled":true,"minimumSpeedMps":8.9408,"onePerSignal":false}')
self.assertIsNotNone(old)
assert old is not None
self.assertFalse(session_policy(old, True).auto_lane_change)
requested = LaneChangePolicy(auto_lane_change=True)
self.assertFalse(session_policy(requested, False).auto_lane_change)
self.assertTrue(session_policy(requested, True).auto_lane_change)
self.assertFalse(session_policy(LaneChangePolicy(enabled=False, auto_lane_change=True), True).enabled)
def test_clock_domains_and_current_calibration(self):
now_mono = 1_000_000_000
now_boot = 10_050_000_000 # includes a long suspend offset from MONOTONIC
sm, frame = submaster(now_mono), model()
self.assertTrue(auto_evidence(sm, frame, -1, 3.0, now_mono_ns=now_mono, now_boot_ns=now_boot, model_valid=True, vehicle_capable=True))
self.assertFalse(auto_evidence(sm, frame, -1, 3.0, now_mono_ns=now_mono, now_boot_ns=now_boot + 200_000_000, model_valid=True, vehicle_capable=True))
sm.values["extrinsicsCalibration"].calStatus = log.ExtrinsicsCalibration.Status.uncalibrated
self.assertFalse(auto_evidence(sm, frame, -1, 3.0, now_mono_ns=now_mono, now_boot_ns=now_boot, model_valid=True, vehicle_capable=True))
sm.values["extrinsicsCalibration"].calStatus = log.ExtrinsicsCalibration.Status.calibrated
sm.logMonoTime["extrinsicsCalibration"] = now_mono - 800_000_000
self.assertFalse(auto_evidence(sm, frame, -1, 3.0, now_mono_ns=now_mono, now_boot_ns=now_boot, model_valid=True, vehicle_capable=True))
def test_suspend_offset_requires_new_calibration_and_controls(self):
before_mono, before_boot = 1_000_000_000, 10_000_000_000
guard = ClockEpochGuard(before_mono, before_boot)
sm = submaster(before_mono)
self.assertTrue(guard.ready(sm, before_mono, before_boot))
# MONOTONIC advanced a short interval; BOOTTIME includes a long suspend.
resumed_mono, resumed_boot = 1_050_000_000, 15_050_000_000
self.assertFalse(guard.ready(sm, resumed_mono, resumed_boot))
self.assertFalse(guard.ready(sm, resumed_mono + 10_000_000, resumed_boot + 10_000_000))
for name in ("carState", "carControl", "extrinsicsCalibration"):
sm.logMonoTime[name] = resumed_mono + 11_000_000
sm.recv_time[name] = (resumed_mono + 11_000_000) / 1e9
self.assertTrue(guard.ready(sm, resumed_mono + 12_000_000, resumed_boot + 12_000_000))
short_guard = ClockEpochGuard(1_000_000_000, 10_000_000_000)
short_sm = submaster(1_020_000_000)
for name in ("carState", "carControl", "extrinsicsCalibration"):
short_sm.logMonoTime[name] = 1_000_000_000 # apparently only 20 ms old on MONOTONIC
short_sm.recv_time[name] = 1.0
fresh_camera = model()
fresh_camera.timestampEof = 10_100_000_000
self.assertTrue(auto_evidence(short_sm, fresh_camera, -1, 3.0, now_mono_ns=1_020_000_000,
now_boot_ns=10_110_000_000, model_valid=True, vehicle_capable=True))
self.assertFalse(short_guard.ready(short_sm, 1_020_000_000, 10_110_000_000))
self.assertFalse(short_guard.ready(short_sm, 1_021_000_000, 10_111_000_000))
self.assertFalse(short_guard.ready(short_sm, 1_021_000_000, 10_111_000_000, sample_skew_ns=2_000_000))
def test_same_frame_geometry_and_engagement(self):
now = 1_000_000_000
sm, frame = submaster(now), model()
self.assertTrue(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True))
frame.laneLineProbs[3] = 0.2
self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True))
frame.laneLineProbs[3] = 0.95
sm.values["carControl"].enabled = False
self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True))
sm.values["carControl"].enabled = True
sm.values["carState"].canTimeout = True
self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True))
sm.values["carState"].canTimeout = False
frame.laneLineProbs[3] = 1.2
self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True))
frame.laneLineProbs[3] = 0.95
frame.frameAge = 2
self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True))
frame.frameAge = 0
self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=False, vehicle_capable=True))
self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=False))
def test_typed_wire_arrival_order_and_neutral_fallback(self):
frame = model()
status = LaneChangeStatus("session", 2, 77, 10_000_000_000, 1_000_000_000, 1_150_000_000,
Phase.WAITING_FOR_DELAY, Direction.LEFT, True, True)
raw = encode(status)
self.assertEqual(decode(raw), status)
zero_eof = LaneChangeStatus(status.producer_session_id, status.sequence, status.model_frame_id, 0,
status.observed_mono_time_ns, status.valid_until_mono_time_ns,
status.phase, status.direction, status.auto_configured, status.engaged)
self.assertIsNone(encode_optional(zero_eof))
self.assertTrue(fresh_for_model(decode(raw), frame, 1_050_000_000, 1_020_000_000))
# Status-first and model-first delivery are neutral until both identify the same frame.
self.assertFalse(fresh_for_model(decode(raw), replace_status_model(frame, 76), 1_050_000_000, 1_020_000_000))
self.assertFalse(fresh_for_model(None, frame, 1_050_000_000, 1_020_000_000))
self.assertFalse(fresh_for_model(decode(raw), frame, 1_400_000_000, 1_020_000_000))
self.assertFalse(fresh_for_model(decode(raw), frame, 1_050_000_000, 1_020_000_000, 3, "session"))
self.assertIsNone(decode(raw[:-8]))
self.assertEqual(alert_wording(None, frame, 1_050_000_000, 1_020_000_000),
("Lane Change Pending", "Check surroundings"))
self.assertEqual(alert_wording(status, frame, 1_050_000_000, 1_020_000_000),
("Automatic Lane Change Pending", "Check surroundings; steer to confirm now"))
self.assertIsNone(alert_wording(replace_status_phase(status, Phase.MANUAL_REQUIRED),
frame, 1_050_000_000, 1_020_000_000))
aol_manual = LaneChangeStatus(status.producer_session_id, status.sequence, status.model_frame_id,
status.model_timestamp_eof_ns, status.observed_mono_time_ns,
status.valid_until_mono_time_ns, Phase.MANUAL_REQUIRED, status.direction, True, False)
self.assertIsNone(alert_wording(aol_manual, frame, 1_050_000_000, 1_020_000_000))
aol_auto = replace_status_phase(aol_manual, Phase.WAITING_FOR_DELAY)
self.assertEqual(alert_wording(aol_auto, frame, 1_050_000_000, 1_020_000_000),
("Lane Change Pending", "Check surroundings"))
self.assertEqual(alert_wording(replace_status_phase(status, Phase.LANE_UNAVAILABLE),
frame, 1_050_000_000, 1_020_000_000),
("Automatic Change Unavailable", "Steer to confirm when safe"))
self.assertEqual(alert_wording(status, frame, 1_050_000_000, 1_020_000_000,
message_mono_ns=800_000_000), ("Lane Change Pending", "Check surroundings"))
after_suspend = replace_status_model(frame, 78)
after_suspend.timestampEof = 15_000_000_000
self.assertEqual(alert_wording(status, after_suspend, 1_050_000_000, 1_020_000_000),
("Lane Change Pending", "Check surroundings"))
def test_real_optional_service_roundtrip(self):
publisher = messaging.PubMaster(["laneChangeAssistWire", "modelV2"])
subscriber = messaging.SubMaster(["laneChangeAssistWire", "modelV2"], ignore_alive=["laneChangeAssistWire"],
ignore_valid=["laneChangeAssistWire"])
self.assertTrue(subscriber.all_checks(["laneChangeAssistWire"])) # optional status cannot hold stock engagement
time.sleep(0.05)
now = time.monotonic_ns()
status = LaneChangeStatus("ipc-session", 1, 77, 10_000_000_000, now, now + 100_000_000,
Phase.MANUAL_REQUIRED, Direction.LEFT, False, True)
msg = messaging.new_message("laneChangeAssistWire", 0)
msg.valid = True
msg.laneChangeAssistWire = encode(status)
publisher.send("laneChangeAssistWire", msg)
subscriber.update(100)
self.assertTrue(subscriber.seen["laneChangeAssistWire"] and subscriber.valid["laneChangeAssistWire"])
self.assertEqual(decode(subscriber["laneChangeAssistWire"]), status)
self.assertEqual(alert_wording(status, subscriber["modelV2"], time.monotonic_ns(),
int(subscriber.recv_time["laneChangeAssistWire"] * 1e9)),
("Lane Change Pending", "Check surroundings")) # status arrived first
def send_model(frame_id):
message = messaging.new_message("modelV2")
message.valid = True
message.modelV2.frameId = frame_id
message.modelV2.timestampEof = 10_000_000_000
message.modelV2.meta.laneChangeDirection = log.LaneChangeDirection.left
publisher.send("modelV2", message)
subscriber.update(100)
send_model(77)
self.assertIsNone(alert_wording(decode(subscriber["laneChangeAssistWire"]), subscriber["modelV2"],
time.monotonic_ns(), int(subscriber.recv_time["laneChangeAssistWire"] * 1e9)))
send_model(78) # model arrived first; old wire must not describe it
self.assertEqual(alert_wording(decode(subscriber["laneChangeAssistWire"]), subscriber["modelV2"],
time.monotonic_ns(), int(subscriber.recv_time["laneChangeAssistWire"] * 1e9)),
("Lane Change Pending", "Check surroundings"))
next_status = LaneChangeStatus("ipc-session", 2, 78, 10_000_000_000, time.monotonic_ns(),
time.monotonic_ns() + 100_000_000, Phase.MANUAL_REQUIRED, Direction.LEFT, False, True)
next_msg = messaging.new_message("laneChangeAssistWire", 0)
next_msg.valid = True
next_msg.laneChangeAssistWire = encode(next_status)
publisher.send("laneChangeAssistWire", next_msg)
subscriber.update(100)
self.assertIsNone(alert_wording(decode(subscriber["laneChangeAssistWire"]), subscriber["modelV2"],
time.monotonic_ns(), int(subscriber.recv_time["laneChangeAssistWire"] * 1e9)))
def replace_status_model(frame, frame_id):
return SimpleNamespace(frameId=frame_id, timestampEof=frame.timestampEof, meta=frame.meta)
def replace_status_phase(status, phase):
return LaneChangeStatus(status.producer_session_id, status.sequence, status.model_frame_id,
status.model_timestamp_eof_ns, status.observed_mono_time_ns,
status.valid_until_mono_time_ns, phase, status.direction,
status.auto_configured, status.engaged)
if __name__ == "__main__":
unittest.main()
openpilot/starpilot/lateral/tests/test_bolt_torque_admission.py
+95
View File
@@ -0,0 +1,95 @@
import ast
from pathlib import Path
import tempfile
from types import SimpleNamespace
import unittest
from unittest.mock import patch
from opendbc.car import structs
from openpilot.starpilot.lateral.bolt_policy import BOLT_GENERATIONS
from openpilot.starpilot.lateral.torque_runtime import TorqueHost, runtime_enabled
from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, FieldChoice, PlatformProfile, serialize_document
class TestBoltTorqueAdmission(unittest.TestCase):
def setUp(self):
temporary = tempfile.TemporaryDirectory()
self.addCleanup(temporary.cleanup)
self.root = Path(temporary.name)
self.params = SimpleNamespace(get_param_path=lambda key: str(self.root / key))
def cp(self, identity):
cp = structs.CarParams.new_message()
cp.brand, cp.carFingerprint = 'gm', identity
cp.steerControlType = structs.CarParams.SteerControlType.torque
tune = cp.lateralTuning.init('torque')
tune.latAccelFactor, tune.latAccelOffset, tune.friction = 2.0, 0.0, 0.1
return cp
def document(self, cp, friction=None):
if friction is None:
friction = FieldChoice('custom', .14)
tune = cp.lateralTuning.torque
basis = (float(tune.latAccelFactor), float(tune.latAccelOffset), float(tune.friction))
raw = serialize_document({str(cp.carFingerprint): PlatformProfile(basis, FieldChoice(), friction)})
(self.root / DOCUMENT_KEY).write_bytes(raw)
def test_defaults_and_canonical_opt_in(self):
for identity in BOLT_GENERATIONS:
with self.subTest(identity=identity):
cp = self.cp(identity)
for file in self.root.iterdir():
file.unlink()
self.assertFalse(runtime_enabled(cp, self.params))
self.document(cp)
self.assertTrue(runtime_enabled(cp, self.params))
(self.root / 'AdvancedLateralTune').write_bytes(b'0')
self.assertTrue(runtime_enabled(cp, self.params))
(self.root / 'AdvancedLateralTune').write_bytes(b'1')
self.assertTrue(runtime_enabled(cp, self.params))
def test_retired_flag_is_irrelevant_and_document_read_failure_denies(self):
cp = self.cp(next(iter(BOLT_GENERATIONS)))
self.document(cp)
for raw in (b'true', b'2', b'', b'1\n'):
(self.root / 'AdvancedLateralTune').write_bytes(raw)
self.assertTrue(runtime_enabled(cp, self.params))
with patch('openpilot.starpilot.lateral.torque_runtime._raw', side_effect=PermissionError):
self.assertFalse(runtime_enabled(cp, self.params))
def test_opt_in_requires_valid_vehicle_friction_document(self):
cp = self.cp(next(iter(BOLT_GENERATIONS)))
(self.root / 'AdvancedLateralTune').write_bytes(b'1')
self.assertFalse(runtime_enabled(cp, self.params))
self.document(cp, FieldChoice())
self.assertFalse(runtime_enabled(cp, self.params))
self.document(self.cp(list(BOLT_GENERATIONS)[1]))
self.assertFalse(runtime_enabled(cp, self.params))
(self.root / DOCUMENT_KEY).write_bytes(b'{bad')
self.assertFalse(runtime_enabled(cp, self.params))
self.document(cp)
(self.root / 'ForceAutoTuneOff').write_bytes(b'bad')
self.assertFalse(runtime_enabled(cp, self.params))
def test_actual_controls_host_assignment_defaults_and_saved_choice(self):
source = Path(__file__).resolve().parents[3] / 'selfdrive/controls/controlsd.py'
tree = ast.parse(source.read_text())
controls = next(node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == 'Controls')
init = next(node for node in controls.body if isinstance(node, ast.FunctionDef) and node.name == '__init__')
assignment = next(node for node in init.body if isinstance(node, ast.Assign) and
any(isinstance(target, ast.Attribute) and target.attr == 'torque_host' for target in node.targets))
code = compile(ast.Module(body=[assignment], type_ignores=[]), str(source), 'exec')
cp = self.cp('CHEVROLET_BOLT_CC_2018_2021')
current = SimpleNamespace(CP=cp, params=self.params, torque_learning_allowed=False,
lateral_controller_selection=SimpleNamespace(policy=None))
environment = {'self': current, 'TorqueHost': TorqueHost, 'torque_runtime_enabled': runtime_enabled}
exec(code, environment)
self.assertIsNone(current.torque_host)
self.document(cp)
(self.root / 'AdvancedLateralTune').write_bytes(b'1')
exec(code, environment)
self.assertIsInstance(current.torque_host, TorqueHost)
if __name__ == '__main__':
unittest.main()
openpilot/starpilot/lateral/tests/test_bolt_torque_policy.py
+116
View File
@@ -0,0 +1,116 @@
import math
from types import SimpleNamespace
import unittest
import numpy as np
from opendbc.car.gm.interface import CarInterface
from opendbc.car.gm.tests.test_bolt_cc import params
from opendbc.car.gm.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.torque_extension import selected_policy
from openpilot.starpilot.lateral.bolt_policy import BOLT_GENERATIONS
from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, replace_mode, selection_from_bytes
class TestBoltTorquePolicy(unittest.TestCase):
def controller(self, identity, mode=None):
cp = params(identity, alpha=True).as_reader()
return cp, LatControlTorque(cp, CarInterface(cp), 0.01, controller_mode=mode)
def state(self):
return SimpleNamespace(vEgo=20.0, steeringAngleDeg=0.0, steeringPressed=False, steeringRateDeg=0.0, standstill=False)
def test_exact_default_selection_and_saved_standard(self):
for identity in BOLT_GENERATIONS:
cp, controller = self.controller(identity)
self.assertEqual(default_selection(cp).mode, ControllerMode.STARPILOT)
self.assertIsNotNone(selected_policy(controller))
choice = selection_from_bytes(cp, replace_mode(None, cp, ControllerMode.STANDARD))
self.assertEqual(choice.mode, ControllerMode.STANDARD)
standard = LatControlTorque(cp, CarInterface(cp), 0.01, controller_mode=choice.mode)
self.assertIsNone(selected_policy(standard))
self.assertEqual(standard.pid._k_i[1], [0.15])
self.assertAlmostEqual(standard.torque_from_lateral_accel(1.0, standard.torque_params), 1.0 / cp.lateralTuning.torque.latAccelFactor)
cp, controller = self.controller(CAR.CHEVROLET_BOLT_EUV)
self.assertEqual(default_selection(cp).mode, ControllerMode.STANDARD)
self.assertIsNone(selected_policy(controller))
valid, _ = self.controller(CAR.CHEVROLET_BOLT_CC_2017)
for field, value in (('brand', 'toyota'), ('notCar', True), ('dashcamOnly', True), ('passive', True)):
bad = valid.as_builder()
setattr(bad, field, value)
self.assertIsNone(default_selection(bad).policy)
def test_generation_gains_and_nonlinear_limits_survive_parameter_updates(self):
coefficients = {
2017: ((2.15, 1.0, 0.129), (2.15, 1.0, 0.145)),
2018: ((1.8, 1.1, 0.27), (2.0, 1.0, 0.205)),
2022: ((2.6531724862969748, 1.1, 0.1919764879840985), (2.7031724862969748, 1.0, 0.1469764879840985)),
}
for identity, generation in BOLT_GENERATIONS.items():
cp, controller = self.controller(identity)
policy = selected_policy(controller)
self.assertEqual(policy.ff_positive, float(np.float32(1.03)))
self.assertEqual(policy.ff_negative, float(np.float32(float(np.float32(1.07)) * (0.9 if generation == 2017 else 1.07))))
multiplier = float(np.float32(float(np.float32(0.93)) * (0.9 if generation == 2017 else 0.93)))
self.assertEqual(controller.pid._k_i[1], [0.35 * multiplier])
values = np.arange(-5.0, 5.0, 0.01)
torques = []
for value in values:
a, b, c = coefficients[generation][0 if value >= 0 else 1]
x = a * value
torques.append(float(np.sign(x) * (1 / (1 + math.exp(-abs(x))) - 0.5) * b + value * c))
expected_limits = (np.interp(1.0, torques, values), np.interp(-1.0, torques, values))
for value in (-2.0, -0.05, 0.0, 0.05, 2.0):
self.assertAlmostEqual(controller.torque_from_lateral_accel(value, controller.torque_params), np.interp(value, values, torques))
self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), expected_limits)
controller.update_torque_parameters(1.1 * cp.lateralTuning.torque.latAccelFactor, 0.01, 0.05)
self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), expected_limits)
self.assertAlmostEqual(controller.torque_params.friction, 0.05)
def test_reset_inactive_priming_driver_release_and_saturation(self):
for identity in BOLT_GENERATIONS:
cp, controller = self.controller(identity)
policy = selected_policy(controller)
vm, cs = VehicleModel(cp), self.state()
live = SimpleNamespace(angleOffsetDeg=0.0, roll=0.0)
controller.pid.i = 0.2
controller.sat_time = 0.3
policy.previous_output = 0.1
policy.curvature_buffer[-1] = 0.001
controller.reset()
self.assertEqual(controller.sat_time, 0.0)
self.assertEqual(controller.pid.i, 0.2)
self.assertEqual(policy.previous_output, 0.1)
self.assertEqual(policy.curvature_buffer[-1], 0.001)
output = controller.update(False, cs, vm, live, False, 0.002, False, 0.2)
self.assertEqual(output[0], 0.0)
self.assertEqual(controller.pid.i, 0.0)
self.assertEqual(policy.previous_output, 0.0)
self.assertEqual(policy.curvature_buffer[-1], 0.002)
self.assertEqual(policy.jerk_filter.x, 0.0)
controller.pid.i = 0.2
policy.previous_pressed = True
controller.update(True, cs, vm, live, True, 0.002, False, 0.2)
self.assertAlmostEqual(controller.pid.i, 0.16)
cs.vEgo = 35.0
for _ in range(80):
_, _, logged = controller.update(True, cs, vm, live, False, 1.0, False, 0.2)
self.assertTrue(logged.saturated)
_, _, logged = controller.update(True, cs, vm, live, True, 1.0, False, 0.2)
self.assertFalse(logged.saturated)
def test_default_kp_survives_parameter_updates_reset_and_inactive_dispatch(self):
for identity in BOLT_GENERATIONS:
cp, controller = self.controller(identity)
self.assertEqual(controller.pid._k_p, ([0], [0.6]))
self.assertEqual(controller.pid._k_p[1][0], 0.6)
controller.update_torque_parameters(3.1, 0.01, 0.09)
controller.reset()
state = self.state()
controller.update(False, state, VehicleModel(cp), SimpleNamespace(angleOffsetDeg=0.0, roll=0.0), False, 0.001, False, 0.2)
self.assertEqual(controller.pid._k_p, ([0], [0.6]))
standard = LatControlTorque(cp, CarInterface(cp), 0.01, controller_mode=ControllerMode.STANDARD)
self.assertEqual(standard.pid._k_p[0], [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30])
self.assertEqual(standard.pid._k_p[1][-1], 0.8)
openpilot/starpilot/lateral/tests/test_controller_selection.py
+171
View File
@@ -0,0 +1,171 @@
"""The optional policy choice is latched at startup and separate from torque sources."""
from dataclasses import replace
import json
import os
import struct
from types import SimpleNamespace
import unittest
from unittest import mock
from opendbc.car import DT_CTRL, structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.controls.controlsd import Controls
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.torque_extension import selected_policy
from openpilot.starpilot.lateral.controller_selection import (
DOCUMENT_KEY, ControllerMode, default_selection, policy_for, read_selection, selection_from_bytes,
)
from openpilot.starpilot.lateral.torque_tuning import TorqueSource
def cp_for(vehicle):
return interfaces[vehicle].get_non_essential_params(vehicle)
def document(**choices):
brands = {'HYUNDAI_IONIQ_6': 'hyundai', 'GENESIS_G70_2020': 'hyundai', 'TOYOTA_COROLLA_TSS2': 'toyota'}
return json.dumps({'version': 1, 'vehicles': {key: {'brand': brands[key], 'mode': value}
for key, value in choices.items()}}, sort_keys=True).encode()
class TestControllerSelection(unittest.TestCase):
def test_missing_choice_exact_policy_and_untuned_matrix(self):
expected = ((HYUNDAI.HYUNDAI_IONIQ_6, 'ioniq6'),
(HYUNDAI.GENESIS_G70_2020, 'genesis_g70_2020'),
(TOYOTA.TOYOTA_COROLLA_TSS2, 'corolla_tss2'),
(HYUNDAI.KIA_EV6, None),
(HYUNDAI.HYUNDAI_SONATA, None),
(TOYOTA.TOYOTA_RAV4_TSS2, None))
for vehicle, policy in expected:
with self.subTest(vehicle=vehicle):
cp = cp_for(vehicle)
self.assertEqual(policy_for(cp), policy)
selected = selection_from_bytes(cp, None)
self.assertEqual(selected.mode, ControllerMode.STARPILOT if policy else ControllerMode.STANDARD)
lateral = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL)
self.assertEqual(lateral.controller_mode, selected.mode)
self.assertEqual(lateral.controller_policy, policy)
self.assertEqual(bool(selected_policy(lateral)), bool(policy))
def test_saved_standard_is_per_vehicle_and_invalid_cannot_enable_policy(self):
ioniq = cp_for(HYUNDAI.HYUNDAI_IONIQ_6)
g70 = cp_for(HYUNDAI.GENESIS_G70_2020)
ev6 = cp_for(HYUNDAI.KIA_EV6)
raw = document(HYUNDAI_IONIQ_6='standard', GENESIS_G70_2020='starpilot')
self.assertEqual(selection_from_bytes(ioniq, raw).mode, ControllerMode.STANDARD)
self.assertEqual(selection_from_bytes(g70, raw).mode, ControllerMode.STARPILOT)
self.assertEqual(selection_from_bytes(ev6, raw), default_selection(ev6))
for invalid in (b'{', b'{"version":true,"vehicles":{}}', b'{"version":2,"vehicles":{}}',
b'{"version":1,"vehicles":{"HYUNDAI_IONIQ_6":{"brand":"toyota","mode":"standard"}}}',
b'{"version":1,"vehicles":{},"vehicles":{}}', b'x' * 4097):
with self.subTest(raw=invalid[:50]):
selected = selection_from_bytes(ioniq, invalid)
self.assertEqual(selected.mode, ControllerMode.STARPILOT)
self.assertEqual(selected.source, 'invalid')
wrong = ioniq.as_reader().as_builder()
wrong.brand = 'toyota'
self.assertIsNone(policy_for(wrong))
wrong = ioniq.as_reader().as_builder()
wrong.lateralTuning.init('pid')
self.assertIsNone(policy_for(wrong))
wrong = ioniq.as_reader().as_builder()
wrong.passive = True
self.assertIsNone(policy_for(wrong))
wrong = ioniq.as_reader().as_builder()
wrong.notCar = True
self.assertIsNone(policy_for(wrong))
with self.assertRaises(ValueError):
LatControlTorque(ev6.as_reader(), interfaces[HYUNDAI.KIA_EV6](ev6), DT_CTRL,
controller_mode=ControllerMode.STARPILOT)
def test_standard_skips_policy_constructor_and_ioniq_factor_multiplier(self):
for vehicle in (HYUNDAI.HYUNDAI_IONIQ_6, HYUNDAI.GENESIS_G70_2020, TOYOTA.TOYOTA_COROLLA_TSS2):
with self.subTest(vehicle=vehicle):
cp = cp_for(vehicle)
standard = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL,
controller_mode=ControllerMode.STANDARD)
policy = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL,
controller_mode=ControllerMode.STARPILOT)
self.assertIsNone(selected_policy(standard))
self.assertIsNotNone(selected_policy(policy))
self.assertEqual(standard.pid._k_i[1], [0.15])
self.assertAlmostEqual(standard.torque_params.latAccelFactor, cp.lateralTuning.torque.latAccelFactor)
if vehicle == HYUNDAI.HYUNDAI_IONIQ_6:
self.assertEqual(standard.pid.pos_limit, cp.lateralTuning.torque.latAccelFactor)
self.assertEqual(policy.pid.pos_limit, cp.lateralTuning.torque.latAccelFactor)
self.assertEqual(policy.torque_params.latAccelFactor,
struct.unpack('f', struct.pack('f', cp.lateralTuning.torque.latAccelFactor * 1.22))[0])
for controller, multiplier in ((standard, 1.0), (policy, 1.22)):
controller.update_torque_parameters(3.3, 0.0, 0.12)
expected = struct.unpack('f', struct.pack('f', 3.3 * multiplier))[0]
self.assertEqual(controller.torque_params.latAccelFactor, expected)
def test_explicit_policy_preserves_legacy_output_through_reset(self):
sequence = ((False, 2.0, 4.0, -0.0005, False),
(True, 8.0, 2.0, 0.0003, False),
(True, 18.0, -1.0, -0.0004, True),
(False, 0.0, 3.0, 0.0, False),
(True, 5.0, 1.0, 0.0002, False))
for vehicle in (HYUNDAI.HYUNDAI_IONIQ_6, HYUNDAI.GENESIS_G70_2020, TOYOTA.TOYOTA_COROLLA_TSS2):
with self.subTest(vehicle=vehicle):
cp = cp_for(vehicle)
legacy = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL)
selected = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL,
controller_mode=ControllerMode.STARPILOT)
vm = VehicleModel(cp)
cs = structs.CarState.new_message()
params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.02)
for active, speed, angle, curvature, pressed in sequence:
cs.vEgo = speed
cs.steeringAngleDeg = angle
cs.steeringPressed = pressed
before = legacy.update(active, cs, vm, params, False, curvature, False, 0.1)
after = selected.update(active, cs, vm, params, False, curvature, False, 0.1)
self.assertEqual(after[0], before[0])
self.assertEqual(after[1], before[1])
self.assertEqual(after[2].to_dict(), before[2].to_dict())
self.assertEqual(selected.pid.i, legacy.pid.i)
if not active:
self.assertEqual(after[0], 0.0)
def test_controlsd_latches_choice_while_manual_source_remains_independent(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0', 'REPLAY': '1',
'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params = Params()
cp = cp_for(HYUNDAI.HYUNDAI_IONIQ_6)
params.put('CarParams', cp.to_bytes(), block=True)
params.put_bool('AdvancedLateralTune', True, block=True)
legacy = Controls()
self.assertEqual(legacy.lateral_controller_selection, default_selection(cp))
self.assertIsNotNone(selected_policy(legacy.LaC))
self.assertEqual(legacy.LaC.pid.pos_limit, cp.lateralTuning.torque.latAccelFactor)
params.put(DOCUMENT_KEY, json.loads(document(HYUNDAI_IONIQ_6='standard')), block=True)
self.assertEqual(read_selection(params, cp).mode, ControllerMode.STANDARD)
first = Controls()
self.assertEqual(first.lateral_controller_selection.mode, ControllerMode.STANDARD)
self.assertIsNone(selected_policy(first.LaC))
self.assertIsNotNone(first.torque_host)
source = replace(first.torque_host.vehicle, source=TorqueSource.USER, lat_accel_factor=3.3, friction=0.12)
self.assertTrue(first.torque_host.apply(first.LaC, source))
self.assertEqual(first.LaC.torque_params.latAccelFactor, struct.unpack('f', struct.pack('f', 3.3))[0])
params.put(DOCUMENT_KEY, json.loads(document(HYUNDAI_IONIQ_6='starpilot')), block=True)
self.assertEqual(first.LaC.controller_mode, ControllerMode.STANDARD)
second = Controls()
self.assertEqual(second.lateral_controller_selection.mode, ControllerMode.STARPILOT)
self.assertIsNotNone(selected_policy(second.LaC))
self.assertIsNotNone(second.torque_host)
self.assertTrue(second.torque_host.apply(second.LaC, source))
self.assertEqual(second.LaC.torque_params.latAccelFactor,
struct.unpack('f', struct.pack('f', 3.3 * 1.22))[0])
self.assertEqual(first.torque_host.vehicle, second.torque_host.vehicle)
if __name__ == '__main__':
unittest.main()
openpilot/starpilot/lateral/tests/test_corolla_tss2_policy.py
+74
View File
@@ -0,0 +1,74 @@
"""Compare runtime output against frozen default original StarPilot traces."""
import json
from pathlib import Path
from types import SimpleNamespace
import pytest
from openpilot.cereal import log
from opendbc.car import structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.toyota.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.torque_extension import selected_policy
from openpilot.starpilot.lateral.corolla_tss2_policy import supported_cp
FIXTURE = Path(__file__).with_name('fixtures') / 'corolla_tss2_ba901b5f.json'
def controller_for(car=CAR.TOYOTA_COROLLA_TSS2):
cp = interfaces[car].get_non_essential_params(car)
return cp, LatControlTorque(cp.as_reader(), interfaces[car](cp), 0.01), VehicleModel(cp)
@pytest.mark.parametrize('case_index', range(5))
def test_frozen_original_numeric_traces(case_index):
fixture = json.loads(FIXTURE.read_text())
cp, controller, vm = controller_for()
assert selected_policy(controller) is not None
params = log.VehicleParameters.new_message(angleOffsetDeg=0.3, roll=0.006)
for index, frame in enumerate(fixture['cases'][case_index]['frames']):
active, speed, angle, pressed, limited, curve, curve_limited, delay = frame['input']
cs = SimpleNamespace(vEgo=speed, steeringAngleDeg=angle, steeringPressed=pressed)
torque, desired_angle, state = controller.update(active, cs, vm, params, limited, curve, curve_limited, delay)
assert desired_angle == 0.0
assert torque == pytest.approx(frame['torque'], abs=1e-12), (case_index, index)
assert [getattr(state, field) for field in fixture['log_fields']] == pytest.approx(frame['log'], abs=1e-12)
assert state.active == frame['active']
assert state.saturated == frame['saturated']
assert abs(torque) <= 1.0
if not active:
assert torque == 0.0
assert controller.pid.i == 0.0
def test_exact_vehicle_admission_and_upstream_gains():
cp, controller, _ = controller_for()
assert supported_cp(cp)
assert controller.pid._k_i[1] == [0.35]
for field, value in [('brand', 'hyundai'), ('dashcamOnly', True), ('passive', True), ('steerControlType', structs.CarParams.SteerControlType.angle)]:
changed = cp.as_reader().as_builder()
setattr(changed, field, value)
assert not supported_cp(changed)
native = LatControlTorque(changed.as_reader(), interfaces[CAR.TOYOTA_COROLLA_TSS2](changed), 0.01)
assert selected_policy(native) is None
assert native.pid._k_i[1] == [0.15]
other_cp, other, _ = controller_for(CAR.TOYOTA_RAV4_TSS2)
assert not supported_cp(other_cp)
assert selected_policy(other) is None
assert other.pid._k_i[1] == [0.15]
def test_live_source_updates_keep_raw_calibration_and_limits():
cp, controller, _ = controller_for()
original = cp.lateralTuning.torque.latAccelFactor
assert controller.torque_params.latAccelFactor == original
for _ in range(3):
controller.update_torque_parameters(original * 1.1, 0.025, 0.08)
assert controller.torque_params.latAccelFactor == pytest.approx(original * 1.1)
assert controller.torque_params.latAccelOffset == pytest.approx(0.025)
assert controller.torque_params.friction == pytest.approx(0.08)
assert controller.pid.pos_limit == controller.lateral_accel_from_torque(1.0, controller.torque_params)
assert controller.pid.neg_limit == controller.lateral_accel_from_torque(-1.0, controller.torque_params)
openpilot/starpilot/lateral/tests/test_ev6_default.py
+134
View File
@@ -0,0 +1,134 @@
"""Untuned EV6 lateral selection and steering command boundaries."""
import unittest
from opendbc.can import CANParser
from opendbc.car import Bus, DT_CTRL, gen_empty_fingerprint, structs
from opendbc.car.hyundai.carcontroller import CarController, MAX_ANGLE_FRAMES
from opendbc.car.hyundai.carstate import CarState
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags, CarControllerParams
from opendbc.car.lateral import apply_driver_steer_torque_limits
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.torque_extension import selected_policy
from openpilot.starpilot.lateral.torque_runtime import runtime_enabled
def ev6(lka_steering: bool):
fingerprint = gen_empty_fingerprint()
if lka_steering:
fingerprint[CanBus(None, fingerprint).CAM][0x50] = 16
return CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], False, False, False)
class NoTorqueSettings:
def get_param_path(self, _key):
raise AssertionError("untuned EV6 must not read saved torque settings")
class TestEV6DefaultLateral(unittest.TestCase):
def test_hda_topologies_keep_vehicle_tune_and_generic_controller(self):
for lka_steering in (False, True):
with self.subTest(lka_steering=lka_steering):
cp = ev6(lka_steering)
self.assertEqual(bool(cp.flags & HyundaiFlags.CANFD_LKA_STEER_MSG), lka_steering)
self.assertEqual(cp.lateralTuning.which(), "torque")
self.assertAlmostEqual(cp.lateralTuning.torque.latAccelFactor, 3.2)
self.assertAlmostEqual(cp.lateralTuning.torque.friction, 0.005)
self.assertFalse(runtime_enabled(cp, NoTorqueSettings()))
lateral = LatControlTorque(cp.as_reader(), CarInterface(cp), DT_CTRL)
self.assertIsNone(selected_policy(lateral))
self.assertAlmostEqual(lateral.torque_params.latAccelFactor, cp.lateralTuning.torque.latAccelFactor)
controller = CarController(DBC[cp.carFingerprint], cp)
self.assertIsNone(controller.ioniq6_longitudinal)
self.assertEqual(controller.params.STEER_MAX, 409)
self.assertEqual(controller.params.STEER_DRIVER_ALLOWANCE, 250)
def test_opposing_driver_torque_limits_can_command_and_angle_request(self):
for lka_steering in (False, True):
with self.subTest(lka_steering=lka_steering):
cp = ev6(lka_steering)
state = CarState(cp)
state.out = state.update(state.get_can_parsers(cp))
control = structs.CarControl()
control.enabled = control.latActive = True
control.actuators.torque = 1.0
controller = CarController(DBC[cp.carFingerprint], cp)
can = CanBus(cp)
name = "LKAS" if lka_steering else "LFA"
bus = can.ACAN if lka_steering else can.ECAN
parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [(name, 0)], bus)
state.out.steeringTorque = 0
state.out.vEgoRaw = 15.0
controller.apply_torque_last = 60
_, sent = controller.update(control.as_reader(), state, 1_000_000_000)
self.assertLessEqual(controller.apply_torque_last, 62)
self.assertEqual(parser.update((1_000_000_000, sent)), {next(msg[0] for msg in sent if msg[2] == bus)})
self.assertEqual(parser.vl[name]["ActToiSta"], 1)
state.out.steeringTorque = -500
for frame in range(30):
previous = controller.apply_torque_last
_, sent = controller.update(control.as_reader(), state, 1_010_000_000 + frame * 10_000_000)
self.assertLessEqual(controller.apply_torque_last, previous)
self.assertEqual(controller.apply_torque_last, 0)
parser.update((1_310_000_000, sent))
self.assertEqual(parser.vl[name]["StrTqReqVal"], 0)
state.out.steeringAngleDeg = 90.0
state.out.steeringTorque = 0
requests = []
for frame in range(MAX_ANGLE_FRAMES + 2):
_, sent = controller.update(control.as_reader(), state, 1_320_000_000 + frame * 10_000_000)
parser.update((1_320_000_000 + frame * 10_000_000, sent))
requests.append(parser.vl[name]["ActToiSta"])
self.assertIn(0, requests[-2:])
def test_torque_speed_transition_unwind_and_zero_crossing(self):
cp = ev6(True)
for speed, up, down in ((0., 10, 8), (14.999, 10, 8), (15., 2, 3), (30., 2, 3),
(-1., 2, 3), (float('nan'), 2, 3), (float('inf'), 2, 3)):
with self.subTest(speed=speed):
limits = CarControllerParams(cp, speed)
self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN), (409, up, down))
for sign in (-1, 1):
first = sign * (min(up, down) if sign > 0 else up)
self.assertEqual(apply_driver_steer_torque_limits(sign * 409, 0, 0, limits), first)
self.assertEqual(apply_driver_steer_torque_limits(sign * 409, sign, 0, limits), sign * (1 + up))
self.assertEqual(apply_driver_steer_torque_limits(0, sign * 409, 0, limits), sign * (409 - down))
self.assertEqual(apply_driver_steer_torque_limits(-sign * 409, sign, 0, limits), sign * (1 - down))
state = CarState(cp)
state.out = state.update(state.get_can_parsers(cp))
control = structs.CarControl()
control.enabled = control.latActive = True
control.actuators.torque = 1.0
controller = CarController(DBC[cp.carFingerprint], cp)
for tick, (speed, previous, expected) in enumerate(((14., 399, 409), (15., 409, 409), (15., 399, 401))):
state.out.vEgoRaw = speed
controller.apply_torque_last = previous
result, _ = controller.update(control.as_reader(), state, 1_000_000_000 + tick * 10_000_000)
self.assertEqual(result.torqueOutputCan, expected)
def test_angle_carnival_and_classic_limits_are_unchanged(self):
cp = ev6(True).as_reader().as_builder()
cp.flags |= int(HyundaiFlags.CANFD_ANGLE_STEERING)
for speed in (0., 15.):
limits = CarControllerParams(cp, speed)
self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN, limits.STEER_THRESHOLD), (270, 2, 3, 175))
cp.flags &= ~int(HyundaiFlags.CANFD_ANGLE_STEERING)
cp.carFingerprint = CAR.KIA_CARNIVAL_HEV_4TH_GEN
for speed in (0., 15.):
limits = CarControllerParams(cp, speed)
self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN), (409, 2, 3))
for vehicle, flags, maximum in ((CAR.HYUNDAI_ELANTRA, 0, 255), (CAR.GENESIS_G70_2020, HyundaiFlags.ALT_LIMITS, 270),
(CAR.GENESIS_G70_2020, HyundaiFlags.ALT_LIMITS_2, 170)):
cp.carFingerprint, cp.flags = vehicle, int(flags)
for speed in (0., 15.):
self.assertEqual(CarControllerParams(cp, speed).STEER_MAX, maximum)
if __name__ == "__main__":
unittest.main()
openpilot/starpilot/lateral/tests/test_genesis_g70_policy.py
+211
View File
@@ -0,0 +1,211 @@
"""Compare runtime output against frozen default original StarPilot traces."""
import json
from collections import deque
from pathlib import Path
from types import SimpleNamespace
from unittest.mock import patch
import pytest
from openpilot.cereal import log
from opendbc.car import structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode
from openpilot.starpilot.lateral.torque_extension import selected_policy
from openpilot.starpilot.lateral.genesis_g70_policy import supported_cp
from openpilot.starpilot.lateral import genesis_g70_policy
FIXTURE = Path(__file__).with_name('fixtures') / 'genesis_g70_ba901b5f.json'
def controller_for(car=CAR.GENESIS_G70_2020):
cp = interfaces[car].get_non_essential_params(car)
return cp, LatControlTorque(cp.as_reader(), interfaces[car](cp), 0.01), VehicleModel(cp)
@pytest.mark.parametrize('case_index', range(5))
@patch.object(genesis_g70_policy, 'get_genesis_g70_center_measurement_damping_gain', return_value=0.0)
@patch.object(genesis_g70_policy, 'get_genesis_g70_traffic_friction_increment', return_value=0.0)
def test_frozen_base_tuning_numeric_traces(_traffic, _damping, case_index):
# This ba901b5f fixture intentionally tests historical base math, as the
# existing damping isolation does. The new traffic stage is exercised by
# test_genesis_dom_leaves.py against actual friction/current driver state.
fixture = json.loads(FIXTURE.read_text())
cp, controller, vm = controller_for()
assert selected_policy(controller) is not None
params = log.VehicleParameters.new_message(angleOffsetDeg=0.3, roll=0.006)
for index, frame in enumerate(fixture['cases'][case_index]['frames']):
active, speed, angle, pressed, limited, curve, curve_limited, delay = frame['input']
cs = SimpleNamespace(vEgo=speed, steeringAngleDeg=angle, steeringPressed=pressed)
torque, desired_angle, state = controller.update(active, cs, vm, params, limited, curve, curve_limited, delay)
assert desired_angle == 0.0
assert torque == pytest.approx(frame['torque'], abs=1e-12, rel=0), (case_index, index)
assert [getattr(state, field) for field in fixture['log_fields']] == pytest.approx(frame['log'], abs=1e-12, rel=0)
assert state.active == frame['active']
assert state.saturated == frame['saturated']
assert abs(torque) <= 1.0
if not active:
assert torque == 0.0
assert controller.pid.i == 0.0
assert selected_policy(controller).prev_output_torque == 0.0
def test_exact_vehicle_admission_and_upstream_gains():
cp, controller, _ = controller_for()
assert supported_cp(cp)
assert controller.pid._k_i[1] == [0.35]
for field, value in [('brand', 'toyota'), ('dashcamOnly', True), ('passive', True), ('steerControlType', structs.CarParams.SteerControlType.angle)]:
changed = cp.as_reader().as_builder()
setattr(changed, field, value)
assert not supported_cp(changed)
native = LatControlTorque(changed.as_reader(), interfaces[CAR.GENESIS_G70_2020](changed), 0.01)
assert selected_policy(native) is None
assert native.pid._k_i[1] == [0.15]
other_cp, other, _ = controller_for(CAR.GENESIS_G70)
assert not supported_cp(other_cp)
assert selected_policy(other) is None
assert other.pid._k_i[1] == [0.15]
non_torque = cp.as_reader().as_builder()
non_torque.lateralTuning.init('pid')
assert not supported_cp(non_torque)
def test_live_source_updates_keep_raw_calibration_and_limits():
cp, controller, _ = controller_for()
original = cp.lateralTuning.torque.latAccelFactor
assert controller.torque_params.latAccelFactor == original
for _ in range(3):
controller.update_torque_parameters(original * 1.1, 0.025, 0.08)
assert controller.torque_params.latAccelFactor == pytest.approx(original * 1.1)
assert controller.torque_params.latAccelOffset == pytest.approx(0.025)
assert controller.torque_params.friction == pytest.approx(0.08)
assert controller.pid.pos_limit == controller.lateral_accel_from_torque(1.0, controller.torque_params)
assert controller.pid.neg_limit == controller.lateral_accel_from_torque(-1.0, controller.torque_params)
def test_driver_override_and_inactive_state_reset():
_, controller, vm = controller_for()
params = log.VehicleParameters.new_message(angleOffsetDeg=0.3, roll=0.006)
cs = SimpleNamespace(vEgo=4.0, steeringAngleDeg=20.3, steeringPressed=False)
with patch.object(genesis_g70_policy, 'get_genesis_g70_low_speed_angle_damping',
wraps=genesis_g70_policy.get_genesis_g70_low_speed_angle_damping) as damping, \
patch.object(genesis_g70_policy, 'get_genesis_g70_stabilized_output',
wraps=genesis_g70_policy.get_genesis_g70_stabilized_output) as smoothing, \
patch.object(controller.pid, 'update', wraps=controller.pid.update) as pid_update:
controller.update(True, cs, vm, params, False, .002, False, .2)
damping.assert_called_once()
smoothing.assert_called_once()
cs.steeringPressed = True
controller.update(True, cs, vm, params, False, .002, False, .2)
assert damping.call_count == 1
assert smoothing.call_count == 1
assert pid_update.call_args.kwargs['freeze_integrator'] is True
cs.steeringPressed = False
controller.update(True, cs, vm, params, True, .002, False, .2)
assert pid_update.call_args.kwargs['freeze_integrator'] is True
torque, _, state = controller.update(False, cs, vm, params, False, .002, False, .2)
policy = selected_policy(controller)
assert torque == 0.0 and not state.active and controller.pid.i == 0.0
assert policy.prev_output_torque == 0.0
assert policy.jerk_filter.x == 0.0
assert policy.measurement_rate_filter.x == 0.0
assert policy.curvature_request_buffer[-1] == .002
def test_preconfigured_manual_torque_values_preserved():
cp = interfaces[CAR.GENESIS_G70_2020].get_non_essential_params(CAR.GENESIS_G70_2020)
cp.lateralTuning.torque.latAccelFactor = 2.45
cp.lateralTuning.torque.latAccelOffset = -.031
cp.lateralTuning.torque.friction = .072
controller = LatControlTorque(cp.as_reader(), interfaces[CAR.GENESIS_G70_2020](cp), .01)
assert controller.torque_params.latAccelFactor == cp.lateralTuning.torque.latAccelFactor
assert controller.torque_params.latAccelOffset == cp.lateralTuning.torque.latAccelOffset
assert controller.torque_params.friction == cp.lateralTuning.torque.friction
assert controller.pid.pos_limit == controller.lateral_accel_from_torque(1.0, controller.torque_params)
@pytest.mark.parametrize('angle', [-85., 0., 85.])
@pytest.mark.parametrize('pid_output', [-.125, 0., .125])
def test_angle_taper_uses_actual_sent_torque_direction(angle, pid_output):
outputs = []
for taper in (lambda *_: 1., genesis_g70_policy.get_genesis_g70_angle_output_scale):
_, controller, vm = controller_for()
cs = SimpleNamespace(vEgo=15., steeringAngleDeg=angle, steeringPressed=True)
params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.)
with patch.object(controller.pid, 'update', return_value=pid_output), \
patch.object(controller, 'torque_from_lateral_accel', side_effect=lambda value, *_: value), \
patch.object(genesis_g70_policy, 'get_genesis_g70_angle_output_scale', taper):
output, _, state = controller.update(True, cs, vm, params, False, 0., False, .2)
assert state.active
outputs.append(output)
baseline, tapered = outputs
if pid_output == 0:
assert baseline == tapered == 0
else:
assert baseline * pid_output < 0
if baseline * angle > 0:
assert 0 < abs(tapered) < abs(baseline)
else:
assert tapered == pytest.approx(baseline)
@pytest.mark.parametrize('mph,desired,measured,jerk,expected', [
(65., 0., .10, 0., .09), (50., 0., .10, 0., 0.), (55., 0., .10, 0., .045),
(65., .25, .10, 0., .045), (65., 0., .25, 0., .045),
(65., .35, .10, 0., 0.), (65., 0., .35, 0., 0.),
(65., 0., .10, .35, .045), (65., 0., .10, .50, 0.),
])
def test_center_measurement_damping_gates(mph, desired, measured, jerk, expected):
for direction in (-1., 1.):
gain = genesis_g70_policy.get_genesis_g70_center_measurement_damping_gain(
mph * .44704, desired * direction, measured * direction, jerk * direction)
assert gain == pytest.approx(expected)
@pytest.mark.parametrize('direction', (-1., 1.))
def test_center_measurement_damping_update_and_recovery(direction):
_, controller, vm = controller_for()
cs = SimpleNamespace(vEgo=65. * .44704, steeringAngleDeg=0., steeringPressed=False)
params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.)
controller.update(True, cs, vm, params, False, 0., False, .2)
cs.steeringAngleDeg = -direction * .5
output, _, moving = controller.update(True, cs, vm, params, False, 0., False, .2)
assert moving.d * direction < 0.
assert abs(moving.d) <= .225
assert abs(output) <= controller.steer_max
for _ in range(150):
_, _, steady = controller.update(True, cs, vm, params, False, 0., False, .2)
assert steady.d == pytest.approx(0., abs=1e-6)
cs.steeringPressed = True
cs.steeringAngleDeg = 0.
_, _, driver = controller.update(True, cs, vm, params, False, 0., False, .2)
assert driver.d == 0.
cs.steeringPressed = False
controller.update(False, cs, vm, params, False, 0., False, .2)
_, _, resumed = controller.update(True, cs, vm, params, False, 0., False, .2)
assert resumed.d == 0.
cs.vEgo = 40. * .44704
cs.steeringAngleDeg = -direction * .5
_, _, low_speed = controller.update(True, cs, vm, params, False, 0., False, .2)
assert low_speed.d == 0.
cs.vEgo = 65. * .44704
curvature = direction * .8 / cs.vEgo**2
policy = selected_policy(controller)
policy.curvature_request_buffer = deque([curvature] * policy.request_buffer_len, maxlen=policy.request_buffer_len)
_, _, curve = controller.update(True, cs, vm, params, False, curvature, False, .2)
assert curve.d == 0.
@pytest.mark.parametrize('car', (CAR.GENESIS_G70_2020, CAR.GENESIS_G70, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, CAR.KIA_EV6, CAR.HYUNDAI_IONIQ_6))
def test_center_damping_isolated_to_selected_g70(car):
cp, controller, vm = controller_for(car)
if car == CAR.GENESIS_G70_2020:
controller = LatControlTorque(cp.as_reader(), interfaces[car](cp), .01, controller_mode=ControllerMode.STANDARD)
cs = SimpleNamespace(vEgo=65. * .44704, steeringAngleDeg=0., steeringPressed=False)
params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.)
with patch.object(genesis_g70_policy, 'get_genesis_g70_center_measurement_damping_gain', side_effect=AssertionError('Wrong vehicle policy')):
controller.update(True, cs, vm, params, False, 0., False, .2)
openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py
+146
View File
@@ -0,0 +1,146 @@
"""Exact GV70 feedforward tuning through the production torque extension."""
from collections import deque
import json
import math
import struct
from pathlib import Path
from types import SimpleNamespace
from unittest.mock import patch
import pytest
from openpilot.cereal import log
from opendbc.car import structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode, policy_for, selection_from_bytes
from openpilot.starpilot.lateral.torque_extension import selected_policy
from openpilot.starpilot.lateral import genesis_gv70_policy as policy
from openpilot.starpilot.lateral import gv70_shaping
def controller_for(vehicle=CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, mode=None):
cp=interfaces[vehicle].get_non_essential_params(vehicle)
controller=LatControlTorque(cp.as_reader(),interfaces[vehicle](cp),.01,controller_mode=mode)
cs=SimpleNamespace(vEgo=25.,steeringAngleDeg=0.,steeringPressed=False)
params=log.VehicleParameters.new_message(angleOffsetDeg=0.,roll=0.)
return cp,controller,VehicleModel(cp),cs,params
def update(controller,vm,cs,params,curvature=.0002,active=True):
return controller.update(active,cs,vm,params,False,curvature,False,.2)
def test_original_derived_default_formula_vectors():
data=json.loads((Path(__file__).with_name('fixtures')/'gv70_385428f6.json').read_text())
assert data['source_commit']=='385428f6ea1c183de4ddcd8604f294abec02a9e8'
for row in data['vectors']:
value=getattr(gv70_shaping,row['function'])(*row['input'])
assert value==pytest.approx(row['expected'],abs=1e-14,rel=0),row
def test_exact_admission_and_saved_standard_keeps_upstream_controller():
cp,controller,*_=controller_for()
assert policy.supported_cp(cp)
assert policy_for(cp)=='genesis_gv70_electrified'
assert isinstance(selected_policy(controller),policy.GenesisGV70TorquePolicy)
# Dom retains the full speed-interpolated schedule, ending at0.6, not flat0.6.
assert controller.pid._k_p==[[1,1.5,2.,3.,5,7.5,10,15,30],[250,120,65,30,11.5,5.5,3.5,2.,.6]]
assert controller.pid._k_i==([0],[.35])
choice=json.dumps({'version':1,'vehicles':{'GENESIS_GV70_ELECTRIFIED_1ST_GEN':{'brand':'hyundai','mode':'standard'}}}).encode()
assert selection_from_bytes(cp,choice).mode==ControllerMode.STANDARD
_,standard,*_=controller_for(mode=ControllerMode.STANDARD)
assert selected_policy(standard) is None
assert standard.pid._k_i==([0],[.15])
assert standard.pid._k_d==([0],[0.])
for field,value in (('brand','toyota'),('passive',True),('dashcamOnly',True),('notCar',True),
('steerControlType',structs.CarParams.SteerControlType.angle)):
changed=cp.as_reader().as_builder();setattr(changed,field,value)
assert not policy.supported_cp(changed)
assert policy_for(changed) is None
changed=cp.as_reader().as_builder();changed.lateralTuning.init('pid')
assert not policy.supported_cp(changed)
for vehicle in (CAR.GENESIS_GV70_1ST_GEN,CAR.GENESIS_GV70_ELECTRIFIED_2ND_GEN):
sibling=interfaces[vehicle].get_non_essential_params(vehicle)
assert not policy.supported_cp(sibling)
assert policy_for(sibling) is None
def test_stabilization_only_changes_feedforward_feedback_remains_immediate():
cp,controller,vm,cs,params=controller_for()
calls=[]
def stable(*values):calls.append(values);return .123
with patch.object(policy,'get_genesis_gv70_stabilized_output',side_effect=stable):
update(controller,vm,cs,params)
assert not calls # first frame has no previous feedforward
output,_,state=update(controller,vm,cs,params)
assert state.f==pytest.approx(.123)
assert output==pytest.approx(-controller.torque_from_lateral_accel(controller.pid.control,controller.torque_params))
prior_p=state.p
cs.steeringAngleDeg=.5
_,_,changed=update(controller,vm,cs,params)
assert changed.f==pytest.approx(.123)
assert changed.p!=pytest.approx(prior_p)
assert calls[-1][1]==pytest.approx(.123)
previous=len(calls)
cs.steeringPressed=True
_,_,pressed=update(controller,vm,cs,params)
assert pressed.d==0
assert len(calls)==previous
cs.steeringPressed=False
update(controller,vm,cs,params)
assert len(calls)==previous # release tick also bypasses smoothing
update(controller,vm,cs,params,active=False)
assert selected_policy(controller).previous_feedforward is None
update(controller,vm,cs,params,curvature=-.0002)
assert len(calls)==previous
def test_measurement_damping_matches_original_schedule_bound_and_has_no_bias():
_,controller,vm,cs,params=controller_for()
cs.vEgo=30.
update(controller,vm,cs,params,curvature=0)
cs.steeringAngleDeg=-5.
output,_,moving=update(controller,vm,cs,params,curvature=0)
# Assert double-precision PID arithmetic and the exact Float32 telemetry separately.
assert controller.pid.d==pytest.approx(-.3,abs=1e-12), controller.pid.d
assert moving.d==struct.unpack("f",struct.pack("f",-.3))[0], moving.d
assert abs(output)<=controller.steer_max
for _ in range(150):_,_,steady=update(controller,vm,cs,params,curvature=0)
assert steady.d==pytest.approx(0,abs=1e-6)
cs.vEgo=3.;cs.steeringAngleDeg=5.
_,_,low=update(controller,vm,cs,params,curvature=0)
assert low.d==0
@pytest.mark.parametrize('direction',(-1.,1.))
def test_overshoot_feedback_bypasses_previous_turn_feedforward(direction):
_,controller,vm,cs,params=controller_for()
cs.vEgo=22.
cs.steeringAngleDeg=math.degrees(vm.get_steer_from_curvature(-direction*2.4/cs.vEgo**2,cs.vEgo,0))
selected=selected_policy(controller)
selected.curvature_request_buffer=deque([direction*.8/cs.vEgo**2]*selected.request_buffer_len,maxlen=selected.request_buffer_len)
selected.previous_feedforward=direction*.8
with patch.object(policy,'get_genesis_gv70_stabilized_output',return_value=direction*.2):
output,_,state=update(controller,vm,cs,params,curvature=direction*.8/cs.vEgo**2)
assert state.p*direction<0
assert output*direction>0
assert abs(output)<=controller.steer_max
@pytest.mark.parametrize('vehicle',(CAR.GENESIS_G70_2020,CAR.GENESIS_GV70_1ST_GEN,CAR.KIA_EV6,CAR.HYUNDAI_IONIQ_6))
def test_measurement_damping_does_not_touch_other_vehicle_policies(vehicle):
_,controller,*_=controller_for(vehicle)
assert not isinstance(selected_policy(controller),policy.GenesisGV70TorquePolicy)
assert max(controller.pid._k_d[1])==0
def test_learned_calibration_updates_stay_raw_without_new_multipliers():
cp,controller,*_=controller_for()
factor=cp.lateralTuning.torque.latAccelFactor
assert controller.torque_params.latAccelFactor==factor
controller.update_torque_parameters(factor*1.1,.025,.08)
assert controller.torque_params.latAccelFactor==pytest.approx(factor*1.1,rel=1e-6)
assert controller.torque_params.latAccelOffset==pytest.approx(.025)
assert controller.torque_params.friction==pytest.approx(.08)
openpilot/starpilot/lateral/tests/test_genesis_torque_context.py
+74
View File
@@ -0,0 +1,74 @@
"""Actual Genesis controller proof for exact mapped Dom leaves."""
from collections import deque
from types import SimpleNamespace
import math
import pytest
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.cereal import log
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode
from openpilot.starpilot.lateral import genesis_gv70_policy as gv70
from openpilot.starpilot.lateral import genesis_g70_policy as g70
@pytest.mark.parametrize('direction', [-1.0, 1.0])
def test_gv70_actual_undertracking_curve_matches_undamped_policy(monkeypatch, direction):
cp = interfaces[CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN].get_non_essential_params(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
def run():
controller = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), 0.01, controller_mode=ControllerMode.STARPILOT)
vm = VehicleModel(cp)
cs = SimpleNamespace(vEgo=30.0, steeringAngleDeg=math.degrees(vm.get_steer_from_curvature(-direction * 0.6 / 900.0, 30.0, 0.0)), steeringPressed=False)
params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0)
request = direction * 0.8 / 900.0
policy = controller.starpilot_extension.policy
policy.curvature_request_buffer = deque([request] * policy.request_buffer_len, maxlen=policy.request_buffer_len)
rows = []
for _ in range(100):
out, _, state = controller.update(True, cs, vm, params, False, request, False, 0.2)
assert math.isfinite(out) and abs(out) <= controller.steer_max + 1e-6
rows.append((out, state.desiredLateralAccel, state.p, state.i, state.d, state.f))
return rows, controller.torque_params.to_dict(), (controller.pid.pos_limit, controller.pid.neg_limit)
actual = run()
for name in ('get_genesis_gv70_unwind_ff_scale', 'get_genesis_gv70_high_speed_error_scale', 'get_genesis_gv70_reversal_output_scale'):
monkeypatch.setattr(gv70, name, lambda *_: 1.0)
assert actual == run()
@pytest.mark.parametrize('direction', [-1.0, 1.0])
@pytest.mark.parametrize('pressed', [False, True])
def test_g70_actual_friction_receives_traffic_increment_only_without_driver_override(monkeypatch, direction, pressed):
cp = interfaces[CAR.GENESIS_G70_2020].get_non_essential_params(CAR.GENESIS_G70_2020)
actual = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), 0.01, controller_mode=ControllerMode.STARPILOT)
actual.update_torque_parameters(3.0, 0.02, 0.12)
tune = actual.torque_params.to_dict()
limits = (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max)
cs = SimpleNamespace(vEgo=5.0, steeringAngleDeg=0.0, steeringPressed=pressed)
params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0)
vm = VehicleModel(cp)
calls = []
original = g70.get_friction
def capture(error, deadzone, threshold, tune):
calls.append(threshold)
return original(error, deadzone, threshold, tune)
monkeypatch.setattr(g70, 'get_friction', capture)
for _ in range(100):
out, _, state = actual.update(True, cs, vm, params, False, direction * 0.0001, False, 0.2)
assert math.isfinite(out) and abs(out) <= actual.steer_max + 1e-6
base = g70.get_genesis_g70_friction_threshold(cs.vEgo, state.desiredLateralAccel, state.desiredLateralJerk)
inc = (
0.0 if pressed else g70.get_genesis_g70_traffic_friction_increment(cs.vEgo, state.desiredLateralAccel, state.desiredLateralJerk, direction * 0.0001, 0.0)
)
assert calls[-1] == pytest.approx(base + inc)
assert (inc == 0.0) is pressed
assert actual.torque_params.to_dict() == tune
assert (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max) == limits
calls.clear()
out, _, state = actual.update(False, cs, vm, params, False, direction * 0.0001, False, 0.2)
assert out == 0.0 and not state.active and calls == []
openpilot/starpilot/lateral/tests/test_hkg_centre_context.py
+111
View File
@@ -0,0 +1,111 @@
"""Dom center leaves qualify only through actual per-vehicle selected controllers."""
from types import SimpleNamespace
import math
import pytest
from openpilot.cereal import log
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection
from openpilot.starpilot.lateral import hkg_vehicle_shaping as hkg
from openpilot.starpilot.lateral import ioniq6_policy as iq6
@pytest.mark.parametrize('vehicle,speed', [(CAR.KIA_NIRO_EV, 65.0 * 0.44704), (CAR.KIA_CARNIVAL_HEV_4TH_GEN, 10.0), (CAR.HYUNDAI_IONIQ_6, 10.0)])
@pytest.mark.parametrize('direction', [-1.0, 1.0])
@pytest.mark.parametrize('pressed', [False, True])
def test_actual_same_frame_threshold_error_output_and_override(monkeypatch, vehicle, speed, direction, pressed):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
def run(neutral=False):
controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), 0.01, controller_mode=ControllerMode.STARPILOT)
controller.update_torque_parameters(3.0, 0.02, 0.12)
_policy = controller.starpilot_extension.policy
cs = SimpleNamespace(vEgo=speed, steeringAngleDeg=0.0, steeringPressed=pressed)
params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0)
calls = []
bound = controller.friction
def capture(error, deadzone, threshold, car, request):
calls.append((error, deadzone, threshold))
return bound(error, deadzone, threshold, car, request)
monkeypatch.setattr(controller, 'friction', capture)
if neutral:
if vehicle == CAR.KIA_NIRO_EV:
monkeypatch.setattr(hkg, 'KIA_NIRO_EV_CENTER_FRICTION_THRESHOLD_GAIN', 0.0)
elif vehicle == CAR.KIA_CARNIVAL_HEV_4TH_GEN:
monkeypatch.setattr(hkg, 'KIA_CARNIVAL_HEV_CENTER_FRICTION_INCREMENT', 0.0)
monkeypatch.setattr(hkg, 'KIA_CARNIVAL_HEV_CENTER_ERROR_REDUCTION', 0.0)
else:
monkeypatch.setattr(iq6, 'get_ioniq_6_2023_center_friction_increment', lambda *_: 0.0)
for _ in range(150):
out, _, state = controller.update(True, cs, VehicleModel(cp), params, False, direction * 0.02 / speed**2, False, 0.2)
assert math.isfinite(out) and abs(out) <= controller.steer_max + 1e-6
live = (out, state.to_dict(), calls[-1], controller.torque_params.to_dict(), (controller.pid.pos_limit, controller.pid.neg_limit, controller.steer_max))
calls.clear()
off, _, offstate = controller.update(False, cs, VehicleModel(cp), params, False, 0.0, False, 0.2)
assert off == 0.0 and not offstate.active and not calls
return live
actual = run()
baseline = run(True)
assert actual[3:] == baseline[3:]
if pressed:
assert actual == baseline
else:
assert actual[2][0:2] == baseline[2][0:2] # Friction error/deadzone retain original stage.
assert actual[2][2] > baseline[2][2]
assert abs(actual[1]['f']) < abs(baseline[1]['f'])
if vehicle == CAR.KIA_CARNIVAL_HEV_4TH_GEN:
assert abs(actual[1]['error']) < abs(baseline[1]['error'])
assert actual[1]['error'] == pytest.approx(0.75 * baseline[1]['error'])
else:
assert actual[1]['error'] == baseline[1]['error']
def test_niro_saved_standard_and_other_factory_have_no_new_profile():
cp = interfaces[CAR.KIA_NIRO_EV].get_non_essential_params(CAR.KIA_NIRO_EV)
assert default_selection(cp).mode == ControllerMode.STANDARD
standard = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), 0.01)
assert standard.starpilot_extension is None
for field in ('passive', 'dashcamOnly', 'notCar'):
other = cp.as_reader().as_builder()
setattr(other, field, True)
from openpilot.starpilot.lateral.hkg_vehicle_policy import policy_for
assert policy_for(other) is None
@pytest.mark.parametrize('future,error,jerk,speed', [(0.5, 0.02, 0.0, 10.0), (0.02, 0.4, 0.0, 10.0), (0.02, 0.02, 1.0, 10.0), (0.02, 0.02, 0.0, 25.0)])
def test_hev_center_leaf_releases_for_turn_error_jerk_and_high_speed(future, error, jerk, speed):
assert hkg.get_kia_carnival_hev_center_weight(speed, 0.02, 0.02 - error, jerk, future) == 0.0
@pytest.mark.parametrize('vehicle', [CAR.KIA_CARNIVAL_HEV_4TH_GEN, CAR.HYUNDAI_IONIQ_6])
def test_actual_previous_driver_release_does_not_immediately_apply_center_delta(monkeypatch, vehicle):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), 0.01, controller_mode=ControllerMode.STARPILOT)
cs = SimpleNamespace(vEgo=10.0, steeringAngleDeg=0.0, steeringPressed=True)
params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0)
vm = VehicleModel(cp)
for _ in range(150):
controller.update(True, cs, vm, params, False, 0.0002, False, 0.2)
calls = []
original = controller.friction
def capture(error, deadzone, threshold, car, request):
calls.append(threshold)
return original(error, deadzone, threshold, car, request)
monkeypatch.setattr(controller, 'friction', capture)
cs.steeringPressed = False
_, _, state = controller.update(True, cs, vm, params, False, 0.0002, False, 0.2)
if vehicle == CAR.KIA_CARNIVAL_HEV_4TH_GEN:
assert controller.starpilot_extension.policy.hev_center_weight == 0.0
assert calls[-1] == pytest.approx(hkg.get_kia_carnival_friction_threshold(10.0, state.desiredLateralAccel, state.desiredLateralJerk))
else:
taper = iq6.get_ioniq_6_center_taper_scale(state.desiredLateralAccel, 10.0)
assert calls[-1] == pytest.approx(iq6.get_ioniq_6_friction_threshold(10.0, state.desiredLateralAccel, state.desiredLateralJerk) / max(taper, 1e-3))
openpilot/starpilot/lateral/tests/test_hkg_highway_friction.py
+156
View File
@@ -0,0 +1,156 @@
import math
from types import SimpleNamespace
import unittest
from unittest import mock
from opendbc.car import structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR, HyundaiFlags
from opendbc.car.lateral import get_friction
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.constants import CV
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib import latcontrol_torque
from openpilot.starpilot.lateral.controller_selection import ControllerMode
def controller_for(vehicle=CAR.KIA_EV6, mode=None, **changes):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
ci = interfaces[vehicle](cp)
for key, value in changes.items():
setattr(cp, key, value)
return cp, LatControlTorque(cp.as_reader(), ci, .01, controller_mode=mode)
class HKGHighwayFrictionTests(unittest.TestCase):
def test_registry_and_wrong_brand_flag_collision(self):
admitted = 0
for vehicle in CAR:
cp = interfaces[vehicle].get_non_essential_params(vehicle)
if cp.lateralTuning.which() != 'torque':
continue
_, controller = controller_for(vehicle)
expected = bool(cp.flags & HyundaiFlags.CANFD and not cp.flags & HyundaiFlags.CANFD_ANGLE_STEERING and
vehicle != CAR.HYUNDAI_IONIQ_6 and
cp.steerControlType == structs.CarParams.SteerControlType.torque and
not (cp.passive or cp.dashcamOnly or cp.notCar))
self.assertEqual(controller.hkg_canfd_torque, expected, str(vehicle))
admitted += expected
self.assertGreater(admitted, 20)
for changes in (
{'brand': 'toyota'}, {'flags': 0}, {'passive': True}, {'dashcamOnly': True}, {'notCar': True},
{'steerControlType': structs.CarParams.SteerControlType.angle},
{'flags': int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)},
):
with self.subTest(changes=changes):
_, controller = controller_for(**changes)
self.assertFalse(controller.hkg_canfd_torque)
cs = SimpleNamespace(vEgo=75 * CV.MPH_TO_MS, steeringPressed=False)
self.assertEqual(controller.friction(.1, 0., .2, cs, .1), get_friction(.1, 0., .2, controller.torque_params))
def test_threshold_floor_soft_deadband_and_existing_larger_threshold(self):
_, controller = controller_for()
for mph, threshold in ((0, .39), (50, .39), (60, .39 + (.65 - .39) * 2 / 3),
(65, .65), (75, .78), (100, .78)):
with self.subTest(mph=mph):
cs = SimpleNamespace(vEgo=mph * CV.MPH_TO_MS, steeringPressed=False)
with mock.patch.object(latcontrol_torque, 'get_friction', wraps=get_friction) as friction:
controller.friction(.1, .01, .39, cs, 0.)
error, deadzone, actual_threshold, params = friction.call_args.args
weight = min(max((mph - 50) / 15, 0.), 1.)
self.assertAlmostEqual(error, .1 - .04 * weight)
self.assertEqual(deadzone, .01)
self.assertAlmostEqual(actual_threshold, threshold)
self.assertIs(params, controller.torque_params)
cs = SimpleNamespace(vEgo=75 * CV.MPH_TO_MS, steeringPressed=False)
with mock.patch.object(latcontrol_torque, 'get_friction', wraps=get_friction) as friction:
controller.friction(.1, 0., .9, cs, 0.)
self.assertEqual(friction.call_args.args[2], .9)
def test_curve_relief_driver_handoff_zero_and_peak_compensation(self):
_, controller = controller_for()
for mph, setpoint, pressed in ((50, 0., False), (75, .65, False), (75, -.65, False),
(75, 2., False), (75, 0., True)):
with self.subTest(mph=mph, setpoint=setpoint, pressed=pressed):
cs = SimpleNamespace(vEgo=mph * CV.MPH_TO_MS, steeringPressed=pressed)
for error in (-1., -.1, 0., .1, 1.):
self.assertEqual(controller.friction(error, 0., .39, cs, setpoint),
get_friction(error, 0., .39, controller.torque_params))
cs = SimpleNamespace(vEgo=75 * CV.MPH_TO_MS, steeringPressed=False)
self.assertAlmostEqual(controller.friction(0., 0., .39, cs, 0.), 0.)
self.assertAlmostEqual(controller.friction(.02, 0., .39, cs, 0.), 0.)
for sign in (-1, 1):
self.assertEqual(controller.friction(sign * 2., 0., .39, cs, 0.),
get_friction(sign * 2., 0., .39, controller.torque_params))
def test_symmetry_continuity_and_friction_bound(self):
_, controller = controller_for()
cap = controller.torque_params.latAccelFactor * controller.torque_params.friction
epsilon = 1e-7
for mph in (30., 50., 60., 65., 75., 100.):
cs = SimpleNamespace(vEgo=mph * CV.MPH_TO_MS, steeringPressed=False)
for accel in (0., .25, .45, .65, 1.):
for error in (-2., -.1, -.04, 0., .04, .1, 2.):
value = controller.friction(error, 0., .39, cs, accel)
negative = controller.friction(-error, 0., .39, cs, -accel)
self.assertTrue(math.isfinite(value))
self.assertAlmostEqual(value, -negative)
self.assertLessEqual(abs(value), cap)
self.assertLessEqual(abs(value), abs(get_friction(error, 0., .39, controller.torque_params)) + 1e-15)
left = controller.friction(error - epsilon, 0., .39, cs, accel - epsilon)
right = controller.friction(error + epsilon, 0., .39, cs, accel + epsilon)
self.assertLess(abs(left - right), 1e-5)
left = controller.friction(.1, 0., .39, SimpleNamespace(vEgo=cs.vEgo - epsilon, steeringPressed=False), accel)
right = controller.friction(.1, 0., .39, SimpleNamespace(vEgo=cs.vEgo + epsilon, steeringPressed=False), accel)
self.assertLess(abs(left - right), 1e-5)
def test_standard_and_custom_paths_only_change_friction(self):
for vehicle in (CAR.KIA_EV6, CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6, CAR.KIA_CARNIVAL_HEV_4TH_GEN,
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, CAR.HYUNDAI_TUCSON_4TH_GEN):
for mode in ControllerMode:
for speed, accel, quiet in ((20., .1, False), (32., .1, True), (32., .8, False)):
with self.subTest(vehicle=str(vehicle), mode=mode, speed=speed, accel=accel):
cp, controller = controller_for(vehicle, mode)
_, baseline = controller_for(vehicle, mode)
baseline.hkg_canfd_torque = False
vm = VehicleModel(cp)
cs = structs.CarState.new_message(vEgo=speed, steeringAngleDeg=0., steeringPressed=False)
cs.gearShifter = structs.CarState.GearShifter.drive
params = SimpleNamespace(angleOffsetDeg=0., roll=0.)
traces = []
for candidate in (controller, baseline):
values = []
for _ in range(100):
candidate.update(False, cs, vm, params, False, accel / speed ** 2, False, .3)
for _ in range(100):
output, _, state = candidate.update(True, cs, vm, params, False, accel / speed ** 2, False, .3)
values.append((output, state.desiredLateralAccel, state.desiredLateralJerk, state.p, state.i, state.f))
traces.append(values)
self.assertEqual(controller.torque_params.to_dict(), baseline.torque_params.to_dict())
self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit, controller.steer_max),
(baseline.pid.pos_limit, baseline.pid.neg_limit, baseline.steer_max))
for changed, original in zip(*traces, strict=True):
self.assertEqual(changed[1:5], original[1:5])
if quiet and vehicle != CAR.HYUNDAI_IONIQ_6:
self.assertLess(abs(traces[0][-1][5]), abs(traces[1][-1][5]))
self.assertLess(abs(traces[0][-1][0]), abs(traces[1][-1][0]))
else:
self.assertEqual(traces[0], traces[1])
def test_ioniq6_preserves_original_threshold_and_error(self):
for mode in ControllerMode:
_, controller = controller_for(CAR.HYUNDAI_IONIQ_6, mode)
self.assertFalse(controller.hkg_canfd_torque)
for mph in (0., 30., 50., 60., 65., 75., 100.):
for setpoint in (-1., -.65, -.25, 0., .25, .65, 1.):
for pressed in (False, True):
cs = SimpleNamespace(vEgo=mph * CV.MPH_TO_MS, steeringPressed=pressed)
for error in (-1., -.1, -.04, -.02, 0., .02, .04, .1, 1.):
for threshold in (.2, .39, .9):
with self.subTest(mode=mode, mph=mph, setpoint=setpoint, pressed=pressed,
error=error, threshold=threshold):
with mock.patch.object(latcontrol_torque, 'get_friction', wraps=get_friction) as friction:
value = controller.friction(error, .01, threshold, cs, setpoint)
self.assertEqual(friction.call_args.args, (error, .01, threshold, controller.torque_params))
self.assertEqual(value, get_friction(error, .01, threshold, controller.torque_params))
openpilot/starpilot/lateral/tests/test_hkg_torque_context.py
+119
View File
@@ -0,0 +1,119 @@
"""Exact Dom leaves must reach actual selected controllers, preserving native ownership."""
from types import SimpleNamespace
import math
import pytest
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.cereal import log
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection
from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping
@pytest.mark.parametrize(
'vehicle,helper,speed,curvature',
[
(CAR.KIA_STINGER_2022, 'get_kia_stinger_2022_friction_threshold', 28.0, 0.00005),
(CAR.HYUNDAI_IONIQ_5, 'get_ioniq_5_low_speed_output_limit', 1.0, 0.02),
(CAR.KIA_CARNIVAL_HEV_4TH_GEN, 'get_kia_carnival_unwind_output_scale', 15.0, 0.0002),
(CAR.HYUNDAI_KONA_EV_2022, 'get_kona_ev_2022_friction_threshold', 110.0 / 3.6, 0.00065),
],
)
@pytest.mark.parametrize('direction', [-1.0, 1.0])
@pytest.mark.parametrize('pressed', [False, True])
def test_actual_controller_context_defaults_live_tune_limits_and_off(monkeypatch, vehicle, helper, speed, curvature, direction, pressed):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
ci = interfaces[vehicle](cp)
assert default_selection(cp).mode == ControllerMode.STANDARD
native = LatControlTorque(cp.as_reader(), ci, 0.01, controller_mode=ControllerMode.STANDARD)
assert native.starpilot_extension is None
actual = LatControlTorque(cp.as_reader(), ci, 0.01, controller_mode=ControllerMode.STARPILOT)
actual.update_torque_parameters(3.0, 0.02, 0.12)
tune = actual.torque_params.to_dict()
limits = (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max)
calls = []
original = getattr(shaping, helper)
def capture(*args, **kwargs):
calls.append((args, kwargs))
return original(*args, **kwargs)
monkeypatch.setattr(shaping, helper, capture)
cs = SimpleNamespace(vEgo=speed, steeringAngleDeg=0.0, steeringPressed=pressed)
params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0)
for _ in range(80):
output, _, state = actual.update(True, cs, VehicleModel(cp), params, False, direction * curvature, False, 0.28)
assert math.isfinite(output) and abs(output) <= actual.steer_max + 1e-6
assert state.active and calls
args, kwargs = calls[-1]
if vehicle == CAR.KIA_STINGER_2022:
assert args[3] == pytest.approx(direction * curvature * speed**2)
assert args[4] is pressed
elif vehicle == CAR.HYUNDAI_IONIQ_5:
assert args[3] == pytest.approx(0.0 if pressed else direction * curvature)
assert math.isfinite(args[4])
elif vehicle == CAR.KIA_CARNIVAL_HEV_4TH_GEN:
assert (kwargs['output_torque'] is None) is pressed
assert actual.torque_params.to_dict() == tune
assert (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max) == limits
calls.clear()
output, _, state = actual.update(False, cs, VehicleModel(cp), params, False, direction * curvature, False, 0.28)
assert output == 0.0 and not state.active and calls == []
@pytest.mark.parametrize('direction', [-1.0, 1.0])
def test_stinger_threshold_increment_bypasses_override_and_future_turn(direction):
speed = 60.0 * 0.44704
old = shaping.get_kia_stinger_2022_friction_threshold(speed, direction * 0.1, 0.0, 0.0, True)
assert shaping.get_kia_stinger_2022_friction_threshold(speed, direction * 0.1, 0.0) == pytest.approx(old + 0.2)
assert shaping.get_kia_stinger_2022_friction_threshold(speed, direction * 0.1, 0.0, direction * 0.4) == pytest.approx(old)
@pytest.mark.parametrize('direction', [-1.0, 1.0])
def test_ioniq5_creep_floor_only_for_requested_turn_direction(direction):
old = shaping.get_ioniq_5_low_speed_output_limit(0.01, 0.0, 1.0)
actual = shaping.get_ioniq_5_low_speed_output_limit(0.01, 0.0, 1.0, direction * 0.02, direction * 0.1)
assert actual >= 0.3 and actual > old
assert shaping.get_ioniq_5_low_speed_output_limit(0.01, 0.0, 1.0, direction * 0.02, -direction * 0.1) == old
@pytest.mark.parametrize('direction', [-1.0, 1.0])
def test_carnival_countersteer_is_retained(direction):
assert shaping.get_kia_carnival_unwind_output_scale(direction * 0.2, direction * 0.5, -direction, 25.0, output_torque=-direction * 0.1) == 1.0
assert shaping.get_kia_carnival_unwind_output_scale(direction * 0.2, direction * 0.5, -direction, 25.0, output_torque=direction * 0.1) < 1.0
@pytest.mark.parametrize(
'vehicle,helper', [(CAR.KIA_STINGER_2022, 'get_kia_stinger_2022_friction_threshold'), (CAR.HYUNDAI_IONIQ_5, 'get_ioniq_5_low_speed_output_limit')]
)
def test_actual_first_driver_release_tick_preserves_dom_override_bypass(monkeypatch, vehicle, helper):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), 0.01, controller_mode=ControllerMode.STARPILOT)
cs = SimpleNamespace(vEgo=28.0, steeringAngleDeg=0.0, steeringPressed=True)
params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0)
vm = VehicleModel(cp)
controller.update(True, cs, vm, params, False, 0.00005, False, 0.2)
calls = []
original = getattr(shaping, helper)
def capture(*args, **kwargs):
calls.append((args, kwargs))
return original(*args, **kwargs)
monkeypatch.setattr(shaping, helper, capture)
cs.steeringPressed = False
controller.update(True, cs, vm, params, False, 0.00005, False, 0.2)
args, _ = calls[-1]
if vehicle == CAR.KIA_STINGER_2022:
assert args[4] is True
else:
assert args[3] == 0.0
calls.clear()
controller.update(True, cs, vm, params, False, 0.00005, False, 0.2)
args, _ = calls[-1]
if vehicle == CAR.KIA_STINGER_2022:
assert args[4] is False
else:
assert args[3] == pytest.approx(0.00005)
openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py
+138
View File
@@ -0,0 +1,138 @@
import pytest
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, replace_mode, selection_from_bytes
from openpilot.starpilot.lateral.hkg_vehicle_policy import HKGVehicleTorquePolicy, PROFILES, policy_for
CASES = [(vehicle, profile, factor) for profile, (vehicles, factor) in PROFILES.items() for vehicle in sorted(vehicles, key=str)]
@pytest.mark.parametrize('vehicle,profile,factor', CASES)
def test_exact_profile_defaults_and_saved_controller_selection(vehicle, profile, factor):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
assert policy_for(cp) == profile
expected = (ControllerMode.STARPILOT if vehicle in (CAR.KIA_FORTE_2019_NON_SCC, CAR.KIA_FORTE_2021_NON_SCC)
else ControllerMode.STANDARD)
assert default_selection(cp).mode == expected
for raw in (b'{', b'{"version":true,"vehicles":{}}', b'{"version":1,"vehicles":{},"vehicles":{}}', b'x' * 4097):
assert selection_from_bytes(cp, raw).mode == expected
unrelated = interfaces[CAR.HYUNDAI_IONIQ_6].get_non_essential_params(CAR.HYUNDAI_IONIQ_6)
assert selection_from_bytes(cp, replace_mode(None, unrelated, ControllerMode.STANDARD)).mode == expected
saved = replace_mode(None, cp, ControllerMode.STARPILOT)
choice = selection_from_bytes(cp, saved)
assert choice.mode == ControllerMode.STARPILOT
assert choice.policy == profile
ci = interfaces[vehicle](cp)
default = LatControlTorque(cp.as_reader(), ci, 0.01)
assert bool(default.starpilot_extension) == (expected == ControllerMode.STARPILOT)
standard_choice = selection_from_bytes(cp, replace_mode(saved, cp, ControllerMode.STANDARD))
standard = LatControlTorque(cp.as_reader(), ci, 0.01, controller_mode=standard_choice.mode)
assert standard.starpilot_extension is None
assert standard.torque_params.latAccelFactor == pytest.approx(cp.lateralTuning.torque.latAccelFactor)
controller = LatControlTorque(cp.as_reader(), ci, 0.01, controller_mode=choice.mode)
policy = controller.starpilot_extension.policy
assert isinstance(policy, HKGVehicleTorquePolicy)
assert policy.profile == profile
assert controller.torque_params.latAccelFactor == pytest.approx(cp.lateralTuning.torque.latAccelFactor * factor)
assert controller.pid.pos_limit == pytest.approx(controller.lateral_accel_from_torque(controller.steer_max, cp.lateralTuning.torque))
for _ in range(3):
controller.update_torque_parameters(3.0, 0.02, 0.12)
assert controller.torque_params.latAccelFactor == pytest.approx(3.0 * factor)
assert controller.torque_params.latAccelOffset == pytest.approx(0.02)
assert controller.torque_params.friction == pytest.approx(0.12)
assert selection_from_bytes(cp, replace_mode(saved, cp, ControllerMode.STANDARD)).mode == ControllerMode.STANDARD
@pytest.mark.parametrize('vehicle,profile,factor', CASES)
def test_unadmitted_and_cross_vehicle_choices_are_not_applied(vehicle, profile, factor):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
for field in ('passive', 'dashcamOnly', 'notCar'):
other = cp.as_reader().as_builder()
setattr(other, field, True)
assert policy_for(other) is None
cp.brand = 'gm'
assert policy_for(cp) is None
cp.brand = 'hyundai'
saved = replace_mode(None, cp, ControllerMode.STARPILOT)
other = interfaces[CAR.HYUNDAI_IONIQ_5].get_non_essential_params(CAR.HYUNDAI_IONIQ_5)
if vehicle != CAR.HYUNDAI_IONIQ_5:
assert selection_from_bytes(other, saved).mode == ControllerMode.STANDARD
@pytest.mark.parametrize('vehicle,profile,factor', CASES)
def test_invalid_torque_contract_cannot_select_vehicle_policy(vehicle, profile, factor):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
for field, value in (('latAccelFactor', 0.0), ('latAccelFactor', float('nan')), ('latAccelOffset', float('inf')), ('friction', -1.0)):
other = cp.as_reader().as_builder()
setattr(other.lateralTuning.torque, field, value)
assert policy_for(other) is None
assert selection_from_bytes(other, None).mode == ControllerMode.STANDARD
cp.lateralTuning.init('pid')
assert policy_for(cp) is None
@pytest.mark.parametrize('kph,desired,increment', (
(60., 0., 0.), (100., .65, 0.), (110., .65, .30), (120., .65, .30),
(120., -.65, .30), (120., 1.6, .15), (120., -1.6, .15),
(120., 2.0, 0.), (140., 0., .30), (140., -2., 0.),
))
def test_kona_2022_high_speed_friction_boundaries(monkeypatch, kph, desired, increment):
from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping
threshold = shaping.get_kona_ev_2022_friction_threshold(kph / 3.6, desired)
monkeypatch.setattr(shaping, 'KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN', 0.)
baseline = shaping.get_kona_ev_2022_friction_threshold(kph / 3.6, desired)
assert threshold == pytest.approx(baseline + increment)
assert baseline <= threshold <= baseline + .30
def test_kona_2022_high_speed_friction_reaches_selected_controller(monkeypatch):
from types import SimpleNamespace
from openpilot.cereal import log
from opendbc.car.vehicle_model import VehicleModel
from opendbc.car.lateral import get_friction
from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping
cp = interfaces[CAR.HYUNDAI_KONA_EV_2022].get_non_essential_params(CAR.HYUNDAI_KONA_EV_2022)
cs = SimpleNamespace(vEgo=120. / 3.6, steeringAngleDeg=0., steeringPressed=False)
params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.)
def run():
controller = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), .01, controller_mode=ControllerMode.STARPILOT)
for _ in range(60):
output, _, state = controller.update(True, cs, VehicleModel(cp), params, False, .0001, False, .28)
return controller, output, state
controller, output, state = run()
threshold = shaping.get_kona_ev_2022_friction_threshold(cs.vEgo, .65)
standard = shaping.get_standard_friction_threshold(cs.vEgo)
for direction in (-1., 1.):
small = get_friction(direction * .1, 0., threshold, controller.torque_params)
assert abs(small) < abs(get_friction(direction * .1, 0., standard, controller.torque_params))
assert small * direction > 0.
assert get_friction(direction, 0., threshold, controller.torque_params) == pytest.approx(
get_friction(direction, 0., standard, controller.torque_params))
monkeypatch.setattr(shaping, 'KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN', 0.)
baseline, base_output, base_state = run()
assert state.active and base_state.active
assert state.desiredLateralAccel == pytest.approx(base_state.desiredLateralAccel)
assert state.p == pytest.approx(base_state.p)
assert abs(state.f) < abs(base_state.f)
assert abs(output) < abs(base_output)
assert controller.steer_max == baseline.steer_max
@pytest.mark.parametrize('vehicle', (CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_KONA_NON_SCC))
def test_kona_2022_friction_does_not_change_sibling_policies(monkeypatch, vehicle):
from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping
def unexpected(*_):
raise AssertionError('Kona 2022 friction used by sibling policy')
monkeypatch.setattr(shaping, 'get_kona_ev_2022_friction_threshold', unexpected)
cp = interfaces[vehicle].get_non_essential_params(vehicle)
controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), .01, controller_mode=ControllerMode.STARPILOT)
threshold, scale = controller.starpilot_extension.policy.friction_context(.1, 0., 0., 120. / 3.6)
assert threshold > 0. and scale > 0.
openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py
+44
View File
@@ -0,0 +1,44 @@
import math
import pytest
from openpilot.starpilot.lateral.ioniq6_policy import get_ioniq_6_low_speed_angle_assist_torque as assist
@pytest.mark.parametrize('speed', [0., 1., 3., 6., 20.])
def test_small_error_sign_change_is_continuous(speed):
epsilon = 1e-5
# No model error means no additional angle-assist torque.
assert assist(0., 0., 0., speed) == 0.
assert abs(assist(epsilon, 0., 0., speed) - assist(-epsilon, 0., 0., speed)) < epsilon
# Same continuity while tracking a turn and releasing error across zero.
assert abs(assist(10. + epsilon, 10., 0., speed) - assist(10. - epsilon, 10., 0., speed)) < epsilon
def test_center_assist_is_bounded_and_does_not_reverse_direction():
for speed in (0., 1., 3., 7., 20.):
for desired in (-.9, -.1, 0., .1, .9):
output = assist(desired, 0., 0., speed)
assert math.isfinite(output) and abs(output) <= 1.
assert output * desired <= 0.
@pytest.mark.parametrize('desired, actual, torque, speed, expected', [
(10., 0., 0., 1., -.39409320626365096),
(-10., 0., 0., 1., .39409320626365096),
(0., 10., 0., 1., .07246171113121364),
(0., -10., 0., 1., -.07246171113121364),
(10., 8., -.5, 3., -.5322587905665043),
])
def test_existing_turn_and_unwind_response_outside_center_is_unchanged(desired, actual, torque, speed, expected):
assert assist(desired, actual, torque, speed) == expected
def test_center_correction_retains_output_saturation_and_symmetry():
for speed in (0., 1., 3., 10., 40.):
for actual in (0., 10., 30.):
for error in (-.9, -.5, -.01, 0., .01, .5, .9):
for torque in (-1., -.99, -.5, 0., .5, .99, 1.):
positive = assist(actual + error, actual, torque, speed)
negative = assist(-actual - error, -actual, -torque, speed)
assert -1. <= positive <= 1.
assert positive == -negative
openpilot/starpilot/lateral/tests/test_ioniq6_firmware_buffers.py
+70
View File
@@ -0,0 +1,70 @@
"""Firmware buffer representation must not change the selected Ioniq 6 controller."""
from types import SimpleNamespace
import pytest
from opendbc.car import structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode
from openpilot.starpilot.lateral.ioniq6_policy import is_ioniq_6_2025_model
from openpilot.starpilot.lateral.torque_extension import selected_policy
class FirmwareReader:
"""Expose a bytes-like firmware boundary while preserving the real factory CarParams."""
def __init__(self, cp, versions):
self.cp = cp
self.carFw = [SimpleNamespace(fwVersion=version) for version in versions]
def __getattr__(self, name):
return getattr(self.cp, name)
@pytest.mark.parametrize('buffer_type', [bytes, bytearray, memoryview])
@pytest.mark.parametrize('versions, expected', [([b'IONIQ6-230915', b'IONIQ6-240206'], True), ([b'IONIQ6-230915'], False), ([], False)])
def test_real_controller_firmware_selection_and_output_match_bytes(buffer_type, versions, expected):
cp = interfaces[CAR.HYUNDAI_IONIQ_6].get_non_essential_params(CAR.HYUNDAI_IONIQ_6)
native = cp.init('carFw', len(versions))
for fw, version in zip(native, versions, strict=True):
fw.fwVersion = version
ci = interfaces[CAR.HYUNDAI_IONIQ_6](cp)
wrapped = FirmwareReader(cp.as_reader(), [buffer_type(version) for version in versions])
assert is_ioniq_6_2025_model(wrapped) is expected
baseline = LatControlTorque(cp.as_reader(), ci, DT_CTRL, controller_mode=ControllerMode.STARPILOT)
actual = LatControlTorque(wrapped, ci, DT_CTRL, controller_mode=ControllerMode.STARPILOT)
actual_policy = selected_policy(actual)
baseline_policy = selected_policy(baseline)
assert actual_policy is not None and baseline_policy is not None
assert actual_policy.is_2025 is expected
assert baseline_policy.is_2025 is expected
assert actual.torque_params.to_dict() == baseline.torque_params.to_dict()
assert (actual.pid.pos_limit, actual.pid.neg_limit) == (baseline.pid.pos_limit, baseline.pid.neg_limit)
state = structs.CarState()
state.gearShifter = structs.CarState.GearShifter.drive
params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.0)
for speed, curvature, pressed in ((2.0, 0.0001, False), (12.0, -0.0003, False), (20.0, 0.0004, True)):
state.vEgo = speed
state.steeringPressed = pressed
args = (True, state, VehicleModel(cp), params, False, curvature, False, 0.1)
actual_output = actual.update(*args)
baseline_output = baseline.update(*args)
assert actual_output[0:2] == baseline_output[0:2]
assert actual_output[2].to_dict() == baseline_output[2].to_dict()
@pytest.mark.parametrize(
'cp',
[
SimpleNamespace(carFingerprint=CAR.HYUNDAI_IONIQ_6),
SimpleNamespace(carFingerprint=CAR.HYUNDAI_IONIQ_6, carFw=None),
SimpleNamespace(carFingerprint=CAR.HYUNDAI_IONIQ_5, carFw=[SimpleNamespace(fwVersion=b'230915 240206')]),
],
)
def test_missing_or_other_vehicle_firmware_keeps_standard_gate(cp):
assert not is_ioniq_6_2025_model(cp)
openpilot/starpilot/lateral/tests/test_ioniq6_policy.py
+159
View File
@@ -0,0 +1,159 @@
"""Frozen 678af783 Ioniq 6 traces and committed-parent non-Ioniq traces."""
import json
import math
from pathlib import Path
from types import SimpleNamespace
import unittest
from opendbc.car import structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.torque_extension import selected_policy
from openpilot.starpilot.lateral import ioniq6_policy as tune
from openpilot.starpilot.lateral.torque_shaping import INTERP_SPEEDS
TESTDATA = Path(__file__).parent / 'testdata'
def _rows(car, sequence, *, profile='2023', matched_frozen_cp=False, passive=False, direct_original_controller=False):
cp = interfaces[car].get_non_essential_params(car)
cp.passive = passive
if matched_frozen_cp:
for name, value in (('mass', 2084.0), ('wheelbase', 2.97), ('centerToFront', 1.188),
('steerRatio', 14.26), ('tireStiffnessFactor', 0.65)):
setattr(cp, name, value)
cp.lateralTuning.torque.latAccelFactor = 3.0
cp.lateralTuning.torque.latAccelOffset = 0.0
cp.lateralTuning.torque.friction = 0.09
cp.lateralTuning.torque.steeringAngleDeadzoneDeg = 0.0
if profile == '2025':
versions = cp.init('carFw', 2)
versions[0].fwVersion = b'IONIQ6-230915'
versions[1].fwVersion = b'IONIQ6-240206'
ci = interfaces[car](cp)
controller = LatControlTorque(cp.as_reader(), ci, DT_CTRL, turn_assist=True)
if direct_original_controller:
# The older frozen vectors called the controller directly, before the
# original controlsd drive loop replaced the PID gain curve each tick.
controller.pid._k_p = [INTERP_SPEEDS, tune.KP_INTERP]
vm = VehicleModel(cp)
cs = structs.CarState.new_message()
cs.gearShifter = structs.CarState.GearShifter.drive
params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.03 if matched_frozen_cp else 0.02)
rows = []
for active, speed, angle, curvature, pressed in sequence:
cs.vEgo = speed
cs.steeringAngleDeg = angle
cs.steeringPressed = pressed
output, _, state = controller.update(active, cs, vm, params, False, curvature, False, 0.1)
rows.append([output, state.desiredLateralAccel, state.desiredLateralJerk, state.p, state.i, state.f])
return controller, rows
class Ioniq6PolicyTests(unittest.TestCase):
def test_ioniq_drive_loop_uses_original_flat_proportional_gain(self):
cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)
cp.lateralTuning.torque.latAccelFactor = 3.0
cp.lateralTuning.torque.friction = 0.09
controller = LatControlTorque(cp.as_reader(), interfaces[HYUNDAI.HYUNDAI_IONIQ_6](cp), DT_CTRL)
self.assertEqual(controller.pid._k_p, [[0.0], [0.6]])
vm = VehicleModel(cp)
cs = structs.CarState.new_message()
params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.0)
sequence = ((False, 2.0, 0.0002, False),
(True, 2.0, 0.0002, False),
(True, 8.0, -0.0003, False),
(True, 8.0, -0.0003, True),
(True, 20.0, 0.0004, True),
(True, 20.0, 0.0004, False),
(False, 0.0, 0.0, False),
(True, 2.0, -0.0002, False))
for active, speed, curvature, pressed in sequence:
cs.vEgo = speed
cs.steeringAngleDeg = 0.0
cs.steeringPressed = pressed
output, _, state = controller.update(active, cs, vm, params, False, curvature, False, 0.1)
self.assertTrue(math.isfinite(output))
self.assertEqual(state.active, active)
if active:
self.assertAlmostEqual(state.p, 0.6 * state.error, places=6)
self.assertAlmostEqual(output, state.output, places=6)
else:
self.assertEqual(output, 0.0)
self.assertEqual(controller.pid._k_p, [[0.0], [0.6]])
controller.update_torque_parameters(3.2, 0.01, 0.10)
self.assertEqual(controller.pid._k_p, [[0.0], [0.6]])
def test_actual_frozen_controller_sequences_and_pure_calibration(self):
sequence = ([(False, 0.3, 10.0, -0.001, False)] * 2 +
[(True, 0.3, 10.0, -0.001, False)] * 4 +
[(True, 2.0, 5.0, -0.001, False)] * 4 +
[(True, 20.0, 2.0, 0.0003, False)] * 4 +
[(True, 20.0, 2.0, -0.0003, True)] * 2 +
[(False, 0.0, 8.0, -0.001, False)] * 2 +
[(True, 3.0, 8.0, -0.001, False)] * 4)
samples = ((0.3, .1, .2, 8., 3., -.2),
(12., -.5, -1.3, -6., -10., .2),
(27., .02, .8, 1., 0., -.1))
helpers = []
for speed, accel, jerk, angle, actual, torque in samples:
helpers.append([
tune.get_ioniq_6_ff_scale(accel, jerk, speed),
tune.get_ioniq_6_directional_taper_scale(accel, jerk, speed),
tune.get_ioniq_6_friction_threshold(speed, accel, jerk),
tune.get_ioniq_6_center_taper_scale(accel, speed),
tune.get_ioniq_6_low_speed_angle_assist_torque(angle, actual, torque, speed),
tune.get_ioniq_6_2025_center_output_scale(accel, speed),
])
for profile in ('2023', '2025'):
with self.subTest(profile=profile):
frozen = json.loads((TESTDATA / f'ioniq6_frozen_{profile}.json').read_text())
controller, actual = _rows(HYUNDAI.HYUNDAI_IONIQ_6, sequence, profile=profile,
matched_frozen_cp=True, direct_original_controller=True)
self.assertIsNotNone(selected_policy(controller))
self.assertEqual(selected_policy(controller).is_2025, profile == '2025')
self.assertAlmostEqual(controller.torque_params.latAccelFactor, frozen['factor'], places=6)
for row, expected in zip(actual, frozen['rows'], strict=True):
for value, reference in zip(row, expected, strict=True):
self.assertAlmostEqual(value, reference, places=6)
for row, expected in zip(helpers, frozen['helpers'], strict=True):
for value, reference in zip(row, expected, strict=True):
self.assertAlmostEqual(value, reference, places=6)
def test_firmware_gate_and_live_factor_do_not_stack(self):
cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)
self.assertFalse(tune.is_ioniq_6_2025_model(cp))
single = cp.init('carFw', 1)
single[0].fwVersion = b'IONIQ6-230915'
self.assertFalse(tune.is_ioniq_6_2025_model(cp))
both = cp.init('carFw', 2)
both[0].fwVersion = b'IONIQ6-230915'
both[1].fwVersion = b'IONIQ6-240206'
self.assertTrue(tune.is_ioniq_6_2025_model(cp))
controller = LatControlTorque(cp.as_reader(), interfaces[HYUNDAI.HYUNDAI_IONIQ_6](cp), DT_CTRL)
raw = cp.lateralTuning.torque.latAccelFactor
for _ in range(3):
controller.update_torque_parameters(raw, 0.02, 0.10)
self.assertAlmostEqual(controller.torque_params.latAccelFactor, raw * 1.22, places=6)
self.assertAlmostEqual(controller.torque_params.latAccelOffset, 0.02, places=6)
self.assertAlmostEqual(controller.torque_params.friction, 0.10, places=6)
def test_other_cars_match_committed_parent_trace(self):
sequence = ([(False, 2., 5., -.0005, False)] * 2 +
[(True, 2., 5., -.0005, False)] * 3 +
[(True, 15., -1., .0004, False)] * 3 +
[(True, 25., -2., .0001, True)] * 2 +
[(False, 0., 5., -.001, False)] * 2)
parent = json.loads((TESTDATA / 'non_ioniq_parent.json').read_text())
for car in (TOYOTA.TOYOTA_COROLLA_TSS2, HYUNDAI.HYUNDAI_IONIQ_5):
with self.subTest(car=str(car)):
controller, actual = _rows(car, sequence, passive=car == TOYOTA.TOYOTA_COROLLA_TSS2)
self.assertIsNone(selected_policy(controller))
for row, expected in zip(actual, parent[str(car)], strict=True):
for value, reference in zip(row, expected, strict=True):
self.assertAlmostEqual(value, reference, places=7)
openpilot/starpilot/lateral/tests/test_lane_centering.py
+322
View File
@@ -0,0 +1,322 @@
"""Native model-message fixtures for the standalone curvature contribution."""
from dataclasses import replace
import math
from types import SimpleNamespace
from typing import Any, cast
import unittest
from unittest.mock import patch
from openpilot.cereal import messaging
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from openpilot.starpilot.lateral.lane_centering import (ControlMode, LaneCenteringController, LaneCenteringInput,
LaneCenteringSettings, MAX_CORRECTION, raw_correction)
XS = [float(i) for i in range(51)]
def model(*, left=-1.5, right=2.1, path=0.0, path_std=0.1, prob=0.9, std=0.1):
message = messaging.new_message("modelV2")
m = message.modelV2
m.init("laneLines", 4)
for line, y in zip(m.laneLines, (0.0, left, right, 0.0), strict=True):
line.x = XS
line.y = [y] * len(XS)
m.laneLineProbs = [0.0, prob, prob, 0.0]
m.laneLineStds = [0.0, std, std, 0.0]
m.position.x = XS
m.position.y = [path] * len(XS)
m.position.yStd = [path_std] * len(XS)
return m
def observation(m=None, **updates):
source = LaneCenteringInput(m if m is not None else model(), 1, True, 0.0002, 20.0,
ControlMode.LATERAL_ONLY, True, LaneCenteringSettings(True, 0.0, 0.0, True), 0.01)
return replace(source, **updates)
def settled(controller, source, ticks=300):
result = None
for _ in range(ticks):
result = controller.update(source)
assert result is not None
return result
class TestLaneCentering(unittest.TestCase):
def test_reader_cache_detects_mutation_samples_and_reset(self):
from openpilot.starpilot.lateral import lane_centering as module
builder = model()
reader = builder.as_reader()
source = observation(reader)
controller = LaneCenteringController()
with patch.object(module, '_path_xy', wraps=module._path_xy) as evaluate:
first = controller.update(source)
second = controller.update(source)
self.assertGreater(second.correction, first.correction)
self.assertEqual(evaluate.call_count, 4)
builder.laneLines[2].y = [2.5] * len(XS)
controller.update(source)
self.assertEqual(evaluate.call_count, 8)
builder.laneLineProbs = [0., .1, .1, 0.]
self.assertEqual(controller.update(source).reason, 'unqualified_model')
self.assertEqual(evaluate.call_count, 12)
controller.update(replace(source, model_sample_id=0))
self.assertEqual(evaluate.call_count, 16)
controller.update(replace(source, model=reader.as_builder().as_reader()))
self.assertEqual(evaluate.call_count, 20)
controller.reset()
controller.update(source)
self.assertEqual(evaluate.call_count, 24)
controller.update(replace(source, time_discontinuity=True))
controller.update(source)
self.assertEqual(evaluate.call_count, 28)
def test_explicit_axis_authority(self):
for mode, allowed in ((ControlMode.OFF, False), (ControlMode.LATERAL_ONLY, True),
(ControlMode.LONGITUDINAL_ONLY, False), (ControlMode.COMBINED, True)):
with self.subTest(mode=mode):
result = LaneCenteringController().update(observation(mode=mode))
self.assertEqual(result.correction > 0.0, allowed)
assert result.candidate_curvature is not None
self.assertAlmostEqual(result.candidate_curvature, 0.0002 + result.correction)
def test_held_model_sample_advances_filter_as_in_frozen_controller(self):
controller = LaneCenteringController()
source = observation()
first = controller.update(source).correction
second = controller.update(source).correction
raw = raw_correction(source.model, 20.0, 0.0, 0.0)
assert raw is not None
expected_target = raw * 0.30 * 1.5
self.assertAlmostEqual(first, expected_target * (1.0 - math.exp(-0.01 / 0.4)))
self.assertAlmostEqual(second, expected_target * (1.0 - math.exp(-0.02 / 0.4)))
self.assertGreater(second, first)
def test_strength_increases_sublimit_correction_without_changing_default(self):
source = observation()
standard, explicit, stronger = LaneCenteringController(), LaneCenteringController(), LaneCenteringController()
for _ in range(100):
before = standard.update(source)
same = explicit.update(replace(source, settings=replace(source.settings, strength=1.5)))
boosted = stronger.update(replace(source, settings=replace(source.settings, strength=3.0)))
self.assertEqual(before, same)
self.assertAlmostEqual(boosted.correction, before.correction * 2, delta=1e-15)
self.assertLess(abs(boosted.correction), MAX_CORRECTION)
def test_strength_preserves_absolute_filter_and_axis_bounds(self):
for strength in (0.5, 1.5, 3.0):
controller = LaneCenteringController()
source = observation(model(left=-0.3, right=3.3), settings=LaneCenteringSettings(True, strength=strength))
previous = 0.0
for index in range(200):
source = replace(source, model=model(left=-0.3, right=3.3) if index < 100 else model(left=-3.3, right=0.3))
correction = controller.update(source).correction
self.assertLessEqual(abs(correction), MAX_CORRECTION)
self.assertLessEqual(abs(correction - previous), 2 * MAX_CORRECTION * (1 - math.exp(-0.01 / 0.4)) + 1e-15)
previous = correction
self.assertEqual(controller.update(replace(source, lateral_active=False)).correction, 0)
self.assertEqual(controller.update(replace(source, time_discontinuity=True)).correction, 0)
for invalid in (True, float("nan"), 0.49, 3.001):
self.assertEqual(LaneCenteringController().update(replace(observation(), settings=LaneCenteringSettings(True, strength=invalid))).reason,
"invalid_input")
def test_stronger_centering_remains_bounded_with_lane_uncertainty_and_sign_changes(self):
for speed in (5., 35.):
controller = LaneCenteringController()
previous = 0.
for index in range(120):
dt = (.01, .02, .05)[index % 3]
displacement = (.079, .081, .159, .161)[index % 4] * (1 if index < 60 else -1)
source = observation(model(left=-1.8 + displacement, right=1.8 + displacement, std=.29999998211860657),
speed_mps=speed, elapsed_seconds=dt,
settings=LaneCenteringSettings(True, e2e_authority=0., strength=3.0))
result = controller.update(source)
self.assertEqual(result.reason, "qualified")
self.assertLessEqual(abs(result.correction), MAX_CORRECTION)
self.assertLessEqual(abs(result.correction - previous),
2 * MAX_CORRECTION * (1 - math.exp(-dt / .4)) + 1e-15)
previous = result.correction
self.assertEqual(controller.update(replace(source, driver_override=True)).correction, 0.)
def test_strength_does_not_replace_model_path_authority(self):
shifted = model(path=0.5)
for authority in (0.0, 0.5, 1.0):
source = observation(shifted, settings=LaneCenteringSettings(True, e2e_authority=authority))
normal = settled(LaneCenteringController(), source).correction
stronger = settled(LaneCenteringController(), replace(source, settings=replace(source.settings, strength=3.0))).correction
self.assertAlmostEqual(stronger, normal * 2, delta=1e-15)
def test_native_builder_trace_matches_frozen_source(self):
# Frozen controller at 678af783; native modelV2, ten held 0.01 s ticks.
expected = (8.147727264677505e-06, 1.6094286426420937e-05, 2.3844644343388891e-05,
3.1403645241574984e-05, 3.8776013742606983e-05, 4.5966357816788907e-05,
5.2979171663232737e-05, 5.9818838518878981e-05, 6.6489633398162377e-05,
7.2995725765035352e-05)
controller = LaneCenteringController()
source = observation()
source = replace(source, settings=replace(source.settings, strength=1.0))
for reference in expected:
self.assertAlmostEqual(controller.update(source).correction, reference, delta=1e-15)
def test_filter_elapsed_time_matches_repeated_frozen_10ms_ticks(self):
source = observation()
ten_ms, twenty_ms = LaneCenteringController(), LaneCenteringController()
for _ in range(10):
result_ten = ten_ms.update(source)
for _ in range(5):
result_twenty = twenty_ms.update(replace(source, elapsed_seconds=0.02))
self.assertAlmostEqual(result_ten.correction, result_twenty.correction, delta=1e-15)
def test_candidate_passes_through_existing_curvature_limiter(self):
result = settled(LaneCenteringController(), observation())
self.assertIsNotNone(result.candidate_curvature)
clipped, _ = clip_curvature(20.0, 0.0, result.candidate_curvature, 0.0)
self.assertGreater(clipped, 0.0)
self.assertLessEqual(clipped, result.candidate_curvature)
def test_both_directions_offset_deadband_and_bounds(self):
right = settled(LaneCenteringController(), observation(model())).correction
left = settled(LaneCenteringController(), observation(model(left=-2.1, right=1.5))).correction
self.assertGreater(right, 0.0)
self.assertLess(left, 0.0)
self.assertEqual(settled(LaneCenteringController(), observation(model(left=-1.75, right=1.85))).correction, 0.0)
settings = LaneCenteringSettings(True, 0.3, 0.0)
narrow = observation(model(left=-1.3, right=1.3), settings=settings)
safe_limit = replace(narrow, settings=replace(settings, offset_m=0.2))
self.assertAlmostEqual(settled(LaneCenteringController(), narrow).correction,
settled(LaneCenteringController(), safe_limit).correction)
extreme = settled(LaneCenteringController(), observation(model(left=0.0, right=3.0)))
self.assertAlmostEqual(extreme.correction, MAX_CORRECTION, delta=1e-6)
def test_e2e_break_in_requires_confident_path(self):
shifted = model(left=-1.0, right=2.6)
no_e2e = settled(LaneCenteringController(), observation(shifted)).correction
full_e2e = settled(LaneCenteringController(), observation(
shifted, settings=LaneCenteringSettings(True, 0.0, 1.0))).correction
uncertain = settled(LaneCenteringController(), observation(
model(left=-1.0, right=2.6, path_std=0.6), settings=LaneCenteringSettings(True, 0.0, 1.0))).correction
self.assertGreater(no_e2e, 0.0)
self.assertAlmostEqual(full_e2e, 0.0)
self.assertAlmostEqual(uncertain, no_e2e)
def test_hard_gates_reset_and_reenable(self):
for change in ("disabled", "override", "lane_change", "low_speed", "invalid_model", "long_only"):
with self.subTest(change=change):
controller = LaneCenteringController()
source = observation()
before = settled(controller, source).correction
if change == "disabled":
blocked = replace(source, settings=replace(source.settings, enabled=False))
elif change == "override":
blocked = replace(source, driver_override=True)
elif change == "low_speed":
blocked = replace(source, speed_mps=4.9)
elif change == "invalid_model":
blocked = replace(source, model_valid=False)
elif change == "long_only":
blocked = replace(source, mode=ControlMode.LONGITUDINAL_ONLY)
else:
changed_model = model()
changed_model.meta.laneChangeState = "laneChangeStarting"
blocked = replace(source, model=changed_model)
self.assertGreater(before, 0.0)
self.assertEqual(controller.update(blocked).correction, 0.0)
reacquired = controller.update(source).correction
self.assertGreater(reacquired, 0.0)
self.assertLess(reacquired, before)
def test_signal_and_confidence_release_then_reacquire(self):
controller = LaneCenteringController()
source = observation()
before = settled(controller, source).correction
signal = controller.update(replace(source, turn_signal_active=True))
self.assertGreater(signal.correction, 0.0)
self.assertLess(signal.correction, before)
self.assertEqual(signal.reason, "signal_release")
no_pause = replace(source, settings=replace(source.settings, pause_on_signal=False), turn_signal_active=True)
self.assertGreater(controller.update(no_pause).correction, signal.correction)
fading = controller.update(replace(source, model=model(prob=0.2)))
self.assertGreater(fading.correction, 0.0)
self.assertLess(fading.correction, before)
self.assertEqual(fading.reason, "unqualified_model")
self.assertGreater(controller.update(source).correction, fading.correction)
def test_signal_pause_precedes_lane_change_as_in_frozen_source(self):
controller = LaneCenteringController()
source = observation()
before = settled(controller, source).correction
changed_model = model()
changed_model.meta.laneChangeState = "laneChangeStarting"
signaled = controller.update(replace(source, model=changed_model, turn_signal_active=True))
self.assertEqual(signaled.reason, "signal_release")
self.assertGreater(signaled.correction, 0.0)
self.assertLess(signaled.correction, before)
unsignaled = controller.update(replace(source, model=changed_model))
self.assertEqual(unsignaled.reason, "lane_change")
self.assertEqual(unsignaled.correction, 0.0)
def test_unqualified_native_models_have_no_new_correction(self):
for bad_model in (model(prob=0.2), model(std=0.4), model(left=-0.5, right=0.5),
model(path=float("nan"))):
with self.subTest(model=bad_model):
self.assertEqual(LaneCenteringController().update(observation(bad_model)).correction, 0.0)
def test_missing_lookahead_and_nonmonotonic_path_rejected(self):
short = model()
short.laneLines[1].x = XS[:10]
short.laneLines[1].y = [-1.5] * 10
self.assertIsNone(raw_correction(short, 20.0, 0.0, 0.0))
reversed_path = model()
reversed_path.position.x = list(reversed(XS))
self.assertIsNone(raw_correction(reversed_path, 20.0, 0.0, 0.0))
def test_malformed_inputs_rejected_without_unsafe_candidate(self):
for updates in ({"base_curvature": float("nan")}, {"speed_mps": float("inf")},
{"elapsed_seconds": float("nan")}, {"elapsed_seconds": -0.01},
{"base_curvature": True}, {"base_curvature": "0.0002"},
{"speed_mps": False}, {"speed_mps": "20"},
{"elapsed_seconds": True}, {"elapsed_seconds": "0.01"},
{"settings": LaneCenteringSettings(True, float("nan"))},
{"settings": LaneCenteringSettings(True, True)},
{"settings": LaneCenteringSettings(True, cast(Any, "0.2"))},
{"settings": LaneCenteringSettings(True, 0.0, cast(Any, "1.0"))},
{"time_discontinuity": "false"}):
with self.subTest(updates=updates):
controller = LaneCenteringController()
settled(controller, observation())
result = controller.update(observation(**updates))
self.assertEqual(result.correction, 0.0)
self.assertEqual(result.reason, "invalid_input")
if "base_curvature" in updates:
self.assertIsNone(result.candidate_curvature)
def test_invalid_observation_type_is_inert(self):
controller = LaneCenteringController()
settled(controller, observation())
result = controller.update(cast(Any, object()))
self.assertEqual(result.reason, "invalid_input")
self.assertIsNone(result.candidate_curvature)
self.assertEqual(result.correction, 0.0)
def test_numeric_strings_in_model_path_rejected(self):
native_model = model()
wrong_path = SimpleNamespace(x=[str(x) for x in XS], y=native_model.position.y,
yStd=native_model.position.yStd)
changed_model = SimpleNamespace(laneLines=native_model.laneLines, laneLineProbs=native_model.laneLineProbs,
laneLineStds=native_model.laneLineStds, position=wrong_path)
self.assertIsNone(raw_correction(changed_model, 20.0, 0.0, 0.0))
def test_time_discontinuity_explicitly_resets(self):
controller = LaneCenteringController()
settled(controller, observation())
result = controller.update(observation(elapsed_seconds=0.2, time_discontinuity=True))
self.assertEqual(result.reason, "time_discontinuity")
self.assertEqual(result.correction, 0.0)
def test_elapsed_time_does_not_infer_a_discontinuity(self):
controller = LaneCenteringController()
result = controller.update(observation(elapsed_seconds=0.2))
self.assertEqual(result.reason, "qualified")
self.assertGreater(result.correction, 0.0)
openpilot/starpilot/lateral/tests/test_lane_change_defaults.py
+22
View File
@@ -0,0 +1,22 @@
import json
from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy, decode, to_value, read_saved
def test_unset_defaults_on_without_writing(tmp_path):
class Source:
def get_param_path(self, key):
return str(tmp_path / key)
source = Source()
for _ in range(3):
assert read_saved(source).policy.one_per_signal
assert not list(tmp_path.iterdir())
def test_explicit_off_survives_document_roundtrip():
policy = LaneChangePolicy(one_per_signal=False)
raw = json.dumps(to_value(policy)).encode()
for _ in range(3):
saved = decode(raw)
assert saved is not None and not saved.one_per_signal
assert json.dumps(to_value(saved)).encode() == raw
openpilot/starpilot/lateral/tests/test_lane_change_smoothing.py
+78
View File
@@ -0,0 +1,78 @@
import math
from dataclasses import replace
from openpilot.cereal import log
from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy, decode, to_value
from openpilot.starpilot.lateral.lane_change_smoothing import LaneChangeSmoother, limit_rate
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
import json
def test_duration_document_roundtrip_and_previous_versions():
for seconds in (3., 5., 8.):
policy = replace(LaneChangePolicy(), duration_s=seconds)
assert decode(json.dumps(to_value(policy)).encode()) == policy
old = to_value(LaneChangePolicy())
old['version'] = 3
old.pop('durationS')
assert decode(json.dumps(old).encode()).duration_s == LaneChangePolicy().duration_s
for value in (2.9, 8.1, True, float('nan')):
current = to_value(LaneChangePolicy()) | {'durationS': value}
assert decode(json.dumps(current).encode()) is None
def test_rate_envelope_and_release_preserve_native_acceleration_limits():
for speed in (1., 10., 30.):
smoother = LaneChangeSmoother()
previous = 0.
factors = []
for tick in range(600):
desired = .005 if tick < 200 else -.005
factor = smoother.factor(active=True, lane_change_state=log.LaneChangeState.laneChangeStarting if tick < 350 else
log.LaneChangeState.off, speed=speed, minimum_speed=0., duration=6.,
previous=previous, desired=desired)
limited = limit_rate(speed, previous, desired, factor)
output, _ = clip_curvature(speed, previous, limited, 0.)
assert 0. < factor <= 1.
assert abs(output) <= .2
assert abs(output - previous) <= 5. / max(speed, 1.) ** 2 * .01 + 1e-12
previous = output
factors.append(factor)
assert factors[0] == math.pi ** 3 * 3.5 / 6. ** 3 * 1.3 / 5.
assert factors[-1] == 1.
assert max(factors[200:350]) > factors[0] # Arrest can recover quicker than turn-in.
def test_stock_duration_inactive_and_regular_driving_remain_exact():
for active, duration, state in ((True, 3., log.LaneChangeState.laneChangeStarting),
(False, 6., log.LaneChangeState.laneChangeStarting),
(True, 6., log.LaneChangeState.off)):
smoother = LaneChangeSmoother()
factor = smoother.factor(active=active, lane_change_state=state, speed=20., minimum_speed=0., duration=duration,
previous=0., desired=.01)
assert factor == 1.
assert limit_rate(20., 0., .01, factor) == .01
def test_intersection_exit_keeps_native_rate_after_crossing_lane_change_floor():
smoother = LaneChangeSmoother()
def factor(speed, state, assist=True, active=True):
return smoother.factor(active=active, lane_change_state=state, speed=speed, minimum_speed=0., duration=7.,
previous=-.005, desired=0., turn_assist=assist)
starting = log.LaneChangeState.laneChangeStarting
assert factor(8., starting) == 1.
assert factor(10., starting) == 1.
assert factor(12., log.LaneChangeState.off) == 1.
assert factor(25., starting) < 1.
def test_intersection_exemption_does_not_survive_disabled_turn_assist_or_lateral():
for disable in ({"assist": False}, {"active": False}):
smoother = LaneChangeSmoother()
def factor(speed, smoother=smoother, **kwargs):
return smoother.factor(active=kwargs.get("active", True), lane_change_state=log.LaneChangeState.laneChangeStarting,
speed=speed, minimum_speed=0., duration=7., previous=-.005, desired=0.,
turn_assist=kwargs.get("assist", True))
assert factor(8.) == 1.
factor(10., **disable)
assert factor(25.) < 1.
openpilot/starpilot/lateral/tests/test_lane_feedback.py
+83
View File
@@ -0,0 +1,83 @@
from types import SimpleNamespace
from unittest.mock import patch
import unittest
import numpy as np
import pyray as rl
from openpilot.cereal import messaging
from openpilot.starpilot.lateral.lane_centering import LaneCenteringResult
from openpilot.starpilot.lateral.lane_feedback import BLUE, MAX_AGE_NS, SERVICE, direction, feedback_message
class TestLaneFeedback(unittest.TestCase):
def reader(self, *, applied=.0003, requested=.0005):
now = 1_000_000_000
event = feedback_message(LaneCenteringResult(.001, requested, 1, 'qualified'), applied, True,
model_mono_time=now, car_control_mono_time=now, valid=True)
event.logMonoTime = now
control = messaging.new_message('carControl', valid=True, logMonoTime=now)
control.carControl.latActive = True
model = messaging.new_message('modelV2', valid=True, logMonoTime=now)
sm = messaging.SubMaster([SERVICE, 'carControl', 'modelV2'], frequency=20)
sm.update_msgs(now / 1e9 - .01, [])
sm.update_msgs(now / 1e9, [event.as_reader(), control.as_reader(), model.as_reader()])
return sm, now
def test_real_messages_show_applied_sign_and_keep_legacy_fields_inactive(self):
for correction, expected in ((.0003, 1), (-.0003, -1), (0., 0), (.0000005, 0)):
sm, now = self.reader(applied=correction, requested=.0005 if correction >= 0 else -.0005)
self.assertEqual(direction(sm, now, 0), expected)
self.assertFalse(sm[SERVICE].active)
self.assertEqual(sm[SERVICE].frictionScale, 0.)
def test_stale_invalid_old_drive_disabled_and_inconsistent_inputs_clear_blue(self):
for service in (SERVICE, 'carControl', 'modelV2'):
for flag in ('alive', 'valid'):
sm, now = self.reader()
getattr(sm, flag)[service] = False
self.assertEqual(direction(sm, now, 0), 0)
sm, now = self.reader()
self.assertEqual(direction(sm, now + MAX_AGE_NS + 1, 0), 0)
self.assertEqual(direction(sm, now - 1, 0), 0)
self.assertEqual(direction(sm, now, sm.frame), 0)
for applied, requested in ((.0013, .0013), (.0006, .0005), (-.0003, .0005), (float('nan'), .0005)):
sm, now = self.reader(applied=applied, requested=requested)
self.assertEqual(direction(sm, now, 0), 0)
for field, value in (('version', 0), ('lateralActive', False), ('modelMonoTime', 1), ('carControlMonoTime', 1)):
sm, now = self.reader()
event = messaging.new_message(SERVICE, valid=True, logMonoTime=now)
event.starpilotLateralState = sm[SERVICE]
setattr(event.starpilotLateralState.laneCentering, field, value)
sm.update_msgs(now / 1e9 + .01, [event.as_reader()])
self.assertEqual(direction(sm, now, 0), 0)
sm, now = self.reader()
control = messaging.new_message('carControl', valid=True, logMonoTime=now)
control.carControl.latActive = False
sm.update_msgs(now / 1e9 + .01, [control.as_reader()])
self.assertEqual(direction(sm, now, 0), 0)
def test_model_renderer_blue_retains_alpha_and_overrides_torque_color(self):
from openpilot.selfdrive.ui.onroad import model_renderer as large
from openpilot.selfdrive.ui.mici.onroad import model_renderer as compact
from openpilot.selfdrive.ui.ui_state import UIStatus
for module in (large, compact):
for sign in (-1, 0, 1):
renderer = module.ModelRenderer.__new__(module.ModelRenderer)
renderer._rect = rl.Rectangle(0, 0, 536, 240)
renderer._lane_lines = [SimpleNamespace(projected_points=np.array([[0., 0.], [5., 0.], [0., 5.]])) for _ in range(4)]
renderer._road_edges = []
renderer._lane_line_probs = [.3, .6, .7, .4]
if module is compact:
renderer._visual_status = lambda: UIStatus.ENGAGED
renderer._torque_filter = SimpleNamespace(x=.9)
with patch.object(module, 'lane_centering_direction', return_value=sign), patch.object(module, 'draw_polygon') as draw:
renderer._draw_lane_lines()
colors = [(c.r, c.g, c.b, c.a) for call in draw.call_args_list for c in [call.args[2]]]
self.assertEqual([i for i, color in enumerate(colors) if color[:3] == BLUE], [1 if sign < 0 else 2] if sign else [])
if sign:
index = 1 if sign < 0 else 2
self.assertEqual(colors[index][3], int(renderer._lane_line_probs[index] * 255))
for index in (0, 3):
self.assertEqual(colors[index][3], int(renderer._lane_line_probs[index] * 255))
openpilot/starpilot/lateral/tests/test_lane_runtime.py
+408
View File
@@ -0,0 +1,408 @@
"""Saved preferences reach actual Controls only with fresh lateral authority."""
import os
import tempfile
from pathlib import Path
from types import SimpleNamespace
import unittest
from unittest import mock
from openpilot.cereal import messaging
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.controls.controlsd import Controls
from openpilot.starpilot.lateral.lane_centering import ControlMode
from openpilot.starpilot.lateral.lane_runtime import LaneCenteringHost, read_settings, runtime_supported
from openpilot.starpilot.lateral.tests.test_lane_centering import model
from openpilot.starpilot.aol.wire import SafetyState, encode_safety
from opendbc.car import gen_empty_fingerprint
from opendbc.car.car_helpers import interfaces
from opendbc.car.honda.interface import CarInterface as HondaCarInterface
from opendbc.car.honda.values import CAR as HondaCAR
from opendbc.car.hyundai.interface import CarInterface as HyundaiCarInterface
from opendbc.car.hyundai.ioniq6_handoff import build_ioniq6_hda2_long_candidate
from opendbc.car.hyundai.values import CAR as HyundaiCAR
from opendbc.car.structs import car
from opendbc.car.toyota.values import CAR
def ioniq_candidate(*, alternate=False):
fingerprint = gen_empty_fingerprint()
fingerprint[2].update({0x110: 32, 0x362: 32} if alternate else {0x50: 16, 0x2A4: 24})
fingerprint[1].update({0x1CF: 8, 0x35: 32, 0x175: 24, 0xA0: 24, 0xEA: 24,
0x1BA: 24, 0x1E5: 16, 0x36A: 16})
fingerprint[0][0x3A5] = 24
fingerprint[0][0x100] = 24
stock = HyundaiCarInterface.get_params(HyundaiCAR.HYUNDAI_IONIQ_6, fingerprint, [], False, False, False)
return stock, build_ioniq6_hda2_long_candidate(stock, fingerprint)
class Frame:
def __init__(self, now_ns):
self.logMonoTime = dict.fromkeys(LaneCenteringHost.REQUIRED, now_ns)
self.checked = True
self.state = SimpleNamespace(canValid=True, canTimeout=False)
def all_checks(self, _services):
return self.checked
def __getitem__(self, _service):
return self.state
def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, override=False, signal=False, model_age=0,
can_valid=True, can_timeout=False, aol_lat_only=False, native_session='lane-aol-session'):
events = []
def msg(service, size=None):
event = messaging.new_message(service, size, valid=True, logMonoTime=timestamp)
events.append(event)
return getattr(event, service)
cs = msg('carState')
cs.vEgo = cs.vEgoRaw = 20.0
cs.vCruise = cs.vCruiseCluster = 100.0
cs.canValid = can_valid
cs.canTimeout = can_timeout
cs.steerFaultTemporary = fault
cs.steeringPressed = override
cs.leftBlinker = signal
state = msg('selfdriveState')
state.active, state.enabled = active, enabled
state.state = 'enabled' if enabled else 'disabled'
if tick % 5 == 0:
lp = msg('vehicleParameters')
lp.stiffnessFactor, lp.steerRatio = 1.0, controls.CP.steerRatio
msg('longitudinalPlan').aTarget = 0.5
msg('lateralDelay').lateralDelay = 0.1
msg('onroadEvents', 0)
if tick % 5 == 0:
event = messaging.new_message('modelV2', valid=True, logMonoTime=timestamp - model_age)
event.modelV2 = model()
event.modelV2.action.desiredCurvature = 0.0002
events.append(event)
if aol_lat_only:
axis = msg('aolAxisState')
axis.qualified = axis.nativeAcknowledged = axis.desiredLateral = axis.lateralActive = True
axis.desiredLongitudinal = axis.longitudinalActive = False
axis.sessionId = 'lane-aol-session'
axis.observedMonoTime = timestamp
axis.validUntilMonoTime = timestamp + 30_000_000
msg('aolSafetyWire', 0)
events[-1].aolSafetyWire = encode_safety(SafetyState(
1, True, timestamp, timestamp + 200_000_000, int(car.CarParams.SafetyModel.hondaBosch),
controls.CP.safetyConfigs[-1].safetyParam, True, False, True, False,
'lane-test-panda', native_session))
controls.sm.update_msgs(timestamp / 1e9, [event.as_reader() for event in events])
class LaneRuntimeTests(unittest.TestCase):
def test_strength_saved_values_and_live_refresh_keep_existing_admission(self):
with tempfile.TemporaryDirectory() as temporary:
root = Path(temporary)
params = SimpleNamespace(get_param_path=lambda key: str(root / key))
(root / 'LaneCentering').write_bytes(b'1')
self.assertEqual(read_settings(params).strength, 1.0)
host = LaneCenteringHost(params)
now = 10_000_000_000
first = host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False)
self.assertEqual(first.settings.strength, 1.0)
(root / 'LaneCenteringStrength').write_bytes(b'1.5')
now += host.REFRESH_NS
updated = host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False)
self.assertEqual(updated.settings.strength, 1.5)
self.assertTrue(updated.time_discontinuity)
for invalid in (b'nan', b'0.49', b'1.51', b'true'):
(root / 'LaneCenteringStrength').write_bytes(invalid)
self.assertFalse(read_settings(params).enabled)
now += host.REFRESH_NS
self.assertIsNone(host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False))
def test_normal_reader_admission_keeps_exact_vehicle_and_saved_preference_gates(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0'}):
params = Params()
for alternate in (False, True):
with self.subTest(alternate=alternate):
stock, tagged = ioniq_candidate(alternate=alternate)
self.assertFalse(runtime_supported(stock))
self.assertTrue(runtime_supported(tagged))
self.assertFalse(read_settings(params).enabled)
params.put_bool('LaneCentering', True, block=True)
self.assertTrue(read_settings(params).enabled)
self.assertFalse(runtime_supported(interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2)))
tagged.safetyConfigs[0].safetyParam = 0
self.assertFalse(runtime_supported(tagged))
params.put_bool('LaneCentering', False, block=True)
_, tagged = ioniq_candidate()
params.put_bool('LaneCentering', True, block=True)
Path(params.get_param_path('LaneCenterOffset')).write_bytes(b'nan')
self.assertFalse(read_settings(params).enabled)
with mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '1'}):
self.assertTrue(runtime_supported(tagged))
def test_actual_tagged_ioniq_saved_host_uses_fresh_lateral_authority(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1',
'AOL_REPLAY_RUNTIME': '0'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params = Params()
stock, tagged = ioniq_candidate()
params.put('LaneCenteringE2EAuthority', 0.0, block=True)
params.put('CarParams', tagged.to_bytes(), block=True)
disabled = Controls()
self.assertIsNotNone(disabled.lane_centering_host)
frame = Frame(1_000_000_000)
self.assertIsNone(disabled.lane_centering_host.sample(frame, now_ns=1_000_000_000,
lateral_active=True, longitudinal_active=False))
params.put_bool('LaneCentering', True, block=True)
controls = disabled
self.assertIsNotNone(controls.lane_centering_host)
for tick in range(35):
now = 2_000_000_000 + tick * 10_000_000
feed(controls, now, tick)
command, _ = controls.state_control()
self.assertTrue(command.latActive)
self.assertIsNotNone(controls.last_lane_centering_result)
self.assertGreater(controls.last_lane_centering_result.correction, 0.0)
feed(controls, now + 10_000_000, 35, model_age=200_000_000)
controls.state_control()
self.assertIsNone(controls.last_lane_centering_result)
params.put_bool('LaneCentering', False, block=True)
feed(controls, 3_000_000_000, 36)
controls.state_control()
self.assertIsNone(controls.last_lane_centering_result)
self.assertEqual(controls.lane_centering_applied, 0.0)
params.put('CarParams', stock.to_bytes(), block=True)
self.assertIsNone(Controls().lane_centering_host)
def test_tagged_ioniq_four_axes_and_stale_reset(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1',
'AOL_REPLAY_RUNTIME': '0'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params = Params()
_, tagged = ioniq_candidate()
params.put('CarParams', tagged.to_bytes(), block=True)
params.put_bool('LaneCentering', True, block=True)
params.put('LaneCenteringE2EAuthority', 0.0, block=True)
controls = Controls()
self.assertIsNotNone(controls.lane_centering_host)
# Inject already-qualified independent-axis results at the consumer seam;
# AOL's native/session checks are covered by its own wire tests.
controls.aol_replay = True
tick = 0
for lat, lon in ((True, True), (True, False), (False, True), (False, False)):
with self.subTest(lat=lat, lon=lon), \
mock.patch('openpilot.selfdrive.controls.controlsd.current_axis', return_value=SimpleNamespace(
sessionId='test-session', nativeAcknowledged=True, lateralActive=lat, desiredLateral=lat,
longitudinalActive=lon, desiredLongitudinal=lon)), \
mock.patch('openpilot.selfdrive.controls.controlsd.current_native', return_value=SimpleNamespace(
requestedLateral=lat, lateralAllowed=lat, requestedLongitudinal=lon, longitudinalAllowed=lon)):
for _ in range(10):
now = 1_000_000_000 + tick * 10_000_000
feed(controls, now, tick, active=lat, enabled=lon)
command, _ = controls.state_control()
tick += 1
self.assertEqual((command.latActive, command.longActive), (lat, lon))
self.assertIsNotNone(controls.last_lane_centering_result)
self.assertEqual(controls.last_lane_centering_result.correction > 0.0, lat)
with mock.patch('openpilot.selfdrive.controls.controlsd.current_axis', return_value=SimpleNamespace(
sessionId='test-session', nativeAcknowledged=True, lateralActive=True, desiredLateral=True,
longitudinalActive=False, desiredLongitudinal=False)), \
mock.patch('openpilot.selfdrive.controls.controlsd.current_native', return_value=SimpleNamespace(
requestedLateral=True, lateralAllowed=True, requestedLongitudinal=False, longitudinalAllowed=False)):
now = 1_000_000_000 + tick * 10_000_000
feed(controls, now, tick, active=False, enabled=False, model_age=200_000_000)
command, _ = controls.state_control()
self.assertTrue(command.latActive)
self.assertFalse(command.longActive)
self.assertIsNone(controls.last_lane_centering_result)
for recovery_tick in range(tick + 1, tick + 6):
now = 1_000_000_000 + recovery_tick * 10_000_000
feed(controls, now, recovery_tick, active=False, enabled=False)
controls.state_control()
self.assertGreater(controls.last_lane_centering_result.correction, 0.0)
def test_saved_settings_bounds_and_corruption(self):
with OpenpilotPrefix():
params = Params()
self.assertFalse(read_settings(params).enabled)
self.assertTrue(read_settings(params).pause_on_signal)
params.put_bool('LaneCentering', True, block=True)
params.put('LaneCenterOffset', -0.2, block=True)
params.put('LaneCenteringE2EAuthority', 0.5, block=True)
settings = read_settings(params)
self.assertEqual((settings.enabled, settings.offset_m, settings.e2e_authority), (True, -0.2, 0.5))
for key, values in (('LaneCenterOffset', (b'bad', b'nan', b'inf', b'0.31')),
('LaneCenteringE2EAuthority', (b'', b'1.01', b'-0.1')),
('LaneCenteringPauseOnSignal', (b'false', b'2'))):
path = Path(params.get_param_path(key))
original = path.read_bytes() if path.exists() else None
for value in values:
with self.subTest(key=key, value=value):
path.write_bytes(value)
self.assertFalse(read_settings(params).enabled)
self.assertTrue(params.get_bool('LaneCentering'))
if original is None:
path.unlink()
else:
path.write_bytes(original)
def test_authority_is_derived_from_actual_axes(self):
with OpenpilotPrefix():
params = Params()
params.put_bool('LaneCentering', True, block=True)
host = LaneCenteringHost(params)
for tick, (lat, lon, mode) in enumerate(((False, False, ControlMode.OFF), (False, True, ControlMode.LONGITUDINAL_ONLY),
(True, False, ControlMode.LATERAL_ONLY), (True, True, ControlMode.COMBINED))):
now = 1_000_000_000 + tick * 10_000_000
request = host.sample(Frame(now), now_ns=now, lateral_active=lat, longitudinal_active=lon)
self.assertIsNotNone(request)
self.assertEqual(request.mode, mode)
def test_stale_future_invalid_input_and_clock_gap(self):
with OpenpilotPrefix():
params = Params()
params.put_bool('LaneCentering', True, block=True)
host = LaneCenteringHost(params)
now = 1_000_000_000
def sample(frame, at=now):
return host.sample(frame, now_ns=at, lateral_active=True, longitudinal_active=True)
self.assertIsNotNone(sample(Frame(now)))
for service in host.REQUIRED:
for age in (200_000_000, -1):
frame = Frame(now)
frame.logMonoTime[service] = now - age
self.assertIsNone(sample(frame))
frame = Frame(now)
frame.checked = False
self.assertIsNone(sample(frame))
frame.checked, frame.state.canValid = True, False
self.assertIsNone(sample(frame))
frame.state.canValid, frame.state.canTimeout = True, True
self.assertIsNone(sample(frame))
frame.state.canTimeout = False
frame.logMonoTime['modelV2'] = 0
self.assertIsNone(sample(frame))
self.assertIsNone(sample(Frame(0), 0))
gap = sample(Frame(now + 100_000_000), now + 100_000_000)
self.assertTrue(gap.time_discontinuity)
self.assertFalse(sample(Frame(now + 110_000_000), now + 110_000_000).time_discontinuity)
self.assertTrue(sample(Frame(now), now).time_discontinuity)
def test_live_settings_refresh_and_corrupt_setting_disables(self):
with OpenpilotPrefix():
params = Params()
params.put_bool('LaneCentering', True, block=True)
host = LaneCenteringHost(params)
for now in (1_000_000_000, 1_010_000_000):
self.assertIsNotNone(host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False))
Path(params.get_param_path('LaneCenterOffset')).write_bytes(b'bad')
now = 2_000_000_000
self.assertIsNone(host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False))
self.assertTrue(params.get_bool('LaneCentering'))
def test_actual_controls_saved_settings_and_permission_gates(self):
scenarios = ('normal', 'disabled_setting', 'off', 'long_only', 'fault', 'override', 'signal', 'stale', 'invalid_can', 'can_timeout')
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'AOL_REPLAY_RUNTIME': '0', 'REPLAY': '1'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params = Params()
cp = interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2)
params.put('CarParams', cp.to_bytes(), block=True)
for scenario in scenarios:
with self.subTest(scenario=scenario):
params.put_bool('LaneCentering', scenario != 'disabled_setting', block=True)
params.put('LaneCenteringE2EAuthority', 0.0, block=True)
with mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0'}):
baseline = Controls()
with mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '1'}):
candidate = Controls()
self.assertIsNone(baseline.lane_centering_host)
previous = 0.0
for tick in range(120):
now = 1_000_000_000 + tick * 10_000_000
phase = scenario if tick >= 80 else 'normal'
options = {'active': phase not in ('off', 'long_only'), 'enabled': phase != 'off',
'fault': phase == 'fault', 'override': phase == 'override', 'signal': phase == 'signal',
'model_age': 200_000_000 if phase == 'stale' else tick % 5 * 10_000_000,
'can_valid': phase != 'invalid_can', 'can_timeout': phase == 'can_timeout'}
for controls in (baseline, candidate):
feed(controls, now, tick, **options)
reference, _ = baseline.state_control()
command, _ = candidate.state_control()
result = candidate.last_lane_centering_result
self.assertEqual(command.latActive, reference.latActive)
self.assertEqual(command.longActive, reference.longActive)
self.assertEqual(command.actuators.accel, reference.actuators.accel)
if scenario == 'disabled_setting':
self.assertEqual(command.to_dict(), reference.to_dict())
elif phase in ('off', 'long_only', 'fault', 'override'):
self.assertEqual(result.correction, 0.0)
elif phase in ('stale', 'invalid_can', 'can_timeout'):
self.assertIsNone(result)
elif phase == 'signal':
self.assertLess(result.correction, previous)
elif tick >= 20:
self.assertGreater(result.correction, previous)
previous = result.correction if result is not None else 0.0
def test_actual_aol_lateral_only_controls_without_longitudinal(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'AOL_REPLAY_RUNTIME': '1', 'LANE_CENTERING_REPLAY_RUNTIME': '1',
'REPLAY': '1'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params = Params()
params.put_bool('LaneCentering', True, block=True)
cp = HondaCarInterface.get_params(HondaCAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], True, False, False)
cp.safetyConfigs[-1].safetyParam |= 0x20 # Card's qualified AOL bit in serialized CarParams.
params.put('CarParams', cp.to_bytes(), block=True)
controls = Controls()
self.assertTrue(controls.aol_replay)
self.assertIsNotNone(controls.lane_centering_host)
for tick in range(40):
now = 1_000_000_000 + tick * 10_000_000
feed(controls, now, tick, active=False, enabled=False, aol_lat_only=True)
command, _ = controls.state_control()
self.assertFalse(command.enabled)
self.assertTrue(command.latActive)
self.assertFalse(command.longActive)
self.assertEqual(command.actuators.accel, 0.0)
assert controls.last_lane_centering_result is not None
self.assertGreater(controls.last_lane_centering_result.correction, 0.0)
previous = controls.last_lane_centering_result.correction
now += 10_000_000
feed(controls, now, 40, active=False, enabled=False, aol_lat_only=True, native_session='old-drive')
command, _ = controls.state_control()
self.assertFalse(command.latActive or command.longActive)
self.assertEqual(controls.last_lane_centering_result.correction, 0.0)
now += 10_000_000
feed(controls, now, 41, active=False, enabled=False, aol_lat_only=True)
command, _ = controls.state_control()
self.assertTrue(command.latActive)
self.assertFalse(command.longActive)
self.assertLess(controls.last_lane_centering_result.correction, previous)
for tick in range(42, 48):
now += 10_000_000
feed(controls, now, tick, active=False, enabled=False)
command, _ = controls.state_control()
self.assertFalse(command.latActive or command.longActive)
self.assertEqual(controls.last_lane_centering_result.correction, 0.0)
def test_stock_acc_lateral_stays_separate_from_aol(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '1',
'REPLAY': '1'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params = Params()
params.put_bool('LaneCentering', True, block=True)
cp = interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2)
cp.openpilotLongitudinalControl = False
cp.pcmCruise = True
params.put('CarParams', cp.to_bytes(), block=True)
controls = Controls()
self.assertFalse(controls.aol_replay)
for tick in range(40):
now = 1_000_000_000 + tick * 10_000_000
feed(controls, now, tick)
command, _ = controls.state_control()
self.assertTrue(command.latActive)
self.assertFalse(command.longActive)
assert controls.last_lane_centering_result is not None
self.assertGreater(controls.last_lane_centering_result.correction, 0.0)
openpilot/starpilot/lateral/tests/test_palisade_policy.py
+87
View File
@@ -0,0 +1,87 @@
import math
from types import SimpleNamespace
from unittest.mock import patch
import pytest
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.tests.test_palisade_2023 import params as topology_params
from opendbc.car.hyundai.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.cereal import log
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, replace_mode, selection_from_bytes
from openpilot.starpilot.lateral.palisade_policy import PalisadeTorquePolicy, supported_cp
from openpilot.starpilot.lateral import palisade_shaping as shaping
from openpilot.starpilot.lateral.torque_shaping import INTERP_SPEEDS, KP_INTERP, KI
@pytest.mark.parametrize('speed,accel,jerk,ff,threshold,friction,center,output', [
(2., .18, .3, 1.1469630171215324, .16001729374366647, 1.034684825233675, .9981430724236608, .9979939519844233),
(8.9408, -.25, -.45, 1.131157979745277, .18376493402775368, 1.0340640585840144, .982732898487096, .9866506253915932),
(22., .8, -.6, 1.1001436880961013, .2366944877586695, 1.0045028671743204, .9999907698683685, .9999871475466129),
(33.528, -1.4, .8, 1.042046705933432, .2738465058489194, 1.0070488018554904, .9999999998280955, .9999999997426313),
])
def test_frozen_phase_and_side_formula_vectors(speed, accel, jerk, ff, threshold, friction, center, output):
actual = (shaping.get_palisade_ff_scale(accel, jerk, speed), shaping.get_palisade_friction_threshold(speed, accel, jerk),
shaping.get_palisade_friction_scale(speed, accel, jerk), shaping.get_palisade_center_taper_scale(accel, speed),
shaping.get_palisade_center_output_scale(accel, speed))
assert actual == pytest.approx((ff, threshold, friction, center, output), abs=1e-14, rel=0)
@pytest.mark.parametrize('vehicle', [CAR.HYUNDAI_PALISADE, CAR.HYUNDAI_PALISADE_2023])
def test_default_choice_startup_and_repeated_live_factor(vehicle):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
ci = interfaces[vehicle](cp)
controller = LatControlTorque(cp.as_reader(), ci, .01)
assert default_selection(cp).policy == 'palisade'
assert isinstance(controller.starpilot_extension.policy, PalisadeTorquePolicy)
assert controller.torque_params.latAccelFactor == pytest.approx(cp.lateralTuning.torque.latAccelFactor * .98)
assert controller.pid._k_p == [INTERP_SPEEDS, KP_INTERP]
assert controller.pid._k_i == ([0.], [KI])
raw_limit = controller.lateral_accel_from_torque(controller.steer_max, cp.lateralTuning.torque)
assert controller.pid.pos_limit == pytest.approx(raw_limit)
for _ in range(3):
controller.update_torque_parameters(3., .02, .12)
assert controller.torque_params.latAccelFactor == pytest.approx(2.94)
assert controller.torque_params.latAccelOffset == pytest.approx(.02)
assert controller.torque_params.friction == pytest.approx(.12)
assert controller.pid.pos_limit == pytest.approx(controller.lateral_accel_from_torque(controller.steer_max, controller.torque_params))
saved = replace_mode(None, cp, ControllerMode.STANDARD)
assert selection_from_bytes(cp, saved).mode == ControllerMode.STANDARD
assert LatControlTorque(cp.as_reader(), ci, .01, controller_mode=ControllerMode.STANDARD).starpilot_extension is None
@pytest.mark.parametrize('topology', ['hdai', 'hdaii', '110'])
def test_actual_selected_controller_output_and_inactive_recovery(topology):
cp = topology_params(topology)
controller = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), .01)
vm = VehicleModel(cp)
state = SimpleNamespace(vEgo=12., steeringAngleDeg=0., steeringPressed=False)
parameters = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=.01)
for curvature in (.0002, -.0002, 0.):
for _ in range(25):
torque, _, trace = controller.update(True, state, vm, parameters, False, curvature, False, .2)
assert math.isfinite(torque) and math.isfinite(trace.f)
limit = max(abs(controller.torque_from_lateral_accel(bound, controller.torque_params))
for bound in (controller.pid.pos_limit, controller.pid.neg_limit))
assert abs(torque) <= limit + 1e-6
assert trace.active
with patch.object(controller.pid, 'update', wraps=controller.pid.update) as update:
state.steeringPressed = True
controller.update(True, state, vm, parameters, False, .0002, False, .2)
assert update.call_args.kwargs['freeze_integrator']
torque, _, trace = controller.update(False, state, vm, parameters, False, .0002, False, .2)
assert torque == 0 and not trace.active and controller.pid.i == 0
state.steeringPressed = False
assert math.isfinite(controller.update(True, state, vm, parameters, False, .0002, False, .2)[0])
def test_unrelated_or_unadmitted_profiles_cannot_select_palisade():
cp = interfaces[CAR.HYUNDAI_PALISADE].get_non_essential_params(CAR.HYUNDAI_PALISADE)
for field in ('passive', 'dashcamOnly', 'notCar'):
candidate = cp.as_reader().as_builder()
setattr(candidate, field, True)
assert not supported_cp(candidate)
cp.carFingerprint = CAR.HYUNDAI_IONIQ_6
assert not supported_cp(cp)
openpilot/starpilot/lateral/tests/test_sonata_hybrid_policy.py
+48
View File
@@ -0,0 +1,48 @@
import pytest
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, replace_mode, selection_from_bytes
from openpilot.starpilot.lateral.sonata_hybrid_policy import SonataHybridTorquePolicy, supported_cp
from openpilot.starpilot.lateral import sonata_hybrid_shaping as shaping
@pytest.mark.parametrize('speed,accel,jerk,ff,threshold,center,output', [
[2.0, 0.18, 0.3, 1.0029066557938373, 0.3172954212921375, 0.9974817827847016, 0.9989658177814709],
[8.0, -0.25, -0.45, 0.8335966434725148, 0.3099892527937427, 0.9999609835312265, 0.9960715403512586],
[22.0, 0.8, -0.6, 0.8570825555968928, 0.3, 0.9999999999996189, 0.9999994360166975],
[33.0, -1.4, 0.8, 0.8442122416154261, 0.3, 1.0, 0.9999999999964582],
])
def test_frozen_phase_side_and_center_vectors(speed, accel, jerk, ff, threshold, center, output):
actual = (shaping.get_sonata_hybrid_ff_scale(accel, jerk, speed), shaping.get_sonata_hybrid_friction_threshold(speed, accel),
shaping.get_sonata_hybrid_center_taper_scale(accel, speed), shaping.get_sonata_hybrid_center_output_scale(accel, speed))
assert actual == pytest.approx((ff, threshold, center, output), abs=1e-14, rel=0)
def test_default_startup_raw_limits_and_live_factor_exactly_once():
cp = interfaces[CAR.HYUNDAI_SONATA_HYBRID].get_non_essential_params(CAR.HYUNDAI_SONATA_HYBRID)
ci = interfaces[cp.carFingerprint](cp)
controller = LatControlTorque(cp.as_reader(), ci, .01)
assert default_selection(cp).policy == 'sonata_hybrid'
assert isinstance(controller.starpilot_extension.policy, SonataHybridTorquePolicy)
assert controller.torque_params.latAccelFactor == pytest.approx(cp.lateralTuning.torque.latAccelFactor * 1.05)
assert controller.pid.pos_limit == pytest.approx(controller.lateral_accel_from_torque(controller.steer_max, cp.lateralTuning.torque))
for _ in range(3):
controller.update_torque_parameters(3., .02, .12)
assert controller.torque_params.latAccelFactor == pytest.approx(3.15)
assert controller.torque_params.latAccelOffset == pytest.approx(.02)
assert controller.torque_params.friction == pytest.approx(.12)
saved = replace_mode(None, cp, ControllerMode.STANDARD)
assert selection_from_bytes(cp, saved).mode == ControllerMode.STANDARD
assert LatControlTorque(cp.as_reader(), ci, .01, controller_mode=ControllerMode.STANDARD).starpilot_extension is None
def test_unrelated_and_unadmitted_do_not_select_hybrid_policy():
cp = interfaces[CAR.HYUNDAI_SONATA_HYBRID].get_non_essential_params(CAR.HYUNDAI_SONATA_HYBRID)
for field in ('passive', 'dashcamOnly', 'notCar'):
other = cp.as_reader().as_builder()
setattr(other, field, True)
assert not supported_cp(other)
cp.carFingerprint = CAR.HYUNDAI_SONATA
assert not supported_cp(cp)
openpilot/starpilot/lateral/tests/test_torque_application.py
+103
View File
@@ -0,0 +1,103 @@
"""Actual source transitions must not silently alter vehicle startup limits."""
from dataclasses import replace
from unittest import mock
import unittest
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.toyota.values import CAR as TOYOTA
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.torque_runtime import TorqueHost
from openpilot.starpilot.lateral.torque_tuning import TorqueSource
class TestTorqueApplication(unittest.TestCase):
def controller(self, vehicle):
cp = interfaces[vehicle].get_non_essential_params(vehicle)
controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL)
return TorqueHost(Params(), cp), controller
def test_initial_vehicle_preserves_ioniq_startup_and_explicit_equal_source_updates(self):
with OpenpilotPrefix():
for source in (TorqueSource.LEARNED, TorqueSource.USER):
with self.subTest(source=source):
host, controller = self.controller(HYUNDAI.HYUNDAI_IONIQ_6)
with mock.patch.object(controller, 'update_torque_parameters', wraps=controller.update_torque_parameters) as update:
initial_limit = controller.pid.pos_limit
for _ in range(3):
self.assertFalse(host.apply(controller, host.vehicle))
update.assert_not_called()
self.assertEqual(controller.pid.pos_limit, initial_limit)
self.assertAlmostEqual(initial_limit, 3.0)
selected = replace(host.vehicle, source=source)
self.assertTrue(host.apply(controller, selected))
self.assertAlmostEqual(controller.pid.pos_limit, 3.0 * 1.22, places=6)
self.assertFalse(host.apply(controller, selected))
self.assertEqual(update.call_count, 1)
self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.0 * 1.22, places=6)
def test_changed_values_fallback_and_new_controller_do_not_inherit_application(self):
with OpenpilotPrefix():
host, controller = self.controller(HYUNDAI.HYUNDAI_IONIQ_6)
custom = replace(host.vehicle, source=TorqueSource.USER, lat_accel_factor=3.3, friction=0.12)
self.assertTrue(host.apply(controller, custom))
self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.3 * 1.22, places=6)
self.assertTrue(host.apply(controller, host.vehicle))
self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.0 * 1.22, places=6)
self.assertAlmostEqual(controller.torque_params.friction, 0.09, places=6)
# A fallback restores values through the normal update API. It does not
# rewind the controller's history to its pre-update initialization state.
self.assertAlmostEqual(controller.pid.pos_limit, 3.0 * 1.22, places=6)
self.assertFalse(host.apply(controller, host.vehicle))
_, other = self.controller(HYUNDAI.HYUNDAI_IONIQ_6)
self.assertFalse(host.apply(other, host.vehicle))
self.assertAlmostEqual(other.pid.pos_limit, 3.0)
self.assertTrue(host.apply(other, custom))
def test_vehicle_only_samples_do_not_unlock_limit(self):
with OpenpilotPrefix():
host, controller = self.controller(HYUNDAI.HYUNDAI_IONIQ_6)
# Inactive or malformed saved settings return the exact vehicle source.
sm = mock.Mock()
sm.all_checks.return_value = False
host.params.put_bool('AdvancedLateralTune', True, block=True)
host.params.put('TorqueOverrideDocument', {'invalid': True}, block=True)
for now, active in ((1_000_000_000, False), (1_010_000_000, True), (2_000_000_000, True)):
tune = host.sample(sm, now_ns=now, lat_active=active)
self.assertEqual(tune, host.vehicle)
self.assertFalse(host.apply(controller, tune))
self.assertAlmostEqual(controller.pid.pos_limit, 3.0)
def test_inactive_fallback_and_failed_application_are_not_cached_as_success(self):
with OpenpilotPrefix():
host, controller = self.controller(HYUNDAI.HYUNDAI_IONIQ_6)
custom = replace(host.vehicle, source=TorqueSource.USER, lat_accel_factor=3.3, friction=0.12)
with mock.patch.object(controller, 'update_torque_parameters', side_effect=ValueError('rejected')):
with self.assertRaises(ValueError):
host.apply(controller, custom)
self.assertTrue(host.apply(controller, custom))
self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.3 * 1.22, places=6)
fallback = host.sample(mock.Mock(), now_ns=1_000_000_000, lat_active=False)
self.assertEqual(fallback, host.vehicle)
self.assertTrue(host.apply(controller, fallback))
self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.0 * 1.22, places=6)
self.assertAlmostEqual(controller.torque_params.friction, 0.09, places=6)
def test_non_ioniq_controller_and_cross_vehicle_rejection(self):
with OpenpilotPrefix():
host, controller = self.controller(TOYOTA.TOYOTA_COROLLA_TSS2)
original = (controller.pid.pos_limit, controller.pid.neg_limit)
self.assertFalse(host.apply(controller, host.vehicle))
self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), original)
self.assertTrue(host.apply(controller, replace(host.vehicle, source=TorqueSource.USER)))
self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), original)
with self.assertRaises(ValueError):
host.apply(controller, replace(host.vehicle, vehicle='HYUNDAI_IONIQ_6'))
if __name__ == '__main__':
unittest.main()
openpilot/starpilot/lateral/tests/test_torque_family.py
+169
View File
@@ -0,0 +1,169 @@
"""Real Toyota CarParams and native controller coverage for the replay-only selector."""
import math
import os
from pathlib import Path
from types import SimpleNamespace
import unittest
from unittest import mock
from opendbc.car import gen_empty_fingerprint, structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.toyota.values import CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.tests.test_torque_tuning import frame
from openpilot.starpilot.lateral.torque_runtime import TorqueHost, development_enabled, read_settings, supported_cp
from openpilot.starpilot.lateral.torque_supported import TOYOTA_VEHICLES
from openpilot.starpilot.lateral.torque_tuning import TorqueSource
def params_for(car_id):
return interfaces[car_id].get_non_essential_params(car_id)
def learner(now_ns, base):
state = SimpleNamespace(useParams=True, valid=True, version=1, latAccelFactorFiltered=base.lat_accel_factor * 1.1,
latAccelOffsetFiltered=0.1, frictionCoefficientFiltered=base.friction * 1.1)
class Sample:
logMonoTime = {'lateralTorqueParameters': now_ns}
def all_checks(self, _names):
return True
def __getitem__(self, _name):
return state
return Sample()
class ToyotaTorqueFamilyTests(unittest.TestCase):
def test_exact_registry_cohort_and_cp_guard(self):
registry = {str(car.value) for car in CAR if (cp := params_for(str(car.value))).brand == 'toyota' and
cp.steerControlType == structs.CarParams.SteerControlType.torque and cp.lateralTuning.which() == 'torque'}
self.assertEqual(len(registry), 38)
self.assertEqual(TOYOTA_VEHICLES, registry)
self.assertFalse(supported_cp(params_for('TOYOTA_RAV4_TSS2_2023')))
self.assertFalse(supported_cp(params_for('CHEVROLET_BOLT_EUV')))
with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}):
for car_id in sorted(TOYOTA_VEHICLES):
cp = params_for(car_id)
with self.subTest(car_id=car_id):
self.assertTrue(development_enabled(cp))
cp.dashcamOnly = True
self.assertFalse(development_enabled(cp))
cp.dashcamOnly = False
cp.steerControlType = structs.CarParams.SteerControlType.angle
self.assertFalse(development_enabled(cp))
cp.steerControlType = structs.CarParams.SteerControlType.torque
cp.lateralTuning.torque.latAccelOffset = float('nan')
self.assertFalse(development_enabled(cp))
cp = params_for('TOYOTA_COROLLA_TSS2')
cp.brand = 'gm'
self.assertFalse(development_enabled(cp))
with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0'}):
self.assertFalse(development_enabled(params_for('TOYOTA_COROLLA_TSS2')))
def test_each_cp_selects_complete_tuple_and_drives_real_controller(self):
for car_id in sorted(TOYOTA_VEHICLES):
with self.subTest(car_id=car_id), OpenpilotPrefix():
cp = params_for(car_id)
saved = Params()
host = TorqueHost(saved, cp.as_reader())
base = host.vehicle
self.assertEqual(base.vehicle, car_id)
self.assertEqual(base.upstream_update(), (cp.lateralTuning.torque.latAccelFactor,
cp.lateralTuning.torque.latAccelOffset, cp.lateralTuning.torque.friction))
now = 1_000_000_000
sm = learner(now, base)
host.settings = read_settings(saved, base)
selected = host._select(sm, now)
self.assertEqual(selected.source, TorqueSource.LEARNED)
self.assertAlmostEqual(selected.lat_accel_factor, base.lat_accel_factor * 1.1)
self.assertAlmostEqual(selected.lat_accel_offset, 0.1)
self.assertAlmostEqual(selected.friction, base.friction * 1.1)
saved.put_bool('AdvancedLateralTune', True, block=True)
saved.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True)
host.settings = read_settings(saved, base)
factor_only = host._select(sm, now)
self.assertEqual(factor_only.source, TorqueSource.USER)
self.assertAlmostEqual(factor_only.lat_accel_factor, base.lat_accel_factor * 1.2)
self.assertAlmostEqual(factor_only.lat_accel_offset, base.lat_accel_offset)
self.assertAlmostEqual(factor_only.friction, base.friction * 1.1)
saved.remove('SteerLatAccel')
saved.put('SteerFriction', min(1.0, max(0.01, base.friction * 1.2)), block=True)
host.settings = read_settings(saved, base)
friction_only = host._select(sm, now)
self.assertEqual(friction_only.source, TorqueSource.USER)
self.assertAlmostEqual(friction_only.lat_accel_factor, base.lat_accel_factor * 1.1)
self.assertAlmostEqual(friction_only.lat_accel_offset, 0.1)
self.assertAlmostEqual(friction_only.friction, min(1.0, max(0.01, base.friction * 1.2)))
saved.put_bool('ForceAutoTuneOff', True, block=True)
host.settings = read_settings(saved, base)
self.assertAlmostEqual(host._select(sm, now).lat_accel_factor, base.lat_accel_factor)
saved.remove('SteerFriction')
Path(saved.get_param_path('SteerLatAccel')).write_bytes(b'nan')
self.assertFalse(read_settings(saved, base).valid)
self.assertEqual(host.sample(sm, now_ns=now, lat_active=False), base)
interface = interfaces[car_id]
controller = LatControlTorque(cp.as_reader(), interface(cp), DT_CTRL)
initial_limit = controller.pid.pos_limit
controller.update_torque_parameters(*factor_only.upstream_update())
self.assertAlmostEqual(controller.torque_params.latAccelFactor, factor_only.lat_accel_factor, delta=1e-6)
self.assertAlmostEqual(controller.torque_params.latAccelOffset, factor_only.lat_accel_offset, delta=1e-6)
self.assertAlmostEqual(controller.torque_params.friction, factor_only.friction, delta=1e-6)
self.assertAlmostEqual(controller.pid.pos_limit, initial_limit * 1.2, delta=1e-6)
vm = VehicleModel(cp)
for _ in range(50):
output, _, state = frame(controller, vm, curvature=0.001)
self.assertTrue(state.active)
self.assertTrue(math.isfinite(output))
self.assertLessEqual(abs(output), controller.steer_max + 1e-6)
output, _, state = frame(controller, vm, active=False, curvature=0.001)
self.assertEqual(output, 0.0)
self.assertFalse(state.active)
def test_stock_markers_do_not_carry_previous_vehicle_tune(self):
first = params_for('TOYOTA_COROLLA_TSS2')
second = params_for('TOYOTA_RAV4_TSS2')
with OpenpilotPrefix():
saved = Params()
old = TorqueHost(saved, first).vehicle
new = TorqueHost(saved, second).vehicle
self.assertNotEqual(old.lat_accel_factor, new.lat_accel_factor)
saved.put_bool('AdvancedLateralTune', True, block=True)
saved.put('SteerLatAccel', old.lat_accel_factor, block=True)
saved.put('SteerLatAccelStock', old.lat_accel_factor, block=True)
saved.put('SteerFriction', old.friction, block=True)
saved.put('SteerFrictionStock', old.friction, block=True)
settings = read_settings(saved, new)
self.assertTrue(settings.valid)
self.assertIsNone(settings.user_factor)
self.assertIsNone(settings.user_friction)
host = TorqueHost(saved, second)
host.settings = settings
self.assertEqual(host._select(learner(1_000_000_000, new), 1_000_000_000).source, TorqueSource.LEARNED)
def test_prius_eps_firmware_deadzone_variant_keeps_cp_and_selector(self):
car_id = 'TOYOTA_PRIUS'
interface = interfaces[car_id]
eps = structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.eps, fwVersion=b'other-prius-eps')
cp = interface.get_params(car_id, gen_empty_fingerprint(), [eps], False, False, False)
self.assertTrue(supported_cp(cp))
self.assertAlmostEqual(cp.lateralTuning.torque.steeringAngleDeadzoneDeg, 0.2)
with OpenpilotPrefix():
host = TorqueHost(Params(), cp)
self.assertEqual(host.vehicle.upstream_update(), (cp.lateralTuning.torque.latAccelFactor,
cp.lateralTuning.torque.latAccelOffset, cp.lateralTuning.torque.friction))
controller = LatControlTorque(cp.as_reader(), interface(cp), DT_CTRL)
self.assertAlmostEqual(controller.steering_angle_deadzone_deg, 0.2)
if __name__ == '__main__':
unittest.main()
openpilot/starpilot/lateral/tests/test_torque_learning.py
+275
View File
@@ -0,0 +1,275 @@
"""Controller-specific learning is enforced by its producer and both consumers."""
import os
from pathlib import Path
from types import SimpleNamespace
import unittest
from unittest import mock
from opendbc.car.car_helpers import interfaces
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.toyota.values import CAR as TOYOTA
from openpilot.cereal import messaging
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.controls.controlsd import Controls
from openpilot.selfdrive.locationd import torqued
from openpilot.starpilot.lateral.controller_selection import (
DOCUMENT_KEY, LEARNING_OFF_KEY, ControllerMode, learning_allowed, replace_mode,
)
from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
from openpilot.starpilot.lateral.tests.test_torque_runtime import LearnedFrame
from openpilot.starpilot.lateral.torque_runtime import TorqueHost, read_settings
from openpilot.starpilot.lateral.torque_tuning import TorqueSource
VEHICLES = (HYUNDAI.HYUNDAI_IONIQ_6, HYUNDAI.GENESIS_G70_2020, TOYOTA.TOYOTA_COROLLA_TSS2)
ENV = {'REPLAY': '1', 'TORQUE_REPLAY_RUNTIME': '0', 'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'}
def cp_for(vehicle):
return interfaces[vehicle].get_non_essential_params(vehicle)
def select(params, cp, mode):
Path(params.get_param_path(DOCUMENT_KEY)).write_bytes(replace_mode(None, cp, mode))
def learned_event(cp, now):
event = messaging.new_message('lateralTorqueParameters', valid=True, logMonoTime=now)
state = event.lateralTorqueParameters
state.useParams = state.valid = True
state.version = torqued.VERSION
state.latAccelFactorFiltered = cp.lateralTuning.torque.latAccelFactor * 1.1
state.latAccelOffsetFiltered = 0.03
state.frictionCoefficientFiltered = cp.lateralTuning.torque.friction * 1.1
return event.as_reader()
class TestTorqueLearning(unittest.TestCase):
def test_exact_policy_defaults_and_saved_off_without_manual_adjustments(self):
with OpenpilotPrefix():
params = Params()
for vehicle in VEHICLES:
with self.subTest(vehicle=vehicle):
cp = cp_for(vehicle)
params.remove(DOCUMENT_KEY)
params.remove(LEARNING_OFF_KEY)
self.assertFalse(learning_allowed(params, cp))
select(params, cp, ControllerMode.STANDARD)
self.assertTrue(learning_allowed(params, cp))
for saved in (b'1', b'invalid', b''):
Path(params.get_param_path(LEARNING_OFF_KEY)).write_bytes(saved)
self.assertFalse(learning_allowed(params, cp))
params.put_bool(LEARNING_OFF_KEY, False, block=True)
self.assertTrue(learning_allowed(params, cp))
params.put_bool(LEARNING_OFF_KEY, True, block=True)
for vehicle in (HYUNDAI.KIA_EV6, HYUNDAI.HYUNDAI_SONATA, TOYOTA.TOYOTA_RAV4_TSS2):
self.assertTrue(learning_allowed(params, cp_for(vehicle)))
def test_learner_does_not_restore_collect_estimate_or_overwrite_disabled_cache(self):
for vehicle in VEHICLES:
with self.subTest(vehicle=vehicle), OpenpilotPrefix(), mock.patch.dict(os.environ, ENV):
params, cp = Params(), cp_for(vehicle)
params.put('CarParams', cp.to_bytes(), block=True)
cache = Path(params.get_param_path('LiveTorqueParameters'))
cache.write_bytes(b'preserved previous learning')
with mock.patch.object(torqued, 'get_cache') as restore:
estimator = torqued.TorqueEstimator(cp, allow_learning=learning_allowed(params, cp))
restore.assert_not_called()
self.assertFalse(estimator.learning_allowed)
estimator.handle_log(1, 'carControl', SimpleNamespace(latActive=True))
estimator.handle_log(1, 'carOutput', SimpleNamespace(actuatorsOutput=SimpleNamespace(torque=0.2)))
self.assertFalse(estimator.raw_points)
with mock.patch.object(estimator, 'estimate_params', side_effect=AssertionError('Learning is disabled')):
msg = estimator.get_msg(with_points=True).lateralTorqueParameters
self.assertFalse(msg.useParams)
self.assertFalse(msg.valid)
self.assertEqual(msg.totalBucketPoints, 0)
self.assertEqual(msg.latAccelFactorFiltered, cp.lateralTuning.torque.latAccelFactor)
sm = mock.Mock(frame=240, updated={})
sm.update.side_effect = [None, StopIteration]
sm.all_checks.return_value = True
with mock.patch.object(torqued, 'prewarm_cache_contracts'), mock.patch.object(torqued, 'config_realtime_process'), \
mock.patch.object(torqued.messaging, 'SubMaster', return_value=sm), mock.patch.object(torqued.messaging, 'PubMaster'), \
mock.patch.object(torqued, 'put_cache') as persist, self.assertRaises(StopIteration):
torqued.main()
persist.assert_not_called()
self.assertEqual(cache.read_bytes(), b'preserved previous learning')
def test_standard_and_untuned_learner_keep_upstream_collection_and_cache(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, ENV):
params = Params()
for vehicle in (*VEHICLES, HYUNDAI.KIA_EV6):
cp = cp_for(vehicle)
if vehicle in VEHICLES:
select(params, cp, ControllerMode.STANDARD)
with self.subTest(vehicle=vehicle), mock.patch.object(torqued, 'get_cache', return_value=None) as restore:
estimator = torqued.TorqueEstimator(cp)
self.assertEqual(restore.call_count, 2)
self.assertTrue(estimator.learning_allowed)
self.assertTrue(estimator.use_params)
estimator.handle_log(1, 'carControl', SimpleNamespace(latActive=True))
self.assertEqual(list(estimator.raw_points['lat_active']), [True])
select(params, cp_for(VEHICLES[0]), ControllerMode.STANDARD)
params.put_bool(LEARNING_OFF_KEY, True, block=True)
cp = cp_for(VEHICLES[0])
estimator = torqued.TorqueEstimator(cp, allow_learning=learning_allowed(params, cp))
self.assertFalse(estimator.learning_allowed)
def test_offline_fitting_is_not_disabled_by_saved_controller_or_learning_preference(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, ENV):
params, cp = Params(), cp_for(HYUNDAI.HYUNDAI_IONIQ_6)
select(params, cp, ControllerMode.STARPILOT)
params.put_bool(LEARNING_OFF_KEY, True, block=True)
self.assertFalse(learning_allowed(params, cp))
with mock.patch.object(torqued, 'get_cache', return_value=None):
estimator = torqued.TorqueEstimator(cp, decimated=True, track_all_points=True)
estimator.handle_log(1, 'carControl', SimpleNamespace(latActive=True))
self.assertEqual(list(estimator.raw_points['lat_active']), [True])
self.assertTrue(estimator.use_params)
with mock.patch.object(estimator.filtered_points, 'is_calculable', return_value=True), \
mock.patch.object(estimator.filtered_points, 'is_valid', return_value=True), \
mock.patch.object(estimator, 'estimate_params', return_value=(3.1, 0.01, 0.10)) as fit:
message = estimator.get_msg().lateralTorqueParameters
fit.assert_called_once()
self.assertTrue(message.valid)
def test_direct_controller_consumer_rejects_learned_messages_and_latches_policy(self):
now = 1_000_000_000
def feed_learning(controls):
# Real SubMaster frequency checks need a measured interval. Host development
# defaults to SIMULATION=1, which otherwise hides an unqualified fixture.
self.assertFalse(controls.sm.simulation)
for tick in range(2):
stamp = now + tick * 250_000_000 # lateralTorqueParameters publishes at 4 Hz.
feed(controls, stamp, 0)
controls.sm.update_msgs(stamp / 1e9, [learned_event(cp, stamp)])
self.assertEqual(controls.sm.all_checks(['lateralTorqueParameters']), tick == 1)
for vehicle in (*VEHICLES, HYUNDAI.KIA_EV6):
with self.subTest(vehicle=vehicle), OpenpilotPrefix(), mock.patch.dict(os.environ, {**ENV, 'SIMULATION': '0'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params, cp = Params(), cp_for(vehicle)
params.put('CarParams', cp.to_bytes(), block=True)
controls = Controls()
self.assertIsNone(controls.torque_host)
feed_learning(controls)
with mock.patch.object(controls.LaC, 'update_torque_parameters') as update:
controls.state_control()
self.assertEqual(update.call_count, 0 if vehicle in VEHICLES else 1)
if vehicle in VEHICLES:
select(params, cp, ControllerMode.STANDARD)
controls.state_control()
update.assert_not_called()
self.assertEqual(controls.LaC.controller_mode, ControllerMode.STARPILOT)
next_drive = Controls()
self.assertTrue(next_drive.torque_learning_allowed)
self.assertEqual(next_drive.LaC.controller_mode, ControllerMode.STANDARD)
feed_learning(next_drive)
with mock.patch.object(next_drive.LaC, 'update_torque_parameters') as standard_update:
next_drive.state_control()
standard_update.assert_called_once()
invalid = learned_event(cp, now + 500_000_000).as_builder()
invalid.valid = False
next_drive.sm.update_msgs((now + 500_000_000) / 1e9, [invalid.as_reader()])
self.assertFalse(next_drive.sm.all_checks(['lateralTorqueParameters']))
next_drive.state_control()
standard_update.assert_called_once()
invalid.valid = True
next_drive.sm.update_msgs((now + 750_000_000) / 1e9, [invalid.as_reader()])
self.assertTrue(next_drive.sm.all_checks(['lateralTorqueParameters']))
next_drive.sm.update_msgs((now + 4_000_000_000) / 1e9, [])
self.assertFalse(next_drive.sm.all_checks(['lateralTorqueParameters']))
next_drive.state_control()
standard_update.assert_called_once()
params.put_bool(LEARNING_OFF_KEY, True, block=True)
disabled = Controls()
self.assertFalse(disabled.torque_learning_allowed)
feed_learning(disabled)
with mock.patch.object(disabled.LaC, 'update_torque_parameters') as disabled_update:
disabled.state_control()
disabled_update.assert_not_called()
def test_manual_factor_and_friction_do_not_inherit_learning_with_starpilot(self):
with OpenpilotPrefix():
params, cp = Params(), cp_for(HYUNDAI.HYUNDAI_IONIQ_6)
params.put_bool('AdvancedLateralTune', True, block=True)
params.put('SteerLatAccel', cp.lateralTuning.torque.latAccelFactor * 1.1, block=True)
host = TorqueHost(params, cp, allow_learning=learning_allowed(params, cp))
base = host.vehicle
now = 1_000_000_000
learned = LearnedFrame(now, factor=base.lat_accel_factor * 1.2, offset=0.03, friction=base.friction * 1.1)
host.sample(learned, now_ns=now, lat_active=True)
self.assertEqual(host.selected.source, TorqueSource.USER)
self.assertAlmostEqual(host.selected.lat_accel_factor, base.lat_accel_factor * 1.1)
self.assertEqual(host.selected.lat_accel_offset, base.lat_accel_offset)
self.assertEqual(host.selected.friction, base.friction)
params.remove('SteerLatAccel')
params.put('SteerFriction', base.friction * 1.2, block=True)
host.settings = read_settings(params, base)
self.assertEqual(host._select(learned, now).lat_accel_factor, base.lat_accel_factor)
self.assertAlmostEqual(host._select(learned, now).friction, base.friction * 1.2)
select(params, cp, ControllerMode.STANDARD)
self.assertFalse(host.allow_learning)
self.assertEqual(host._select(learned, now).lat_accel_factor, base.lat_accel_factor)
params.put_bool('AdvancedLateralTune', False, block=True)
params.put_bool(LEARNING_OFF_KEY, True, block=True)
host.settings = read_settings(params, base)
self.assertFalse(host.settings.advanced)
self.assertFalse(TorqueHost(params, cp, allow_learning=learning_allowed(params, cp)).allow_learning)
def test_running_manual_host_latches_both_learning_directions_and_keeps_manual_edits(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, ENV), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params, cp = Params(), cp_for(HYUNDAI.HYUNDAI_IONIQ_6)
params.put('CarParams', cp.to_bytes(), block=True)
params.put_bool('AdvancedLateralTune', True, block=True)
select(params, cp, ControllerMode.STANDARD)
controls = Controls()
host, base = controls.torque_host, controls.torque_host.vehicle
now = 1_000_000_000
learned = LearnedFrame(now, factor=base.lat_accel_factor * 1.2, offset=0.03, friction=base.friction * 1.1)
host.sample(learned, now_ns=now, lat_active=True)
self.assertEqual(host.selected.source, TorqueSource.LEARNED)
select(params, cp, ControllerMode.STARPILOT)
params.put_bool(LEARNING_OFF_KEY, True, block=True)
for tick in range(1, 102):
stamp = now + tick * 10_000_000
learned.logMonoTime['lateralTorqueParameters'] = stamp
host.sample(learned, now_ns=stamp, lat_active=True)
self.assertEqual(controls.LaC.controller_mode, ControllerMode.STANDARD)
self.assertEqual(host.selected.source, TorqueSource.LEARNED)
params.put('SteerLatAccel', base.lat_accel_factor * 1.1, block=True)
host.last_refresh_ns = None
host.sample(learned, now_ns=stamp + 10_000_000, lat_active=True)
self.assertEqual(host.selected.source, TorqueSource.USER)
self.assertAlmostEqual(host.selected.lat_accel_factor, base.lat_accel_factor * 1.1)
self.assertEqual(host.selected.friction, learned.state.frictionCoefficientFiltered)
next_drive = Controls()
disabled = next_drive.torque_host
self.assertFalse(disabled.allow_learning)
select(params, cp, ControllerMode.STANDARD)
params.put_bool(LEARNING_OFF_KEY, False, block=True)
disabled.sample(learned, now_ns=stamp + 20_000_000, lat_active=True)
self.assertEqual(next_drive.LaC.controller_mode, ControllerMode.STARPILOT)
self.assertEqual(disabled.selected.source, TorqueSource.USER)
self.assertEqual(disabled.selected.friction, base.friction)
def test_unrelated_manual_host_preserves_master_off_behavior(self):
with OpenpilotPrefix():
params, cp = Params(), cp_for(TOYOTA.TOYOTA_RAV4_TSS2)
host = TorqueHost(params, cp)
for raw in (b'1', b'invalid'):
Path(params.get_param_path(LEARNING_OFF_KEY)).write_bytes(raw)
settings = read_settings(params, host.vehicle)
self.assertTrue(settings.valid)
self.assertFalse(settings.advanced)
self.assertFalse(settings.force_auto_off)
self.assertIsNone(TorqueHost(params, cp).allow_learning)
if __name__ == '__main__':
unittest.main()
openpilot/starpilot/lateral/tests/test_torque_runtime.py
+291
View File
@@ -0,0 +1,291 @@
"""Native Corolla torque source selection, saved choices, and controller gates."""
import math
import os
from pathlib import Path
from types import SimpleNamespace
import unittest
from unittest import mock
from openpilot.cereal import messaging
from openpilot.common.params import ParamKeyFlag, Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.controls.controlsd import Controls
from openpilot.selfdrive.locationd.torqued import TorqueEstimator
from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
from openpilot.starpilot.lateral.controller_selection import DOCUMENT_KEY as CONTROLLER_KEY, ControllerMode, replace_mode
from openpilot.starpilot.lateral.torque_runtime import TorqueHost, development_enabled, read_settings
from openpilot.starpilot.lateral.torque_tuning import TorqueSource
from opendbc.car.car_helpers import interfaces
from opendbc.car.toyota.values import CAR
def corolla():
return interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2)
class LearnedFrame:
def __init__(self, now_ns, *, factor=2.0, offset=0.1, friction=0.1, version=1, valid=True):
self.logMonoTime = {'lateralTorqueParameters': now_ns}
self.checked = True
self.state = SimpleNamespace(useParams=True, valid=valid, version=version, latAccelFactorFiltered=factor,
latAccelOffsetFiltered=offset, frictionCoefficientFiltered=friction)
def all_checks(self, _names):
return self.checked
def __getitem__(self, _name):
return self.state
class TorqueRuntimeTests(unittest.TestCase):
def test_saved_partial_precedence_and_corrupt_raw_bytes(self):
with OpenpilotPrefix():
params, cp = Params(), corolla()
host = TorqueHost(params, cp)
base = host.vehicle
self.assertFalse(read_settings(params, base).advanced)
params.put_bool('AdvancedLateralTune', True, block=True)
params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True)
params.put('SteerFriction', min(1.0, base.friction * 1.2), block=True)
settings = read_settings(params, base)
self.assertTrue(settings.valid)
host.settings = settings
learned = LearnedFrame(1_000_000_000, factor=base.lat_accel_factor * 1.1,
offset=0.1, friction=base.friction * 1.1)
selected = host._select(learned, 1_000_000_000)
self.assertEqual(selected.source, TorqueSource.USER)
self.assertEqual(selected.lat_accel_factor, settings.user_factor)
self.assertEqual(selected.lat_accel_offset, base.lat_accel_offset)
self.assertEqual(selected.friction, settings.user_friction)
Path(params.get_param_path('SteerLatAccel')).write_bytes(b'nan')
self.assertFalse(read_settings(params, base).valid)
self.assertTrue(params.get_bool('AdvancedLateralTune'))
host.settings = read_settings(params, base)
self.assertEqual(host._select(learned, 1_000_000_000), base)
self.assertEqual(host.sample(learned, now_ns=1_000_000_000, lat_active=True), base)
def test_frozen_auto_synced_stock_and_individual_overrides(self):
with OpenpilotPrefix():
params, cp = Params(), corolla()
host = TorqueHost(params, cp)
base = host.vehicle
now = 1_000_000_000
learned = LearnedFrame(now, factor=base.lat_accel_factor * 1.1,
offset=0.1, friction=base.friction * 1.1)
params.put_bool('AdvancedLateralTune', True, block=True)
params.put('SteerLatAccel', base.lat_accel_factor, block=True)
params.put('SteerFriction', base.friction, block=True)
host.settings = read_settings(params, base)
self.assertIsNone(host.settings.user_factor)
self.assertIsNone(host.settings.user_friction)
self.assertEqual(host._select(learned, now).source, TorqueSource.LEARNED)
params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True)
host.settings = read_settings(params, base)
factor_only = host._select(learned, now)
self.assertEqual(factor_only.source, TorqueSource.USER)
self.assertAlmostEqual(factor_only.lat_accel_factor, base.lat_accel_factor * 1.2)
self.assertEqual(factor_only.lat_accel_offset, base.lat_accel_offset)
self.assertAlmostEqual(factor_only.friction, base.friction * 1.1)
params.put('SteerLatAccel', base.lat_accel_factor, block=True)
params.put('SteerFriction', base.friction * 1.2, block=True)
host.settings = read_settings(params, base)
friction_only = host._select(learned, now)
self.assertEqual(friction_only.source, TorqueSource.USER)
self.assertAlmostEqual(friction_only.lat_accel_factor, base.lat_accel_factor * 1.1)
self.assertAlmostEqual(friction_only.lat_accel_offset, 0.1)
self.assertAlmostEqual(friction_only.friction, base.friction * 1.2)
# A frozen stock-sync file distinguishes an old stock save from a user edit
# after a later CarParams retune changes the current stock value.
old_stock = base.lat_accel_factor * 0.9
params.put('SteerLatAccel', old_stock, block=True)
params.put('SteerLatAccelStock', old_stock, block=True)
params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START)
self.assertAlmostEqual(params.get('SteerLatAccelStock'), old_stock)
host.settings = read_settings(params, base)
self.assertIsNone(host.settings.user_factor)
self.assertAlmostEqual(host._select(learned, now).lat_accel_factor, learned.state.latAccelFactorFiltered)
# Old untouched stock may be outside today's safe custom range: it is a
# stock-tracking marker, not a custom override to reject or apply.
older_stock = base.lat_accel_factor * 2.0
params.put('SteerLatAccel', older_stock, block=True)
params.put('SteerLatAccelStock', older_stock, block=True)
host.settings = read_settings(params, base)
self.assertTrue(host.settings.valid)
self.assertIsNone(host.settings.user_factor)
self.assertAlmostEqual(host._select(learned, now).lat_accel_factor, learned.state.latAccelFactorFiltered)
params.put('SteerFriction', base.friction, block=True)
params.put_bool('ForceAutoTuneOff', True, block=True)
host.settings = read_settings(params, base)
self.assertEqual(host._select(learned, now), base)
def test_malformed_saved_bool_and_numeric_settings_fail_closed(self):
with OpenpilotPrefix():
params, cp = Params(), corolla()
host = TorqueHost(params, cp)
params.put_bool('AdvancedLateralTune', True, block=True)
for key, bad in (('AdvancedLateralTune', b'yes'), ('ForceAutoTuneOff', b'2'),
('SteerLatAccel', b'nan'), ('SteerFriction', b'inf'),
('SteerLatAccelStock', b'bad'), ('SteerFrictionStock', b'bad')):
with self.subTest(key=key):
path = Path(params.get_param_path(key))
original = path.read_bytes() if path.exists() else None
path.write_bytes(bad)
self.assertFalse(read_settings(params, host.vehicle).valid)
if original is None:
path.unlink()
else:
path.write_bytes(original)
def test_learned_source_freshness_version_and_override(self):
with OpenpilotPrefix():
params, cp = Params(), corolla()
host = TorqueHost(params, cp)
base = host.vehicle
frame = LearnedFrame(1_000_000_000, factor=base.lat_accel_factor * 1.1,
offset=0.1, friction=base.friction * 1.1)
self.assertEqual(host._select(frame, 1_000_000_000).source, TorqueSource.LEARNED)
frame.state.version = 99
self.assertEqual(host._select(frame, 1_000_000_000), base)
frame.state.version = 1
frame.logMonoTime['lateralTorqueParameters'] = 1
self.assertEqual(host._select(frame, 1_000_000_000), base)
frame.logMonoTime['lateralTorqueParameters'] = 1_000_000_000
frame.checked = False
self.assertEqual(host._select(frame, 1_000_000_000), base)
params.put_bool('AdvancedLateralTune', True, block=True)
params.put_bool('ForceAutoTuneOff', True, block=True)
host.settings = read_settings(params, base)
frame.checked = True
self.assertEqual(host._select(frame, 1_000_000_000), base)
def test_exact_car_development_policy(self):
cp = corolla()
with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}):
self.assertTrue(development_enabled(cp))
cp.carFingerprint = 'TOYOTA_RAV4_TSS2_2023'
self.assertFalse(development_enabled(cp))
with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0'}):
self.assertFalse(development_enabled(corolla()))
def test_opt_in_learner_skips_old_cache_without_deleting_it(self):
cp = corolla()
with OpenpilotPrefix(), mock.patch('openpilot.selfdrive.locationd.torqued.get_cache', return_value=None) as get_cache:
with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}):
fresh = TorqueEstimator(cp.as_reader())
self.assertEqual(get_cache.call_count, 0)
self.assertEqual(fresh.filtered_params['latAccelFactor'].x, cp.lateralTuning.torque.latAccelFactor)
cp.carFingerprint = 'TOYOTA_RAV4_TSS2_2023'
with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}):
TorqueEstimator(cp.as_reader())
self.assertEqual(get_cache.call_count, 2)
def test_native_controls_default_equivalence_and_selected_torque(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0',
'LANE_CENTERING_REPLAY_RUNTIME': '0'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params, cp = Params(), corolla()
params.put('CarParams', cp.to_bytes(), block=True)
with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0'}):
baseline = Controls()
with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}):
selected = Controls()
self.assertIsNone(baseline.torque_host)
self.assertIsNotNone(selected.torque_host)
for tick in range(110):
now = 1_000_000_000 + tick * 10_000_000
for controls in (baseline, selected):
feed(controls, now, tick)
a, _ = baseline.state_control()
b, _ = selected.state_control()
self.assertEqual(a.to_dict(), b.to_dict())
params.put_bool('AdvancedLateralTune', True, block=True)
native_factor = selected.torque_host.vehicle.lat_accel_factor
params.put('SteerLatAccel', native_factor * 1.3, block=True)
torque_differences = []
for tick in range(110, 230):
now = 1_000_000_000 + tick * 10_000_000
for controls in (baseline, selected):
feed(controls, now, tick)
reference, _ = baseline.state_control()
command, _ = selected.state_control()
torque_differences.append(abs(command.actuators.torque - reference.actuators.torque))
self.assertTrue(command.latActive)
self.assertGreater(selected.torque_host.applied.lat_accel_factor, native_factor)
self.assertLessEqual(selected.torque_host.applied.lat_accel_factor, native_factor * 1.3)
self.assertGreater(selected.LaC.pid.pos_limit, baseline.LaC.pid.pos_limit)
self.assertGreater(max(torque_differences), 1e-6)
feed(selected, now + 10_000_000, 230, active=False, enabled=False)
off, _ = selected.state_control()
self.assertFalse(off.latActive)
self.assertEqual(off.actuators.torque, 0.0)
self.assertEqual(selected.torque_host.applied, selected.torque_host.vehicle)
self.assertEqual(selected.LaC.pid.i, 0.0)
feed(selected, now + 20_000_000, 231, can_valid=False)
invalid, _ = selected.state_control()
self.assertFalse(invalid.latActive)
self.assertEqual(invalid.actuators.torque, 0.0)
feed(selected, now + 30_000_000, 232)
resumed, _ = selected.state_control()
self.assertTrue(resumed.latActive)
self.assertLess(abs(resumed.actuators.torque), selected.LaC.steer_max)
feed(selected, now + 40_000_000, 233, can_timeout=True)
timeout, _ = selected.state_control()
self.assertFalse(timeout.latActive)
self.assertEqual(timeout.actuators.torque, 0.0)
feed(selected, now + 50_000_000, 234, override=True)
prior_integral = selected.LaC.pid.i
override, _ = selected.state_control()
self.assertTrue(override.latActive)
self.assertTrue(math.isfinite(override.actuators.torque))
self.assertEqual(selected.LaC.pid.i, prior_integral)
def test_actual_controls_learned_to_user_to_stale_source(self):
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1', 'TORQUE_REPLAY_RUNTIME': '1',
'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'}), \
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
params, cp = Params(), corolla()
Path(params.get_param_path(CONTROLLER_KEY)).write_bytes(replace_mode(None, cp, ControllerMode.STANDARD))
params.put('CarParams', cp.to_bytes(), block=True)
controls = Controls()
base = controls.torque_host.vehicle
target = base.lat_accel_factor * 1.1
for tick in range(120):
now = 1_000_000_000 + tick * 10_000_000
feed(controls, now, tick)
if tick % 25 == 0:
event = messaging.new_message('lateralTorqueParameters', valid=True, logMonoTime=now)
state = event.lateralTorqueParameters
state.useParams = state.valid = True
state.version = 1
state.latAccelFactorFiltered = target
state.latAccelOffsetFiltered = 0.1
state.frictionCoefficientFiltered = base.friction
controls.sm.update_msgs(now / 1e9, [event.as_reader()])
controls.state_control()
self.assertEqual(controls.torque_host.selected.source, TorqueSource.LEARNED)
self.assertGreater(controls.torque_host.applied.lat_accel_factor, base.lat_accel_factor)
params.put_bool('AdvancedLateralTune', True, block=True)
params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True)
for tick in range(120, 230):
now = 1_000_000_000 + tick * 10_000_000
feed(controls, now, tick)
controls.state_control()
self.assertEqual(controls.torque_host.selected.source, TorqueSource.USER)
self.assertEqual(controls.torque_host.selected.lat_accel_offset, base.lat_accel_offset)
self.assertGreater(controls.torque_host.applied.lat_accel_factor, target)
params.put_bool('AdvancedLateralTune', False, block=True)
for tick in range(230, 340):
now = 1_000_000_000 + tick * 10_000_000
feed(controls, now, tick)
controls.state_control()
self.assertEqual(controls.torque_host.selected, base)
self.assertAlmostEqual(controls.torque_host.applied.lat_accel_factor, base.lat_accel_factor)
if __name__ == '__main__':
unittest.main()
openpilot/starpilot/lateral/tests/test_torque_settings.py
+86
View File
@@ -0,0 +1,86 @@
"""Versioned Torque preference document against real temporary Params."""
from pathlib import Path
import json
import unittest
from openpilot.common.params import ParamKeyType, Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.starpilot.lateral.torque_runtime import TorqueHost, read_settings
from openpilot.starpilot.lateral.torque_settings import (
DOCUMENT_KEY, LEGACY_KEYS, FieldChoice, LegacyMode, PlatformProfile, interpret_legacy, parse_document,
replace_field, resolve_document, serialize_document,
)
from opendbc.car.car_helpers import interfaces
def corolla():
return interfaces["TOYOTA_COROLLA_TSS2"].get_non_essential_params("TOYOTA_COROLLA_TSS2")
class TorqueDocumentTests(unittest.TestCase):
def test_native_key_is_optional_nondefault_json(self):
with OpenpilotPrefix():
params = Params()
self.assertEqual(params.get_type(DOCUMENT_KEY), ParamKeyType.JSON)
self.assertIsNone(params.get(DOCUMENT_KEY, return_default=True))
def test_legacy_absence_and_explicit_document_ignore_old_bytes(self):
with OpenpilotPrefix():
params = Params()
host = TorqueHost(params, corolla())
basis = (host.vehicle.lat_accel_factor, host.vehicle.lat_accel_offset, host.vehicle.friction)
params.put_bool("AdvancedLateralTune", True, block=True)
old_factor = basis[0] * 1.2
params.put("SteerLatAccel", old_factor, block=True)
before = {key: Path(params.get_param_path(key)).read_bytes() if Path(params.get_param_path(key)).exists() else None
for key in LEGACY_KEYS}
self.assertEqual(read_settings(params, host.vehicle).user_factor, old_factor)
raw = serialize_document({host.vehicle.vehicle: PlatformProfile(basis, FieldChoice("custom", basis[0]), FieldChoice())})
params.put(DOCUMENT_KEY, json.loads(raw), block=True)
after = read_settings(params, host.vehicle)
self.assertTrue(after.valid)
self.assertIsNone(after.user_factor)
self.assertEqual(parse_document(raw)[host.vehicle.vehicle].factor.mode, "custom")
self.assertEqual({key: Path(params.get_param_path(key)).read_bytes() if Path(params.get_param_path(key)).exists() else None
for key in LEGACY_KEYS}, before)
def test_partial_fields_round_trip_and_model_scope(self):
cp = corolla()
host = TorqueHost(Params(), cp)
basis = (host.vehicle.lat_accel_factor, host.vehicle.lat_accel_offset, host.vehicle.friction)
profiles = replace_field({}, host.vehicle.vehicle, basis, "factor", "custom", basis[0] * 1.1)
profiles = replace_field(profiles, host.vehicle.vehicle, basis, "friction", "custom", basis[2] * 1.2)
profiles = replace_field(profiles, host.vehicle.vehicle, basis, "factor", "source", None)
parsed = parse_document(serialize_document(profiles))
self.assertEqual(parsed[host.vehicle.vehicle].factor.custom_value, basis[0] * 1.1)
self.assertEqual(resolve_document(parsed, host.vehicle.vehicle, basis), (None, basis[2] * 1.2, False))
self.assertEqual(resolve_document(parsed, "TOYOTA_CAMRY", basis), (None, None, False))
changed_basis = (basis[0] * 1.1, basis[1], basis[2])
self.assertEqual(resolve_document(parsed, host.vehicle.vehicle, changed_basis), (None, None, True))
def test_bad_document_fails_closed_without_legacy_fallback(self):
with OpenpilotPrefix():
params = Params()
host = TorqueHost(params, corolla())
params.put_bool("AdvancedLateralTune", True, block=True)
params.put("SteerLatAccel", host.vehicle.lat_accel_factor * 1.2, block=True)
for raw in (b"{}", b'{"schemaVersion":1,"schemaVersion":1,"vehicles":{}}',
b'{"schemaVersion":1,"vehicles":{"TOYOTA_COROLLA_TSS2":{"basis":{},"factor":{},"friction":{}}}}',
b'[' * 1100 + b'0' + b']' * 1100,
b'{"schemaVersion":1,"vehicles":{}}' + b' ' * 20_000,
b'{"schemaVersion":' + b'9' * 5000 + b',"vehicles":{}}',
b'{"schemaVersion":1,"vehicles":{"TOYOTA_COROLLA_TSS2":{"basis":{"latAccelFactor":' +
b'9' * 350 + b',"latAccelOffset":0,"friction":0},"factor":{"mode":"source","customValue":null},' +
b'"friction":{"mode":"source","customValue":null}}}}'):
Path(params.get_param_path(DOCUMENT_KEY)).write_bytes(raw)
self.assertFalse(read_settings(params, host.vehicle).valid)
self.assertEqual(Path(params.get_param_path(DOCUMENT_KEY)).read_bytes(), raw)
def test_shared_legacy_parser_rejects_bad_stock_marker(self):
basis = (2.0, 0.1, 0.12)
with self.assertRaises(ValueError):
interpret_legacy({"SteerLatAccel": b"2.1", "SteerLatAccelStock": b"bad"}, basis)
old_stock = interpret_legacy({"SteerLatAccel": b"2.2", "SteerLatAccelStock": b"2.2"}, basis)
self.assertEqual((old_stock.factor_mode, old_stock.factor, old_stock.factor_saved),
(LegacyMode.STOCK, None, 2.2))
openpilot/starpilot/lateral/tests/test_torque_tuning.py
+128
View File
@@ -0,0 +1,128 @@
"""Explicit torque values exercised against the current native controller."""
import math
import unittest
from openpilot.cereal import log
from opendbc.car.structs import car
from opendbc.car.car_helpers import interfaces
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.torque_tuning import TorqueSource, TorqueTuning, from_record
PLATFORM = TOYOTA.TOYOTA_RAV4_TSS2
def native_controller():
interface = interfaces[PLATFORM]
cp = interface.get_non_essential_params(PLATFORM)
controller = LatControlTorque(cp.as_reader(), interface(cp), DT_CTRL)
return cp, controller, VehicleModel(cp)
def record(cp, source="vehicle"):
tune = cp.lateralTuning.torque
return {"source": source, "vehicle": cp.carFingerprint, "latAccelFactor": tune.latAccelFactor,
"latAccelOffset": tune.latAccelOffset, "friction": tune.friction}
def frame(controller, vm, *, active=True, curvature=0.0, speed=20.0, steering_angle=0.0,
steer_limited=False, steering_pressed=False):
cs = car.CarState.new_message()
cs.vEgo = speed
cs.steeringAngleDeg = steering_angle
cs.steeringPressed = steering_pressed
return controller.update(active, cs, vm, log.VehicleParameters.new_message(),
steer_limited, curvature, False, 0.2)
class TestTorqueTuning(unittest.TestCase):
def test_stock_values_leave_native_limits_unchanged(self):
cp, controller, _ = native_controller()
stock = from_record(record(cp))
self.assertIs(stock.source, TorqueSource.VEHICLE)
self.assertEqual(stock.vehicle, cp.carFingerprint)
for selected, actual in zip(stock.upstream_update(), (controller.torque_params.latAccelFactor,
controller.torque_params.latAccelOffset,
controller.torque_params.friction), strict=True):
self.assertAlmostEqual(selected, actual)
original_limits = controller.pid.neg_limit, controller.pid.pos_limit
controller.update_torque_parameters(*stock.upstream_update())
for selected, actual in zip((controller.pid.neg_limit, controller.pid.pos_limit), original_limits, strict=True):
self.assertAlmostEqual(selected, actual)
def test_selected_complete_values_change_native_parameters_and_limits(self):
cp, controller, _ = native_controller()
selected = record(cp, "user")
selected["latAccelFactor"] *= 1.2
selected["latAccelOffset"] = 0.07
selected["friction"] += 0.02
tuning = from_record(selected)
self.assertIs(tuning.source, TorqueSource.USER)
initial_limit = controller.pid.pos_limit
controller.update_torque_parameters(*tuning.upstream_update())
self.assertAlmostEqual(controller.pid.pos_limit, initial_limit * 1.2, delta=1e-6)
self.assertAlmostEqual(controller.pid.neg_limit, -controller.pid.pos_limit)
self.assertAlmostEqual(controller.torque_params.latAccelOffset, 0.07)
self.assertAlmostEqual(controller.torque_params.friction, selected["friction"])
def test_inactive_nonzero_command_and_reengagement_use_native_buffer(self):
cp, controller, vm = native_controller()
learned = record(cp, "learned")
learned["latAccelFactor"] *= 1.05
controller.update_torque_parameters(*from_record(learned).upstream_update())
for _ in range(controller.lat_accel_request_buffer_len):
output, _, state = frame(controller, vm, active=False, curvature=0.001)
self.assertEqual(output, 0.0)
self.assertFalse(state.active)
self.assertTrue(all(math.isclose(value, 0.4, abs_tol=1e-6) for value in controller.lat_accel_request_buffer))
output, _, state = frame(controller, vm, curvature=0.001)
self.assertTrue(state.active)
self.assertAlmostEqual(state.desiredLateralAccel, 0.4)
self.assertTrue(math.isfinite(output))
self.assertNotEqual(output, 0.0)
def test_native_saturation_sets_then_clears_under_bounded_inputs(self):
_, controller, vm = native_controller()
for _ in range(100):
output, _, state = frame(controller, vm, curvature=0.02)
self.assertLessEqual(abs(output), controller.steer_max + 1e-6)
self.assertTrue(state.saturated)
self.assertAlmostEqual(controller.sat_time, controller.sat_limit)
for _ in range(controller.lat_accel_request_buffer_len + 100):
output, _, state = frame(controller, vm, curvature=0.0)
self.assertLessEqual(abs(output), controller.steer_max + 1e-6)
self.assertFalse(state.saturated)
self.assertAlmostEqual(controller.sat_time, 0.0)
def test_invalid_numeric_inputs_are_rejected(self):
values = [("latAccelFactor", 0), ("latAccelFactor", -1), ("latAccelFactor", float("nan")),
("latAccelFactor", True), ("latAccelOffset", float("inf")), ("latAccelOffset", False),
("friction", -0.01), ("friction", float("nan")), ("friction", True), ("latAccelFactor", 10 ** 1000)]
for field, value in values:
with self.subTest(field=field, value=value):
source = {"source": "vehicle", "vehicle": "test-platform", "latAccelFactor": 2.0,
"latAccelOffset": 0.0, "friction": 0.1}
source[field] = value
with self.assertRaises(ValueError):
from_record(source)
def test_incomplete_unknown_and_legacy_fields_are_rejected(self):
source = {"source": "vehicle", "vehicle": "test-platform", "latAccelFactor": 2.0,
"latAccelOffset": 0.0, "friction": 0.1}
for extra in ("kp", "ki", "kd", "kf", "steeringAngleDeadzoneDeg", "unknown"):
with self.subTest(extra=extra), self.assertRaises(ValueError):
from_record(source | {extra: 1.0})
with self.assertRaises(ValueError):
from_record({key: value for key, value in source.items() if key != "friction"})
with self.assertRaises(ValueError):
from_record(source | {"source": "unrecognized"})
with self.assertRaises(ValueError):
TorqueTuning(TorqueSource.VEHICLE, "", 2.0, 0.0, 0.1)
if __name__ == "__main__":
unittest.main()
openpilot/starpilot/lateral/tests/test_turn_lane_change_smoothing.py
+89
View File
@@ -0,0 +1,89 @@
import json
from pathlib import Path
from openpilot.cereal import log
from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from openpilot.starpilot.lateral.lane_change_smoothing import LaneChangeSmoother, limit_rate
def factor(smoother, speed, state, previous=0., desired=.01, assist=True):
return smoother.factor(active=True, lane_change_state=state, speed=speed, minimum_speed=0., duration=5.777777777777778,
previous=previous, desired=desired, turn_assist=assist)
def test_turn_assist_releases_intersection_curve_and_prior_lane_change_tail():
smoother = LaneChangeSmoother()
assert factor(smoother, 20., log.LaneChangeState.laneChangeStarting) < 1.
for speed in (0., .8, 2., 4., 8.2, LANE_CHANGE_SPEED_MIN - 1e-6):
assert factor(smoother, speed, log.LaneChangeState.laneChangeStarting) == 1.
assert smoother.release == 0.
assert smoother.entry_sign == 0.
assert smoother.previous_factor == 1.
# An intersection stays exempt until the model ends that lane-change state.
assert factor(smoother, LANE_CHANGE_SPEED_MIN, log.LaneChangeState.laneChangeStarting) == 1.
assert factor(smoother, LANE_CHANGE_SPEED_MIN, log.LaneChangeState.off) == 1.
assert factor(smoother, LANE_CHANGE_SPEED_MIN, log.LaneChangeState.laneChangeStarting) < 1.
def test_manual_and_highway_lane_change_shaping_stays_exact():
for speed, assist in ((1., False), (8.2, False), (LANE_CHANGE_SPEED_MIN, True), (30., True)):
baseline, candidate = LaneChangeSmoother(), LaneChangeSmoother()
previous = 0.
for tick in range(700):
state = log.LaneChangeState.laneChangeStarting if tick < 400 else log.LaneChangeState.off
desired = .01 if tick < 250 else -.01
expected = factor(baseline, speed, state, previous, desired, False)
actual = factor(candidate, speed, state, previous, desired, assist)
assert actual == expected
previous, _ = clip_curvature(speed, previous, limit_rate(speed, previous, desired, actual), 0.)
def test_recorded_intersection_requests_retain_native_curvature_envelope():
recorded = json.loads((Path(__file__).parent / 'fixtures/oct2_turns.json').read_text())
for sequence in recorded['turns']:
smoother = LaneChangeSmoother()
previous = sequence[0][3]
exercised = 0
for _, speed, desired, _, state_name in sequence:
state = getattr(log.LaneChangeState, state_name)
shaping = factor(smoother, speed, state, previous, desired)
limited = limit_rate(speed, previous, desired, shaping)
output, _ = clip_curvature(speed, previous, limited, 0.)
native, _ = clip_curvature(speed, previous, desired, 0.)
if speed < LANE_CHANGE_SPEED_MIN:
assert shaping == 1.
assert output == native
exercised += 1
assert abs(output) <= .2
assert abs(output) <= 3. / max(speed, 1.) ** 2 + 1e-12
previous = output
assert exercised > 500
def test_recorded_turn_onset_and_exit_are_not_delayed_by_lane_change_pacing():
recorded = json.loads((Path(__file__).parent / 'fixtures/oct2_turns.json').read_text())
results = []
# The recorded sustained curvature-rate factor is 0.0863065, matching this
# supported duration. This is a fixed-input response, not a vehicle simulation.
duration = 6.888888888888889
for sequence in recorded['turns']:
tracks = []
for assist in (False, True):
smoother = LaneChangeSmoother()
previous = sequence[0][3]
trace = []
for time, speed, desired, _, state_name in sequence:
shaping = smoother.factor(active=True, lane_change_state=getattr(log.LaneChangeState, state_name),
speed=speed, minimum_speed=0., duration=duration,
previous=previous, desired=desired, turn_assist=assist)
previous, _ = clip_curvature(speed, previous, limit_rate(speed, previous, desired, shaping), 0.)
trace.append((time, previous))
onset = next(time for time, curvature in trace if curvature < -.02)
peak = min(range(len(trace)), key=lambda index: trace[index][1])
exit_time = next(time for time, curvature in trace[peak:] if abs(curvature) < .01)
tracks.append((onset, exit_time))
results.append(tracks)
# First bookmark is the unwind complaint; second is late turn initiation.
assert results[0][0][1] - results[0][1][1] > 1.
assert results[1][0][0] - results[1][1][0] > 2.
openpilot/starpilot/lateral/tests/test_turn_model_runtime.py
+88
View File
@@ -0,0 +1,88 @@
"""Saved assist reaches the model/torque path only with existing rolling authority."""
import json
import os
import unittest
from unittest.mock import patch
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.controls.controlsd import Controls
from openpilot.starpilot.lateral.tests.test_lane_runtime import feed, ioniq_candidate
from openpilot.starpilot.lateral.controller_selection import ControllerMode, DOCUMENT_KEY, replace_mode
def rolling_feed(controls, timestamp, tick, *, speed=.5, standstill=False, fault=False, active=True, gear='drive'):
captured = []
with patch.object(controls.sm, 'update_msgs', side_effect=lambda _time, events: captured.extend(events)):
feed(controls, timestamp, tick, fault=fault, active=active, enabled=active, signal=True)
events = [event.as_builder() for event in captured]
cs = next(event.carState for event in events if event.which() == 'carState')
cs.vEgo = cs.vEgoRaw = speed
cs.standstill = standstill
cs.gearShifter = gear
cs.aEgo = -.1
cs.leftBlinker = False
cs.rightBlinker = True
if tick % 5 == 0:
md = next(event.modelV2 for event in events if event.which() == 'modelV2')
md.position.x = [float(x) for x in range(51)]
md.position.y = [.1 * x * x for x in range(51)]
md.action.desiredCurvature = .0001
controls.sm.update_msgs(timestamp / 1e9, [event.as_reader() for event in events])
class TestTurnModelRuntime(unittest.TestCase):
def construct(self, enabled, mode=ControllerMode.STARPILOT):
cp, _ = ioniq_candidate()
params = Params()
params.put('CarParams', cp.to_bytes(), block=True)
params.put_bool('TurnAssist', enabled, block=True)
params.put_bool('OpenpilotEnabledToggle', True, block=True)
params.put(DOCUMENT_KEY, json.loads(replace_mode(None, cp, mode)), block=True)
return Controls()
def test_off_and_standard_preserve_existing_commands(self):
for mode in (ControllerMode.STARPILOT, ControllerMode.STANDARD):
with self.subTest(mode=mode), OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}):
selected = self.construct(False, mode)
reference = self.construct(mode == ControllerMode.STANDARD, mode)
for tick in range(20):
now = 1_000_000_000 + tick * 10_000_000
rolling_feed(selected, now, tick)
rolling_feed(reference, now, tick)
actual, _ = selected.state_control()
expected, _ = reference.state_control()
self.assertEqual(actual.to_bytes(), expected.to_bytes())
self.assertEqual(selected.model_turn_assist.turn_hold_curvature, 0.)
def test_model_bias_reaches_actual_curvature_and_torque_limits(self):
with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}):
controls = self.construct(True)
reference = self.construct(False)
before = controls.LaC.torque_params.to_dict()
maximum_hold = maximum_difference = 0.
with patch.object(controls.model_turn_assist, 'update', wraps=controls.model_turn_assist.update) as update:
for tick in range(120):
rolling_feed(controls, 1_000_000_000 + tick * 10_000_000, tick)
rolling_feed(reference, 1_000_000_000 + tick * 10_000_000, tick)
command, _ = controls.state_control()
reference.state_control()
maximum_hold = max(maximum_hold, abs(controls.model_turn_assist.turn_hold_curvature))
maximum_difference = max(maximum_difference, abs(controls.desired_curvature - reference.desired_curvature))
self.assertLessEqual(abs(command.actuators.torque), 1.)
self.assertEqual(update.call_count, 120)
self.assertGreater(maximum_hold, 0.)
self.assertGreater(maximum_difference, 0.)
self.assertEqual(controls.LaC.torque_params.to_dict(), before)
def test_standstill_speed_fault_reverse_and_inactive_deny_assist(self):
for gate in ({'speed': 0.}, {'speed': .044}, {'standstill': True}, {'fault': True}, {'gear': 'reverse'}, {'active': False}):
with self.subTest(gate=gate), OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}):
controls = self.construct(True)
with patch.object(controls.model_turn_assist, 'update', wraps=controls.model_turn_assist.update) as update:
for tick in range(10):
rolling_feed(controls, 1_000_000_000 + tick * 10_000_000, tick, **gate)
controls.state_control()
update.assert_not_called()
self.assertEqual(controls.model_turn_assist.turn_hold_curvature, 0.)
openpilot/starpilot/lateral/tests/testdata/README.md
Vendored
+8
View File
@@ -0,0 +1,8 @@
# Lateral controller regression vectors
These source-derived vectors are inputs to `test_ioniq6_policy.py`.
- `ioniq6_frozen_2023.json` and `ioniq6_frozen_2025.json` cover the two Ioniq 6 controller variants through stop, crawl, pull-away, highway, steering override, and restart. They include controller output, requested lateral acceleration and jerk, PID terms, and feedforward. Their `helpers` entries cover the corresponding shaping functions.
- `non_ioniq_parent.json` checks that Corolla TSS2 and Ioniq 5 controller output remains unchanged by the Ioniq-specific policy.
The tests use matching vehicle parameters for each comparison. These vectors check software behavior; they do not establish physical steering performance.
openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2023.json
Vendored
+208
View File
@@ -0,0 +1,208 @@
{
"profile": "2023",
"factor": 3.6600000858306885,
"rows": [
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.10946385067439653,
-6.3100724219111726e-06,
-6.310072058113292e-05,
0.1341981738805771,
1.8787743556458736e-06,
0.00032710772939026356
],
[
-0.10883246512145582,
-1.2177733879070729e-05,
-0.00012177733879070729,
0.1318926364183426,
3.725271199073177e-06,
0.00031992016010917723
],
[
-0.10824537411387734,
-1.7634001778787933e-05,
-0.00017634002142585814,
0.12974874675273895,
5.541753580473596e-06,
0.00031323812436312437
],
[
-0.10769947128273318,
-2.2707719836034812e-05,
-0.00022707720927428454,
0.12775516510009766,
7.330325843213359e-06,
0.00030702617368660867
],
[
-0.14305795162332793,
-0.00030156326829455793,
-0.00301563274115324,
0.6839030981063843,
4.415587682160549e-05,
-0.22402147948741913
],
[
-0.1359612868675231,
-0.0005608677165582776,
-0.00560867739841342,
0.6581742167472839,
7.959602953633294e-05,
-0.22430188953876495
],
[
-0.12936356042287359,
-0.000801991845946759,
-0.008019918575882912,
0.6342493295669556,
0.00011374791210982949,
-0.22455888986587524
],
[
-0.1232297452015651,
-0.0010262103751301765,
-0.010262103751301765,
0.6120017766952515,
0.00014670185919385403,
-0.2247941493988037
],
[
0.18898263724481582,
-0.3834262788295746,
0.16573716700077057,
-0.363513708114624,
-0.0006830576458014548,
-0.3340778648853302
],
[
0.17043036934689756,
-0.3670603334903717,
0.3293967843055725,
-0.3358006775379181,
-0.0014495592331513762,
-0.29714497923851013
],
[
0.15329577144049716,
-0.3518418073654175,
0.4815819263458252,
-0.31003066897392273,
-0.002157237846404314,
-0.26136893033981323
],
[
0.13885098210221994,
-0.3376902937889099,
0.6230970621109009,
-0.2860674262046814,
-0.0028102179057896137,
-0.23191167414188385
],
[
0.18527992219918452,
-0.3245309591293335,
0.7546903491020203,
-0.2637842893600464,
-0.0028102179057896137,
-0.42906454205513
],
[
0.17206466577559282,
-0.31229427456855774,
0.8770573139190674,
-0.24306349456310272,
-0.0028102179057896137,
-0.4004751741886139
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.18071963917045453,
-0.008999999612569809,
0.0,
0.8138636350631714,
9.495075937593356e-05,
-0.2816694974899292
],
[
-0.18078095032621577,
-0.008999999612569809,
0.0,
0.8138636350631714,
0.00018990151875186712,
-0.28154006600379944
],
[
-0.31736123368584657,
0.0018796906806528568,
-0.008203093893826008,
1.3015185594558716,
0.0003417453553993255,
-0.2694607377052307
],
[
-0.3078279365475176,
0.0011168947676196694,
-0.015831053256988525,
1.267328143119812,
0.0004896002938039601,
-0.27031010389328003
]
],
"helpers": [
[
1.152007017756061,
0.9895813688219441,
0.37665466606411585,
0.9995785592537053,
-0.48882890713177274,
0.999949749695639
],
[
0.7143736867598727,
0.705142017608503,
0.3198,
0.9980951598655177,
0.2,
0.9989500339924398
],
[
0.9990463679058011,
0.9936653590982446,
0.38946576694327467,
0.8866936494147638,
-0.1,
0.7281708549099692
]
]
}
openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2025.json
Vendored
+208
View File
@@ -0,0 +1,208 @@
{
"profile": "2025",
"factor": 3.6600000858306885,
"rows": [
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.1000419596173212,
-6.3100724219111726e-06,
-6.310072058113292e-05,
0.09995962679386139,
1.399434836457658e-06,
0.00010129198199138045
],
[
-0.09957267603318784,
-1.2177733879070729e-05,
-0.00012177733879070729,
0.09824388474225998,
2.774849235720467e-06,
9.798955579753965e-05
],
[
-0.09913630335274988,
-1.7634001778787933e-05,
-0.00017634002142585814,
0.09664839506149292,
4.127926786168246e-06,
9.492012031842023e-05
],
[
-0.0987305336073542,
-2.2707719836034812e-05,
-0.00022707720927428454,
0.0951647162437439,
5.460232841869583e-06,
9.206731920130551e-05
],
[
-0.09746098608574076,
-0.00030156326829455793,
-0.00301563274115324,
0.521321177482605,
3.353137071826495e-05,
-0.22827710211277008
],
[
-0.09216927405030517,
-0.0005608677165582776,
-0.00560867739841342,
0.5020400881767273,
6.0564299928955734e-05,
-0.22839277982711792
],
[
-0.08724454159432636,
-0.000801991845946759,
-0.008019918575882912,
0.4840916097164154,
8.663076732773334e-05,
-0.22849681973457336
],
[
-0.08266166023362777,
-0.0010262103751301765,
-0.010262103751301765,
0.46738457679748535,
0.0001117976353270933,
-0.22859011590480804
],
[
0.17274071128924318,
-0.3834262788295746,
0.16573716700077057,
-0.3635135591030121,
-0.000717961578629911,
-0.30158889293670654
],
[
0.15695589094557277,
-0.3670603334903717,
0.3293967843055725,
-0.3358005881309509,
-0.0014844628749415278,
-0.2747289836406708
],
[
0.14488035756024556,
-0.3518418073654175,
0.4815819263458252,
-0.31003063917160034,
-0.0021921414881944656,
-0.2578660845756531
],
[
0.13203018780276538,
-0.3376902937889099,
0.6230970621109009,
-0.286067396402359,
-0.0028451215475797653,
-0.2338174432516098
],
[
0.17666741388048487,
-0.3245309591293335,
0.7546903491020203,
-0.2637842893600464,
-0.0028451215475797653,
-0.4359583258628845
],
[
0.16482470244305603,
-0.31229427456855774,
0.8770573139190674,
-0.24306349456310272,
-0.0028451215475797653,
-0.4119570255279541
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.13034704739944014,
-0.008999999612569809,
0.0,
0.639947772026062,
7.466057286364958e-05,
-0.2920238971710205
],
[
-0.1304028061718463,
-0.008999999612569809,
0.0,
0.639947772026062,
0.00014932114572729915,
-0.29189446568489075
],
[
-0.23666653614857244,
0.0018796906806528568,
-0.008203093893826008,
1.0229543447494507,
0.0002686658117454499,
-0.28603285551071167
],
[
-0.22962545662414624,
0.0011168947676196694,
-0.015831053256988525,
0.9974353313446045,
0.0003850332577712834,
-0.2864045202732086
]
],
"helpers": [
[
1.152007017756061,
0.9895813688219441,
0.37665466606411585,
0.9995785592537053,
-0.48882890713177274,
0.999949749695639
],
[
0.7143736867598727,
0.705142017608503,
0.3198,
0.9980951598655177,
0.2,
0.9989500339924398
],
[
0.9990463679058011,
0.9936653590982446,
0.38946576694327467,
0.8866936494147638,
-0.1,
0.7281708549099692
]
]
}
openpilot/starpilot/lateral/tests/testdata/non_ioniq_parent.json
Vendored
+198
View File
@@ -0,0 +1,198 @@
{
"TOYOTA_COROLLA_TSS2": [
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.1741040538961501,
0.0,
-0.0125821428373456,
0.5361478328704834,
0.0,
-0.18948359787464142
],
[
-0.1743630122313323,
0.0,
-0.011699984781444073,
0.5361478328704834,
0.0,
-0.1889679729938507
],
[
-0.1746038145029732,
0.0,
-0.010879676789045334,
0.5361478328704834,
0.0,
-0.18848849833011627
],
[
0.21482844409667973,
0.0,
0.31239789724349976,
-0.2552758753299713,
-0.0001914569002110511,
-0.1722845435142517
],
[
0.12667966987594909,
0.0,
0.6130098700523376,
-0.2552758753299713,
-0.0003829138004221022,
0.0034227967262268066
],
[
0.13939243693119238,
0.0,
0.5700305700302124,
-0.2552758753299713,
-0.0005743707297369838,
-0.021698547527194023
],
[
0.5066447263279833,
0.0,
0.4336607754230499,
-0.4848564863204956,
-0.0005743707297369838,
-0.52336585521698
],
[
0.5066447263279833,
0.0,
0.3068521022796631,
-0.4848564863204956,
-0.0005743707297369838,
-0.52336585521698
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"HYUNDAI_IONIQ_5": [
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.09367755417862601,
0.0,
-0.0125821428373456,
0.4897056818008423,
0.0,
-0.19247344136238098
],
[
-0.09380461008165825,
0.0,
-0.011699984781444073,
0.4897056818008423,
0.0,
-0.19207030534744263
],
[
-0.09392275785262694,
0.0,
-0.010879676789045334,
0.4897056818008423,
0.0,
-0.191695436835289
],
[
0.11027891320132133,
0.0,
0.31239789724349976,
-0.21928606927394867,
-0.00016446455265395343,
-0.13045662641525269
],
[
0.06703405539612153,
0.0,
0.6130098700523376,
-0.21928606927394867,
-0.00032892910530790687,
0.006920696701854467
],
[
0.07327613223245147,
0.0,
0.5700305700302124,
-0.21928606927394867,
-0.0004933936288580298,
-0.012720505706965923
],
[
0.29068416058013297,
0.0,
0.4336607754230499,
-0.4834653437137604,
-0.0004933936288580298,
-0.4383614659309387
],
[
0.29068416058013297,
0.0,
0.3068521022796631,
-0.4834653437137604,
-0.0004933936288580298,
-0.4383614659309387
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
-0.0,
0.0,
0.0,
0.0,
0.0,
0.0
]
]
}
openpilot/starpilot/lateral/torque_extension.py
+102
View File
@@ -0,0 +1,102 @@
"""Selected StarPilot torque behavior outside the upstream controller."""
from openpilot.starpilot.lateral.controller_selection import ControllerMode
class TorqueExtension:
def __init__(self, policy, *, parameter_factor=None):
self.policy = policy
self.parameter_factor = parameter_factor
self.ratio_scale = getattr(policy, 'steer_ratio_scale', None)
def transform_torque_parameters(self, factor, offset, friction):
if self.parameter_factor is not None:
factor *= self.parameter_factor
return factor, offset, friction
def vehicle_model_ratio(self, ratio, speed):
return ratio if self.ratio_scale is None else ratio * self.ratio_scale(speed)
def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay):
return self.policy.update(active, cs, vm, params, safety_limited, curvature, curvature_limited, delay)
def create_extension(parent, cp, mode, selected, *, turn_assist=False):
if mode != ControllerMode.STARPILOT:
return None
from openpilot.starpilot.lateral.hkg_vehicle_policy import HKGVehicleTorquePolicy, PROFILES, policy_for
if selected in PROFILES:
if policy_for(cp) != selected:
raise ValueError('HKG torque selection does not match CarParams')
policy = HKGVehicleTorquePolicy(parent, cp)
return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT)
if selected == 'ioniq6':
from openpilot.starpilot.lateral.ioniq6_policy import Ioniq6TorquePolicy
policy = Ioniq6TorquePolicy(parent, cp, turn_assist=turn_assist)
return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT)
if selected == 'genesis_gv70_electrified':
from openpilot.starpilot.lateral.genesis_gv70_policy import GenesisGV70TorquePolicy, supported_cp
if supported_cp(cp):
return TorqueExtension(GenesisGV70TorquePolicy(parent, cp))
if selected == 'genesis_g70_2020':
from openpilot.starpilot.lateral.genesis_g70_policy import GenesisG70TorquePolicy, supported_cp
if supported_cp(cp):
return TorqueExtension(GenesisG70TorquePolicy(parent, cp))
if selected == 'rav4_tss2':
from openpilot.starpilot.lateral.rav4_tss2_policy import Rav4TSS2TorquePolicy, supported_cp
if supported_cp(cp):
return TorqueExtension(Rav4TSS2TorquePolicy(parent, cp))
if selected == 'corolla_tss2':
from openpilot.starpilot.lateral.corolla_tss2_policy import CorollaTSS2TorquePolicy, supported_cp
if supported_cp(cp):
return TorqueExtension(CorollaTSS2TorquePolicy(parent, cp))
if selected in ('ordinary_camera', 'silverado_cc'):
from openpilot.starpilot.lateral.camera_policy import CameraTorquePolicy
return TorqueExtension(CameraTorquePolicy(parent, cp))
if selected == 'suburban':
from openpilot.starpilot.lateral.suburban_policy import SuburbanTorquePolicy
return TorqueExtension(SuburbanTorquePolicy(parent, cp))
if selected == 'ordinary_cc':
from openpilot.starpilot.lateral.ordinary_cc_policy import OrdinaryCcTorquePolicy
return TorqueExtension(OrdinaryCcTorquePolicy(parent, cp))
if selected == 'ordinary_sdgm':
from openpilot.starpilot.lateral.sdgm_policy import SdgmTorquePolicy
return TorqueExtension(SdgmTorquePolicy(parent, cp))
if selected == 'ordinary_ascm':
from openpilot.starpilot.lateral.ascm_policy import AscmTorquePolicy
return TorqueExtension(AscmTorquePolicy(parent, cp))
if selected == 'sonata_hybrid':
from openpilot.starpilot.lateral.sonata_hybrid_policy import SonataHybridTorquePolicy
policy = SonataHybridTorquePolicy(parent, cp)
return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT)
if selected == 'palisade':
from openpilot.starpilot.lateral.palisade_policy import PalisadeTorquePolicy
policy = PalisadeTorquePolicy(parent, cp)
return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT)
if selected == 'volt':
from openpilot.starpilot.lateral.volt_policy import VoltTorquePolicy
return TorqueExtension(VoltTorquePolicy(parent, cp))
if selected == 'bolt':
from openpilot.starpilot.lateral.bolt_policy import BoltTorquePolicy
return TorqueExtension(BoltTorquePolicy(parent, cp))
raise ValueError('No StarPilot torque policy for CarParams')
def selected_policy(controller):
extension = controller.starpilot_extension
return None if extension is None else extension.policy
def create_turn_assist(cp, enabled):
from openpilot.starpilot.lateral.controller_selection import turn_assist_supported
if not enabled or not turn_assist_supported(cp):
return None
from openpilot.starpilot.lateral.torque_turn_assist import TorqueTurnAssist
return TorqueTurnAssist(cp)
def apply_turn_assist(parent, cs, vm, params, curvature, output):
if parent.turn_assist is None:
return output
return parent.turn_assist.apply(cs, vm, params, curvature, output, parent.steer_max)
openpilot/starpilot/lateral/torque_runtime.py
+234
View File
@@ -0,0 +1,234 @@
"""Exact-platform torque source selection for native controlsd."""
from __future__ import annotations
from dataclasses import dataclass
import math
import os
from pathlib import Path
from opendbc.car import structs
from openpilot.cereal.services import SERVICE_LIST
from openpilot.starpilot.lateral.torque_tuning import TorqueSource, TorqueTuning
from openpilot.starpilot.lateral.torque_supported import BOLT_VEHICLES, GM_VEHICLES, IONIQ6_VEHICLES, TOYOTA_VEHICLES
from openpilot.starpilot.lateral.torque_settings import (
DOCUMENT_KEY, LEGACY_KEYS, MAX_DOCUMENT_BYTES, interpret_legacy, parse_document, resolve_document,
)
LEARNER_VERSION = 1
REFRESH_NS = 1_000_000_000
def _move(old: float, new: float, step: float) -> float:
return min(max(new, old - step), old + step)
GM_TORQUE_POLICIES = frozenset({'bolt', 'volt', 'ordinary_ascm', 'ordinary_sdgm',
'ordinary_cc', 'ordinary_camera', 'silverado_cc', 'suburban'})
def gm_manual_supported_cp(CP) -> bool:
"""Reuse exact finalized torque policies; no brand-wide or controller-mode admission."""
if (str(CP.carFingerprint) not in GM_VEHICLES or str(CP.brand) != 'gm' or
CP.notCar or CP.passive or CP.dashcamOnly or
CP.steerControlType != structs.CarParams.SteerControlType.torque or CP.lateralTuning.which() != 'torque'):
return False
from openpilot.starpilot.lateral.controller_selection import policy_for
if str(CP.carFingerprint) not in BOLT_VEHICLES:
from opendbc.car.gm.aol import qualified_gm
if not qualified_gm(CP):
return False
return policy_for(CP) in GM_TORQUE_POLICIES
def supported_cp(CP) -> bool:
fingerprint = str(CP.carFingerprint)
if fingerprint in BOLT_VEHICLES:
from openpilot.starpilot.lateral.bolt_policy import supported_cp as bolt_supported_cp
return bolt_supported_cp(CP)
platform = ((fingerprint in TOYOTA_VEHICLES and str(CP.brand) == 'toyota') or
(fingerprint in IONIQ6_VEHICLES and str(CP.brand) == 'hyundai'))
return (platform and
CP.steerControlType == structs.CarParams.SteerControlType.torque and not CP.dashcamOnly and
CP.lateralTuning.which() == 'torque' and
math.isfinite(float(CP.lateralTuning.torque.latAccelFactor)) and
float(CP.lateralTuning.torque.latAccelFactor) > 0 and
math.isfinite(float(CP.lateralTuning.torque.latAccelOffset)) and
math.isfinite(float(CP.lateralTuning.torque.friction)) and
float(CP.lateralTuning.torque.friction) >= 0)
def production_supported_cp(CP) -> bool:
"""Exact production consumer scope; replay qualification does not expand UI."""
return (supported_cp(CP) and str(CP.carFingerprint) in IONIQ6_VEHICLES and
not CP.passive and not CP.notCar)
def manual_overrides_present(CP, params) -> bool:
"""Numeric intent alone admits the custom host and invalidates stale learning."""
if params is None or not (production_supported_cp(CP) or os.getenv('TORQUE_REPLAY_RUNTIME') == '1' and supported_cp(CP)):
return False
settings = read_settings(params, TorqueHost(params, CP).vehicle)
return settings.valid and (settings.user_factor is not None or settings.user_friction is not None)
def runtime_enabled(CP, params=None) -> bool:
if not supported_cp(CP):
return False
if str(CP.carFingerprint) in BOLT_VEHICLES:
if params is None:
return False
settings = read_settings(params, TorqueHost(params, CP).vehicle)
return settings.advanced and settings.valid and settings.user_friction is not None
if os.getenv('TORQUE_REPLAY_RUNTIME') == '1':
return True
# Preserve native/default startup behavior. A first custom edit is saved for
# the next drive; a custom host already admitted can refresh/reset live.
return manual_overrides_present(CP, params)
development_enabled = runtime_enabled
@dataclass(frozen=True)
class TorqueSettings:
force_auto_off: bool = False
user_factor: float | None = None
user_friction: float | None = None
valid: bool = True
def _raw(params, key: str) -> bytes | None:
try:
with Path(params.get_param_path(key)).open('rb') as source:
return source.read(MAX_DOCUMENT_BYTES + 1) if key == DOCUMENT_KEY else source.read()
except FileNotFoundError:
return None
def _bool(raw: bytes | None, default: bool) -> bool:
if raw is None:
return default
if raw not in (b'0', b'1'):
raise ValueError('Malformed torque setting')
return raw == b'1'
def read_settings(params, vehicle: TorqueTuning, *, allow_learning: bool | None = None) -> TorqueSettings:
try:
force_off = _bool(_raw(params, 'ForceAutoTuneOff'), False) if allow_learning is None else not allow_learning
basis = (vehicle.lat_accel_factor, vehicle.lat_accel_offset, vehicle.friction)
document = _raw(params, DOCUMENT_KEY)
if vehicle.vehicle in BOLT_VEHICLES and document is None:
return TorqueSettings(valid=False)
if document is None:
legacy = interpret_legacy({key: _raw(params, key) for key in LEGACY_KEYS}, basis)
factor, friction = legacy.factor, legacy.friction
else:
factor, friction, review = resolve_document(parse_document(document), vehicle.vehicle, basis)
if review:
return TorqueSettings(valid=False)
return TorqueSettings(force_off, factor, friction)
except (OSError, ValueError, OverflowError):
return TorqueSettings(valid=False)
class TorqueHost:
"""Choose one complete source and rate-bound changes to the native controller."""
def __init__(self, params, CP, *, allow_learning: bool | None = None):
if not supported_cp(CP):
raise ValueError('Unsupported torque host')
tune = CP.lateralTuning.torque
self.vehicle = TorqueTuning(TorqueSource.VEHICLE, str(CP.carFingerprint),
float(tune.latAccelFactor), float(tune.latAccelOffset), float(tune.friction))
self.params = params
# An exact controller supplies its startup decision. None preserves the
# existing live manual preference for other optional replay-host vehicles.
self.allow_learning = allow_learning
self.settings = TorqueSettings()
self.last_refresh_ns: int | None = None
self.last_tick_ns: int | None = None
self.applied = self.vehicle
self.selected = self.vehicle
self.applied_controller = None
self.last_application = self.vehicle
def apply(self, controller, tune: TorqueTuning) -> bool:
"""Apply a complete source transition, preserving an untouched vehicle tune.
This host exclusively owns parameter updates on its controller. Reapplying
the initial vehicle values is not a harmless refresh: the Ioniq controller
intentionally retains its original startup PID limits until an actual
learned/custom update. Source identity matters even when numbers are equal.
"""
if not isinstance(tune, TorqueTuning) or tune.vehicle != self.vehicle.vehicle:
raise ValueError('Torque application does not match vehicle')
if controller is not self.applied_controller:
self.applied_controller = controller
self.last_application = self.vehicle
if tune == self.last_application:
return False
controller.update_torque_parameters(*tune.upstream_update())
self.last_application = tune
return True
def _learned(self, sm, now_ns: int) -> TorqueTuning | None:
name = 'lateralTorqueParameters'
stamp = int(sm.logMonoTime[name])
if not sm.all_checks([name]) or stamp <= 0 or not 0 <= now_ns - stamp <= int(2e9 / SERVICE_LIST[name].frequency):
return None
state = sm[name]
if not state.useParams or not state.valid or int(state.version) != LEARNER_VERSION:
return None
factor = float(state.latAccelFactorFiltered)
offset = float(state.latAccelOffsetFiltered)
friction = float(state.frictionCoefficientFiltered)
vehicle = self.vehicle
if (not all(math.isfinite(v) for v in (factor, offset, friction)) or
not 0.7 * vehicle.lat_accel_factor <= factor <= 1.3 * vehicle.lat_accel_factor or
abs(offset) > min(0.5, 0.5 * vehicle.lat_accel_factor) or
not 0.5 * vehicle.friction <= friction <= max(0.1, 1.5 * vehicle.friction)):
return None
return TorqueTuning(TorqueSource.LEARNED, vehicle.vehicle, factor, offset, friction)
def _select(self, sm, now_ns: int) -> TorqueTuning:
vehicle = self.vehicle
settings = self.settings
if not settings.valid:
return vehicle
allow_learning = not settings.force_auto_off if self.allow_learning is None else self.allow_learning
learned = self._learned(sm, now_ns) if allow_learning else None
base = learned or vehicle
if settings.user_factor is None and settings.user_friction is None:
return base
return TorqueTuning(TorqueSource.USER, vehicle.vehicle,
settings.user_factor if settings.user_factor is not None else base.lat_accel_factor,
vehicle.lat_accel_offset if settings.user_factor is not None else base.lat_accel_offset,
settings.user_friction if settings.user_friction is not None else base.friction)
def sample(self, sm, *, now_ns: int, lat_active: bool) -> TorqueTuning:
if now_ns <= 0 or (self.last_tick_ns is not None and not 0 < now_ns - self.last_tick_ns <= 50_000_000):
self.applied = self.vehicle
self.last_refresh_ns = None
self.last_tick_ns = now_ns
self.selected = self.vehicle
return self.vehicle
dt = 0.01 if self.last_tick_ns is None else (now_ns - self.last_tick_ns) / 1e9
self.last_tick_ns = now_ns
if self.last_refresh_ns is None or now_ns - self.last_refresh_ns >= REFRESH_NS:
self.settings = read_settings(self.params, self.vehicle, allow_learning=self.allow_learning)
self.last_refresh_ns = now_ns
if not lat_active or not self.settings.valid:
self.applied = self.selected = self.vehicle
return self.vehicle
self.selected = self._select(sm, now_ns)
prior, target = self.applied, self.selected
factor_step = 0.5 * self.vehicle.lat_accel_factor * dt
offset_step = 0.1 * dt
friction_step = max(0.05, self.vehicle.friction) * dt
self.applied = TorqueTuning(target.source, target.vehicle,
_move(prior.lat_accel_factor, target.lat_accel_factor, factor_step),
_move(prior.lat_accel_offset, target.lat_accel_offset, offset_step),
_move(prior.friction, target.friction, friction_step))
return self.applied
openpilot/starpilot/lateral/torque_settings.py
+285
View File
@@ -0,0 +1,285 @@
"""Strict saved Torque policy shared by the optional host and native settings UI."""
from __future__ import annotations
from dataclasses import dataclass, replace
from enum import StrEnum
import json
import math
from typing import cast
from openpilot.starpilot.lateral.torque_supported import BOLT_VEHICLES, SUPPORTED_VEHICLES
DOCUMENT_KEY = "TorqueOverrideDocument"
LEGACY_KEYS = ("SteerLatAccel", "SteerFriction", "SteerLatAccelStock", "SteerFrictionStock")
MAX_DOCUMENT_BYTES = 16_384
@dataclass(frozen=True)
class FieldChoice:
mode: str = "source"
custom_value: float | None = None
@dataclass(frozen=True)
class GainBasis:
controller: str
source_table: tuple[tuple[float, ...], tuple[float, ...]]
torque_basis: tuple[float, float, float]
@dataclass(frozen=True)
class GeometryProfile:
basis: tuple[float, float]
ratio: FieldChoice = FieldChoice()
full_delay: FieldChoice = FieldChoice()
automatic_delay: bool = True
learning: str = "source"
@dataclass(frozen=True)
class PlatformProfile:
basis: tuple[float, float, float]
factor: FieldChoice
friction: FieldChoice
proportional_gain: FieldChoice = FieldChoice()
gain_basis: GainBasis | None = None
class LegacyMode(StrEnum):
ABSENT = "absent"
STOCK = "stock"
CUSTOM = "custom"
@dataclass(frozen=True)
class LegacyChoice:
factor: float | None
friction: float | None
factor_mode: LegacyMode
friction_mode: LegacyMode
factor_saved: float | None
friction_saved: float | None
def bounds(basis: tuple[float, float, float], field: str) -> tuple[float, float]:
factor, _, friction = basis
if field == "factor":
return 0.5 * factor, 1.5 * factor
if field == "friction":
return 0.0, min(1.0, max(0.1, 2.0 * friction))
raise ValueError("Unknown torque field")
def _finite_number(value: object) -> bool:
if type(value) not in (float, int):
return False
try:
return math.isfinite(cast(float | int, value))
except OverflowError:
return False
def valid_basis(basis: tuple[float, float, float]) -> bool:
return (len(basis) == 3 and all(_finite_number(v) for v in basis) and
basis[0] > 0 and basis[2] >= 0)
def _number(raw: bytes | None) -> float | None:
if raw is None:
return None
value = float(raw)
if not math.isfinite(value):
raise ValueError("Invalid torque number")
return value
def interpret_legacy(raw: dict[str, bytes | None], basis: tuple[float, float, float]) -> LegacyChoice:
"""Preserve the old stock-marker and two-decimal CP equality semantics."""
if not valid_basis(basis):
raise ValueError("Invalid torque basis")
values = {key: _number(raw.get(key)) for key in LEGACY_KEYS}
result: list[float | None] = []
modes: list[LegacyMode] = []
saved: list[float | None] = []
for key, marker_key, field, stock in ((LEGACY_KEYS[0], LEGACY_KEYS[2], "factor", basis[0]),
(LEGACY_KEYS[1], LEGACY_KEYS[3], "friction", basis[2])):
value, marker = values[key], values[marker_key]
saved.append(value)
if value is None:
result.append(None)
modes.append(LegacyMode.ABSENT)
elif round(value, 2) == round(stock, 2) or marker is not None and math.isclose(value, marker, abs_tol=1e-6):
result.append(None)
modes.append(LegacyMode.STOCK)
else:
low, high = bounds(basis, field)
if not low <= value <= high:
raise ValueError("Out-of-range torque setting")
result.append(value)
modes.append(LegacyMode.CUSTOM)
return LegacyChoice(result[0], result[1], modes[0], modes[1], saved[0], saved[1])
def empty_document() -> dict:
return {"schemaVersion": 1, "vehicles": {}}
def _pairs(items: list[tuple[str, object]]) -> dict:
result = {}
for key, value in items:
if key in result:
raise ValueError("Duplicate torque document member")
result[key] = value
return result
def _bad_constant(_value: str):
raise ValueError("Nonfinite torque document number")
def _choice(value: object) -> FieldChoice:
if not isinstance(value, dict) or set(value) != {"mode", "customValue"}:
raise ValueError("Malformed torque field")
mode, custom = value["mode"], value["customValue"]
if mode not in ("source", "custom") or (custom is not None and not _finite_number(custom)) or \
(mode == "custom" and custom is None):
raise ValueError("Malformed torque field")
return FieldChoice(mode, custom)
def parse_document(raw: bytes) -> dict[str, PlatformProfile]:
if len(raw) > MAX_DOCUMENT_BYTES:
raise ValueError("Oversized torque document")
try:
document = json.loads(raw.decode("utf-8"), object_pairs_hook=_pairs, parse_constant=_bad_constant)
except (RecursionError, OverflowError) as exc:
raise ValueError("Malformed torque document") from exc
if not isinstance(document, dict) or set(document) != {"schemaVersion", "vehicles"} or \
type(document["schemaVersion"]) is not int or document["schemaVersion"] not in (1, 2) or \
not isinstance(document["vehicles"], dict) or len(document["vehicles"]) > len(SUPPORTED_VEHICLES):
raise ValueError("Malformed torque document")
profiles: dict[str, PlatformProfile] = {}
for fingerprint, value in document["vehicles"].items():
expected = {"basis", "factor", "friction"}
if document["schemaVersion"] == 2 and isinstance(value, dict) and "proportionalGain" in value:
expected |= {"proportionalGain", "gainBasis"}
if fingerprint not in SUPPORTED_VEHICLES or not isinstance(value, dict) or set(value) != expected:
raise ValueError("Malformed torque profile")
basis_obj = value["basis"]
if not isinstance(basis_obj, dict) or set(basis_obj) != {"latAccelFactor", "latAccelOffset", "friction"}:
raise ValueError("Malformed torque basis")
basis = tuple(basis_obj[key] for key in ("latAccelFactor", "latAccelOffset", "friction"))
if not valid_basis(basis):
raise ValueError("Malformed torque basis")
gain = FieldChoice()
gain_basis = None
if "proportionalGain" in value:
if fingerprint not in BOLT_VEHICLES:
raise ValueError("Unsupported gain profile")
gain = _choice(value["proportionalGain"])
gain_basis = parse_gain_basis(value["gainBasis"])
if gain.mode == "custom" and (gain.custom_value is None or not 0.3 <= gain.custom_value <= 0.9):
raise ValueError("Out-of-range proportional gain")
profiles[fingerprint] = PlatformProfile(basis, _choice(value["factor"]), _choice(value["friction"]), gain, gain_basis)
return profiles
def serialize_document(profiles: dict[str, PlatformProfile]) -> bytes:
vehicles = {}
version = 2 if any(p.gain_basis is not None for p in profiles.values()) else 1
for fingerprint, profile in profiles.items():
vehicles[fingerprint] = {
"basis": dict(zip(("latAccelFactor", "latAccelOffset", "friction"), profile.basis, strict=True)),
"factor": {"mode": profile.factor.mode, "customValue": profile.factor.custom_value},
"friction": {"mode": profile.friction.mode, "customValue": profile.friction.custom_value},
}
if profile.gain_basis is None and profile.proportional_gain != FieldChoice():
raise ValueError("Gain choice requires a bound basis")
if profile.gain_basis is not None:
vehicles[fingerprint]["proportionalGain"] = {"mode": profile.proportional_gain.mode, "customValue": profile.proportional_gain.custom_value}
vehicles[fingerprint]["gainBasis"] = {"controller": profile.gain_basis.controller,
"sourceTable": profile.gain_basis.source_table,
"torqueBasis": profile.gain_basis.torque_basis}
raw = json.dumps({"schemaVersion": version, "vehicles": vehicles}, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()
parse_document(raw)
return raw
def resolve_document(profiles: dict[str, PlatformProfile], fingerprint: str,
basis: tuple[float, float, float]) -> tuple[float | None, float | None, bool]:
profile = profiles.get(fingerprint)
if profile is None:
return None, None, False
if (profile.factor.mode == "custom" or profile.friction.mode == "custom") and profile.basis != basis:
return None, None, True
if fingerprint in BOLT_VEHICLES and profile.factor.mode == "custom":
return None, None, True
result = []
for field, choice in (("factor", profile.factor), ("friction", profile.friction)):
if choice.mode == "custom":
low, high = bounds(basis, field)
if choice.custom_value is None or not low <= choice.custom_value <= high:
raise ValueError("Out-of-range torque profile")
result.append(None if round(choice.custom_value, 2) == round(basis[0 if field == "factor" else 2], 2) else choice.custom_value)
else:
result.append(None)
return result[0], result[1], False
def replace_field(profiles: dict[str, PlatformProfile], fingerprint: str, basis: tuple[float, float, float],
field: str, mode: str, value: float | None) -> dict[str, PlatformProfile]:
if fingerprint in BOLT_VEHICLES and field == "factor":
raise ValueError("Bolt factor editing is not supported")
if mode not in ("source", "custom") or field not in ("factor", "friction") or not valid_basis(basis):
raise ValueError("Invalid torque edit")
if value is not None and not _finite_number(value):
raise ValueError("Invalid torque edit")
if mode == "custom":
low, high = bounds(basis, field)
if value is None or not low <= value <= high:
raise ValueError("Out-of-range torque edit")
profile = profiles.get(fingerprint, PlatformProfile(basis, FieldChoice(), FieldChoice()))
if profile.basis != basis and (profile.factor.mode == "custom" or profile.friction.mode == "custom"):
raise ValueError("Torque basis needs review")
current = profile.factor if field == "factor" else profile.friction
choice = FieldChoice(mode, current.custom_value if mode == "source" else value)
updated = replace(profile, basis=basis, factor=choice if field == "factor" else profile.factor,
friction=choice if field == "friction" else profile.friction)
result = dict(profiles)
result[fingerprint] = updated
return result
def parse_gain_basis(value: object) -> GainBasis:
if not isinstance(value, dict) or set(value) != {"controller", "sourceTable", "torqueBasis"}:
raise ValueError("Malformed gain basis")
table, basis = value["sourceTable"], value["torqueBasis"]
if (value["controller"] not in ("standard", "starpilot") or
not isinstance(table, (list, tuple)) or len(table) != 2 or
any(not isinstance(row, (list, tuple)) or not row for row in table) or
len(table[0]) != len(table[1]) or len(table[0]) > 32 or
any(not _finite_number(n) for row in table for n in row) or
any(a >= b for a, b in zip(table[0], table[0][1:], strict=False)) or
any(n <= 0 for n in table[1]) or
not isinstance(basis, (list, tuple)) or not valid_basis(tuple(basis))):
raise ValueError("Malformed gain basis")
return GainBasis(value["controller"], tuple(tuple(row) for row in table), tuple(basis))
def replace_gain(profiles: dict[str, PlatformProfile], fingerprint: str, basis: GainBasis,
mode: str, value: float | None, *, review: bool = False) -> dict[str, PlatformProfile]:
if fingerprint not in BOLT_VEHICLES or mode not in ("source", "custom"):
raise ValueError("Unsupported gain edit")
if value is not None and (not _finite_number(value) or not 0.3 <= value <= 0.9):
raise ValueError("Out-of-range proportional gain")
if mode == "custom" and value is None:
raise ValueError("Missing proportional gain")
prior = profiles.get(fingerprint, PlatformProfile(basis.torque_basis, FieldChoice(), FieldChoice()))
if mode == "custom" and prior.proportional_gain.mode == "custom" and prior.gain_basis != basis and not review:
raise ValueError("Gain basis needs review")
result = dict(profiles)
result[fingerprint] = replace(prior, proportional_gain=FieldChoice(mode, value if mode == "custom" else prior.proportional_gain.custom_value),
gain_basis=basis)
return result
openpilot/starpilot/lateral/torque_shaping.py
+50
View File
@@ -0,0 +1,50 @@
"""Shared default torque-shaping calibration and pure math.
Vehicle admission, calibration overrides, state and output remain in each policy.
Defaults derive from the frozen StarPilot torque controller tune.
"""
import math
import numpy as np
def sigmoid(x: float) -> float:
if x >= 0.0:
z = math.exp(-x)
return 1.0 / (1.0 + z)
z = math.exp(x)
return z / (1.0 + z)
KP = 0.6
KI = 0.35
INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
LOW_SPEED_X = [0, 10, 20, 30]
LOW_SPEED_Y = [12, 10.5, 8, 5]
MAX_LAT_JERK_UP = 2.5
LP_FILTER_CUTOFF_HZ = 1.2
JERK_GAIN = 0.22
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
MIN_SPEED = 1.0
UNWIND_D_DES_THRESHOLD = -1.0
UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3
FF_ROLL_OFFSET_FADE_BP = [0.5, 2.5]
FF_ROLL_OFFSET_FADE_V = [0.0, 1.0]
CENTER_CHATTER_JERK_DEADZONE_SPEED_BP = [0.0, 5.0, 12.0, 25.0]
CENTER_CHATTER_JERK_DEADZONE_SPEED_V = [0.08, 0.12, 0.18, 0.18]
CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_BP = [0.0, 0.18, 0.35]
CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_V = [1.0, 1.0, 0.0]
def center_chatter_friction_jerk_deadzone(v_ego: float, setpoint: float, vehicle_deadzone: float) -> float:
speed_deadzone = np.interp(max(v_ego, 0.0), CENTER_CHATTER_JERK_DEADZONE_SPEED_BP,
CENTER_CHATTER_JERK_DEADZONE_SPEED_V)
center_weight = np.interp(abs(setpoint), CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_BP,
CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_V)
return max(vehicle_deadzone, float(speed_deadzone * center_weight))
def get_standard_friction_threshold(v_ego):
return 0.30
openpilot/starpilot/lateral/torque_supported.py
+63
View File
@@ -0,0 +1,63 @@
"""Exact development-only Torque platform cohorts."""
TOYOTA_VEHICLES = frozenset({
'TOYOTA_ALPHARD_TSS2', 'TOYOTA_AVALON', 'TOYOTA_AVALON_2019', 'TOYOTA_AVALON_TSS2',
'TOYOTA_CAMRY', 'TOYOTA_CAMRY_TSS2', 'TOYOTA_CHR', 'TOYOTA_CHR_TSS2',
'TOYOTA_COROLLA', 'TOYOTA_COROLLA_TSS2', 'TOYOTA_HIGHLANDER', 'TOYOTA_HIGHLANDER_TSS2',
'TOYOTA_PRIUS', 'TOYOTA_PRIUS_V', 'TOYOTA_PRIUS_TSS2', 'TOYOTA_RAV4', 'TOYOTA_RAV4H',
'TOYOTA_RAV4_TSS2', 'TOYOTA_RAV4_TSS2_2022', 'TOYOTA_RAV4_PRIME', 'TOYOTA_YARIS',
'TOYOTA_MIRAI', 'TOYOTA_SIENNA', 'TOYOTA_SIENNA_4TH_GEN', 'LEXUS_CTH', 'LEXUS_ES',
'LEXUS_ES_TSS2', 'LEXUS_IS', 'LEXUS_IS_TSS2', 'LEXUS_NX', 'LEXUS_NX_TSS2',
'LEXUS_LC_TSS2', 'LEXUS_RC', 'LEXUS_RC_TSS2', 'LEXUS_RX', 'LEXUS_RX_TSS2',
'LEXUS_GS_F', 'LEXUS_LS',
})
IONIQ6_VEHICLES = frozenset({'HYUNDAI_IONIQ_6'})
BOLT_VEHICLES = frozenset({
'CHEVROLET_BOLT_CC_2017', 'CHEVROLET_BOLT_CC_2018_2021', 'CHEVROLET_BOLT_CC_2022_2023',
'CHEVROLET_BOLT_ACC_2022_2023', 'CHEVROLET_BOLT_ACC_2022_2023_PEDAL',
})
GM_VEHICLES = frozenset({
'BUICK_BABYENCLAVE',
'BUICK_LACROSSE_ASCM',
'BUICK_LACROSSE_ASCM_19US',
'CADILLAC_CT6_CC',
'CADILLAC_ESCALADE_ASCM',
'CADILLAC_ESCALADE_ESV_2019_ASCM',
'CADILLAC_XT4',
'CADILLAC_XT4_CC',
'CADILLAC_XT5',
'CADILLAC_XT5_CC',
'CADILLAC_XT6',
'CHEVROLET_BLAZER',
'CHEVROLET_BOLT_ACC_2022_2023',
'CHEVROLET_BOLT_ACC_2022_2023_PEDAL',
'CHEVROLET_BOLT_CC_2017',
'CHEVROLET_BOLT_CC_2018_2021',
'CHEVROLET_BOLT_CC_2022_2023',
'CHEVROLET_EQUINOX',
'CHEVROLET_EQUINOX_CC',
'CHEVROLET_MALIBU_ASCM',
'CHEVROLET_MALIBU_CC',
'CHEVROLET_MALIBU_HYBRID_CC',
'CHEVROLET_MALIBU_SDGM',
'CHEVROLET_SILVERADO',
'CHEVROLET_SILVERADO_CC',
'CHEVROLET_SUBURBAN',
'CHEVROLET_SUBURBAN_ASCM',
'CHEVROLET_SUBURBAN_CAMERA',
'CHEVROLET_SUBURBAN_CC',
'CHEVROLET_TRAILBLAZER',
'CHEVROLET_TRAILBLAZER_CC',
'CHEVROLET_TRAVERSE',
'CHEVROLET_TRAX',
'CHEVROLET_VOLT',
'CHEVROLET_VOLT_2019',
'CHEVROLET_VOLT_ASCM',
'CHEVROLET_VOLT_CAMERA',
'CHEVROLET_VOLT_CC',
'GMC_YUKON',
'GMC_YUKON_CC',
})
GAIN_VEHICLES = GM_VEHICLES
SUPPORTED_VEHICLES = TOYOTA_VEHICLES | IONIQ6_VEHICLES | GM_VEHICLES
openpilot/starpilot/lateral/torque_tuning.py
+82
View File
@@ -0,0 +1,82 @@
"""Validated torque values for the optional Corolla upstream controller update.
Source selection lives in torque_runtime; this record does not infer a merge
order or interpret legacy gain fields.
"""
from dataclasses import dataclass
from enum import StrEnum
import math
from collections.abc import Mapping
class TorqueSource(StrEnum):
VEHICLE = "vehicle"
USER = "user"
LEARNED = "learned"
_RECORD_FIELDS = frozenset({"source", "vehicle", "latAccelFactor", "latAccelOffset", "friction"})
def _finite_number(value: object, field: str) -> float:
if isinstance(value, bool) or not isinstance(value, (int, float)):
raise ValueError(f"{field} must be a finite number")
try:
result = float(value)
except OverflowError as exc:
raise ValueError(f"{field} must be a finite number") from exc
if not math.isfinite(result):
raise ValueError(f"{field} must be a finite number")
return result
@dataclass(frozen=True)
class TorqueTuning:
"""One complete source's values in the upstream lateral acceleration domain."""
source: TorqueSource
vehicle: str
lat_accel_factor: float
lat_accel_offset: float
friction: float
def __post_init__(self) -> None:
if not isinstance(self.source, TorqueSource):
raise ValueError("source must be a TorqueSource")
if not isinstance(self.vehicle, str) or not self.vehicle.strip():
raise ValueError("vehicle must identify the selected platform")
factor = _finite_number(self.lat_accel_factor, "latAccelFactor")
offset = _finite_number(self.lat_accel_offset, "latAccelOffset")
friction = _finite_number(self.friction, "friction")
if factor <= 0.0:
raise ValueError("latAccelFactor must be positive")
if friction < 0.0:
raise ValueError("friction must be nonnegative")
object.__setattr__(self, "lat_accel_factor", factor)
object.__setattr__(self, "lat_accel_offset", offset)
object.__setattr__(self, "friction", friction)
def upstream_update(self) -> tuple[float, float, float]:
"""Arguments for LatControlTorque.update_torque_parameters, in its API order."""
return self.lat_accel_factor, self.lat_accel_offset, self.friction
def from_record(record: Mapping[str, object]) -> TorqueTuning:
"""Parse only the named fields; serialized legacy gains are deliberately rejected."""
if not isinstance(record, Mapping) or set(record) != _RECORD_FIELDS:
raise ValueError(f"torque record fields must be exactly {sorted(_RECORD_FIELDS)}")
raw_source = record["source"]
if not isinstance(raw_source, str):
raise ValueError("unknown torque source")
try:
source = TorqueSource(raw_source)
except (TypeError, ValueError) as exc:
raise ValueError("unknown torque source") from exc
vehicle = record["vehicle"]
if not isinstance(vehicle, str):
raise ValueError("vehicle must identify the selected platform")
return TorqueTuning(source, vehicle,
_finite_number(record["latAccelFactor"], "latAccelFactor"),
_finite_number(record["latAccelOffset"], "latAccelOffset"),
_finite_number(record["friction"], "friction"))
openpilot/starpilot/lateral/torque_turn_assist.py
+21
View File
@@ -0,0 +1,21 @@
import math
from opendbc.car import structs
from openpilot.starpilot.lateral.ioniq6_policy import get_ioniq_6_low_speed_angle_assist_torque
class TorqueTurnAssist:
def __init__(self, cp):
minimum = float(cp.minSteerSpeed)
self.minimum_speed = max(0.044704, minimum) if math.isfinite(minimum) else math.inf
def apply(self, cs, vm, params, curvature, output, limit):
if (not math.isfinite(cs.vEgo) or cs.vEgo < self.minimum_speed or cs.standstill or cs.steeringPressed or
cs.steerFaultTemporary or cs.steerFaultPermanent or cs.gearShifter not in (structs.CarState.GearShifter.drive, structs.CarState.GearShifter.low)):
return output
desired = math.degrees(vm.get_steer_from_curvature(-curvature, cs.vEgo, params.roll))
actual = cs.steeringAngleDeg - params.angleOffsetDeg
if not all(math.isfinite(value) for value in (desired, actual, output, limit)) or limit < 0:
return output
adjusted = get_ioniq_6_low_speed_angle_assist_torque(desired, actual, output, cs.vEgo)
return max(-limit, min(limit, adjusted))
openpilot/starpilot/lateral/volt_policy.py
+53
View File
@@ -0,0 +1,53 @@
"""Vehicle-owned default torque controller for the five Volt identities."""
import math
import numpy as np
from opendbc.car import structs
from opendbc.car.gm.values import CAR
from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy
VOLT_IDENTITIES = {
CAR.CHEVROLET_VOLT,
CAR.CHEVROLET_VOLT_ASCM,
CAR.CHEVROLET_VOLT_CAMERA,
CAR.CHEVROLET_VOLT_CC,
CAR.CHEVROLET_VOLT_2019,
}
def supported_cp(cp):
return (
cp.brand == 'gm'
and cp.carFingerprint in VOLT_IDENTITIES
and not cp.notCar
and not cp.passive
and not cp.dashcamOnly
and cp.steerControlType == structs.CarParams.SteerControlType.torque
and cp.lateralTuning.which() == 'torque'
and math.isfinite(cp.lateralTuning.torque.latAccelFactor)
and cp.lateralTuning.torque.latAccelFactor > 0
and math.isfinite(cp.lateralTuning.torque.latAccelOffset)
and math.isfinite(cp.lateralTuning.torque.friction)
and cp.lateralTuning.torque.friction >= 0
)
class VoltTorquePolicy(GMOrdinaryTorquePolicy):
def __init__(self, parent, cp):
self.parent = parent
if not supported_cp(cp):
raise ValueError('Unsupported Volt torque profile')
self.dt = parent.dt
coefficients = ((1.525, 1.05, 0.155, 0.0), (1.525, 0.95, 0.150, 0.0))
lateral_values = np.arange(-5.0, 5.0, 0.01)
torque_values = []
for value in lateral_values:
a, b, c, d = coefficients[0 if value >= 0 else 1]
sig_input = a * value
sigmoid = np.sign(sig_input) * (1 / (1 + math.exp(-abs(sig_input))) - 0.5)
torque_values.append(float(sigmoid * b + value * c + d))
parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral_values, torque_values)
parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque_values, lateral_values)
super().__init__(parent, cp)