Volt startup handoff

openpilot/selfdrive/car/card.py

@@ -56,7 +56,8 @@ from openpilot.starpilot.aol.wire import IntentState, encode_intent
 from openpilot.starpilot.conditional_mode.manual import Button, ButtonTracker, IoniqMediaMapCache, WheelMapCache, Press, ioniq6_media_eligible
 from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_long_eligible
 from opendbc.car.gm.aol import native_bootstrap_supported, qualified_gm
-from opendbc.car.gm.values import is_volt_sdgm_profile
+from opendbc.car.gm.values import CAR as GM_CAR, GMFlags, is_volt_ascm_longitudinal, is_volt_sdgm_profile
+from opendbc.car.gm.lateral import lane_centering_supported as gm_lane_centering_supported
 from opendbc.car.gm.ordinary_cc import control_transport_required as gm_cc_transport_required
 from openpilot.starpilot.longitudinal.toyota_output_policy import CLOCK_PAIR_MAX_SKEW_NS, clock_pair_ns
 from openpilot.starpilot.longitudinal.cruise_intervals import read_cruise_intervals
@@ -344,6 +345,8 @@ class Car:
       self.CP.safetyConfigs[0].safetyParam |= aol_policy.safety_param_addition
       self.CP.alternativeExperience |= aol_policy.alternative_experience_addition
     self.vehicle_startup.finalize_aol_configuration(self.CI)
+    if isinstance(self.CI, GMInterface) and self.CI.CC is not None and self.CI.CC.volt_sng_plan_input is not None:
+      self.CI.CC.volt_sng_plan_input.boottime = self.volt_startup_keepalive()
 
     # Reserve optional transport from finalized capability, not saved admission.
     self.slc_transport_available = slc_transport_capable(self.CP, os.environ)
@@ -996,7 +999,7 @@ class Car:
                 len(pandas) == len(self.CP.safetyConfigs) and
                 all(ps.safetyModel == cfg.safetyModel and ps.safetyParam == cfg.safetyParam and
                     ps.alternativeExperience == self.CP.alternativeExperience and
-                    (not ps.safetyRxChecksInvalid or inactive_keepalive and self.volt_sdgm_startup_keepalive())
+                    (not ps.safetyRxChecksInvalid or inactive_keepalive and self.volt_startup_keepalive())
                     for ps, cfg in zip(pandas, self.CP.safetyConfigs, strict=True)))
 
   def publish_sendcan(self, frames, valid=True):
@@ -1006,6 +1009,10 @@ class Car:
       builder = messaging.log_from_bytes(packet).as_builder()
       builder.logMonoTime = int(getattr(self, 'volt_cc_now_boot_ns', 0))
       packet = builder.to_bytes()
+    elif self.volt_startup_keepalive():
+      builder = messaging.log_from_bytes(packet).as_builder()
+      builder.logMonoTime = time.clock_gettime_ns(time.CLOCK_BOOTTIME)
+      packet = builder.to_bytes()
     if self.vehicle_startup.owner is not None:
       with self.vehicle_startup.send_lock:
         self.pm.send('sendcan', packet)
@@ -1048,6 +1055,13 @@ class Car:
   def volt_sdgm_startup_keepalive(self):
     return (is_volt_sdgm_profile(self.CP, longitudinal=True) and self.CP.safetyConfigs[0].safetyParam == 0x5007)
 
+  def volt_startup_keepalive(self):
+    return (self.volt_sdgm_startup_keepalive() or
+            self.CP.alternativeExperience == 32 and not self.CP.flags & GMFlags.PEDAL_LONG and
+            (is_volt_ascm_longitudinal(self.CP) or
+             self.CP.carFingerprint == GM_CAR.CHEVROLET_VOLT_ASCM and self.CP.pcmCruise and
+             not self.CP.openpilotLongitudinalControl and gm_lane_centering_supported(self.CP)))
+
   def controls_update(self, CS: car.CarState, CC: car.CarControl, *, initialize_only=False):
     """control update loop, driven by carControl"""
 
@@ -1129,7 +1143,7 @@ class Car:
       self.ci_initialized = True
 
     if initialize_only:
-      if (isinstance(self.CI, GMInterface) and self.volt_sdgm_startup_keepalive() and
+      if (isinstance(self.CI, GMInterface) and self.volt_startup_keepalive() and
           self.startup_panda_configured(inactive_keepalive=True)):
         inactive = car.CarControl.new_message().as_reader()
         self.last_actuators_output, can_sends = self.CI.apply(
@@ -1173,7 +1187,7 @@ class Car:
       # send car controls over can
       now_nanos = (self.volt_cc_now_boot_ns if getattr(self, 'volt_cc_selected', False) else
                    self.can_log_mono_time if REPLAY else
-                   time.clock_gettime_ns(time.CLOCK_BOOTTIME) if self.volt_sdgm_startup_keepalive() else int(time.monotonic() * 1e9))
+                   time.clock_gettime_ns(time.CLOCK_BOOTTIME) if self.volt_startup_keepalive() else int(time.monotonic() * 1e9))
       self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos)
       self.publish_sendcan(can_sends, valid=CS.canValid)
 
@@ -1190,7 +1204,7 @@ class Car:
                    self.sm.seen['onroadEvents'])
     if not self.CP.passive and initialized:
       self.controls_update(CS, self.sm['carControl'])
-    elif ((not self.ci_initialized or self.volt_sdgm_startup_keepalive()) and self.sm.seen['onroadEvents'] and
+    elif ((not self.ci_initialized or self.volt_startup_keepalive()) and self.sm.seen['onroadEvents'] and
           any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and
           self.aol_replay and native_bootstrap_supported(self.CP)):
       self.controls_update(CS, self.sm['carControl'], initialize_only=True)

openpilot/selfdrive/car/tests/test_init_lifecycle.py

@@ -220,5 +220,231 @@ class CardInitLifecycleTest(unittest.TestCase):
       self.assertFalse(apply.call_args.kwargs)
 
 
+  def test_volt_ascm_inactive_startup_has_no_six_second_handoff_gap(self):
+    self._assert_volt_ascm_inactive_startup(alpha=True)
+
+  def test_volt_stock_ascm_inactive_startup_has_no_six_second_steering_gap(self):
+    self._assert_volt_ascm_inactive_startup(alpha=False)
+
+  def _assert_volt_ascm_inactive_startup(self, *, alpha):
+    from opendbc.car import Bus, structs
+    from opendbc.can import CANPacker
+    from opendbc.car.gm.values import DBC
+    from opendbc.car.gm.tests.test_ascm_intercept import params as ascm_params
+    from opendbc.car.gm.tests.test_volt_camera_control import feed_camera
+    cp = ascm_params(GMCar.CHEVROLET_VOLT_ASCM, sascm=True, alpha=alpha, accelerator=True, radar=True)
+    word = 0xD114 if alpha else 0xA05
+    if alpha:
+      cp.safetyConfigs[0].safetyParam = word
+    self.assertEqual(cp.safetyConfigs[0].safetyParam, word)
+    cadence = ((0x180, 110_000_000), (0x2CB, 50_000_000), (0x315, 50_000_000)) if alpha else ((0x180, 110_000_000),)
+    cp.alternativeExperience = 32
+    card = Car.__new__(Car)
+    card.CP = cp
+    card.CI = GMInterface(cp)
+    card.vehicle_startup = VehicleStartupOwner()
+    card.ci_initialized = False
+    card.initialized_prev = False
+    card.aol_replay = True
+    card.ioniq6_long_prearmed = False
+    card.ioniq6_long_selected = False
+    card.can_callbacks = (lambda wait_for_one=False: [], lambda _: None)
+    params = SimpleNamespace(put_bool=Mock())
+    self.enterContext(patch.object(card, 'params', params, create=True))
+    card.state_publish = Mock()
+    packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
+    cs, _ = feed_camera(card.CI, packer, 22_000_000_000, active=False)
+    self.assertTrue(cs.canValid)
+    counter = [1]
+
+    def state_update():
+      state, _ = feed_camera(card.CI, packer, boot, counter=counter[0] % 4, speed=0., active=False)
+      counter[0] += 1
+      self.assertTrue(state.canValid)
+      return state, None
+
+    self.enterContext(patch.object(card, 'state_update', state_update))
+    control = structs.CarControl.new_message().as_reader()
+    panda = SimpleNamespace(safetyModel=cp.safetyConfigs[0].safetyModel, safetyParam=word,
+                            alternativeExperience=cp.alternativeExperience, safetyRxChecksInvalid=True)
+    mono, boot = 20_000_000_000, 22_000_000_000
+
+    class SubMaster:
+      seen = dict.fromkeys(('carControl', 'pandaStates', 'onroadEvents'), True)
+      valid = dict.fromkeys(('carControl', 'pandaStates'), True)
+      alive = dict(valid)
+      logMonoTime = {'carControl': mono, 'pandaStates': boot}
+      recv_time = dict.fromkeys(('carControl', 'pandaStates'), mono / 1e9)
+      events = [SimpleNamespace(name=EventName.selfdriveInitializing)]
+
+      def __getitem__(self, key):
+        return {'carControl': control, 'pandaStates': [panda], 'onroadEvents': self.events}[key]
+
+      def all_alive(self, services):
+        return all(self.alive[name] for name in services)
+
+    sm = SubMaster()
+    self.enterContext(patch.object(card, 'sm', sm, create=True))
+    sent = []
+    timestamps = {}
+    envelopes = []
+
+    def capture_packet(service, packet):
+      self.assertEqual(service, 'sendcan')
+      event = messaging.log_from_bytes(packet)
+      self.assertTrue(event.valid)
+      self.assertEqual(event.logMonoTime, boot)
+      # Match pandad's unsigned BOOTTIME subtraction and strict one-second bound.
+      self.assertLess((boot - int(event.logMonoTime)) & ((1 << 64) - 1), 1_000_000_000)
+      envelopes.append(event.logMonoTime)
+
+    self.enterContext(patch.object(card, 'pm', SimpleNamespace(send=capture_packet), create=True))
+
+    def publish(frames, valid=True):
+      Car.publish_sendcan(card, frames, valid=valid)
+      self.assertTrue(valid)
+      sent.extend(frames)
+      for address, _, _ in frames:
+        timestamps.setdefault(address, []).append(boot)
+
+    self.enterContext(patch.object(card, 'publish_sendcan', publish))
+    # These saved requests cannot acquire startup brake ownership.
+    card.CI.CC.gm_auto_hold = True
+    card.CI.CC.volt_one_pedal = True
+    with patch('openpilot.selfdrive.car.card.time.monotonic', side_effect=lambda: mono / 1e9), \
+         patch('openpilot.selfdrive.car.card.time.monotonic_ns', side_effect=lambda: mono), \
+         patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', side_effect=lambda _clock: boot), \
+         patch.object(card.CI, 'apply', wraps=card.CI.apply) as apply, \
+         patch.object(card.CI, 'init', wraps=card.CI.init) as init:
+      self.assertFalse(card.startup_panda_configured())
+      self.assertTrue(card.startup_panda_configured(inactive_keepalive=True))
+      for _ in range(601):
+        sm.logMonoTime.update(carControl=mono, pandaStates=boot)
+        sm.recv_time.update(carControl=mono / 1e9, pandaStates=mono / 1e9)
+        card.step()
+        mono += 10_000_000
+        boot += 10_000_000
+      sm.logMonoTime.update(carControl=mono, pandaStates=boot)
+      sm.recv_time.update(carControl=mono / 1e9, pandaStates=mono / 1e9)
+      self.assertEqual(apply.call_count, 601)
+      init.assert_called_once()
+      params.put_bool.assert_called_once_with('ControlsReady', True)
+      self.assertEqual(card.CI.CC.gm_auto_hold_state.drive_ns, 0)
+      self.assertEqual(card.CI.CC.apply_brake, 0)
+      for call in apply.call_args_list:
+        cc, timestamp = call.args
+        self.assertGreaterEqual(timestamp, 22_000_000_000)
+        self.assertLessEqual(timestamp, 28_000_000_000)
+        self.assertTrue(call.kwargs['startup_keepalive'])
+        self.assertFalse(cc.enabled or cc.latActive or cc.longActive)
+        self.assertEqual((cc.actuators.torque, cc.actuators.accel), (0., 0.))
+      for address, max_gap in cadence:
+        times = timestamps[address]
+        self.assertLessEqual(times[0], 22_100_000_000)
+        self.assertGreaterEqual(times[-1], 27_900_000_000)
+        self.assertLessEqual(max(b - a for a, b in zip(times, times[1:], strict=False)), max_gap)
+      self.assertTrue(sent)
+      if not alpha:
+        self.assertFalse(any(address in (0x2CB, 0x315) for address, _, _ in sent))
+        self.assertTrue(card.CP.pcmCruise)
+        self.assertFalse(card.CP.openpilotLongitudinalControl)
+      for address, data, _ in sent:
+        if address == 0x180:
+          self.assertEqual(data[0] & 15, 0)
+          self.assertEqual(data[1], 0)
+        elif address == 0x315:
+          self.assertEqual(((data[0] & 15) << 8) | data[1], 0)
+        elif address == 0x2CB:
+          self.assertEqual(data[0] & 1, 0)
+          self.assertEqual(data[1] & 0x20, 0)
+          self.assertEqual((((data[1] & 7) << 16) | (data[2] << 8) | data[3]) * .125 - 22534, -650)
+      panda.safetyParam = word ^ 1
+      card.step()
+      self.assertEqual(apply.call_count, 601)
+      panda.safetyParam = word
+      sm.logMonoTime['pandaStates'] = boot - 300_000_001
+      card.step()
+      self.assertEqual(apply.call_count, 601)
+      sm.logMonoTime['pandaStates'] = boot
+      sm.valid['carControl'] = False
+      card.step()
+      self.assertEqual(apply.call_count, 601)
+      sm.valid['carControl'] = True
+      sm.logMonoTime['carControl'] = mono - 150_000_001
+      card.step()
+      self.assertEqual(apply.call_count, 601)
+      sm.logMonoTime['carControl'] = mono
+      active = structs.CarControl.new_message(latActive=True)
+      control = active.as_reader()
+      card.step()
+      self.assertEqual(apply.call_count, 601)
+      control = structs.CarControl.new_message().as_reader()
+      sm.events = []
+      # The physical CAN clock remains continuous after inactive startup.
+      for _ in range(10):
+        sm.logMonoTime.update(carControl=mono, pandaStates=boot)
+        sm.recv_time.update(carControl=mono / 1e9, pandaStates=mono / 1e9)
+        card.step()
+        self.assertEqual(apply.call_args.args[1], boot)
+        self.assertFalse(apply.call_args.kwargs)
+        mono += 10_000_000
+        boot += 10_000_000
+      self.assertEqual(apply.call_count, 611)
+      self.assertEqual(len(envelopes), 611)
+      self.assertEqual(envelopes, [call.args[1] for call in apply.call_args_list])
+      self.assertGreaterEqual((boot - mono) & ((1 << 64) - 1), 1_000_000_000)
+      for before, after in zip(apply.call_args_list, apply.call_args_list[1:], strict=False):
+        self.assertEqual(after.args[1] - before.args[1], 10_000_000)
+      for address, max_gap in cadence:
+        times = timestamps[address]
+        self.assertGreaterEqual(times[-1], 28_060_000_000)
+        self.assertLessEqual(max(b - a for a, b in zip(times, times[1:], strict=False)), max_gap)
+
+
+  def test_volt_startup_selector_denies_other_profiles_and_permissions(self):
+    from opendbc.car.gm.tests.test_ascm_intercept import params as ascm_params
+    from opendbc.car.gm.tests.test_volt_camera_control import camera_params
+    card = Car.__new__(Car)
+    for radar in (False, True):
+      for accelerator in (False, True):
+        cp = ascm_params(GMCar.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True,
+                         accelerator=accelerator, radar=radar)
+        cp.alternativeExperience = 32
+        card.CP = cp
+        self.assertTrue(card.volt_startup_keepalive())
+        for field, value in (('alternativeExperience', 0), ('openpilotLongitudinalControl', False),
+                             ('pcmCruise', True), ('passive', True), ('dashcamOnly', True),
+                             ('flags', int(cp.flags) | int(GMFlags.PEDAL_LONG))):
+          saved = getattr(cp, field)
+          setattr(cp, field, value)
+          self.assertFalse(card.volt_startup_keepalive())
+          setattr(cp, field, saved)
+    for radar in (False, True):
+      for accelerator in (False, True):
+        cp = ascm_params(GMCar.CHEVROLET_VOLT_ASCM, sascm=True, alpha=False,
+                         accelerator=accelerator, radar=radar)
+        cp.alternativeExperience = 32
+        card.CP = cp
+        self.assertTrue(card.volt_startup_keepalive())
+        for field, value in (('alternativeExperience', 0), ('openpilotLongitudinalControl', True),
+                             ('pcmCruise', False), ('passive', True), ('dashcamOnly', True),
+                             ('radarUnavailable', not cp.radarUnavailable),
+                             ('flags', int(cp.flags) | int(GMFlags.PEDAL_LONG))):
+          saved = getattr(cp, field)
+          setattr(cp, field, value)
+          self.assertFalse(card.volt_startup_keepalive())
+          setattr(cp, field, saved)
+        word = cp.safetyConfigs[0].safetyParam
+        cp.safetyConfigs[0].safetyParam = word | 0x4000
+        self.assertFalse(card.volt_startup_keepalive())
+        cp.safetyConfigs[0].safetyParam = word
+    cc = GMInterface.get_params(GMCar.CHEVROLET_VOLT_CC, gen_empty_fingerprint(), [], True, False, False)
+    for cp in (cc, camera_params(), GMInterface.get_params(GMCar.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
+                                                       gen_empty_fingerprint(), [], True, False, False)):
+      cp.alternativeExperience = 32
+      card.CP = cp
+      self.assertFalse(card.volt_startup_keepalive())
+
+
 if __name__ == '__main__':
   unittest.main()

openpilot/starpilot/controller_extensions.py

@@ -150,9 +150,10 @@ def configure_controller(CI, params):
   from opendbc.car.gm.values import is_volt_longitudinal
   if controller is not None and is_volt_longitudinal(cp) and controller.volt_sng:
     try:
-      from opendbc.car.gm.values import is_volt_sdgm_profile
+      from opendbc.car.gm.values import GMFlags, is_volt_ascm_longitudinal, is_volt_sdgm_profile
       controller.volt_sng_plan_input = ResumePlanInputs(
-        boottime=is_volt_sdgm_profile(cp, longitudinal=True) and cp.safetyConfigs[0].safetyParam == 0x5007)
+        boottime=(is_volt_sdgm_profile(cp, longitudinal=True) and cp.safetyConfigs[0].safetyParam == 0x5007 or
+                  cp.alternativeExperience == 32 and not cp.flags & GMFlags.PEDAL_LONG and is_volt_ascm_longitudinal(cp)))
     except OSError:
       controller.volt_sng_plan_input = None
       cloudlog.exception('Optional Volt resume input transport unavailable')

openpilot/starpilot/longitudinal/tests/test_gm_volt_stop.py

@@ -346,6 +346,57 @@ class TestVoltStop(unittest.TestCase):
     time.sleep(.16)
     self.assertFalse(provider.update(time.monotonic_ns()))
 
+  def test_ascm_card_resume_uses_physical_clock_and_monotonic_plan_with_two_second_offset(self):
+    from opendbc.car.gm.tests.test_ascm_intercept import params as ascm_params
+    from openpilot.starpilot.controller_extensions import configure_controller
+    from openpilot.starpilot.longitudinal.tests.test_gm_volt_long_policy import SubMasterFixture
+    mono, boot = 20_000_000_000, 22_000_000_000
+    cp = ascm_params(CAR.CHEVROLET_VOLT_ASCM, sascm=True, alpha=True, accelerator=True, radar=True)
+    cp.safetyConfigs[0].safetyParam = 0xD114
+    self.assertEqual(cp.alternativeExperience, 0)
+    from openpilot.common.params import Params
+    from openpilot.selfdrive.car.card import Car
+    self.enterContext(OpenpilotPrefix())
+    self.enterContext(patch.dict('os.environ', {'SIMULATION': '1', 'AOL_REPLAY_RUNTIME': '0'}))
+    saved = Params()
+    for key in ('OpenpilotEnabledToggle', 'AlwaysOnLateral', 'VoltSNG', 'GMAutoHold', 'VoltOnePedalMode'):
+      saved.put_bool(key, True, block=True)
+    saved.put_bool('SafeMode', False, block=True)
+    ci = CarInterface(cp)
+    card = Car(CI=ci, RI=CarInterface.RadarInterface(cp))
+    self.assertEqual(card.CP.alternativeExperience, 32)
+    self.assertEqual(card.CP.safetyConfigs[0].safetyParam, 0xD114)
+    self.assertTrue(card.volt_startup_keepalive())
+    provider = ci.CC.volt_sng_plan_input
+    sm = SubMasterFixture(mono)
+    self.enterContext(patch.object(sm, 'update', lambda timeout: None, create=True))
+    self.enterContext(patch.object(provider, 'sm', sm))
+    self.assertTrue(provider.boottime)
+    provider.freshness.offset_ns = boot - mono
+    provider.freshness.floor_ns = mono - 1_000_000_000
+    with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=mono), \
+         patch('openpilot.starpilot.controller_extensions.time.clock_gettime_ns', return_value=boot), \
+         patch('openpilot.starpilot.longitudinal.inputs.clock_pair_ns', return_value=(mono, boot)):
+      self.assertTrue(provider.update(boot))
+      self.assertFalse(provider.update(mono))
+      sm['longitudinalPlan'].shouldStop = True
+      self.assertFalse(provider.update(boot))
+      sm['longitudinalPlan'].shouldStop = False
+      sm.logMonoTime['longitudinalPlan'] = mono - 150_000_001
+      self.assertFalse(provider.update(boot))
+      sm.logMonoTime['longitudinalPlan'] = boot - 20_000_000
+      self.assertFalse(provider.update(boot))
+      sm.logMonoTime['longitudinalPlan'] = mono - 20_000_000
+      sm.valid['longitudinalPlan'] = False
+      self.assertFalse(provider.update(boot))
+      sm.valid['longitudinalPlan'] = True
+      self.assertTrue(provider.update(boot))
+    # Outside the exact ASCM independent-axis profile, retain the prior clock.
+    cp.alternativeExperience = 0
+    with patch('openpilot.starpilot.controller_extensions.messaging.SubMaster', return_value=sm):
+      configure_controller(ci, None)
+    self.assertFalse(ci.CC.volt_sng_plan_input.boottime)
+
   def test_frozen_card_opt_in_qualifies_plan_when_controls_snapshot_is_off(self):
     from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
     from openpilot.starpilot.controller_extensions import configure_controller