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

[torqued] Update offline values (#26261)

Browse Source 
* add qlog mode to torqued

* update offline valujes from qlogs

* resollve comments

* update refs

* resolve comments
pull/25811/head^2
Vivek Aithal 3 years ago committed by GitHub
parent ba570b963f
commit b158c016cb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 33 additions and 22 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      selfdrive/car/torque_data/override.yaml
  2. 29
      selfdrive/car/torque_data/params.yaml
  3. 23
      selfdrive/locationd/torqued.py
  4. 2
      selfdrive/test/process_replay/ref_commit

1
selfdrive/car/torque_data/override.yaml
Unescape View File

@ -21,7 +21,6 @@ FORD FOCUS 4TH GEN: [.nan, 1.5, .nan]
COMMA BODY: [.nan, 1000, .nan]
# Totally new cars
KIA EV6 2022: [3.5, 3.0, 0.0]
RAM 1500 5TH GEN: [2.0, 2.0, 0.0]
RAM HD 5TH GEN: [1.4, 1.4, 0.0]
SUBARU OUTBACK 6TH GEN: [2.3, 2.3, 0.11]

29
selfdrive/car/torque_data/params.yaml
Unescape View File

@ -39,17 +39,18 @@ HYUNDAI SONATA 2020: [2.9638737459977467, 2.1259108157250735, 0.0781366561692759
HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488717382]
JEEP GRAND CHEROKEE 2019: [1.7321233388827006, 1.289689569171081, 0.15046331002097185]
JEEP GRAND CHEROKEE V6 2018: [1.8776598027756923, 1.4057367824262523, 0.11725947414922003]
KIA EV6 2022: [3.2, 3.0, 0.05]
KIA K5 2021: [2.405339728085138, 1.460032270828705, 0.11650989850813716]
KIA NIRO EV 2020: [2.9215954981365337, 2.1500583840260044, 0.09236802474810267]
KIA SORENTO GT LINE 2018: [2.464854685101844, 1.5335274218367956, 0.12056170567599558]
KIA STINGER GT2 2018: [2.7499043387418967, 1.849652021986449, 0.12048334239559202]
LEXUS ES 2019: [2.0203086922726112, 2.134803912579666, 0.12757526789308554]
LEXUS ES HYBRID 2019: [2.392442298703042, 1.863360677810788, 0.17690002108856212]
LEXUS ES 2019: [1.935835, 2.134803912579666, 0.093439]
LEXUS ES HYBRID 2019: [2.135678, 1.863360677810788, 0.109627]
LEXUS NX 2018: [2.302625600642627, 2.1382378491466625, 0.14986840878892838]
LEXUS NX 2020: [2.4331999786982936, 2.1045680431705414, 0.14099899317761067]
LEXUS NX HYBRID 2018: [2.4025593501080955, 1.8080446063815507, 0.15349361249519017]
LEXUS RX 2016: [1.5876816543130423, 1.0427699298523752, 0.21334066732397142]
LEXUS RX 2020: [1.5228812994274734, 1.431102486563665, 0.2093316728710659]
LEXUS RX 2020: [1.5228812994274734, 1.431102486563665, 0.164117]
LEXUS RX HYBRID 2017: [1.6984261557042386, 1.3211501880159107, 0.1820354534928893]
LEXUS RX HYBRID 2020: [1.5522309889823778, 1.255230465866663, 0.2220954003055114]
MAZDA CX-9 2021: [1.7601682915983443, 1.0889677335154337, 0.17713792194297195]
@ -62,31 +63,31 @@ TOYOTA AVALON 2019: [1.7036141952825095, 1.239619084240008, 0.08459830394899492]
TOYOTA AVALON 2022: [2.3154403649717357, 2.7777922854327124, 0.11453999639164605]
TOYOTA C-HR 2018: [1.5591084333664578, 1.271271459066948, 0.20259087058453193]
TOYOTA C-HR 2021: [1.7678810166088303, 1.3742176337919942, 0.2319674583741509]
TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.13519250664782062]
TOYOTA CAMRY 2021: [2.6922769557433055, 2.3476510120007434, 0.1450430192989234]
TOYOTA CAMRY HYBRID 2018: [2.0974120828287774, 1.7996193116697359, 0.13823613467632756]
TOYOTA CAMRY HYBRID 2021: [2.6426668350384457, 2.3901492458927986, 0.16103875108816076]
TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.105192506]
TOYOTA CAMRY 2021: [2.446083, 2.3476510120007434, 0.121615]
TOYOTA CAMRY HYBRID 2018: [1.996333, 1.7996193116697359, 0.112565]
TOYOTA CAMRY HYBRID 2021: [2.263582, 2.3901492458927986, 0.115257]
TOYOTA COROLLA 2017: [3.117154369115421, 1.8438132575043773, 0.12289685869250652]
TOYOTA COROLLA HYBRID TSS2 2019: [1.9079729107361805, 1.8118712531729109, 0.22251440891543514]
TOYOTA COROLLA TSS2 2019: [2.0742917676766712, 1.9258612322678952, 0.16888685704519352]
TOYOTA HIGHLANDER 2017: [1.8696367437248915, 1.626293990451463, 0.17485372210240796]
TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054457]
TOYOTA HIGHLANDER HYBRID 2018: [1.9421825202382728, 1.6433903296845025, 0.16928956792275918]
TOYOTA HIGHLANDER HYBRID 2020: [2.103373061114133, 2.104015182965606, 0.14447040132184993]
TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600]
TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993]
TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565]
TOYOTA PRIUS 2017: [2.0183401513314294, 1.5023147650693636, 0.20856908464957724]
TOYOTA PRIUS TSS2 2021: [2.327639738920072, 1.9104337425537743, 0.2030762265549664]
TOYOTA PRIUS 2017: [1.746445, 1.5023147650693636, 0.151515]
TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968]
TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975]
TOYOTA RAV4 2019: [2.5038362866776835, 2.0993589721530252, 0.1552425356342368]
TOYOTA RAV4 2019: [2.331293, 2.0993589721530252, 0.129822]
TOYOTA RAV4 2019 8965: [2.5084506298290377, 2.4216520504763475, 0.11992835265067918]
TOYOTA RAV4 2019 x02: [2.7209621987605024, 2.2148637653781593, 0.10862567142268198]
TOYOTA RAV4 HYBRID 2017: [1.9796257271652042, 1.7503987331707576, 0.14628860048885406]
TOYOTA RAV4 HYBRID 2019: [2.2271858492309153, 2.074844961405639, 0.14382216826893632]
TOYOTA RAV4 HYBRID 2019 8965: [2.1077397198131336, 1.8162215166877735, 0.13891369391200137]
TOYOTA RAV4 HYBRID 2019 x02: [2.803624333289342, 2.272367966572498, 0.11364569214387774]
TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.1565442715453653]
TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.112174]
TOYOTA RAV4 HYBRID 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736]
TOYOTA SIENNA 2018: [1.8660896232147548, 1.3208264576110418, 0.18799149615227198]
TOYOTA SIENNA 2018: [1.689726, 1.3208264576110418, 0.140456]
VOLKSWAGEN ARTEON 1ST GEN: [1.45136518053819, 1.3639364049316804, 0.23806361745695032]
VOLKSWAGEN ATLAS 1ST GEN: [1.4677006726964945, 1.6733266634075656, 0.12959584092073367]
VOLKSWAGEN GOLF 7TH GEN: [1.3750394140491293, 1.5814743077200641, 0.2018321939386586]

23
selfdrive/locationd/torqued.py
Unescape View File

@ -16,7 +16,9 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
HISTORY = 5 # secs
POINTS_PER_BUCKET = 1500
MIN_POINTS_TOTAL = 4000
MIN_POINTS_TOTAL_QLOG = 800
FIT_POINTS_TOTAL = 2000
FIT_POINTS_TOTAL_QLOG = 800
MIN_VEL = 15 # m/s
FRICTION_FACTOR = 1.5 # ~85% of data coverage
FACTOR_SANITY = 0.3
@ -26,7 +28,7 @@ MIN_FILTER_DECAY = 50
MAX_FILTER_DECAY = 250
LAT_ACC_THRESHOLD = 1
STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
MIN_BUCKET_POINTS = [100, 300, 500, 500, 500, 500, 300, 100]
MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
MAX_RESETS = 5.0
MAX_INVALID_THRESHOLD = 10
MIN_ENGAGE_BUFFER = 2 # secs
@ -58,10 +60,11 @@ class NPQueue:
class PointBuckets:
def __init__(self, x_bounds, min_points):
def __init__(self, x_bounds, min_points, min_points_total):
self.x_bounds = x_bounds
self.buckets = {bounds: NPQueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds}
self.buckets_min_points = {bounds: min_point for bounds, min_point in zip(x_bounds, min_points)}
self.min_points_total = min_points_total
def bucket_lengths(self):
return [len(v) for v in self.buckets.values()]
@ -70,7 +73,7 @@ class PointBuckets:
return sum(self.bucket_lengths())
def is_valid(self):
return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= MIN_POINTS_TOTAL)
return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= self.min_points_total)
def add_point(self, x, y):
for bound_min, bound_max in self.x_bounds:
@ -90,9 +93,17 @@ class PointBuckets:
class TorqueEstimator:
def __init__(self, CP):
def __init__(self, CP, decimated=False):
self.hist_len = int(HISTORY / DT_MDL)
self.lag = CP.steerActuatorDelay + .2 # from controlsd
if decimated:
self.min_bucket_points = MIN_BUCKET_POINTS / 10
self.min_points_total = MIN_POINTS_TOTAL_QLOG
self.fit_points = FIT_POINTS_TOTAL_QLOG
else:
self.min_bucket_points = MIN_BUCKET_POINTS
self.min_points_total = MIN_POINTS_TOTAL
self.fit_points = FIT_POINTS_TOTAL
self.offline_friction = 0.0
self.offline_latAccelFactor = 0.0
@ -157,10 +168,10 @@ class TorqueEstimator:
self.invalid_values_tracker = 0.0
self.decay = MIN_FILTER_DECAY
self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len))
self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=MIN_BUCKET_POINTS)
self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total)
def estimate_params(self):
points = self.filtered_points.get_points(FIT_POINTS_TOTAL)
points = self.filtered_points.get_points(self.fit_points)
# total least square solution as both x and y are noisy observations
# this is empirically the slope of the hysteresis parallelogram as opposed to the line through the diagonals
try:

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
fbafde6f1c46b50c219c3ec3121b94ee0848a7be
f4ea2499a95e198914fd275a8380178e8ff65a31
Write Preview
Loading…
Cancel
Save