Hongqi HS5

selfdrive/car/hongqi/carcontroller.py

@@ -0,0 +1,41 @@
+from selfdrive.car import apply_std_steer_torque_limits
+from selfdrive.car.hongqi import hongqican
+from selfdrive.car.hongqi.values import DBC_FILES, CANBUS, CarControllerParams as P
+from opendbc.can.packer import CANPacker
+
+class CarController():
+  def __init__(self, dbc_name, CP, VM):
+    self.apply_steer_last = 0
+    self.CP = CP
+
+    self.packer_pt = CANPacker(DBC_FILES.hongqi)
+
+  def update(self, c, CS, frame, actuators):
+    """ Controls thread """
+
+    can_sends = []
+
+    # **** Steering Controls ************************************************ #
+
+    if frame % P.LKAS_STEP == 0:
+      if c.latActive:
+        new_steer = int(round(actuators.steer * P.STEER_MAX))
+        apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
+      else:
+        apply_steer = 0
+
+      self.apply_steer_last = apply_steer
+      can_sends.append(hongqican.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, c.latActive))
+
+    # **** HUD Controls ***************************************************** #
+
+    # TODO
+
+    # **** ACC Button Controls ********************************************** #
+
+    # TODO
+
+    new_actuators = actuators.copy()
+    new_actuators.steer = self.apply_steer_last / P.STEER_MAX
+
+    return new_actuators, can_sends

selfdrive/car/hongqi/carstate.py

@@ -0,0 +1,155 @@
+import numpy as np
+from cereal import car
+from selfdrive.car.interfaces import CarStateBase
+from opendbc.can.parser import CANParser
+from opendbc.can.can_define import CANDefine
+from selfdrive.car.hongqi.values import DBC_FILES, CANBUS, GearShifter, CarControllerParams
+
+class CarState(CarStateBase):
+  def __init__(self, CP):
+    super().__init__(CP)
+    can_define = CANDefine(DBC_FILES.hongqi)
+    # TODO: populate this
+    # self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"]
+    self.lkas_status_values = can_define.dv["EPS_2"]["LKAS_STATUS"]
+
+  def update(self, pt_cp, cam_cp):
+    ret = car.CarState.new_message()
+    # Update vehicle speed and acceleration from ABS wheel speeds.
+    ret.wheelSpeeds = self.get_wheel_speeds(
+      pt_cp.vl["ABS_1"]["FRONT_LEFT"],
+      pt_cp.vl["ABS_1"]["FRONT_RIGHT"],
+      pt_cp.vl["ABS_2"]["REAR_LEFT"],
+      pt_cp.vl["ABS_2"]["REAR_RIGHT"],
+    )
+
+    ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
+    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
+    ret.standstill = ret.vEgo < 0.1
+
+    # Update steering angle, rate, yaw rate, and driver input torque. FAW send
+    # the sign/direction in a separate signal so they must be recombined.
+    ret.steeringAngleDeg = pt_cp.vl["EPS_1"]["STEER_ANGLE"] * (1, -1)[int(pt_cp.vl["EPS_1"]["STEER_ANGLE_DIRECTION"])]
+    ret.steeringRateDeg = pt_cp.vl["EPS_1"]["STEER_RATE"] * (1, -1)[int(pt_cp.vl["EPS_1"]["STEER_RATE_DIRECTION"])]
+    # FIXME: don't have a solid direction bit for this yet, borrow from overall EPS output
+    ret.steeringTorque = pt_cp.vl["EPS_2"]["DRIVER_INPUT_TORQUE"] * (1, -1)[int(pt_cp.vl["EPS_2"]["EPS_TORQUE_DIRECTION"])]
+    ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
+    # TODO: populate this
+    # ret.yawRate = pt_cp.vl["ESP_02"]["ESP_Gierrate"] * (1, -1)[int(pt_cp.vl["ESP_02"]["ESP_VZ_Gierrate"])] * CV.DEG_TO_RAD
+
+    # Verify EPS readiness to accept steering commands
+    lkas_status = self.lkas_status_values.get(pt_cp.vl["EPS_2"]["LKAS_STATUS"])
+    ret.steerFaultPermanent = lkas_status == "FAULT"
+    ret.steerFaultTemporary = lkas_status == "INITIALIZING"
+
+    # Update gas, brakes, and gearshift.
+    ret.gas = pt_cp.vl["ECM_1"]["DRIVER_THROTTLE"]
+    ret.gasPressed = ret.gas > 0
+    ret.brake = pt_cp.vl["ABS_3"]["DRIVER_BRAKE_PRESSURE"]
+    ret.brakePressed = bool(pt_cp.vl["MAYBE_ABS"]["BRAKE_PRESSED"])
+    # TODO: populate this
+    # ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"])  # FIXME: need to include an EPB check as well
+
+    # Update gear and/or clutch position data.
+    # TODO: populate this
+    # ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["Getriebe_11"]["GE_Fahrstufe"], None))
+    ret.gearShifter = GearShifter.drive
+
+    # Update door and trunk/hatch lid open status.
+    # TODO: find trunk signal
+    ret.doorOpen = any([pt_cp.vl["DOOR_FL"]["OPEN"],
+                        pt_cp.vl["DOOR_FR"]["OPEN"],
+                        pt_cp.vl["DOOR_RL"]["OPEN"],
+                        pt_cp.vl["DOOR_RR"]["OPEN"]])
+
+    # Update seatbelt fastened status.
+    # TODO: populate this
+    # ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3
+
+    # Consume blind-spot monitoring info/warning LED states, if available.
+    # Infostufe: BSM LED on, Warnung: BSM LED flashing
+    # TODO: populate this
+    # if self.CP.enableBsm:
+    #   ret.leftBlindspot = bool(ext_cp.vl["SWA_01"]["SWA_Infostufe_SWA_li"]) or bool(ext_cp.vl["SWA_01"]["SWA_Warnung_SWA_li"])
+    #   ret.rightBlindspot = bool(ext_cp.vl["SWA_01"]["SWA_Infostufe_SWA_re"]) or bool(ext_cp.vl["SWA_01"]["SWA_Warnung_SWA_re"])
+
+    # TODO: populate if possible
+    # ret.stockFcw = bool(ext_cp.vl["ACC_10"]["AWV2_Freigabe"])
+    # ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"])
+
+    # Update ACC radar status.
+    # TODO: populate this properly, need an available signal and overrides (11 avail?, 27 gas override?, 19 coastdown?)
+    ret.cruiseState.available = True
+    ret.cruiseState.enabled = cam_cp.vl["ACC"]["STATUS"] in (4, 5, 6, 7)
+
+    # Update ACC setpoint.
+    # TODO: populate this
+    #    ret.cruiseState.speed = 0
+
+    # Update control button states for turn signals and ACC controls.
+    ret.leftBlinker = bool(pt_cp.vl["TURN_SIGNALS"]["LEFT_TURN_SIGNALING"])
+    ret.rightBlinker = bool(pt_cp.vl["TURN_SIGNALS"]["RIGHT_TURN_SIGNALING"])
+
+    # TODO: populate this
+    # ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0
+
+    return ret
+
+  @staticmethod
+  def get_can_parser(CP):
+    signals = [
+      # sig_name, sig_address
+      ("DRIVER_THROTTLE", "ECM_1"),
+      ("DRIVER_THROTTLE", "ECM_1"),
+      ("FRONT_LEFT", "ABS_1"),
+      ("FRONT_RIGHT", "ABS_1"),
+      ("REAR_LEFT", "ABS_2"),
+      ("REAR_RIGHT", "ABS_2"),
+      ("DRIVER_BRAKE_PRESSURE", "ABS_3"),
+      ("BRAKE_PRESSED", "MAYBE_ABS"),
+      ("STEER_ANGLE", "EPS_1"),
+      ("STEER_ANGLE_DIRECTION", "EPS_1"),
+      ("STEER_RATE", "EPS_1"),
+      ("STEER_RATE_DIRECTION", "EPS_1"),
+      ("EPS_TORQUE", "EPS_2"),
+      ("EPS_TORQUE_DIRECTION", "EPS_2"),
+      ("DRIVER_INPUT_TORQUE", "EPS_2"),
+      ("LKAS_STATUS", "EPS_2"),
+      ("OPEN", "DOOR_FL"),
+      ("OPEN", "DOOR_FR"),
+      ("OPEN", "DOOR_RL"),
+      ("OPEN", "DOOR_RR"),
+      ("LEFT_TURN_SIGNALING", "TURN_SIGNALS"),
+      ("RIGHT_TURN_SIGNALING", "TURN_SIGNALS"),
+    ]
+
+    checks = [
+      # sig_address, frequency
+      ("ECM_1", 100),
+      ("ABS_1", 100),
+      ("ABS_2", 100),
+      ("MAYBE_ABS", 100),  # FIXME: figure out a better name
+      ("ABS_3", 50),
+      ("EPS_1", 50),
+      ("EPS_2", 50),
+      ("DOOR_FL", 10),
+      ("DOOR_FR", 10),
+      ("DOOR_RL", 10),
+      ("DOOR_RR", 10),
+      ("TURN_SIGNALS", 10),
+    ]
+
+    return CANParser(DBC_FILES.hongqi, signals, checks, CANBUS.pt)
+
+  @staticmethod
+  def get_cam_can_parser(CP):
+    signals = [
+      # sig_name, sig_address
+      ("STATUS", "ACC")
+    ]
+    checks = [
+      # sig_address, frequency
+      ("ACC", 50),
+    ]
+
+    return CANParser(DBC_FILES.hongqi, signals, checks, CANBUS.cam)

selfdrive/car/hongqi/hongqican.py

@@ -0,0 +1,9 @@
+def create_steering_control(packer, bus, apply_steer, lkas_enabled):
+  values = {
+    "LKAS_TORQUE_ACTIVE": lkas_enabled,
+    "LKAS_TORQUE": abs(apply_steer) if lkas_enabled else 1022,
+    "LKAS_TORQUE_DIRECTION": 1 if apply_steer < 0 else 0,
+    "LKAS_ACTIVE": lkas_enabled,
+    "MAYBE_HUD_LANE_STATES": 3 if lkas_enabled else 1,
+  }
+  return packer.make_can_msg("LKAS", bus, values)

selfdrive/car/hongqi/interface.py

@@ -0,0 +1,58 @@
+from cereal import car
+from selfdrive.car.hongqi.values import CAR, GearShifter
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, get_safety_config
+from selfdrive.car.interfaces import CarInterfaceBase
+
+EventName = car.CarEvent.EventName
+
+
+class CarInterface(CarInterfaceBase):
+  # def __init__(self, CP, CarController, CarState):
+  #   super().__init__(CP, CarController, CarState)
+
+  @staticmethod
+  def _get_params(ret, candidate, fingerprint, car_fw, experimental_long):
+    ret.carName = "hongqi"
+    ret.radarOffCan = True
+
+    # Set global Hongqi parameters
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hongqi)]
+    # TODO: identify BSM signals
+    # ret.enableBsm = 0x30F in fingerprint[0]  # SWA_01
+
+    # Global lateral tuning defaults, can be overridden per-vehicle
+
+    ret.steerActuatorDelay = 0.1
+    ret.steerLimitTimer = 0.4
+    ret.steerRatio = 15.6  # Let the params learner figure this out
+    tire_stiffness_factor = 1.0  # Let the params learner figure this out
+    CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+
+    # Per-chassis tuning values, override tuning defaults here if desired
+
+    if candidate == CAR.HS5_G1:
+      ret.mass = 1780 + STD_CARGO_KG
+      ret.wheelbase = 2.87
+
+    else:
+      raise ValueError(f"unsupported car {candidate}")
+
+    ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
+    ret.centerToFront = ret.wheelbase * 0.45
+    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
+                                                                         tire_stiffness_factor=tire_stiffness_factor)
+    return ret
+
+  # returns a car.CarState
+  def _update(self, c):
+    ret = self.CS.update(self.cp, self.cp_cam)
+
+    events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic])
+    ret.events = events.to_msg()
+
+    return ret
+
+  def apply(self, c):
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators)
+    self.frame += 1
+    return ret

selfdrive/car/hongqi/radar_interface.py

@@ -0,0 +1,5 @@
+#!/usr/bin/env python3
+from selfdrive.car.interfaces import RadarInterfaceBase
+
+class RadarInterface(RadarInterfaceBase):
+  pass

selfdrive/car/hongqi/values.py

@@ -0,0 +1,85 @@
+from collections import defaultdict
+from dataclasses import dataclass
+from enum import Enum
+from typing import Dict, List, Union
+
+from cereal import car
+from panda.python import uds
+from selfdrive.car import dbc_dict
+from selfdrive.car.docs_definitions import CarInfo, Harness
+from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
+
+Ecu = car.CarParams.Ecu
+GearShifter = car.CarState.GearShifter
+
+
+class CarControllerParams:
+  LKAS_STEP = 2                  # LKAS message frequency 50Hz
+
+  # TODO: placeholder values pending discovery of true EPS limits
+  STEER_MAX = 300                # As-yet unknown fault boundary, guessing 300 / 3.0Nm for now
+  STEER_DELTA_UP = 6             # 10 unit/sec observed from factory LKAS, fault boundary unknown
+  STEER_DELTA_DOWN = 10          # 10 unit/sec observed from factory LKAS, fault boundary unknown
+  STEER_DRIVER_ALLOWANCE = 12
+  STEER_DRIVER_MULTIPLIER = 3    # weight driver torque heavily
+  STEER_DRIVER_FACTOR = 1        # from dbc
+
+
+class CANBUS:
+  pt = 0
+  cam = 2
+
+
+class DBC_FILES:
+  hongqi = "hongqi_hs5"
+
+
+DBC = defaultdict(lambda: dbc_dict(DBC_FILES.hongqi, None))  # type: Dict[str, Dict[str, str]]
+
+
+class CAR:
+  HS5_G1 = "HONGQI HS5 1ST GEN"          # First generation FAW Hongqi HS5 SUV
+
+
+@dataclass
+class HongqiCarInfo(CarInfo):
+  package: str = "Who Knows"  # FIXME
+  harness: Enum = Harness.custom
+
+
+CAR_INFO: Dict[str, Union[HongqiCarInfo, List[HongqiCarInfo]]] = {
+  CAR.HS5_G1: HongqiCarInfo("Hongqi HS5 2020"),
+}
+
+
+HONGQI_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
+  p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER)
+HONGQI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
+  p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER)
+
+FW_QUERY_CONFIG = FwQueryConfig(
+  requests=[
+    Request(
+      [HONGQI_VERSION_REQUEST],
+      [HONGQI_VERSION_RESPONSE],
+    ),
+  ],
+)
+
+
+FW_VERSIONS = {
+  CAR.HS5_G1: {
+    (Ecu.engine, 0x7e0, None): [
+      b'3601015-DD21    ',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'3611015-DD01\x00\x00\x00\x00',
+    ],
+    (Ecu.eps, 0x7a2, None): [
+      b'3418310-DD01\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x722, None): [
+      b'3616215-DD03-B  ',
+    ],
+  },
+}

selfdrive/car/tests/routes.py

@@ -2,6 +2,7 @@
 from collections import namedtuple
 
 from selfdrive.car.chrysler.values import CAR as CHRYSLER
+from selfdrive.car.hongqi.values import CAR as HONGQI
 from selfdrive.car.gm.values import CAR as GM
 from selfdrive.car.ford.values import CAR as FORD
 from selfdrive.car.honda.values import CAR as HONDA
@@ -45,6 +46,8 @@
   CarTestRoute("62241b0c7fea4589|2022-09-01--15-32-49", FORD.EXPLORER_MK6),
   #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION),
 
+  CarTestRoute("7d3307665954222a|2022-08-07--10-20-43", HONGQI.HS5_G1),
+
   CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA),
   CarTestRoute("aa20e335f61ba898|2019-02-05--16-59-04", GM.BUICK_REGAL),
   CarTestRoute("46460f0da08e621e|2021-10-26--07-21-46", GM.ESCALADE_ESV),

selfdrive/car/torque_data/override.yaml

@@ -37,6 +37,7 @@ GENESIS GV70 1ST GEN: [2.42, 2.42, 0.1]
 KIA SORENTO PLUG-IN HYBRID 4TH GEN: [2.5, 2.5, 0.1]
 GENESIS GV60 ELECTRIC 1ST GEN: [2.5, 2.5, 0.1]
 KIA SORENTO 4TH GEN: [2.5, 2.5, 0.1]
+HONGQI HS5 1ST GEN: [1.5, 1.5, 0.1]
 
 # Dashcam or fallback configured as ideal car
 mock: [10.0, 10, 0.0]