From c44108b0303b5740e140db106b40721245c1ffdb Mon Sep 17 00:00:00 2001 From: elkoled Date: Sat, 13 Dec 2025 14:09:11 +0100 Subject: [PATCH 01/77] port longitudinal --- opendbc/car/psa/carcontroller.py | 85 ++++++++++++++++++++++++++------ opendbc/car/psa/carstate.py | 26 ++++++---- opendbc/car/psa/fingerprints.py | 7 ++- opendbc/car/psa/interface.py | 7 ++- opendbc/car/psa/psacan.py | 73 ++++++++++++++++++++++++++- opendbc/car/psa/values.py | 39 ++++++++++++--- opendbc/safety/modes/psa.h | 49 +++++++++++++----- opendbc/safety/tests/test_psa.py | 22 ++++----- 8 files changed, 247 insertions(+), 61 deletions(-) diff --git a/opendbc/car/psa/carcontroller.py b/opendbc/car/psa/carcontroller.py index 792deccee9d..31579159d65 100644 --- a/opendbc/car/psa/carcontroller.py +++ b/opendbc/car/psa/carcontroller.py @@ -1,9 +1,12 @@ from opendbc.can.packer import CANPacker -from opendbc.car import Bus +from opendbc.car import Bus, structs, make_tester_present_msg from opendbc.car.lateral import apply_std_steer_angle_limits from opendbc.car.interfaces import CarControllerBase -from opendbc.car.psa.psacan import create_lka_steering +from opendbc.car.psa.psacan import create_lka_steering, create_resume_acc, create_disable_radar, create_HS2_DYN1_MDD_ETAT_2B6, create_HS2_DYN_MDD_ETAT_2F6 from opendbc.car.psa.values import CarControllerParams +from numpy import interp + +LongCtrlState = structs.CarControl.Actuators.LongControlState class CarController(CarControllerBase): @@ -11,31 +14,83 @@ def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) self.packer = CANPacker(dbc_names[Bus.main]) self.apply_angle_last = 0 + self.radar_disabled = 0 self.status = 2 def update(self, CC, CS, now_nanos): can_sends = [] actuators = CC.actuators + # longitudinal + starting = actuators.longControlState == LongCtrlState.starting and CS.out.vEgo <= self.CP.vEgoStarting + # stopping = actuators.longControlState == LongCtrlState.stopping # lateral control - if self.frame % 5 == 0: - apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg, CC.latActive, CarControllerParams.ANGLE_LIMITS) - # EPS disengages on steering override, activation sequence 2->3->4 to re-engage - # STATUS - 0: UNAVAILABLE, 1: UNSELECTED, 2: READY, 3: AUTHORIZED, 4: ACTIVE - if not CC.latActive: - self.status = 2 - elif not CS.eps_active and not CS.out.steeringPressed: - self.status = 2 if self.status == 4 else self.status + 1 - else: - self.status = 4 + # EPS disengages on steering override, activation sequence 2->3->4 to re-engage + # STATUS - 0: UNAVAILABLE, 1: UNSELECTED, 2: READY, 3: AUTHORIZED, 4: ACTIVE + if not CC.latActive: + self.status = 2 + elif not CS.eps_active and not CS.out.steeringPressed: + self.status = 2 if self.status == 4 else self.status + 1 + else: + self.status = 4 + + + # # emulate resume button every 4 seconds to prevent autohold timeout + # if CC.latActive and CS.out.standstill and CC.hudControl.leadVisible: + # # map: {frame:status} - 0, 0, 1, 1 + # status = {0: 0, 5: 0, 10: 1, 15: 1}.get(self.frame % 400) + # if status is not None: + # msg = CS.hs2_dat_mdd_cmd_452 + # counter = (msg['COUNTER'] + 1) % 16 + # can_sends.append(create_resume_acc(self.packer, counter, status, msg)) + + # longitudinal control + # TUNING + # >=-0.8: Engine brakes only + # <-0.8: Add friction brakes + brake_accel = -0.5 + + # torque lookup + ACCEL_LOOKUP = [-2.0, -1.0, -0.5, 0.0, 0.5, 1.0, 1.5, 2.0] + TORQUE_LOOKUP = [-600, -400, -100, 150, 400, 700, 900, 1000] + + # calculate Torque + torque_nm = interp(actuators.accel, ACCEL_LOOKUP, TORQUE_LOOKUP) + torque = max(-600, min(torque_nm, 1000)) + + # engine/friction brake transition + braking = actuators.accel < brake_accel and not CS.out.gasPressed + + # # twitchy on gas/accel transition but ok car following and braking + # torque = actuators.accel * 1000 + # braking = torque < -300 and not CS.out.gasPressed + if self.CP.openpilotLongitudinalControl: + # disable radar ECU by setting to programming mode + if self.radar_disabled == 0: + can_sends.append(create_disable_radar()) + self.radar_disabled = 1 + + # keep radar ECU disabled by sending tester present + if self.frame % 100 == 0 and self.frame>0: # TODO check if disable_radar is sent 100 frames before + can_sends.append(make_tester_present_msg(0x6b6, 1, suppress_response=False)) + + # TODO: tune torque multiplier + # TODO: tune braking threshold + # Highest torque seen without gas input: ~1000 + # Lowest torque seen without break mode: -560 (but only when transitioning from brake to accel mode, else -248) + # Lowest brake mode accel seen: -4.85m/s² - can_sends.append(create_lka_steering(self.packer, CC.latActive, apply_angle, self.status)) + if self.frame % 2 == 0: + can_sends.append(create_HS2_DYN1_MDD_ETAT_2B6(self.packer, self.frame // 2, actuators.accel, CS.out.cruiseState.enabled, CS.out.gasPressed, braking, CS.out.brakePressed, CS.out.standstill, torque)) + can_sends.append(create_HS2_DYN_MDD_ETAT_2F6(self.packer, braking)) - self.apply_angle_last = apply_angle + can_sends.append(create_lka_steering(self.packer, CC.latActive, apply_angle, self.status)) + self.apply_angle_last = apply_angle new_actuators = actuators.as_builder() - new_actuators.steeringAngleDeg = self.apply_angle_last + new_actuators.steeringAngleDeg = apply_angle self.frame += 1 return new_actuators, can_sends diff --git a/opendbc/car/psa/carstate.py b/opendbc/car/psa/carstate.py index 81335ef5985..91004e0566b 100644 --- a/opendbc/car/psa/carstate.py +++ b/opendbc/car/psa/carstate.py @@ -1,7 +1,8 @@ +import copy from opendbc.car import structs, Bus from opendbc.can.parser import CANParser from opendbc.car.common.conversions import Conversions as CV -from opendbc.car.psa.values import DBC, CarControllerParams +from opendbc.car.psa.values import CAR, DBC, CarControllerParams from opendbc.car.interfaces import CarStateBase GearShifter = structs.CarState.GearShifter @@ -23,18 +24,22 @@ def update(self, can_parsers) -> structs.CarState: cp.vl['Dyn4_FRE']['P266_VehV_VPsvValWhlBckR'], ) ret.yawRate = cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VITESSE_LACET_BRUTE'] * CV.DEG_TO_RAD - ret.standstill = bool(cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VEHICLE_STANDSTILL']) + ret.standstill = cp.vl['Dyn4_FRE']['P263_VehV_VPsvValWhlFrtL'] < 0.1 # gas - ret.gasPressed = cp.vl['Dyn_CMM']['P002_Com_rAPP'] > 0 + ret.gasPressed = cp_cam.vl['DRIVER']['GAS_PEDAL'] > 0 # brake ret.brakePressed = bool(cp_cam.vl['Dat_BSI']['P013_MainBrake']) ret.parkingBrake = cp.vl['Dyn_EasyMove']['P337_Com_stPrkBrk'] == 1 # 0: disengaged, 1: engaged, 3: brake actuator moving # steering wheel - ret.steeringAngleDeg = cp.vl['STEERING_ALT']['ANGLE'] # EPS - ret.steeringRateDeg = cp.vl['STEERING_ALT']['RATE'] * (2 * cp.vl['STEERING_ALT']['RATE_SIGN'] - 1) # convert [0,1] to [-1,1] EPS: rot. speed * rot. sign + STEERING_ALT_BUS = { + CAR.PSA_PEUGEOT_208: cp.vl, + } + bus = STEERING_ALT_BUS[self.CP.carFingerprint] + ret.steeringAngleDeg = bus['STEERING_ALT']['ANGLE'] # EPS + ret.steeringRateDeg = bus['STEERING_ALT']['RATE'] * (1 - 2 * bus['STEERING_ALT']['RATE_SIGN']) # convert [0,1] to [1,-1] EPS: rot. speed * rot. sign ret.steeringTorque = cp.vl['STEERING']['DRIVER_TORQUE'] ret.steeringTorqueEps = cp.vl['IS_DAT_DIRA']['EPS_TORQUE'] ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5) @@ -43,10 +48,13 @@ def update(self, can_parsers) -> structs.CarState: # cruise ret.cruiseState.speed = cp_adas.vl['HS2_DAT_MDD_CMD_452']['SPEED_SETPOINT'] * CV.KPH_TO_MS # set to 255 when ACC is off, -2 kph offset from dash speed ret.cruiseState.enabled = cp_adas.vl['HS2_DAT_MDD_CMD_452']['RVV_ACC_ACTIVATION_REQ'] == 1 - ret.cruiseState.available = cp_adas.vl['HS2_DYN1_MDD_ETAT_2B6']['ACC_STATUS'] > 2 - ret.cruiseState.nonAdaptive = cp_adas.vl['HS2_DAT_MDD_CMD_452']['LONGITUDINAL_REGULATION_TYPE'] != 3 # 0: None, 1: CC, 2: Limiter, 3: ACC - ret.cruiseState.standstill = bool(cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VEHICLE_STANDSTILL']) - ret.accFaulted = cp_adas.vl['HS2_DYN_UCF_MDD_32D']['ACC_ETAT_DECEL_OR_ESP_STATUS'] == 3 # 0: Inhibited, 1: Waiting, 2: Active, 3: Fault + ret.cruiseState.available = True # not available for CC-only + ret.cruiseState.nonAdaptive = False # not available for CC-only + ret.cruiseState.standstill = False # not available for CC-only + ret.accFaulted = False # not available for CC-only + # resume request + self.hs2_dat_mdd_cmd_452 = copy.copy(cp_adas.vl['HS2_DAT_MDD_CMD_452']) + self.accel_longi_calib = cp_adas.vl['HS2_DYN_UCF_MDD_32D']['ACCEL_LONGI_CALIB'] # gear if bool(cp_cam.vl['Dat_BSI']['P103_Com_bRevGear']): diff --git a/opendbc/car/psa/fingerprints.py b/opendbc/car/psa/fingerprints.py index 497f8bb134d..531998898c0 100644 --- a/opendbc/car/psa/fingerprints.py +++ b/opendbc/car/psa/fingerprints.py @@ -6,8 +6,11 @@ FW_VERSIONS = { CAR.PSA_PEUGEOT_208: { - (Ecu.fwdRadar, 0x6b6, None): [ - b'212053276', + # ARTIV - Radar + (Ecu.fwdRadar, 0x6B6, None): [ + b'212053276', # Peugeot e208 Allure Pack 2021 + b'194504751', # Peugeot e208 GT 2020 + b'222256113', # Peugeot e208 GT NZ 2022 ], }, } diff --git a/opendbc/car/psa/interface.py b/opendbc/car/psa/interface.py index f719a3d6aaa..a9f2a86ea42 100644 --- a/opendbc/car/psa/interface.py +++ b/opendbc/car/psa/interface.py @@ -16,7 +16,7 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_lo ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.psa)] - ret.dashcamOnly = True + ret.dashcamOnly = False ret.steerActuatorDelay = 0.3 ret.steerLimitTimer = 0.1 @@ -25,6 +25,9 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_lo ret.steerControlType = structs.CarParams.SteerControlType.angle ret.radarUnavailable = True - ret.alphaLongitudinalAvailable = False + ret.alphaLongitudinalAvailable = True + ret.openpilotLongitudinalControl = True + ret.startingState = True + ret.startAccel = 1.0 return ret \ No newline at end of file diff --git a/opendbc/car/psa/psacan.py b/opendbc/car/psa/psacan.py index a039e752a0e..c9405af49f3 100644 --- a/opendbc/car/psa/psacan.py +++ b/opendbc/car/psa/psacan.py @@ -1,5 +1,8 @@ +from opendbc.car.can_definitions import CanData + + def psa_checksum(address: int, sig, d: bytearray) -> int: - chk_ini = {0x452: 0x4, 0x38D: 0x7, 0x42D: 0xC}.get(address, 0xB) + chk_ini = {0x452: 0x4, 0x4f8: 0x4, 0x208: 0x5, 0x38D: 0x7, 0x2f6: 0x8, 0x2b6: 0xC, 0x42D: 0xC}.get(address, 0xB) byte = sig.start_bit // 8 d[byte] &= 0x0F if sig.start_bit % 8 >= 4 else 0xF0 checksum = sum((b >> 4) + (b & 0xF) for b in d) @@ -16,3 +19,71 @@ def create_lka_steering(packer, lat_active: bool, apply_angle: float, status: in } return packer.make_can_msg('LANE_KEEP_ASSIST', 0, values) + + +def create_resume_acc(packer, counter, status, hs2_dat_mdd_cmd_452): + hs2_dat_mdd_cmd_452['COUNTER'] = counter + hs2_dat_mdd_cmd_452['COCKPIT_GO_ACC_REQUEST'] = status + return packer.make_can_msg('HS2_DAT_MDD_CMD_452', 1, hs2_dat_mdd_cmd_452) + + +def create_drive_away_request(packer, hs2_dyn_mdd_etat_2f6): + hs2_dyn_mdd_etat_2f6['DRIVE_AWAY_REQUEST'] = 0 + return packer.make_can_msg('HS2_DYN_MDD_ETAT_2F6', 1, hs2_dyn_mdd_etat_2f6) + + +# Radar, 50 Hz +def create_HS2_DYN1_MDD_ETAT_2B6(packer, frame: int, accel: float, enabled: bool, gasPressed: bool, braking: int, brakePressed: int, standstill: bool, torque: int): + # TODO: if gas pressed, ACC_STATUS is set to suspended and decel can be set negative (about -300 Nm / -0.6m/s²) with brake mode inactive + # TODO: tune torque multiplier + # TODO: check difference between GMP_POTENTIAL_WHEEL_TORQUE and GMP_WHEEL_TORQUE + # TODO: transition from waiting to active enables torque control. For now, deactivate autohold or enable on brake pressed + + values = { + 'MDD_DESIRED_DECELERATION': accel if braking and enabled else 2.05, # m/s² + 'POTENTIAL_WHEEL_TORQUE_REQUEST': (2 if braking else 1) if enabled else 0, + 'MIN_TIME_FOR_DESIRED_GEAR': 0.0 if braking or not enabled else 6.2, + 'GMP_POTENTIAL_WHEEL_TORQUE': torque if not braking and enabled else -4000, + 'ACC_STATUS': (5 if gasPressed else 2 if brakePressed and not standstill else 4) if enabled else (2 if brakePressed else 3), + 'GMP_WHEEL_TORQUE': torque if not braking and enabled else -4000, + 'WHEEL_TORQUE_REQUEST': 1 if enabled else 0, # TODO: test 1: high torque range 2: low torque range + 'AUTO_BRAKING_STATUS': 3, # AEB # TODO: testing ALWAYS ENABLED to resolve DTC errors if enabled else 3, # maybe disabled on too high steering angle + 'MDD_DECEL_TYPE': int(braking), + 'MDD_DECEL_CONTROL_REQ': int(braking), + } + + return packer.make_can_msg('HS2_DYN1_MDD_ETAT_2B6', 1, values) + + +# Radar, 50 Hz +def create_HS2_DYN_MDD_ETAT_2F6(packer, braking): + values = { + # 'TARGET_DETECTED': 0, # TODO: + # 'REQUEST_TAKEOVER': 0, # TODO potential signal for HUD message from OP + # 'BLIND_SENSOR': 0, + # 'REQ_VISUAL_COLL_ALERT_ARC': 0, + # 'REQ_AUDIO_COLL_ALERT_ARC': 0, + # 'REQ_HAPTIC_COLL_ALERT_ARC': 0, + 'INTER_VEHICLE_DISTANCE': 255.5, # TODO: if enabled else 255.5, + 'ARC_STATUS': 6, # 12 after 50 frames (1 sec) after AUTO_BRAKING_STATUS else 6 + # 'AUTO_BRAKING_IN_PROGRESS': 0, + # 'AEB_ENABLED': 0, + # 'DRIVE_AWAY_REQUEST': 0, # TODO: potential RESUME request? + 'DISPLAY_INTERVEHICLE_TIME': 6.2, # TODO: