Restore Dom mode transitions for qualified Volt longitudinal control

This commit is contained in:
firestar5683
2026-10-22 22:57:39 -05:00
parent 687bc6835d
commit 482e3cd7f2
6 changed files with 330 additions and 23 deletions
+29 -10
View File
@@ -1,4 +1,4 @@
"""Mode-transition output shaping for admitted Bolt longitudinal owners."""
"""Mode-transition output shaping for admitted GM longitudinal owners."""
class BoltModeTransition:
@@ -37,7 +37,7 @@ class BoltModeTransition:
if output < 0.0 and output < last_output:
progress = min(1.0, self.timer / self.duration)
urgency = abs(output / -4.0)
urgency_smooth = min(1.0, urgency ** 0.4)
urgency_smooth = min(1.0, urgency**0.4)
blend = 1.0 - (1.0 - progress) * (1.0 - urgency_smooth)
output = last_output + (output - last_output) * blend
return output
@@ -45,16 +45,32 @@ class BoltModeTransition:
def policy_for(cp):
from opendbc.car.structs import CarParams
from opendbc.car.gm.values import (CAR, GMFlags, GMSafetyFlags, is_bolt_cc_profile, is_bolt_euv_longitudinal,
is_ordinary_cc_profile, is_ordinary_camera_profile, is_conventional_cc_pedal_profile)
from opendbc.car.gm.values import (
CAR,
GMFlags,
GMSafetyFlags,
is_bolt_cc_profile,
is_bolt_euv_longitudinal,
is_ordinary_cc_profile,
is_ordinary_camera_profile,
is_conventional_cc_pedal_profile,
is_volt_longitudinal,
)
from opendbc.car.gm.longitudinal import policy_for as pedal_policy_for
if not cp.openpilotLongitudinalControl:
return None
if (is_conventional_cc_pedal_profile(cp) or is_ordinary_camera_profile(cp, longitudinal=True) or
is_ordinary_cc_profile(cp) or is_bolt_cc_profile(cp) or is_bolt_euv_longitudinal(cp)):
if (
is_volt_longitudinal(cp)
or is_conventional_cc_pedal_profile(cp)
or is_ordinary_camera_profile(cp, longitudinal=True)
or is_ordinary_cc_profile(cp)
or is_bolt_cc_profile(cp)
or is_bolt_euv_longitudinal(cp)
):
return BoltModeTransition()
from opendbc.car.gm.values import gm_control_word
base = GMSafetyFlags.HW_CAM | GMSafetyFlags.EV | GMSafetyFlags.PEDAL_LONG | GMSafetyFlags.PADDLE_SCHED
words = {
CAR.CHEVROLET_BOLT_CC_2017: base | GMSafetyFlags.NO_ACC | GMSafetyFlags.BOLT_2017,
@@ -62,9 +78,12 @@ def policy_for(cp):
CAR.CHEVROLET_BOLT_CC_2022_2023: base | GMSafetyFlags.NO_ACC | GMSafetyFlags.BOLT_GEN2,
CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL: base | GMSafetyFlags.BOLT_ACC_PEDAL | GMSafetyFlags.BOLT_GEN2,
}
if (pedal_policy_for(cp) is not None and not cp.flags & GMFlags.CC_LONG.value and
len(cp.safetyConfigs) == 1 and
cp.networkLocation == CarParams.NetworkLocation.fwdCamera and
gm_control_word(cp) == words.get(cp.carFingerprint)):
if (
pedal_policy_for(cp) is not None
and not cp.flags & GMFlags.CC_LONG.value
and len(cp.safetyConfigs) == 1
and cp.networkLocation == CarParams.NetworkLocation.fwdCamera
and gm_control_word(cp) == words.get(cp.carFingerprint)
):
return BoltModeTransition()
return None
@@ -1,4 +1,5 @@
"""Exact development-only manual Volt CC native admission and arbitration."""
import unittest
from opendbc.can import CANPacker
@@ -15,7 +16,7 @@ class TestGmVoltCc(unittest.TestCase):
self.safety = libsafety_py.libsafety
self.release = self.safety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) != 0
fp = gen_empty_fingerprint()
fp[0].update({0xbe: 6, 0x3d1: 8, 0xc9: 8, 0x1e1: 7, 0x1f5: 8, 0x34a: 5, 0x1c4: 8, 0xbd: 7})
fp[0].update({0xBE: 6, 0x3D1: 8, 0xC9: 8, 0x1E1: 7, 0x1F5: 8, 0x34A: 5, 0x1C4: 8, 0xBD: 7})
self.cp = CarInterface.get_params(CAR.CHEVROLET_VOLT_CC, fp, [], False, False, False)
self.packer = CANPacker(DBC[self.cp.carFingerprint][Bus.pt])
@@ -28,12 +29,13 @@ class TestGmVoltCc(unittest.TestCase):
self.safety.init_tests()
self.safety.set_timer(1_000_000)
def frames(self, counter=0, speed=20., gas=False, brake=False):
def frames(self, counter=0, speed=20.0, gas=False, brake=False):
frames = pt_frames(self.packer, counter=counter, gas=gas, brake=brake)
replacements = [self.packer.make_can_msg('EBCMWheelSpdRear', 0,
{'RLWheelSpd': speed * 3.6, 'RRWheelSpd': speed * 3.6, 'RLWheelDir': 1, 'RRWheelDir': 1}),
gmcan.create_buttons(self.packer, 0, counter, 1),
self.packer.make_can_msg('EBCMRegenPaddle', 0, {})]
replacements = [
self.packer.make_can_msg('EBCMWheelSpdRear', 0, {'RLWheelSpd': speed * 3.6, 'RRWheelSpd': speed * 3.6, 'RLWheelDir': 1, 'RRWheelDir': 1}),
gmcan.create_buttons(self.packer, 0, counter, 1),
self.packer.make_can_msg('EBCMRegenPaddle', 0, {}),
]
addresses = {frame[0] for frame in replacements}
return [frame for frame in frames if frame[0] not in addresses] + replacements
@@ -102,7 +104,7 @@ class TestGmVoltCc(unittest.TestCase):
self.assertFalse(self.tx(counter=2, button=6))
def test_actual_controller_direction_to_disable_native_acceptance(self):
for accel in (-2., 2.):
for accel in (-2.0, 2.0):
for offset_frames in range(1, 5):
for override in ('none', 'gas', 'brake'):
self.reset()
@@ -118,7 +120,7 @@ class TestGmVoltCc(unittest.TestCase):
command.actuators.accel = accel
command.hudControl.leadVisible = True
_, frames = ci.apply(command.as_reader(), 1_000_000_000)
buttons = [frame for frame in frames if frame[0] == 0x1e1]
buttons = [frame for frame in frames if frame[0] == 0x1E1]
self.assertEqual(len(buttons), 1)
self.assertEqual(self.safety.safety_tx_hook(self.packet(buttons[0])), not self.release)
elapsed = offset_frames * 10_000
@@ -132,13 +134,13 @@ class TestGmVoltCc(unittest.TestCase):
command.longActive = False
self.safety.set_controls_allowed(False)
_, frames = ci.apply(command.as_reader(), 1_000_000_000 + elapsed * 1000)
buttons = [frame for frame in frames if frame[0] == 0x1e1]
buttons = [frame for frame in frames if frame[0] == 0x1E1]
self.assertEqual(len(buttons), 1)
self.assertEqual((buttons[0][1][5] >> 4) & 7, 6)
self.assertEqual(self.safety.safety_tx_hook(self.packet(buttons[0])), not self.release)
def test_low_speed_and_gas_are_not_lateral_propulsion_gates(self):
for speed, gas in ((4., False), (20., True)):
for speed, gas in ((4.0, False), (20.0, True)):
self.reset()
self.feed(speed=speed)
self.assertEqual(self.safety.get_controls_allowed(), not self.release)
@@ -152,7 +154,7 @@ class TestGmVoltCc(unittest.TestCase):
self.assertFalse(self.tx(counter=2 if gas else 1))
def test_individual_source_staleness_and_forbidden_authority(self):
required = {0x3d1, 0x1e1, 0xc9, 0xbe, 0x1f5, 0x1c4, 0xbd, 0x34a}
required = {0x3D1, 0x1E1, 0xC9, 0xBE, 0x1F5, 0x1C4, 0xBD, 0x34A}
for missing in required:
self.reset()
self.feed()
@@ -166,9 +168,35 @@ class TestGmVoltCc(unittest.TestCase):
self.reset()
self.feed()
self.safety.set_controls_allowed(True)
for addr, size in ((0x200, 6), (0x2cb, 8), (0x315, 5), (0x3d1, 8)):
for addr, size in ((0x200, 6), (0x2CB, 8), (0x315, 5), (0x3D1, 8)):
self.assertFalse(self.safety.safety_tx_hook(self.packet((addr, bytes(size), 0))))
if __name__ == '__main__':
unittest.main()
def test_volt_f1_uses_six_count_threshold_and_independent_c9_without_malibu_bit():
safety = libsafety_py.libsafety
release = safety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) != 0
safety.init_tests()
safety.set_alternative_experience(32)
assert safety.set_safety_hooks(structs.CarParams.SafetyModel.gm, 0xC163) == 0
if release:
# RELEASE never acquires this development-only no-ACC actuator owner.
assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(0x180, 0, bytes(4)))
assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(0x1E1, 0, bytes(7)))
return
for raw in range(256):
for malibu_bit in (False, True):
for c9 in (False, True):
safety.init_tests()
safety.set_alternative_experience(32)
assert safety.set_safety_hooks(structs.CarParams.SafetyModel.gm, 0xC163) == 0
safety.set_timer(1_000_000)
# The same physical bit means Malibu brake, but is not Volt's source.
f1 = bytes([int(malibu_bit) << 1, raw, 0, 0, 0, 0])
status = bytes([0, 0, 0, 0, 0, int(c9), 0, 0])
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0xF1, 0, f1))
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0xC9, 0, status))
assert bool(safety.get_brake_pressed_prev()) == bool(raw >= 6 or c9), (raw, malibu_bit, c9)
@@ -0,0 +1,121 @@
"""Card's exact raw companion can precede the cached SubMaster projection."""
from contextlib import ExitStack
from unittest.mock import patch
import pytest
from opendbc.car.gm.tests.test_volt_sdgm_control import sdgm_params
from opendbc.car.structs import car
from openpilot.cereal import log, messaging
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.common.realtime import Ratekeeper
from openpilot.selfdrive.car.cruise import VCruiseHelper
from openpilot.selfdrive.car.tests.publication_fixture import initialize_publication_sources
from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
from openpilot.starpilot.car.gm.steering_companion import SteeringCompanion
@pytest.mark.parametrize(
'source_age,cached_age',
[
(3_759_000, 32_550_000),
(4_483_000, 32_710_000),
(3_931_000, 32_760_000),
(3_792_000, 32_630_000),
(3_910_000, 34_330_000),
(4_470_000, 33_450_000),
(3_910_000, 32_820_000),
(3_710_000, 32_850_000),
(3_430_000, 32_800_000),
],
)
@pytest.mark.parametrize('c9', [False])
@pytest.mark.parametrize('full_op', [False, True])
@pytest.mark.parametrize('condition', ['sm_expired', 'sm_expired_fault', 'raw_fault'])
def test_exact_card_raw_companion_survives_older_cached_projection(c9, full_op, condition, source_age, cached_age):
from openpilot.selfdrive.car.card import Car
from openpilot.starpilot.aol.tests.test_gm_source_deadline import bind_steering_receipts
actual_recv, actual_optional = messaging.recv_one, messaging.recv_one_or_none
case = fixture.IoniqAvailabilityTests(methodName='runTest')
with ExitStack() as cleanup, OpenpilotPrefix():
cleanup.callback(case.doCleanups)
messaging.reset_context()
sd, _, step = case._authority_callers(full_op, cp=sdgm_params(alpha=True, brake_c9=c9))
step = bind_steering_receipts(sd, step, cleanup)
assert step(0)[0].lateralActive
pm = messaging.PubMaster(['carState', 'carOutput', 'starpilotCarState'])
sd.car_state_sock = messaging.sub_sock('carState', timeout=100)
sd.steering_companion = SteeringCompanion(messaging.sub_sock('starpilotCarState', conflate=False))
latest = messaging.sub_sock('starpilotCarState', conflate=True, timeout=100)
card = Car.__new__(Car)
card.CP, card.pm = sd.CP, pm
initialize_publication_sources(card)
authority = card.CI.CS.steering_authority
card.sm = messaging.SubMaster(['carControl'], ignore_avg_freq=['carControl'])
neutral = messaging.new_message('carControl', valid=True, logMonoTime=10_000_000_000)
card.sm.update_msgs(10.0, [neutral.as_reader()])
card.sm.frame = 1
card.car_params_published = True
card.last_actuators_output = car.CarControl.Actuators.new_message()
card.slc_replay = card.curve_replay = card.aol_replay = False
card.can_rcv_cum_timeout_counter = 0
card.rk = Ratekeeper(100.0, print_delay_threshold=None)
card.v_cruise_helper = VCruiseHelper(card.CP)
source = 10_010_000_000
now = source + source_age
older = now - cached_age
original_message = messaging.new_message
def publish(stamp, *, fault=False, valid=True, enabled=True):
authority.enabled, authority.latched = enabled, fault
cs = fixture.car_state()
cs.canValid = valid
cs.cruiseState.available = True
with patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=lambda *a, **kw: original_message(*a, **dict(kw, logMonoTime=stamp))):
card.state_publish(cs, None)
publish(older, fault=condition in ('sm_fault', 'sm_expired_fault'), valid=condition not in ('sm_invalid', 'sm_expired_invalid'))
assert actual_recv(sd.car_state_sock).logMonoTime == older
cached = actual_recv(latest).as_builder()
assert cached.logMonoTime == older
if condition == 'sm_wrong_source':
cached.starpilotCarState.sourceCarStateMonoTime -= 1
if condition == 'sm_future':
cached.logMonoTime = now + 1
cached.starpilotCarState.sourceCarStateMonoTime = now + 1
def receive_first(sock):
publish(source, fault=condition == 'raw_fault', enabled=condition != 'missing_exact')
first = actual_recv(sock)
assert first.logMonoTime == source
return first
def receive_cached(_timeout):
sd.sm.update_msgs(now / 1e9, [cached.as_reader()])
def receive_raw(sock):
if sock is sd.car_state_sock:
return None
message = actual_optional(sock)
if message is not None and condition == 'raw_invalid' and message.logMonoTime == source:
message = message.as_builder()
message.valid = False
return message.as_reader()
return message
sd.sm.update.side_effect = receive_cached
with (
patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one', side_effect=receive_first),
patch('openpilot.starpilot.car.gm.steering_companion.messaging.recv_one_or_none', side_effect=receive_raw),
patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=now),
):
axis, _, cc = step(1)
healthy = condition in ('healthy', 'sm_expired')
assert axis.sourceCarStateMonoTime == source
assert bool(axis.lateralActive) == healthy and bool(cc.latActive) == healthy
assert bool(cc.longActive) == (healthy and full_op)
assert (log.OnroadEvent.EventName.controlsMismatch in sd.events.names) == (not healthy)
if healthy:
assert sd.sm.logMonoTime['starpilotCarState'] == older
assert sd.steering_companion.samples[source][0] == source
assert axis.validUntilMonoTime == axis.observedMonoTime + 30_000_000
@@ -0,0 +1,136 @@
"""Volt mode transitions reach actual factory/LongControl/Controls callers."""
from types import SimpleNamespace
import pytest
from opendbc.car import gen_empty_fingerprint
from opendbc.car.gm.interface import CarInterface
from opendbc.car.gm.values import CAR
from opendbc.car.gm.bolt_mode import policy_for
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.starpilot.longitudinal.extension import LongitudinalContext
def sdgm(*, alpha=True, c9=False):
fp = gen_empty_fingerprint()
fp[0][0x2FF] = 8
if not c9:
fp[0][0xBE] = 6
return CarInterface.get_params(CAR.CHEVROLET_VOLT_2019, fp, [], alpha, False, False)
def physical(speed=15.25):
return SimpleNamespace(
vEgo=speed, aEgo=0.72, brakePressed=False, gasPressed=False, canValid=True, canTimeout=False, cruiseState=SimpleNamespace(standstill=False)
)
@pytest.mark.parametrize('c9', (False, True))
def test_exact_sdgm_factory_restores_dom_mode_handoff(c9):
cp = sdgm(c9=c9)
assert cp.openpilotLongitudinalControl and not cp.pcmCruise
controller = LongControl(cp)
assert controller.extension.bolt_mode is not None
state = physical()
for _ in range(120):
controller.update(True, state, 0.0, False, (-4.0, 1.4), context=LongitudinalContext(experimental_mode=True, has_lead=True))
controller.reset()
controller.last_output_accel = 0.0
controller.update(False, state, 0.668, False, (-4.0, 1.4), context=LongitudinalContext(experimental_mode=False))
assert controller.extension.bolt_mode.current_mode == 'blended'
# Recorded Mary handoff: the new positive request must follow Dom's one-second
# mode ramp; it may not jump straight to the raw .668 m/s² request.
output = controller.update(True, state, 0.668, False, (-4.0, 1.4), context=LongitudinalContext(experimental_mode=False, has_lead=True))
assert controller.extension.bolt_mode.leaving_experimental
assert controller.pid.i == 0.0
assert output == pytest.approx(float(controller.pid.control) * 0.01)
assert output < 0.01
# Brake/disabled LONG cannot resume or advance its mode owner on their own.
mode_time = controller.extension.bolt_mode.timer
assert controller.update(False, state, 0.668, False, (-4.0, 1.4), context=LongitudinalContext(experimental_mode=True)) == 0.0
assert controller.extension.bolt_mode.timer == mode_time
@pytest.mark.parametrize('c9', (False, True))
@pytest.mark.parametrize('fault', ('stock', 'passive', 'dashcam', 'not_car', 'wrong_word', 'wrong_identity', 'missing_sascm'))
def test_sdgm_mode_policy_never_admits_unqualified_owner(c9, fault):
cp = sdgm(alpha=fault != 'stock', c9=c9)
if fault == 'passive':
cp.passive = True
if fault == 'dashcam':
cp.dashcamOnly = True
if fault == 'not_car':
cp.notCar = True
if fault == 'wrong_word':
cp.safetyConfigs[0].safetyParam = 0xFFFF
if fault == 'wrong_identity':
cp.carFingerprint = 'UNQUALIFIED_VOLT'
if fault == 'missing_sascm':
fp = gen_empty_fingerprint()
fp[0][0xBE] = 6
cp = CarInterface.get_params(CAR.CHEVROLET_VOLT_2019, fp, [], True, False, False)
assert policy_for(cp) is None
@pytest.mark.parametrize('c9', (False, True))
def test_actual_controls_uses_only_fresh_sdgm_mode_telemetry(c9, monkeypatch):
from openpilot.starpilot.longitudinal.tests import test_bolt_mode_transition as fixture
case = fixture.TestBoltModeTransition(methodName='test_actual_controls_transports_only_healthy_mode_state')
cp = sdgm(c9=c9)
monkeypatch.setattr(fixture, 'params', lambda *_args, **_kwargs: cp)
try:
case.test_actual_controls_transports_only_healthy_mode_state()
finally:
case.doCleanups()
@pytest.mark.parametrize('identity', (CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_ASCM, CAR.CHEVROLET_VOLT_CAMERA, CAR.CHEVROLET_VOLT_2019))
@pytest.mark.parametrize('alpha', (False, True))
@pytest.mark.parametrize('c9', (False, True))
def test_each_volt_factory_keeps_its_own_mode_selection(identity, alpha, c9):
fp = gen_empty_fingerprint()
fp[0][0x2FF] = 8
fp[1][0x460] = 8
if identity == CAR.CHEVROLET_VOLT_CAMERA:
fp[2][0x320] = 6
if not c9:
fp[0][0xBE] = 6
cp = CarInterface.get_params(identity, fp, [], alpha, False, False)
# Gateway owns ordinary OP LONG; the other three require explicit Alpha.
expected = identity == CAR.CHEVROLET_VOLT or alpha
assert cp.openpilotLongitudinalControl == expected
assert (policy_for(cp) is not None) == expected
if expected:
control = LongControl(cp)
assert control.extension.bolt_mode is not None
cs = physical()
control.update(True, cs, -0.5, False, (-4.0, 1.4), context=LongitudinalContext(experimental_mode=True))
assert control.extension.bolt_mode.current_mode == 'blended'
corrupt = cp.as_reader().as_builder()
corrupt.safetyConfigs[0].safetyParam = 0xFFFF
assert policy_for(corrupt) is None
@pytest.mark.parametrize('topology', ('camera', 'gateway', 'ascm', 'sdgm'))
@pytest.mark.parametrize('alpha', (False, True))
@pytest.mark.parametrize('be', (False, True))
@pytest.mark.parametrize('release', (False, True))
def test_pedal_volt_siblings_keep_stock_and_release_ownership(topology, alpha, be, release):
from opendbc.car.gm.tests import test_volt_transitions as factory
build = getattr(factory, 'volt_' + topology + '_pedal_params')
options = {'alpha': alpha, 'be': be, 'release': release}
if topology in ('ascm', 'sdgm'):
options['sascm'] = True
cp = build(**options)
# Actual factory admission owns this distinction. A pedal observation does
# not itself permit LONG, particularly in stock/release configurations.
admitted = cp.openpilotLongitudinalControl and not cp.dashcamOnly and not cp.passive
mode = policy_for(cp)
assert (mode is not None) == admitted, (topology, alpha, be, release, cp.to_dict())
if mode is not None:
controller = LongControl(cp)
controller.update(False, physical(), 1.0, False, (-4.0, 1.4), context=LongitudinalContext(experimental_mode=True))
assert controller.last_output_accel == 0.0
assert not controller.extension.bolt_mode.transitioning
+1 -1
View File
@@ -1 +1 @@
{"protected_inventory_sha256": "390a406438710d6c8e3dce0034dda5111a0b3dc6cb9361b8903cc5ea56c32f07", "schema_version": 1}
{"protected_inventory_sha256": "eff942f1623e2892fac4dcb88b5562f7d6db7c8bad2458c1d2d77a55598ed227", "schema_version": 1}
+3
View File
@@ -50,6 +50,9 @@ AOL_LIFECYCLE_TARGETS = (
"openpilot/starpilot/aol/tests/test_gm_steering_receive_order.py",
"openpilot/starpilot/aol/tests/test_gm_main_response.py",
"openpilot/starpilot/aol/tests/test_gm_held_native.py",
'openpilot/starpilot/longitudinal/tests/test_volt_mode_transition.py',
'openpilot/starpilot/aol/tests/test_gm_route1c_receive_order.py',
"openpilot/starpilot/aol/tests/test_gm_ordinary_warning.py",
"openpilot/starpilot/aol/tests/test_gm_startup_main.py",
"openpilot/starpilot/aol/tests/test_gm_pedal_order.py",