StarPilot/openpilot/starpilot/lateral/tests/test_ev6_default.py

"""Untuned EV6 lateral selection and steering command boundaries."""

import unittest

from opendbc.can import CANParser
from opendbc.car import Bus, DT_CTRL, gen_empty_fingerprint, structs
from opendbc.car.hyundai.carcontroller import CarController, MAX_ANGLE_FRAMES
from opendbc.car.hyundai.carstate import CarState
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags, CarControllerParams
from opendbc.car.lateral import apply_driver_steer_torque_limits
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.starpilot.lateral.torque_extension import selected_policy
from openpilot.starpilot.lateral.torque_runtime import runtime_enabled


def ev6(lka_steering: bool):
  fingerprint = gen_empty_fingerprint()
  if lka_steering:
    fingerprint[CanBus(None, fingerprint).CAM][0x50] = 16
  return CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], False, False, False)


class NoTorqueSettings:
  def get_param_path(self, _key):
    raise AssertionError("untuned EV6 must not read saved torque settings")


class TestEV6DefaultLateral(unittest.TestCase):
  def test_hda_topologies_keep_vehicle_tune_and_generic_controller(self):
    for lka_steering in (False, True):
      with self.subTest(lka_steering=lka_steering):
        cp = ev6(lka_steering)
        self.assertEqual(bool(cp.flags & HyundaiFlags.CANFD_LKA_STEER_MSG), lka_steering)
        self.assertEqual(cp.lateralTuning.which(), "torque")
        self.assertAlmostEqual(cp.lateralTuning.torque.latAccelFactor, 3.2)
        self.assertAlmostEqual(cp.lateralTuning.torque.friction, 0.005)
        self.assertFalse(runtime_enabled(cp, NoTorqueSettings()))
        lateral = LatControlTorque(cp.as_reader(), CarInterface(cp), DT_CTRL)
        self.assertIsNone(selected_policy(lateral))
        self.assertAlmostEqual(lateral.torque_params.latAccelFactor, cp.lateralTuning.torque.latAccelFactor)
        controller = CarController(DBC[cp.carFingerprint], cp)
        self.assertIsNone(controller.ioniq6_longitudinal)
        self.assertEqual(controller.params.STEER_MAX, 409)
        self.assertEqual(controller.params.STEER_DRIVER_ALLOWANCE, 250)

  def test_opposing_driver_torque_limits_can_command_and_angle_request(self):
    for lka_steering in (False, True):
      with self.subTest(lka_steering=lka_steering):
        cp = ev6(lka_steering)
        state = CarState(cp)
        state.out = state.update(state.get_can_parsers(cp))
        control = structs.CarControl()
        control.enabled = control.latActive = True
        control.actuators.torque = 1.0
        controller = CarController(DBC[cp.carFingerprint], cp)
        can = CanBus(cp)
        name = "LKAS" if lka_steering else "LFA"
        bus = can.ACAN if lka_steering else can.ECAN
        parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [(name, 0)], bus)

        state.out.steeringTorque = 0
        state.out.vEgoRaw = 15.0
        controller.apply_torque_last = 60
        _, sent = controller.update(control.as_reader(), state, 1_000_000_000)
        self.assertLessEqual(controller.apply_torque_last, 62)
        self.assertEqual(parser.update((1_000_000_000, sent)), {next(msg[0] for msg in sent if msg[2] == bus)})
        self.assertEqual(parser.vl[name]["ActToiSta"], 1)

        state.out.steeringTorque = -500
        for frame in range(30):
          previous = controller.apply_torque_last
          _, sent = controller.update(control.as_reader(), state, 1_010_000_000 + frame * 10_000_000)
          self.assertLessEqual(controller.apply_torque_last, previous)
        self.assertEqual(controller.apply_torque_last, 0)
        parser.update((1_310_000_000, sent))
        self.assertEqual(parser.vl[name]["StrTqReqVal"], 0)

        state.out.steeringAngleDeg = 90.0
        state.out.steeringTorque = 0
        requests = []
        for frame in range(MAX_ANGLE_FRAMES + 2):
          _, sent = controller.update(control.as_reader(), state, 1_320_000_000 + frame * 10_000_000)
          parser.update((1_320_000_000 + frame * 10_000_000, sent))
          requests.append(parser.vl[name]["ActToiSta"])
        self.assertIn(0, requests[-2:])


  def test_torque_speed_transition_unwind_and_zero_crossing(self):
    cp = ev6(True)
    for speed, up, down in ((0., 10, 8), (14.999, 10, 8), (15., 2, 3), (30., 2, 3),
                            (-1., 2, 3), (float('nan'), 2, 3), (float('inf'), 2, 3)):
      with self.subTest(speed=speed):
        limits = CarControllerParams(cp, speed)
        self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN), (409, up, down))
        for sign in (-1, 1):
          first = sign * (min(up, down) if sign > 0 else up)
          self.assertEqual(apply_driver_steer_torque_limits(sign * 409, 0, 0, limits), first)
          self.assertEqual(apply_driver_steer_torque_limits(sign * 409, sign, 0, limits), sign * (1 + up))
          self.assertEqual(apply_driver_steer_torque_limits(0, sign * 409, 0, limits), sign * (409 - down))
          self.assertEqual(apply_driver_steer_torque_limits(-sign * 409, sign, 0, limits), sign * (1 - down))
    state = CarState(cp)
    state.out = state.update(state.get_can_parsers(cp))
    control = structs.CarControl()
    control.enabled = control.latActive = True
    control.actuators.torque = 1.0
    controller = CarController(DBC[cp.carFingerprint], cp)
    for tick, (speed, previous, expected) in enumerate(((14., 399, 409), (15., 409, 409), (15., 399, 401))):
      state.out.vEgoRaw = speed
      controller.apply_torque_last = previous
      result, _ = controller.update(control.as_reader(), state, 1_000_000_000 + tick * 10_000_000)
      self.assertEqual(result.torqueOutputCan, expected)

  def test_angle_carnival_and_classic_limits_are_unchanged(self):
    cp = ev6(True).as_reader().as_builder()
    cp.flags |= int(HyundaiFlags.CANFD_ANGLE_STEERING)
    for speed in (0., 15.):
      limits = CarControllerParams(cp, speed)
      self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN, limits.STEER_THRESHOLD), (270, 2, 3, 175))
    cp.flags &= ~int(HyundaiFlags.CANFD_ANGLE_STEERING)
    cp.carFingerprint = CAR.KIA_CARNIVAL_HEV_4TH_GEN
    for speed in (0., 15.):
      limits = CarControllerParams(cp, speed)
      self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN), (409, 2, 3))
    for vehicle, flags, maximum in ((CAR.HYUNDAI_ELANTRA, 0, 255), (CAR.GENESIS_G70_2020, HyundaiFlags.ALT_LIMITS, 270),
                                  (CAR.GENESIS_G70_2020, HyundaiFlags.ALT_LIMITS_2, 170)):
      cp.carFingerprint, cp.flags = vehicle, int(flags)
      for speed in (0., 15.):
        self.assertEqual(CarControllerParams(cp, speed).STEER_MAX, maximum)


if __name__ == "__main__":
  unittest.main()