Psa

opendbc/car/psa/carcontroller.py

@@ -1,41 +1,110 @@
 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
+from cereal import messaging
+import math
+
+LongCtrlState = structs.CarControl.Actuators.LongControlState
+sm = messaging.SubMaster(['modelV2'], poll='modelV2')
 
 
 class CarController(CarControllerBase):
   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
+    self.bars = 4
 
   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
+    # 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
+
+    # TUNING
+    # >=-0.5: Engine brakes only
+    # <-0.5: Add friction brakes
+    pitch = CC.orientationNED[1] if len(CC.orientationNED) == 3 else 0.0
+    accel_slope = math.sin(pitch) * 9.81
+    accel_cmd = actuators.accel + accel_slope
+
+    brake_accel = -0.5
+
+    # torque lookup
+    ACCEL_LOOKUP = [-1.0, -0.5, 0.0, 0.5, 1.0, 1.5, 2.0]
+    TORQUE_LOOKUP = [-400, -300, 120, 350, 550, 800, 1000]
+
+    # calculate Torque
+    torque_nm = interp(accel_cmd, ACCEL_LOOKUP, TORQUE_LOOKUP)
+    torque = max(-400, min(torque_nm, 1000))
+
+    braking = accel_cmd < brake_accel and not CS.out.gasPressed
+    if self.CP.openpilotLongitudinalControl:
+      if CC.hudControl.leadVisible:
+        sm.update(0)
+        leads_v3 = sm['modelV2'].leadsV3
+        if leads_v3 and leads_v3[0].x:
+          r = leads_v3[0].x[0] / (5 + CS.out.vEgo)
+          if self.bars > 3:  # initialize from "no lead"
+            self.bars = min(3, int(r))
+          elif r > self.bars + 1.2:
+            self.bars = min(3, self.bars + 1)
+          elif r < self.bars - 0.2:
+            self.bars = max(0, self.bars - 1)
       else:
-        self.status = 4
+        self.bars = 4
+
+      # 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))
+
+      # 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²
+
+      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, CC.hudControl.leadVisible, self.bars))
 
-      can_sends.append(create_lka_steering(self.packer, CC.latActive, apply_angle, self.status))
+    # stock long
+    # emulate resume button every 3 seconds to prevent autohold timeout
+    elif CC.latActive and CS.out.standstill and CC.hudControl.leadVisible:
+      # map: {frame:status} - 0, 1
+      status = {0: 0, 5: 1}.get(self.frame % 300)
+      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))
 
-      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

opendbc/car/psa/carstate.py

@@ -1,7 +1,9 @@
+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.mazda.values import LKAS_LIMITS
+from opendbc.car.psa.values import CAR, DBC, CarControllerParams
 from opendbc.car.interfaces import CarStateBase
 
 GearShifter = structs.CarState.GearShifter
@@ -26,27 +28,63 @@ def update(self, can_parsers) -> structs.CarState:
     ret.standstill = bool(cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VEHICLE_STANDSTILL'])
 
     # gas
-    ret.gasPressed = cp.vl['Dyn_CMM']['P002_Com_rAPP'] > 0
+    # gas
+    if self.CP.carFingerprint == CAR.PSA_PEUGEOT_3008:
+      ret.gasPressed = cp.vl['Dyn5_CMM']['P334_ACCPed_Position'] > 0
+    else:
+      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
 
+    # brake pressure
+    if self.CP.carFingerprint == CAR.PSA_PEUGEOT_3008:
+      raw = cp.vl["Dyn2_FRE"]["BRAKE_PRESSURE"]
+      ret.brake = max(0.0, float(raw) - 550.0)  # clamp a 0
+
     # 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,
+      CAR.PSA_PEUGEOT_508: cp_cam.vl,
+      CAR.PSA_PEUGEOT_3008: cp.vl,
+
+    }
+    bus = STEERING_ALT_BUS[self.CP.carFingerprint]
+    ret.steeringAngleDeg = bus['STEERING_ALT']['ANGLE'] # EPS
+    if self.CP.carFingerprint == CAR.PSA_PEUGEOT_3008:
+      # PSA EPS encodes the steering rotation direction bit inverted from the driver's perspective:
+      #   RATE_SIGN = 0 → clockwise (right turn)
+      #   RATE_SIGN = 1 → anticlockwise (left turn)
+      # Invert the sign to match OpenPilot's convention: right = positive, left = negative.
+      ret.steeringRateDeg = bus['STEERING_ALT']['RATE'] * (1 - 2 * bus['STEERING_ALT']['RATE_SIGN'])
+    else:
+      # Convert EPS direction bit [0,1] to signed multiplier [-1,+1]
+      # Standard convention: 0 → left (negative), 1 → right (positive)
+     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)
+
+    if self.CP.carFingerprint == CAR.PSA_PEUGEOT_3008:
+        # Peugeot 3008: EPS_TORQUE represents only driver-applied torque (no motor assist).
+        # The signal is already smoothed by the EPS ECU, so update_steering_pressed is unnecessary.
+        ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD
+    else:
+        ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
+
+    # ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
     self.eps_active = cp.vl['IS_DAT_DIRA']['EPS_STATE_LKA'] == 3 # 0: Unauthorized, 1: Authorized, 2: Available, 3: Active, 4: Defect
 
     # 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 = cp_adas.vl['HS2_DYN_UCF_MDD_32D']['ACC_ETAT_DECEL_OR_ESP_STATUS'] == 3
+    # resume request
+    self.hs2_dat_mdd_cmd_452 = copy.copy(cp_adas.vl['HS2_DAT_MDD_CMD_452'])
 
     # gear
     if bool(cp_cam.vl['Dat_BSI']['P103_Com_bRevGear']):
@@ -56,8 +94,22 @@ def update(self, can_parsers) -> structs.CarState:
 
     # blinkers
     blinker = cp_cam.vl['HS2_DAT7_BSI_612']['CDE_CLG_ET_HDC']
-    ret.leftBlinker = blinker == 1
-    ret.rightBlinker = blinker == 2
+    if self.CP.carFingerprint == CAR.PSA_PEUGEOT_3008:
+      ret.leftBlinker = blinker == 2
+      ret.rightBlinker = blinker == 1
+    else:
+      ret.leftBlinker = blinker == 1
+      ret.rightBlinker = blinker == 2
+
+    # Blind sensor ( there is not left and right )
+    if self.CP.carFingerprint == CAR.PSA_PEUGEOT_3008:
+      ret.leftBlindspot = cp_adas.vl["HS2_DYN_MDD_ETAT_2F6"]["BLIND_SENSOR"] != 0
+      ret.rightBlindspot = cp_adas.vl["HS2_DYN_MDD_ETAT_2F6"]["BLIND_SENSOR"] != 0
+
+    # Auto Braking in progress
+    if self.CP.carFingerprint == CAR.PSA_PEUGEOT_3008:
+      ret.stockAeb = cp_adas.vl["HS2_DYN1_MDD_ETAT_2B6"]["AUTO_BRAKING_STATUS"] == 1
+
 
     # lock info
     ret.doorOpen = any((cp_cam.vl['Dat_BSI']['DRIVER_DOOR'], cp_cam.vl['Dat_BSI']['PASSENGER_DOOR']))

opendbc/car/psa/fingerprints.py

@@ -6,8 +6,64 @@
 
 FW_VERSIONS = {
   CAR.PSA_PEUGEOT_208: {
-    (Ecu.fwdRadar, 0x6b6, None): [
-      b'212053276',
+    (Ecu.abs, 0x6ad, None): [
+      b'085095700857210527',
+      b'085095706198220818',
+      b'085095700473220908',
+      b'085135705863191103',
+      b'285160381930060025B',
+      b'085095705963200902',
+      b'085095709268220917',
+      b'085135709971211107',
+      b'085135702270200218',
+    ],
+  },
+  CAR.PSA_PEUGEOT_508: {
+    (Ecu.abs, 0x6ad, None): [
+      b'085065315928191130',
+      b'085095308910190312',
+      b'085095701207240115',
+      b'085135702688200218',
+      b'085044414569231210',
+      b'369042321125160806',
+    ],
+  },
+
+  CAR.PSA_PEUGEOT_3008: {
+    # ARTIV - Front Radar ADAS
+    (Ecu.fwdRadar, 0x6B6, None): [ ],
+
+    # DIRECTN - Electronic power steering
+    (Ecu.eps, 0x6B5, None): [ ],
+
+    # HCU2 - Hybrid Control Unit
+    (Ecu.hybrid, 0x6A6, None): [  ],
+
+    # BOITEVIT - Automatic transmission  (EAT6/8)
+    (Ecu.transmission, 0x6A9, None): [
+        ## Peugeot 3008 II (Phase I, 2016) 1.6 PureTech (180 Hp) Automatic S&S /2018, 2019, 2020
+        b'1614191B101502000000',
+        b'\xff\xff\x00\x000`\x08\x01\x13\x01%\x06\x08\xff\xff\xff\x00\x02\x00\x00\x01\x934t',
+        #
+    ],
+
+    # FREINEBB - Electronic Brake Booster
+    (Ecu.electricBrakeBooster, 0x5D0, None): [],
+
+    # INJ - Engine (VCU)
+    (Ecu.engine, 0x6A8, None): [
+        ## Peugeot 3008 II (Phase I, 2016) 1.6 PureTech (180 Hp) Automatic S&S /2018, 2019, 2020
+        b'000D170047100',
+        b'\xff\xff\x00\x00\x03&\t\x02\x13\x01C1\x04\xff\xff\xff\x00\x02\x00\x00\x01\x93YW',
+        #
+    ],
+
+    # ABRASR - ABS/ESP
+    (Ecu.abs, 0x6AD, None): [
+        ## Peugeot 3008 II (Phase I, 2016) 1.6 PureTech (180 Hp) Automatic S&S /2018, 2019, 2020
+        b'085065201906190129',
+        b'\x00\x00\x00\x00\x03\x92\x01\x06\x11\x01\x06\x18\x02\xff\xff\xff\x00\x02\x00\x00\x01\x92\x83\x12',
+        #
     ],
   },
 }

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,12 @@ 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 = alpha_long
+    ret.startingState = True
+    ret.startAccel = 1.0
+
+    # ret.longitudinalTuning.kiBP = [0., 35.]
+    # ret.longitudinalTuning.kiV = [0.5, 0.2]
 
     return ret