Resynchronize Polestar angle commands on engagement

opendbc_repo/opendbc/car/volvo/cma_carcontroller.py

@@ -35,6 +35,8 @@ class CarControllerCMA(CarControllerBase):
 
     # Counter management for LCA_5 (formerly SPEED_1)
     self.lca_5_counter = None  # Will grab initial value from CarState
+    self.lca_5_primed = False
+    self.lca_5_angle_last = 0.0
 
     self.last_lat_active = False  # Track state
 
@@ -79,6 +81,9 @@ class CarControllerCMA(CarControllerBase):
     lat_active = bool(CC.latActive and CS.out.canValid and not CS.out.canTimeout and
                       CS.out.gearShifter == structs.CarState.GearShifter.drive and
                       not CS.out.steerFaultTemporary and not CS.out.steerFaultPermanent)
+    if not lat_active:
+      self.lca_5_primed = False
+      self.lca_5_angle_last = CS.out.steeringAngleDeg
 
     # lateral control - angle-based steering
     # NOTE: LCA message is sent every frame (even when inactive) to replace stock LCA
@@ -247,14 +252,27 @@ class CarControllerCMA(CarControllerBase):
     # LCA_5 (formerly SPEED_1) - 0x67 - 50 Hz
     # Contains wheel speeds + LCA signals (LCA_TURN_BITS, LCA_5_STEER)
     if self.frame % 2 == 0: # 50 Hz
-      if not lat_active or self.lca_5_counter is None:
+      lca_5_active = lat_active
+      lca_5_angle = apply_angle
+      if self.lca_2_modulus == 15 and lat_active:
+        if not self.lca_5_primed:
+          # Stock transport seeds the native angle history before a new request.
+          lca_5_active = False
+          self.lca_5_primed = True
+          self.lca_5_angle_last = CS.out.steeringAngleDeg
+        else:
+          lca_5_angle = apply_std_steer_angle_limits(
+            apply_angle, self.lca_5_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg,
+            True, CarControllerParams.LCA_5_ANGLE_LIMITS)
+          self.lca_5_angle_last = lca_5_angle
+      if not lca_5_active or self.lca_5_counter is None:
         self.lca_5_counter = CS.msg_lca_5['COUNTER']
 
       # Increment counter by +4, wrap at 15 (0xF never used)
-      if lat_active:
+      if lca_5_active:
         self.lca_5_counter = (self.lca_5_counter + 4) % 15
 
-      can_sends.append(create_lca_5_message(self.packer, lat_active, apply_angle,
+      can_sends.append(create_lca_5_message(self.packer, lca_5_active, lca_5_angle,
                                             CS.msg_lca_5, self.lca_5_counter))
 
     # LCA_4 - 0x90 - 29 Hz
@@ -287,7 +305,8 @@ class CarControllerCMA(CarControllerBase):
       pass
 
     new_actuators = actuators.as_builder()
-    new_actuators.steeringAngleDeg = self.apply_angle_last
+    new_actuators.steeringAngleDeg = (self.lca_5_angle_last if self.lca_2_modulus == 15 and lat_active
+                                    else self.apply_angle_last)
     self.frame += 1
     self.last_lat_active = CC.latActive
     return new_actuators, can_sends

opendbc_repo/opendbc/car/volvo/cma_params.py

@@ -70,3 +70,9 @@ class CarControllerCMAParams:
     ([0., 5., 25.], [2.5, 1.5, .2]),  # rate up limits at different speeds
     ([0., 5., 25.], [5., 2., .3]),    # rate down limits at different speeds
   )
+
+  LCA_5_ANGLE_LIMITS = AngleSteeringLimits(
+    ANGLE_LIMITS.STEER_ANGLE_MAX,
+    (ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP[0], [value * 2 for value in ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP[1]]),
+    (ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN[0], [value * 2 for value in ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN[1]]),
+  )

opendbc_repo/opendbc/car/volvo/tests/test_cma_primary_angle.py

@@ -0,0 +1,90 @@
+"""Polestar primary-angle activation through the production parser and controller."""
+import pytest
+from opendbc.can import CANPacker
+from opendbc.car import Bus, gen_empty_fingerprint, structs
+from opendbc.car.volvo.cma_can import create_lca_5_message
+from opendbc.car.volvo.interface import CarInterface
+from opendbc.car.volvo.values import CAR, DBC
+
+
+def _sources(identity, cruise=True, *, frame=0):
+  modulus = 15 if identity == 'POLESTAR_2' else 16
+  mid = CANPacker('volvo_mid_1')
+  pt = CANPacker(DBC[CAR(identity)][Bus.pt])
+  result = [mid.make_can_msg(name, bus, values) for name, bus, values in [
+    ('SPEED', 0, {'SPEED': 3000}), ('SPEED_2', 0, {}), ('GEAR_POSITION', 0, {'GEAR_POSITION': 3}),
+    ('LCA', 0, {}), ('LCA_2', 0, {'COUNTER_1': ((frame // 2) * 2) % modulus, 'COUNTER_2': ((frame // 2) * 4) % modulus}), ('LCA_3', 0, {}), ('LCA_4', 0, {}),
+    ('LCA_5', 0, {}), ('LCA_6', 0, {}), ('LCA_7', 0, {}),
+    ('PSCM', 2, {}), ('DRIVER_INPUT', 2, {}), ('EGSM', 2, {}), ('PSCM_RELATED', 2, {'NEW_SIGNAL_2': 8}), ('SAS', 2, {}),
+  ]]
+  spa = identity != 'VOLVO_XC40_RECHARGE'
+  result.append(pt.make_can_msg('ECM_1', 1, {'ACCELERATOR_PEDAL_POS': 0 if spa else 20}))
+  result.append(pt.make_can_msg('BUS1_CRUISE_CONTROL', 1,
+                              {'CRUISE_CONTROL_SPA_ENABLED' if spa else 'CRUISE_CONTROL_ENABLED': int(cruise)}))
+  from opendbc.car.volvo.helpers import (checksum_lca_2_message, checksum_2_0x69_message,
+    checksum_1_pscm_related_message, checksum_2_pscm_related_message, checksum_lca_5_message)
+  checked = []
+  for address, payload, bus in result:
+    if address == 0x69 and frame % 2:
+      continue  # This source is 50Hz in the actual receiver contract.
+    d = bytearray(payload)
+    if address == 0x69:
+      d[6] = checksum_lca_2_message(d[0], d[5])
+      d[2] = checksum_2_0x69_message(d[0], d[1], d[3], d[4])
+    elif address == 0x17:
+      d[0] = checksum_1_pscm_related_message(d[1], d[2])
+      d[3] = checksum_2_pscm_related_message(d[2])
+    elif address == 0x67:
+      d[2] = checksum_lca_5_message(d[0], d[1], d[3], d[4], d[5])
+    checked.append((address, bytes(d), bus))
+  return checked
+
+
+@pytest.mark.parametrize('withdrawal', ['disabled', 'invalid', 'timeout', 'park', 'temporary', 'permanent'])
+def test_polestar_primary_primes_on_cold_start_and_after_withdrawal(withdrawal):
+  cp = CarInterface.get_params(CAR.POLESTAR_2, gen_empty_fingerprint(), [], False, False, False)
+  ci = CarInterface(cp)
+  for frame in range(40):
+    ci.update([(1_000_000_000 + frame * 10_000_000, _sources('POLESTAR_2', frame=frame))])
+  assert ci.CS.out.canValid
+  assert ci.CS.out.gearShifter == structs.CarState.GearShifter.drive
+  cc = structs.CarControl.new_message(latActive=True)
+  cc.actuators.steeringAngleDeg = 12.0
+  controller = ci.CC
+  assert controller.lca_2_modulus == 15
+  stock = create_lca_5_message(controller.packer, False, 0., ci.CS.msg_lca_5, ci.CS.msg_lca_5['COUNTER'])
+  _, output = ci.apply(cc.as_reader(), 2_000_000_000)
+  assert next(message for message in output if message[0] == 0x67) == stock
+  assert controller.lca_5_primed
+  for frame in range(1, 7):
+    actuators, output = ci.apply(cc.as_reader(), 2_000_000_000 + frame * 10_000_000)
+    assert actuators.steeringAngleDeg == pytest.approx(controller.lca_5_angle_last)
+  assert controller.lca_5_angle_last > 0
+  healthy = ci.CS.out
+  blocked = structs.CarState.new_message(**healthy.to_dict())
+  if withdrawal == 'disabled':
+    cc.latActive = False
+  elif withdrawal == 'invalid':
+    blocked.canValid = False
+  elif withdrawal == 'timeout':
+    blocked.canTimeout = True
+  elif withdrawal == 'park':
+    blocked.gearShifter = structs.CarState.GearShifter.park
+  elif withdrawal == 'temporary':
+    blocked.steerFaultTemporary = True
+  else:
+    blocked.steerFaultPermanent = True
+  ci.CS.out = blocked.as_reader()
+  for frame in (7, 8):
+    _, output = ci.apply(cc.as_reader(), 2_000_000_000 + frame * 10_000_000)
+    assert not controller.lca_5_primed
+    for message in output:
+      if message[0] == 0x67:
+        assert message == stock
+  ci.CS.out = healthy
+  cc.latActive = True
+  for frame in (9, 10):
+    _, output = ci.apply(cc.as_reader(), 2_000_000_000 + frame * 10_000_000)
+    for message in output:
+      if message[0] == 0x67:
+        assert message == stock

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "f8e452106b5698682f884462e64e20b46664de48f31ca48b964759a66f05e54b", "schema_version": 1}
+{"protected_inventory_sha256": "5be51fa226cc1a2b656de5be17465024850933ba6b46b99148e4f68aabd8dd54", "schema_version": 1}