mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-06 20:43:46 +08:00
firestar5683
137 lines
7.0 KiB
Python
137 lines
7.0 KiB
Python
"""Untuned EV6 lateral selection and steering command boundaries."""
|
|
|
|
import unittest
|
|
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.prefix import OpenpilotPrefix
|
|
|
|
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 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), OpenpilotPrefix():
|
|
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)
|
|
# Global custom torque admission reads the saved numeric choices. An
|
|
# empty real store must still select the exact vehicle/default tune.
|
|
original_tune = cp.lateralTuning.to_dict()
|
|
self.assertFalse(runtime_enabled(cp, Params()))
|
|
self.assertEqual(cp.lateralTuning.to_dict(), original_tune)
|
|
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()
|