↔️ Use Torque for Lateral Controller

Pulled commit from future Comma work.

The KIA commit was pulled just to make my life easier.

commit 0eae5b3
Author: Shane Smiskol <shane@smiskol.com>
Date:   Thu Jun 2 01:06:14 2022 -0700

    Move some Toyotas to the torque tune for testing

commit cb91f7c
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Fri May 27 10:21:02 2022 -0700

    Tssp prius torque control (commaai#24669)

    * use llk

    * use steering sensor at low speed stil

    * Try more simple

    * rm prius tune

    * updated ref

commit baaa47f
Author: HaraldSchafer <harald.the.engineer@gmail.com>
Date:   Tue May 24 17:52:33 2022 -0700

    Rerevert torque control (commaai#24649)

    * Revert "Revert torque control (commaai#24565)"

    This reverts commit 9f8b037.

    * Move tune out of car specific stuff

    * Update ref commit

commit 7c69036
Author: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date:   Mon May 23 19:16:17 2022 -0700

    Kia EV6 (commaai#24485)

    * ev6 squash

    * test route

    * cleanup

    * remove those

    * resume spam

    * setme

    * more reliable button pressing

    * new steer signals

    * for now

    * update docs

release/files_common

@@ -168,7 +168,6 @@ selfdrive/controls/lib/events.py
 selfdrive/controls/lib/lane_planner.py
 selfdrive/controls/lib/latcontrol_angle.py
 selfdrive/controls/lib/latcontrol_indi.py
-selfdrive/controls/lib/latcontrol_lqr.py
 selfdrive/controls/lib/latcontrol_torque.py
 selfdrive/controls/lib/latcontrol_pid.py
 selfdrive/controls/lib/latcontrol.py
@@ -491,6 +490,7 @@ opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc
 opendbc/honda_insight_ex_2019_can_generated.dbc
 opendbc/acura_ilx_2016_nidec.dbc
 
+opendbc/kia_ev6.dbc
 opendbc/hyundai_kia_generic.dbc
 opendbc/hyundai_kia_mando_front_radar.dbc
 

selfdrive/car/hyundai/carcontroller.py

@@ -3,8 +3,8 @@
 from common.numpy_fast import clip, interp
 from common.conversions import Conversions as CV
 from selfdrive.car import apply_std_steer_torque_limits
-from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc, create_acc_commands, create_acc_opt, create_frt_radar_opt
-from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR
+from selfdrive.car.hyundai import hda2can, hyundaican
+from selfdrive.car.hyundai.values import Buttons, CarControllerParams, HDA2_CAR, CAR
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -44,13 +44,12 @@ def __init__(self, dbc_name, CP, VM):
     self.apply_steer_last = 0
     self.car_fingerprint = CP.carFingerprint
     self.steer_rate_limited = False
-    self.last_resume_frame = 0
+    self.last_button_frame = 0
     self.accel = 0
 
   def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
-    pcm_cancel_cmd = CC.cruiseControl.cancel
 
     # Steering Torque
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
@@ -67,58 +66,75 @@ def update(self, CC, CS):
 
     can_sends = []
 
-    # tester present - w/ no response (keeps radar disabled)
-    if self.CP.openpilotLongitudinalControl:
-      if self.frame % 100 == 0:
-        can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
-
-    can_sends.append(create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive,
-                                   CS.lkas11, sys_warning, sys_state, CC.enabled,
-                                   hud_control.leftLaneVisible, hud_control.rightLaneVisible,
-                                   left_lane_warning, right_lane_warning))
-
-    if not self.CP.openpilotLongitudinalControl:
-      if pcm_cancel_cmd:
-        can_sends.append(create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL))
-      elif CS.out.cruiseState.standstill:
-        # send resume at a max freq of 10Hz
-        if (self.frame - self.last_resume_frame) * DT_CTRL > 0.1:
-          # send 25 messages at a time to increases the likelihood of resume being accepted
-          can_sends.extend([create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
-          self.last_resume_frame = self.frame
-
-    if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
-      accel = actuators.accel
-      jerk = 0
-
-      if CC.longActive:
-        jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
-        if accel < 0:
-          accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
-
-      accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
-
-      lead_visible = False
-      stopping = actuators.longControlState == LongCtrlState.stopping
-      set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
-      can_sends.extend(create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible,
-                                           set_speed_in_units, stopping, CS.out.gasPressed))
-      self.accel = accel
-
-    # 20 Hz LFA MFA message
-    if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
-                                                        CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
-                                                        CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
-                                                        CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
-      can_sends.append(create_lfahda_mfc(self.packer, CC.enabled))
-
-    # 5 Hz ACC options
-    if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
-      can_sends.extend(create_acc_opt(self.packer))
-
-    # 2 Hz front radar options
-    if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
-      can_sends.append(create_frt_radar_opt(self.packer))
+    if self.CP.carFingerprint in HDA2_CAR:
+      # steering control
+      can_sends.append(hda2can.create_lkas(self.packer, CC.enabled, self.frame, CC.latActive, apply_steer))
+
+      # cruise cancel
+      if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
+        if CC.cruiseControl.cancel:
+          for _ in range(20):
+            can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, True, False))
+          self.last_button_frame = self.frame
+
+        # cruise standstill resume
+        elif CC.enabled and CS.out.cruiseState.standstill:
+          can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, False, True))
+          self.last_button_frame = self.frame
+    else:
+
+      # tester present - w/ no response (keeps radar disabled)
+      if self.CP.openpilotLongitudinalControl:
+        if self.frame % 100 == 0:
+          can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
+
+      can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive,
+                                     CS.lkas11, sys_warning, sys_state, CC.enabled,
+                                     hud_control.leftLaneVisible, hud_control.rightLaneVisible,
+                                     left_lane_warning, right_lane_warning))
+
+      if not self.CP.openpilotLongitudinalControl:
+        if CC.cruiseControl.cancel:
+          can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL))
+        elif CS.out.cruiseState.standstill:
+          # send resume at a max freq of 10Hz
+          if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
+            # send 25 messages at a time to increases the likelihood of resume being accepted
+            can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
+            self.last_button_frame = self.frame
+
+      if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
+        accel = actuators.accel
+        jerk = 0
+
+        if CC.longActive:
+          jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
+          if accel < 0:
+            accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
+
+        accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+
+        lead_visible = False
+        stopping = actuators.longControlState == LongCtrlState.stopping
+        set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible,
+                                             set_speed_in_units, stopping, CS.out.gasPressed))
+        self.accel = accel
+
+      # 20 Hz LFA MFA message
+      if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
+                                                          CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
+                                                          CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
+                                                          CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
+        can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
+
+      # 5 Hz ACC options
+      if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
+        can_sends.extend(hyundaican.create_acc_opt(self.packer))
+
+      # 2 Hz front radar options
+      if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
+        can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
 
     new_actuators = actuators.copy()
     new_actuators.steer = apply_steer / self.params.STEER_MAX

selfdrive/car/hyundai/carstate.py

@@ -5,7 +5,7 @@
 from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR, Buttons
+from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, HDA2_CAR, EV_CAR, HYBRID_CAR, Buttons
 from selfdrive.car.interfaces import CarStateBase
 
 PREV_BUTTON_SAMPLES = 4
@@ -19,14 +19,23 @@ def __init__(self, CP):
     self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
     self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
 
-    if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
+    if CP.carFingerprint in HDA2_CAR:
+      self.shifter_values = can_define.dv["ACCELERATOR"]["GEAR"]
+    elif self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
       self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
     elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
       self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
     else:  # preferred and elect gear methods use same definition
       self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
 
+    self.brake_error = False
+    self.park_brake = False
+    self.buttons_counter = 0
+
   def update(self, cp, cp_cam):
+    if self.CP.carFingerprint in HDA2_CAR:
+      return self.update_hda2(cp, cp_cam)
+
     ret = car.CarState.new_message()
 
     ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
@@ -120,8 +129,55 @@ def update(self, cp, cp_cam):
 
     return ret
 
+  def update_hda2(self, cp, cp_cam):
+    ret = car.CarState.new_message()
+
+    ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255.
+    ret.gasPressed = ret.gas > 1e-3
+    ret.brakePressed = cp.vl["BRAKE"]["BRAKE_PRESSED"] == 1
+
+    ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR_OPEN"] == 1
+    ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT_LATCHED"] == 0
+
+    gear = cp.vl["ACCELERATOR"]["GEAR"]
+    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
+
+    # TODO: figure out positions
+    ret.wheelSpeeds = self.get_wheel_speeds(
+      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
+      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
+      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
+      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
+    )
+    ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
+    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
+    ret.standstill = ret.vEgoRaw < 0.1
+
+    ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
+    ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
+    ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
+    ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
+    ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
+
+    ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"],
+                                                                      cp.vl["BLINKERS"]["RIGHT_LAMP"])
+
+    ret.cruiseState.available = True
+    ret.cruiseState.enabled = cp.vl["SCC1"]["CRUISE_ACTIVE"] == 1
+    ret.cruiseState.standstill = cp.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1
+
+    speed_factor = CV.MPH_TO_MS if cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] == 1 else CV.KPH_TO_MS
+    ret.cruiseState.speed = cp.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor
+
+    self.buttons_counter = cp.vl["CRUISE_BUTTONS"]["_COUNTER"]
+
+    return ret
+
   @staticmethod
   def get_can_parser(CP):
+    if CP.carFingerprint in HDA2_CAR:
+      return CarState.get_can_parser_hda2(CP)
+
     signals = [
       # sig_name, sig_address
       ("WHL_SPD_FL", "WHL_SPD11"),
@@ -256,6 +312,9 @@ def get_can_parser(CP):
 
   @staticmethod
   def get_cam_can_parser(CP):
+    if CP.carFingerprint in HDA2_CAR:
+      return None
+
     signals = [
       # sig_name, sig_address
       ("CF_Lkas_LdwsActivemode", "LKAS11"),
@@ -280,3 +339,50 @@ def get_cam_can_parser(CP):
     ]
 
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+
+  @staticmethod
+  def get_can_parser_hda2(CP):
+    signals = [
+      ("WHEEL_SPEED_1", "WHEEL_SPEEDS"),
+      ("WHEEL_SPEED_2", "WHEEL_SPEEDS"),
+      ("WHEEL_SPEED_3", "WHEEL_SPEEDS"),
+      ("WHEEL_SPEED_4", "WHEEL_SPEEDS"),
+
+      ("ACCELERATOR_PEDAL", "ACCELERATOR"),
+      ("GEAR", "ACCELERATOR"),
+      ("BRAKE_PRESSED", "BRAKE"),
+
+      ("STEERING_RATE", "STEERING_SENSORS"),
+      ("STEERING_ANGLE", "STEERING_SENSORS"),
+      ("STEERING_COL_TORQUE", "MDPS"),
+      ("STEERING_OUT_TORQUE", "MDPS"),
+
+      ("CRUISE_ACTIVE", "SCC1"),
+      ("SET_SPEED", "CRUISE_INFO"),
+      ("CRUISE_STANDSTILL", "CRUISE_INFO"),
+      ("_COUNTER", "CRUISE_BUTTONS"),
+
+      ("DISTANCE_UNIT", "CLUSTER_INFO"),
+
+      ("LEFT_LAMP", "BLINKERS"),
+      ("RIGHT_LAMP", "BLINKERS"),
+
+      ("DRIVER_DOOR_OPEN", "DOORS_SEATBELTS"),
+      ("DRIVER_SEATBELT_LATCHED", "DOORS_SEATBELTS"),
+    ]
+
+    checks = [
+      ("WHEEL_SPEEDS", 100),
+      ("ACCELERATOR", 100),
+      ("BRAKE", 100),
+      ("STEERING_SENSORS", 100),
+      ("MDPS", 100),
+      ("SCC1", 50),
+      ("CRUISE_INFO", 50),
+      ("CRUISE_BUTTONS", 50),
+      ("CLUSTER_INFO", 4),
+      ("BLINKERS", 4),
+      ("DOORS_SEATBELTS", 4),
+    ]
+
+    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 5)

selfdrive/car/hyundai/hda2can.py

@@ -0,0 +1,23 @@
+def create_lkas(packer, enabled, frame, lat_active, apply_steer):
+  values = {
+    "LKA_MODE": 2,
+    "LKA_ICON": 2 if enabled else 1,
+    "TORQUE_REQUEST": apply_steer,
+    "LKA_ASSIST": 0,
+    "STEER_REQ": 1 if lat_active else 0,
+    "STEER_MODE": 0,
+    "SET_ME_1": 0,
+    "NEW_SIGNAL_1": 0,
+    "NEW_SIGNAL_2": 0,
+  }
+  return packer.make_can_msg("LKAS", 4, values, frame % 255)
+
+
+def create_buttons(packer, cnt, cancel, resume):
+  values = {
+    "_COUNTER": cnt % 0xf,
+    "SET_ME_1": 1,
+    "DISTANCE_BTN": 1 if resume else 0,
+    "PAUSE_RESUME_BTN": 1 if cancel else 0,
+  }
+  return packer.make_can_msg("CRUISE_BUTTONS", 5, values)

selfdrive/car/hyundai/interface.py

@@ -2,7 +2,7 @@
 from cereal import car
 from panda import Panda
 from common.conversions import Conversions as CV
-from selfdrive.car.hyundai.values import CAR, DBC, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
+from selfdrive.car.hyundai.values import CAR, DBC, HDA2_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
 from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
@@ -34,7 +34,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     # These cars have been put into dashcam only due to both a lack of users and test coverage.
     # These cars likely still work fine. Once a user confirms each car works and a test route is
     # added to selfdrive/car/tests/routes.py, we can remove it from this list.
-    ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
+    ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} or candidate in HDA2_CAR
 
     ret.steerActuatorDelay = 0.1  # Default delay
     ret.steerRateCost = 0.5
@@ -243,6 +243,21 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       tire_stiffness_factor = 0.5
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+    elif candidate == CAR.KIA_EV6:
+      ret.mass = 2055 + STD_CARGO_KG
+      ret.wheelbase = 2.9
+      ret.steerRatio = 16.
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput),
+                           get_safety_config(car.CarParams.SafetyModel.hyundaiHDA2)]
+      tire_stiffness_factor = 0.65
+
+      max_lat_accel = 2.
+      ret.lateralTuning.init('torque')
+      ret.lateralTuning.torque.useSteeringAngle = True
+      ret.lateralTuning.torque.kp = 1.0 / max_lat_accel
+      ret.lateralTuning.torque.kf = 1.0 / max_lat_accel
+      ret.lateralTuning.torque.ki = 0.1 / max_lat_accel
+      ret.lateralTuning.torque.friction = 0.01
 
     # Genesis
     elif candidate == CAR.GENESIS_G70:
@@ -321,7 +336,8 @@ def _update(self, c):
     # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
     # Main button also can trigger an engagement on these cars
     allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons)
-    events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable)
+    allow_enable = allow_enable or self.CP.carFingerprint in HDA2_CAR
+    events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable or True)
 
     if self.CS.brake_error:
       events.add(EventName.brakeUnavailable)