diff --git a/opendbc_repo/opendbc/car/volvo/cma_carcontroller.py b/opendbc_repo/opendbc/car/volvo/cma_carcontroller.py index 40c8ddfd62..7d8337107a 100644 --- a/opendbc_repo/opendbc/car/volvo/cma_carcontroller.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/volvo/cma_params.py b/opendbc_repo/opendbc/car/volvo/cma_params.py index 15a134f03d..b61f89d3b5 100644 --- a/opendbc_repo/opendbc/car/volvo/cma_params.py +++ b/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]]), + ) diff --git a/opendbc_repo/opendbc/car/volvo/tests/test_cma_primary_angle.py b/opendbc_repo/opendbc/car/volvo/tests/test_cma_primary_angle.py new file mode 100644 index 0000000000..7d4f626b8a --- /dev/null +++ b/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 diff --git a/prebuilt.json b/prebuilt.json index 6cf3304b87..c21fc5e869 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "f8e452106b5698682f884462e64e20b46664de48f31ca48b964759a66f05e54b", "schema_version": 1} +{"protected_inventory_sha256": "5be51fa226cc1a2b656de5be17465024850933ba6b46b99148e4f68aabd8dd54", "schema_version": 1}