From 5501541aa2ae6ee62f251a5df0b57cb669604cde Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 26 Oct 2018 17:26:00 +0200 Subject: [PATCH 1/3] Improve Toyota radar filtering (#409) --- selfdrive/car/toyota/radar_interface.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 2ae8fd3db..9711f1393 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -33,7 +33,7 @@ class RadarInterface(object): def __init__(self, CP): # radar self.pts = {} - self.seen_valid = {key: False for key in RADAR_A_MSGS} + self.valid_cnt = {key: 0 for key in RADAR_A_MSGS} self.track_id = 0 self.delay = 0.0 # Delay of radar @@ -57,8 +57,7 @@ class RadarInterface(object): while 1: tm = int(sec_since_boot() * 1e9) updated_messages.update(self.rcp.update(tm, True)) - # TODO: do not hardcode last msg - if 0x22f in updated_messages: + if RADAR_B_MSGS[-1] in updated_messages: break errors = [] @@ -71,17 +70,18 @@ class RadarInterface(object): if ii in RADAR_A_MSGS: cpt = self.rcp.vl[ii] - if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']: - self.seen_valid[ii] = False # reset validity - - if cpt['LONG_DIST'] < 255 and cpt['VALID']: - self.seen_valid[ii] = True + if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']: + self.valid_cnt[ii] = 0 # reset counter + if cpt['VALID'] and cpt['LONG_DIST'] < 255: + self.valid_cnt[ii] += 1 + else: + self.valid_cnt[ii] = max(self.valid_cnt[ii] -1, 0) score = self.rcp.vl[ii+16]['SCORE'] - # print ii, score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST'] + # print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST'] # radar point only valid if it's a valid measurement and score is above 50 - if (cpt['VALID'] or score > 50) and cpt['LONG_DIST'] < 255 and self.seen_valid[ii]: + if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0): if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii] = car.RadarState.RadarPoint.new_message() self.pts[ii].trackId = self.track_id From 2f8034b2ec63bfa1a7e67e64999814f547c86b49 Mon Sep 17 00:00:00 2001 From: Jafar Al-Gharaibeh Date: Tue, 30 Oct 2018 16:13:18 -0500 Subject: [PATCH 2/3] boardd: keep defined safety models in sync with panda repo (#412) This is a noop for most users, but for those of us working on new car ports where we have our own additional defines, having those defines out of sync creates merge conflicts in one place (good so we can fix it) but not the other ( not good becasue they will go unnoticed). I learned this the hardway!. Signed-off-by: Jafar Al-Gharaibeh --- selfdrive/boardd/boardd.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index e037555b7..aae2b665a 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -40,8 +40,11 @@ #define SAFETY_FORD 5 #define SAFETY_CADILLAC 6 #define SAFETY_HYUNDAI 7 +#define SAFETY_TESLA 8 +#define SAFETY_TOYOTA_IPAS 0x1335 #define SAFETY_TOYOTA_NOLIMITS 0x1336 #define SAFETY_ALLOUTPUT 0x1337 +#define SAFETY_ELM327 0xE327 namespace { From 00429e6bbb27d79f452da81a4b7210db53b03f01 Mon Sep 17 00:00:00 2001 From: dekerr Date: Tue, 30 Oct 2018 17:30:09 -0400 Subject: [PATCH 3/3] Improve VM dynamic sol accuracy (#391) * use solve and eye func * remove uneeded import from vehicle model --- selfdrive/controls/lib/vehicle_model.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index a42b8112a..7aac7d0a9 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import numpy as np -from numpy.linalg import inv +from numpy.linalg import solve # dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## # Xdot = A*X + B*U @@ -33,7 +33,7 @@ def kin_ss_sol(sa, u, VM): def dyn_ss_sol(sa, u, VM): # Dynamic solution, useful when speed > 0 A, B = create_dyn_state_matrices(u, VM) - return - np.matmul(inv(A), B) * sa + return - solve(A, B) * sa def calc_slip_factor(VM): @@ -87,7 +87,7 @@ class VehicleModel(object): # U is the matrix of the controls # u is the long speed A, B = create_dyn_state_matrices(u, self) - return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt + return np.matmul((A * self.dt + np.eye(2)), self.state) + B * sa * self.dt def yaw_rate(self, sa, u): return self.calc_curvature(sa, u) * u