diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 36b2e534b9..5d936ead54 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -73,10 +73,14 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.01 ret.steerRatio = 16.5 ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 3.5 - ret.lateralTuning.indi.outerLoopGain = 2.0 - ret.lateralTuning.indi.timeConstant = 1.4 - ret.lateralTuning.indi.actuatorEffectiveness = 2.3 + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [3.5] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [2.0] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [2.3] ret.minSteerSpeed = 60 * CV.KPH_TO_MS elif candidate == CAR.KONA: ret.lateralTuning.pid.kf = 0.00005 @@ -157,10 +161,14 @@ class CarInterface(CarInterfaceBase): # Genesis elif candidate == CAR.GENESIS_G70: ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 2.5 - ret.lateralTuning.indi.outerLoopGain = 3.5 - ret.lateralTuning.indi.timeConstant = 1.4 - ret.lateralTuning.indi.actuatorEffectiveness = 1.8 + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [2.5] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [3.5] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] ret.steerActuatorDelay = 0.1 ret.mass = 1640.0 + STD_CARGO_KG ret.wheelbase = 2.84 diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index be05c9d065..9eb6f275fe 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -39,7 +39,7 @@ class TestCarInterfaces(unittest.TestCase): elif tuning == 'lqr': self.assertTrue(len(car_params.lateralTuning.lqr.a)) elif tuning == 'indi': - self.assertGreater(car_params.lateralTuning.indi.outerLoopGain, 1e-3) + self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) # Run car interface CC = car.CarControl.new_message() diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a22d836010..af46242a89 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -45,10 +45,14 @@ class CarInterface(CarInterfaceBase): ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 4.0 - ret.lateralTuning.indi.outerLoopGain = 3.0 - ret.lateralTuning.indi.timeConstant = 1.0 - ret.lateralTuning.indi.actuatorEffectiveness = 1.0 + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [4.0] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [3.0] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.0] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.0] ret.steerActuatorDelay = 0.5 elif candidate in [CAR.RAV4, CAR.RAV4H]: diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 0e685fd1a4..09bfbcc5a9 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -3,7 +3,7 @@ import numpy as np from cereal import log from common.realtime import DT_CTRL -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from selfdrive.car.toyota.values import CarControllerParams from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.controls.lib.drive_helpers import get_steer_max @@ -28,16 +28,18 @@ class LatControlINDI(): [7.29394177e+00, 1.39159419e-02], [1.71022442e+01, 3.38495381e-02]]) + self.speed = 0. + self.K = K self.A_K = A - np.dot(K, C) self.x = np.array([[0.], [0.], [0.]]) self.enforce_rate_limit = CP.carName == "toyota" - self.RC = CP.lateralTuning.indi.timeConstant - self.G = CP.lateralTuning.indi.actuatorEffectiveness - self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain - self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain + self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) + self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV) + self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) + self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV) self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) self.sat_count_rate = 1.0 * DT_CTRL @@ -45,10 +47,27 @@ class LatControlINDI(): self.reset() + @property + def RC(self): + return interp(self.speed, self._RC[0], self._RC[1]) + + @property + def G(self): + return interp(self.speed, self._G[0], self._G[1]) + + @property + def outer_loop_gain(self): + return interp(self.speed, self._outer_loop_gain[0], self._outer_loop_gain[1]) + + @property + def inner_loop_gain(self): + return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1]) + def reset(self): self.delayed_output = 0. self.output_steer = 0. self.sat_count = 0.0 + self.speed = 0. def _check_saturation(self, control, check_saturation, limit): saturated = abs(control) == limit @@ -63,6 +82,7 @@ class LatControlINDI(): return self.sat_count > self.sat_limit def update(self, active, CS, CP, path_plan): + self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) diff --git a/selfdrive/test/test_models.py b/selfdrive/test/test_models.py index b0c4723051..74339f75fe 100755 --- a/selfdrive/test/test_models.py +++ b/selfdrive/test/test_models.py @@ -91,7 +91,7 @@ class TestCarModel(unittest.TestCase): elif tuning == 'lqr': self.assertTrue(len(self.CP.lateralTuning.lqr.a)) elif tuning == 'indi': - self.assertGreater(self.CP.lateralTuning.indi.outerLoopGain, 1e-3) + self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV)) self.assertTrue(self.CP.enableCamera)