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.] diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 3db04a26f..2ed5771f3 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -92,6 +92,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 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