IQ.Pilot Prebuilt Release @ 7a91404

2026-08-31 23:04:09 -05:00
commit e2b219bcf7
2545 changed files with 677873 additions and 0 deletions
--- a/artifacts/package_runtime/iqdbc/car/psa/carstate.py
+++ b/artifacts/package_runtime/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),
+    }