IQ.Pilot Prebuilt Release @ ab07000

iqdbc_repo/iqdbc/car/psa/carcontroller.py

@@ -0,0 +1,41 @@
+from iqdbc.can.packer import CANPacker
+from iqdbc.car import Bus
+from iqdbc.car.lateral import apply_std_steer_angle_limits
+from iqdbc.car.interfaces import CarControllerBase
+from iqdbc.car.psa.psacan import create_lka_steering
+from iqdbc.car.psa.values import CarControllerParams
+
+
+class CarController(CarControllerBase):
+  def __init__(self, dbc_names, CP, CP_IQ):
+    super().__init__(dbc_names, CP, CP_IQ)
+    self.packer = CANPacker(dbc_names[Bus.main])
+    self.apply_angle_last = 0
+    self.status = 2
+
+  def update(self, CC, CC_IQ, CS, now_nanos):
+    can_sends = []
+    actuators = CC.actuators
+
+    # lateral control
+    if self.frame % 5 == 0:
+      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
+
+      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
+    self.frame += 1
+    return new_actuators, can_sends

iqdbc_repo/iqdbc/car/psa/carstate.py

@@ -0,0 +1,74 @@
+from iqdbc.car import structs, Bus
+from iqdbc.can.parser import CANParser
+from iqdbc.car.common.conversions import Conversions as CV
+from iqdbc.car.psa.values import DBC, CarControllerParams
+from iqdbc.car.interfaces import CarStateBase
+
+GearShifter = structs.CarState.GearShifter
+TransmissionType = structs.CarParams.TransmissionType
+
+
+class CarState(CarStateBase):
+  def update(self, can_parsers) -> structs.CarState:
+    cp = can_parsers[Bus.main]
+    cp_adas = can_parsers[Bus.adas]
+    cp_cam = can_parsers[Bus.cam]
+    ret = structs.CarState()
+    ret_iq = structs.IQCarState()
+
+    # car speed
+    self.parse_wheel_speeds(ret,
+      cp.vl['Dyn4_FRE']['P263_VehV_VPsvValWhlFrtL'],
+      cp.vl['Dyn4_FRE']['P264_VehV_VPsvValWhlFrtR'],
+      cp.vl['Dyn4_FRE']['P265_VehV_VPsvValWhlBckL'],
+      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'])
+
+    # gas
+    ret.gasPressed = cp.vl['Dyn_CMM']['P002_Com_rAPP'] > 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
+    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)
+    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
+
+    # gear
+    if bool(cp_cam.vl['Dat_BSI']['P103_Com_bRevGear']):
+      ret.gearShifter = GearShifter.reverse
+    else:
+      ret.gearShifter = GearShifter.drive
+
+    # blinkers
+    blinker = cp_cam.vl['HS2_DAT7_BSI_612']['CDE_CLG_ET_HDC']
+    ret.leftBlinker = blinker == 1
+    ret.rightBlinker = blinker == 2
+
+    # lock info
+    ret.doorOpen = any((cp_cam.vl['Dat_BSI']['DRIVER_DOOR'], cp_cam.vl['Dat_BSI']['PASSENGER_DOOR']))
+    ret.seatbeltUnlatched = cp_cam.vl['RESTRAINTS']['DRIVER_SEATBELT'] != 2
+    return ret, ret_iq
+
+  @staticmethod
+  def get_can_parsers(CP, CP_IQ):
+    return {
+      Bus.main: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0),
+      Bus.adas: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1),
+      Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
+    }

iqdbc_repo/iqdbc/car/psa/fingerprints.py

@@ -0,0 +1,13 @@
+""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
+from iqdbc.car.structs import CarParams
+from iqdbc.car.psa.values import CAR
+
+Ecu = CarParams.Ecu
+
+FW_VERSIONS = {
+  CAR.PSA_PEUGEOT_208: {
+    (Ecu.fwdRadar, 0x6b6, None): [
+      b'212053276',
+    ],
+  },
+}

iqdbc_repo/iqdbc/car/psa/interface.py

@@ -0,0 +1,30 @@
+from iqdbc.car import structs, get_safety_config
+from iqdbc.car.interfaces import CarInterfaceBase
+from iqdbc.car.psa.carcontroller import CarController
+from iqdbc.car.psa.carstate import CarState
+
+TransmissionType = structs.CarParams.TransmissionType
+
+
+class CarInterface(CarInterfaceBase):
+  CarState = CarState
+  CarController = CarController
+
+  @staticmethod
+  def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
+    ret.brand = 'psa'
+
+    ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.psa)]
+
+    ret.dashcamOnly = True
+
+    ret.steerActuatorDelay = 0.3
+    ret.steerLimitTimer = 0.1
+    ret.steerAtStandstill = True
+
+    ret.steerControlType = structs.CarParams.SteerControlType.angle
+    ret.radarUnavailable = True
+
+    ret.alphaLongitudinalAvailable = False
+
+    return ret

iqdbc_repo/iqdbc/car/psa/psacan.py

@@ -0,0 +1,18 @@
+def psa_checksum(address: int, sig, d: bytearray) -> int:
+  chk_ini = {0x452: 0x4, 0x38D: 0x7, 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)
+  return (chk_ini - checksum) & 0xF
+
+
+def create_lka_steering(packer, lat_active: bool, apply_angle: float, status: int):
+  values = {
+    'DRIVE': 1,
+    'STATUS': status,
+    'LXA_ACTIVATION': 1,
+    'TORQUE_FACTOR': lat_active * 100,
+    'SET_ANGLE': apply_angle,
+  }
+
+  return packer.make_can_msg('LANE_KEEP_ASSIST', 0, values)

iqdbc_repo/iqdbc/car/psa/values.py

@@ -0,0 +1,54 @@
+from dataclasses import dataclass, field
+
+from iqdbc.car.structs import CarParams
+from iqdbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
+from iqdbc.car.lateral import AngleSteeringLimits
+from iqdbc.car.docs_definitions import CarDocs, CarHarness, CarParts
+from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+
+Ecu = CarParams.Ecu
+
+
+class CarControllerParams:
+  STEER_STEP = 1
+
+  ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
+    390, # deg
+    ([0., 5., 25.], [2.5, 1.5, .2]),
+    ([0., 5., 25.], [5., 2., .3]),
+  )
+  STEER_DRIVER_ALLOWANCE = 5  # Driver intervention threshold, 0.5 Nm
+
+
+@dataclass
+class PSACarDocs(CarDocs):
+  package: str = "Adaptive Cruise Control (ACC) & Lane Assist"
+  car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.psa_a]))
+
+
+@dataclass
+class PSAPlatformConfig(PlatformConfig):
+  dbc_dict: DbcDict = field(default_factory=lambda: {
+    Bus.pt: 'psa_aee2010_r3',
+  })
+
+
+class CAR(Platforms):
+  PSA_PEUGEOT_208 = PSAPlatformConfig(
+    [PSACarDocs("Peugeot 208 2019-25")],
+    CarSpecs(mass=1530, wheelbase=2.54, steerRatio=17.6),
+  )
+
+
+# Placeholder, FW Query will be added in separate PR
+FW_QUERY_CONFIG = FwQueryConfig(
+  requests=[
+    Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
+      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
+      bus=0,
+    ),
+  ],
+)
+
+DBC = CAR.create_dbc_map()