mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-28 10:23:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #434 from commaai/devel

Browse Source 
calibration improvements and devel synch up
This commit is contained in:
rbiasini
2018-11-13 18:50:56 -08:00
committed by GitHub
parent 7c0202a101 b4dd562c43
commit 9a42f2f1ad
5 changed files with 32 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/boardd/boardd.cc
+3
View File
@@ -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 {
selfdrive/car/toyota/values.py
+6 -2
View File
@@ -52,8 +52,8 @@ STATIC_MSGS = [
(0x365, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
(0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'),
(0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
(0x470, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'),
(0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x00\x00\x01\x79'),
(0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'),
(0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H), 1, 100, '\x00\x00\x01\x79'),
(0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
(0x292, ECU.APGS, (CAR.PRIUS), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
@@ -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, 740: 5, 742: 8, 743: 8, 800: 8, 830: 7, 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, 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, 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, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 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
selfdrive/controls/lib/vehicle_model.py
+3 -3
View File
@@ -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
selfdrive/locationd/calibrationd.py
+19 -5
View File
@@ -30,6 +30,7 @@ EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
VP_VALIDITY_CORNERS = np.array([[-150., -200.], [150., 200.]]) + VP_INIT
GRID_WEIGHT_INIT = 2e6
MAX_LINES = 500 # max lines to avoid over computation
HOOD_HEIGHT = H*3/4 # the part of image usually free from the car's hood
DEBUG = os.getenv("DEBUG") is not None
@@ -72,8 +73,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 +97,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
@@ -127,7 +136,12 @@ class Calibrator(object):
return
rot_speeds = np.array([0.,0.,-yaw_rate])
uvs[:,1,:] = denormalize(correct_pts(normalize(uvs[:,1,:]), rot_speeds, self.dt))
good_tracks = np.linalg.norm(uvs[:,1,:] - uvs[:,0,:], axis=1) > 10
# exclude tracks where:
# - pixel movement was less than 10 pixels
# - tracks are in the "hood region"
good_tracks = np.all([np.linalg.norm(uvs[:,1,:] - uvs[:,0,:], axis=1) > 10,
uvs[:,0,1] < HOOD_HEIGHT,
uvs[:,1,1] < HOOD_HEIGHT], axis = 0)
uvs = uvs[good_tracks]
if uvs.shape[0] > MAX_LINES:
uvs = uvs[np.random.choice(uvs.shape[0], MAX_LINES, replace=False), :]
@@ -136,7 +150,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 +203,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
selfdrive/locationd/get_vp.c
+1 -1
View File
@@ -11,7 +11,7 @@ int get_intersections(double *lines, double *intersections, long long n) {
Dx = L1[2] * L2[1] - L1[1] * L2[2];
Dy = L1[0] * L2[2] - L1[2] * L2[0];
// only intersect lines from different quadrants and only left-right crossing
if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0) && (L1[0]*L2[0] < 0)){
if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0) && (L1[1]*L2[1] < 0)){
x = Dx / D;
y = Dy / D;
if ((0 < x) &&