From fdbf1c5938b70a9fa389cb6a117f123b6cbf5a2e 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 b7c029c92c46c48a13e6eecab3ae5705b0fe0ad5 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 7db592d3b8982698c0e5206faa04d014d7a7a73f 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