From c6be3e2ed54924319742a6475d11d839fd4ea185 Mon Sep 17 00:00:00 2001 From: kegman Date: Wed, 31 Oct 2018 14:09:48 -0400 Subject: [PATCH 1/3] Honda Pilot tuning / lane centering fixes (#408) Tuned kP and kI and Tire Stiffness Factor settings. Fixes lane centering issues in the following situations: 1. Hugging of left lane line during fast left curving roads 2. Hugging of right lane line during fast right curving roads 3. Left bias in fast lane on crowned roads (sloping down to the left) 4. Right bias in slow lane on crowned roads (sloping down to the right) Works better when lane_width = 2.85 (vs 3.7 default) in pathplanner.py --- selfdrive/car/honda/interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 42eb041f3..3098f3768 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -294,8 +294,8 @@ class CarInterface(object): ret.wheelbase = 2.81 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # as spec - tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] + tire_stiffness_factor = 0.82 + ret.steerKpV, ret.steerKiV = [[0.5], [0.22]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] From 736cf2fe327abd610eda750e4c8c8f7484ec2593 Mon Sep 17 00:00:00 2001 From: ErichMoraga <33645296+ErichMoraga@users.noreply.github.com> Date: Mon, 5 Nov 2018 20:43:50 -0600 Subject: [PATCH 2/3] Added fingerprint for Chinese RAV4 (#422) Confirmed working on 2017-2018 Chinese RAV4 by @Fiftycentsjj --- selfdrive/car/toyota/values.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 1afab157e..5546137db 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -83,6 +83,10 @@ FINGERPRINTS = { }], CAR.RAV4H: [{ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }, + # Chinese RAV4 + { + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1043: 8, 1056: 8, 1059: 1, 1114: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8 }], CAR.PRIUS: [{ 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 From 2b8e62a0d64ccf2ef9adb37f0dc8c7b617f2d8c8 Mon Sep 17 00:00:00 2001 From: dekerr Date: Tue, 6 Nov 2018 16:51:37 -0500 Subject: [PATCH 3/3] Faster calibration filtering (#421) * separable filter * add missing args * formatting * fix casing --- selfdrive/locationd/calibrationd.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 523302f11..05ba209ed 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -72,8 +72,15 @@ def gaussian_kernel(sizex, sizey, stdx, stdy, dx, dy): g = np.exp(-((x - dx)**2 / (2. * stdx**2) + (y - dy)**2 / (2. * stdy**2))) return g / g.sum() -def blur_image(img, kernel): - return cv2.filter2D(img.astype(np.uint16), -1, kernel) +def gaussian_kernel_1d(kernel): + #creates separable gaussian filter + u,s,v = np.linalg.svd(kernel) + x = u[:,0]*np.sqrt(s[0]) + y = np.sqrt(s[0])*v[0,:] + return x, y + +def blur_image(img, kernel_x, kernel_y): + return cv2.sepFilter2D(img.astype(np.uint16), -1, kernel_x, kernel_y) def is_calibration_valid(vp): return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \ @@ -89,6 +96,7 @@ class Calibrator(object): self.l100_last_updated = 0 self.prev_orbs = None self.kernel = gaussian_kernel(11, 11, 2.35, 2.35, 0, 0) + self.kernel_x, self.kernel_y = gaussian_kernel_1d(self.kernel) self.vp = copy.copy(VP_INIT) self.cal_status = Calibration.UNCALIBRATED @@ -136,7 +144,7 @@ class Calibrator(object): increment_grid_c(self.grid, lines, len(lines)) self.frame_counter += 1 if (self.frame_counter % FRAMES_NEEDED) == 0: - grid = blur_image(self.grid, self.kernel) + grid = blur_image(self.grid, self.kernel_x, self.kernel_y) argmax_vp = np.unravel_index(np.argmax(grid), grid.shape)[::-1] self.rescale_grid() self.vp_unfilt = np.array(argmax_vp) @@ -189,7 +197,7 @@ class Calibrator(object): self.yaw_rate = log.live100.curvature * self.speed def handle_debug(self): - grid_blurred = blur_image(self.grid, self.kernel) + grid_blurred = blur_image(self.grid, self.kernel_x, self.kernel_y) grid_grey = np.clip(grid_blurred/(0.1 + np.max(grid_blurred))*255, 0, 255) grid_color = np.repeat(grid_grey[:,:,np.newaxis], 3, axis=2) grid_color[:,:,0] = 0