mawei
/
openpilot0
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add indi breakpoints (#19664) 

* Added BP, V to latcontrol_indi

* hyundai

* toyota

* Tests for INDI outerLoopGain
old-commit-hash: 0a65e87394
This commit is contained in:
Igor 2021-01-07 09:55:46 -05:00 committed by GitHub
parent 0f9ba9ab91
commit b1c6f804e5
5 changed files with 51 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
selfdrive/car/hyundai/interface.py
View File

@ -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

2
selfdrive/car/tests/test_car_interfaces.py
View File

@ -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()

12
selfdrive/car/toyota/interface.py
View File

@ -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]:

30
selfdrive/controls/lib/latcontrol_indi.py
View File

@ -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)

2
selfdrive/test/test_models.py
View File

@ -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)