firestar5683
172 lines
8.6 KiB
Python
172 lines
8.6 KiB
Python
from parameterized import parameterized
|
|
from types import SimpleNamespace
|
|
|
|
from cereal import car, custom, log
|
|
import openpilot.selfdrive.controls.lib.latcontrol_torque as latcontrol_torque
|
|
from opendbc.car.car_helpers import interfaces
|
|
from opendbc.car.honda.values import CAR as HONDA
|
|
from opendbc.car.toyota.values import CAR as TOYOTA
|
|
from opendbc.car.nissan.values import CAR as NISSAN
|
|
from opendbc.car.gm.values import CAR as GM
|
|
from opendbc.car.vehicle_model import VehicleModel
|
|
from openpilot.common.realtime import DT_CTRL
|
|
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
|
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
|
from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
|
LatControlTorque,
|
|
get_friction_threshold,
|
|
get_bolt_2017_base_torque_scale,
|
|
get_bolt_2017_torque_scale,
|
|
get_bolt_2022_2023_ff_scale,
|
|
get_bolt_2022_2023_friction_scale,
|
|
get_bolt_2022_2023_friction_threshold,
|
|
get_bolt_2018_2021_dynamic_torque_scale,
|
|
get_bolt_2018_2021_friction_scale,
|
|
get_bolt_2018_2021_friction_threshold,
|
|
get_bolt_2018_2021_torque_scale,
|
|
)
|
|
|
|
|
|
class TestLatControl:
|
|
|
|
@staticmethod
|
|
def _build_torque_controller(car_name):
|
|
CarInterface = interfaces[car_name]
|
|
CP = CarInterface.get_non_essential_params(car_name)
|
|
CI = CarInterface(CP, custom.StarPilotCarParams.new_message())
|
|
controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL)
|
|
VM = VehicleModel(CP)
|
|
|
|
CS = car.CarState.new_message()
|
|
CS.vEgo = 22
|
|
CS.steeringPressed = False
|
|
CS.steeringAngleDeg = 1.0
|
|
|
|
params = log.LiveParametersData.new_message()
|
|
params.steerRatio = CP.steerRatio
|
|
params.stiffnessFactor = 1.0
|
|
params.roll = 0.0
|
|
params.angleOffsetDeg = 0.0
|
|
|
|
starpilot_toggles = SimpleNamespace()
|
|
return controller, VM, CS, params, starpilot_toggles
|
|
|
|
def test_bolt_2017_testing_ground_scale_curve(self):
|
|
assert get_bolt_2017_base_torque_scale(0.1) == 1.0
|
|
assert get_bolt_2017_base_torque_scale(-0.1) == 1.0
|
|
assert get_bolt_2017_base_torque_scale(0.5) > get_bolt_2017_base_torque_scale(-0.5)
|
|
assert 1.0 < get_bolt_2017_base_torque_scale(1.2) < get_bolt_2017_base_torque_scale(0.5)
|
|
assert get_bolt_2017_base_torque_scale(-2.5) < 1.0
|
|
assert get_bolt_2017_torque_scale(0.6, 0.6, 8.0) > get_bolt_2017_torque_scale(0.6, 0.0, 8.0) > get_bolt_2017_torque_scale(0.6, -0.6, 8.0)
|
|
assert get_bolt_2017_torque_scale(-0.6, -0.6, 8.0) > get_bolt_2017_torque_scale(-0.6, 0.0, 8.0) > get_bolt_2017_torque_scale(-0.6, 0.6, 8.0)
|
|
assert get_bolt_2017_torque_scale(0.6, 0.6, 8.0) > get_bolt_2017_torque_scale(-0.6, -0.6, 8.0)
|
|
|
|
def test_bolt_2018_2021_testing_ground_scale_curve(self):
|
|
assert get_bolt_2018_2021_torque_scale(0.0) == 1.0
|
|
assert get_bolt_2018_2021_torque_scale(0.2) > get_bolt_2018_2021_torque_scale(0.08)
|
|
assert get_bolt_2018_2021_torque_scale(0.4) > get_bolt_2018_2021_torque_scale(-0.4)
|
|
assert get_bolt_2018_2021_torque_scale(2.0) < get_bolt_2018_2021_torque_scale(0.8)
|
|
assert get_bolt_2018_2021_dynamic_torque_scale(0.08, 0.0, 25.0) < get_bolt_2018_2021_dynamic_torque_scale(0.08, 0.0, 8.0)
|
|
assert get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.8, 20.0) < get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.1, 20.0)
|
|
assert get_bolt_2018_2021_dynamic_torque_scale(0.6, -0.6, 8.0) < get_bolt_2018_2021_dynamic_torque_scale(0.6, 0.6, 8.0)
|
|
assert get_bolt_2018_2021_dynamic_torque_scale(-0.6, 0.6, 8.0) < get_bolt_2018_2021_dynamic_torque_scale(-0.6, -0.6, 8.0)
|
|
|
|
def test_bolt_2018_2021_friction_threshold_curve(self):
|
|
base = get_friction_threshold(6.0)
|
|
left_turn_in = get_bolt_2018_2021_friction_threshold(6.0, 0.7, 0.8)
|
|
right_turn_in = get_bolt_2018_2021_friction_threshold(6.0, -0.7, -0.8)
|
|
left_unwind = get_bolt_2018_2021_friction_threshold(6.0, 0.7, -0.8)
|
|
right_unwind = get_bolt_2018_2021_friction_threshold(6.0, -0.7, 0.8)
|
|
assert left_turn_in < right_turn_in < base < left_unwind < right_unwind
|
|
assert get_bolt_2018_2021_friction_threshold(25.0, 0.7, 0.8) > left_turn_in
|
|
|
|
def test_bolt_2018_2021_friction_scale_curve(self):
|
|
base = get_bolt_2018_2021_friction_scale(25.0, 0.7, 0.8)
|
|
center_base = get_bolt_2018_2021_friction_scale(25.0, 0.0, 0.0)
|
|
left_turn_in = get_bolt_2018_2021_friction_scale(6.0, 0.7, 0.8)
|
|
right_turn_in = get_bolt_2018_2021_friction_scale(6.0, -0.7, -0.8)
|
|
left_unwind = get_bolt_2018_2021_friction_scale(6.0, 0.7, -0.8)
|
|
right_unwind = get_bolt_2018_2021_friction_scale(6.0, -0.7, 0.8)
|
|
assert center_base < 1.02
|
|
assert left_turn_in > right_turn_in > base
|
|
assert base > left_unwind > right_unwind
|
|
|
|
def test_bolt_2022_2023_ff_scale_curve(self):
|
|
assert get_bolt_2022_2023_ff_scale(0.0, 0.0, 20.0) == 1.0
|
|
assert get_bolt_2022_2023_ff_scale(0.5, 0.0, 20.0) > get_bolt_2022_2023_ff_scale(-0.5, 0.0, 20.0)
|
|
assert get_bolt_2022_2023_ff_scale(0.6, 0.7, 8.0) > get_bolt_2022_2023_ff_scale(-0.6, -0.7, 8.0)
|
|
assert get_bolt_2022_2023_ff_scale(-0.6, -0.7, 8.0) > get_bolt_2022_2023_ff_scale(-0.6, 0.0, 8.0)
|
|
assert get_bolt_2022_2023_ff_scale(0.6, -0.7, 8.0) < get_bolt_2022_2023_ff_scale(0.6, 0.0, 8.0)
|
|
assert get_bolt_2022_2023_ff_scale(0.6, -0.7, 6.0) < get_bolt_2022_2023_ff_scale(0.6, -0.7, 20.0)
|
|
|
|
def test_bolt_2022_2023_friction_threshold_curve(self):
|
|
base = get_friction_threshold(6.0)
|
|
left_turn_in = get_bolt_2022_2023_friction_threshold(6.0, 0.7, 0.8)
|
|
right_turn_in = get_bolt_2022_2023_friction_threshold(6.0, -0.7, -0.8)
|
|
left_unwind = get_bolt_2022_2023_friction_threshold(6.0, 0.7, -0.8)
|
|
right_unwind = get_bolt_2022_2023_friction_threshold(6.0, -0.7, 0.8)
|
|
assert left_turn_in < right_turn_in < base < right_unwind < left_unwind
|
|
|
|
def test_bolt_2022_2023_friction_scale_curve(self):
|
|
base = get_bolt_2022_2023_friction_scale(25.0, 0.7, 0.8)
|
|
left_turn_in = get_bolt_2022_2023_friction_scale(6.0, 0.7, 0.8)
|
|
right_turn_in = get_bolt_2022_2023_friction_scale(6.0, -0.7, -0.8)
|
|
left_unwind = get_bolt_2022_2023_friction_scale(6.0, 0.7, -0.8)
|
|
right_unwind = get_bolt_2022_2023_friction_scale(6.0, -0.7, 0.8)
|
|
assert left_turn_in > right_turn_in > base
|
|
assert base > right_unwind > left_unwind
|
|
|
|
def test_bolt_2017_testing_ground_update_path(self, monkeypatch):
|
|
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_CC_2017)
|
|
monkeypatch.setattr(latcontrol_torque, "bolt_2017_lateral_testing_ground_active", lambda: True)
|
|
|
|
_, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
|
|
|
|
assert lac_log.active
|
|
|
|
def test_bolt_2018_2021_testing_ground_update_path(self, monkeypatch):
|
|
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_CC_2018_2021)
|
|
monkeypatch.setattr(latcontrol_torque, "bolt_2018_2021_lateral_testing_ground_active", lambda: True)
|
|
|
|
_, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
|
|
|
|
assert lac_log.active
|
|
|
|
def test_bolt_2022_2023_testing_ground_update_path(self, monkeypatch):
|
|
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023)
|
|
monkeypatch.setattr(latcontrol_torque, "bolt_2022_2023_lateral_testing_ground_active", lambda: True)
|
|
|
|
_, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
|
|
|
|
assert lac_log.active
|
|
|
|
@parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
|
|
(NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_ACC_2022_2023, LatControlTorque)])
|
|
def test_saturation(self, car_name, controller):
|
|
CarInterface = interfaces[car_name]
|
|
CP = CarInterface.get_non_essential_params(car_name)
|
|
CI = CarInterface(CP, custom.StarPilotCarParams.new_message())
|
|
VM = VehicleModel(CP)
|
|
|
|
controller = controller(CP.as_reader(), CI, DT_CTRL)
|
|
|
|
CS = car.CarState.new_message()
|
|
CS.vEgo = 30
|
|
CS.steeringPressed = False
|
|
|
|
params = log.LiveParametersData.new_message()
|
|
starpilot_toggles = SimpleNamespace()
|
|
|
|
# Saturate for curvature limited and controller limited
|
|
for _ in range(1000):
|
|
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, True, 0.2, None, None, starpilot_toggles)
|
|
assert lac_log.saturated
|
|
|
|
for _ in range(1000):
|
|
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, False, 0.2, None, None, starpilot_toggles)
|
|
assert not lac_log.saturated
|
|
|
|
for _ in range(1000):
|
|
_, _, lac_log = controller.update(True, CS, VM, params, False, 1, False, 0.2, None, None, starpilot_toggles)
|
|
assert lac_log.saturated
|