diff --git a/.gitignore b/.gitignore
index ca50a23656..623c45d79e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -25,11 +25,12 @@ clcache
board/obj/
selfdrive/boardd/boardd
-selfdrive/visiond/visiond
selfdrive/logcatd/logcatd
+selfdrive/mapd/default_speeds_by_region.json
selfdrive/proclogd/proclogd
selfdrive/ui/ui
selfdrive/test/tests/plant/out
+selfdrive/visiond/visiond
/src/
one
diff --git a/.travis.yml b/.travis.yml
index 74411b5de8..3dfe338239 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -7,6 +7,8 @@ install:
- docker build -t tmppilot -f Dockerfile.openpilot .
script:
- - docker run --rm
+ - docker run
-v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
+ - docker run
+ tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
diff --git a/README.md b/README.md
index b1ed6a1a69..f8052b2859 100644
--- a/README.md
+++ b/README.md
@@ -114,12 +114,11 @@ Community Maintained Cars
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
| Honda | Fit 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Inverted Nidec |
-| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph | Custom9|
-| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph | Custom9|
+| Tesla | Model S 2012-13 | All | Yes | Not yet | Not applicable | 0mph | Custom8|
[[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266).
[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246)
-9Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla)
+8Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla)
Community Maintained Cars are not confirmed by comma.ai to meet our [safety model](https://github.com/commaai/openpilot/blob/devel/SAFETY.md). Be extra cautious using them.
diff --git a/RELEASES.md b/RELEASES.md
index ae4c8f14a9..ef96e09e5a 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,15 @@
+Version 0.5.10 (2019-03-19)
+========================
+ * Self-tuning vehicle parameters: steering offset, tires stiffness and steering ratio
+ * Improve longitudinal control at low speed when lead vehicle harshly decelerates
+ * Fix panda bug going unexpectedly in DCP mode when EON is connected
+ * Reduce white panda power consumption by 500mW when EON is disconnected by turning off WIFI
+ * New Driver Monitoring Model
+ * Support QR codes for login using comma connect
+ * Refactor comma pedal FW and use CRC-8 checksum algorithm for safety. Reflashing pedal is required.
+ Please see `#hw-pedal` on [discord](discord.comma.ai) for assistance updating comma pedal.
+ * Additional speed limit rules for Germany thanks to arne182
+
Version 0.5.9 (2019-02-10)
========================
* Improve calibration using a dedicated neural network
@@ -8,7 +20,7 @@ Version 0.5.9 (2019-02-10)
* Kia Optima support thanks to emmertex!
* Buick Regal 2018 support thanks to HOYS!
* Comma pedal support for Toyota thanks to wocsor! Note: tuning needed and not maintained by comma
- * Chrysler Pacifica and Jeep Grand Cherokee suppor thanks to adhintz!
+ * Chrysler Pacifica and Jeep Grand Cherokee support thanks to adhintz!
Version 0.5.8 (2019-01-17)
========================
diff --git a/SAFETY.md b/SAFETY.md
index a2f77a5509..b5800a0d50 100644
--- a/SAFETY.md
+++ b/SAFETY.md
@@ -89,7 +89,7 @@ GM/Chevrolet
and openpilot to 350. This is approximately 0.3g of braking.
- Steering torque is controlled through the 0x180 CAN message and it's limited by the panda firmware and by
- openpilot to a value between -255 and 255. In addition, the vehicle EPS unit will not fault when
+ openpilot to a value between -300 and 300. In addition, the vehicle EPS unit will fault for
commands outside these limits. A steering torque rate limit is enforced by the panda firmware and by
openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
0.75s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's
@@ -105,5 +105,34 @@ GM/Chevrolet
- GM CAN uses both a counter and a checksum to ensure integrity and prevent
replay of the same message.
-**Extra note"**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or
+Hyundai/Kia (Lateral only)
+------
+
+ - While the system is engaged, steer commands are subject to the same limits used by
+ the stock system.
+
+ - Steering torque is controlled through the 0x340 CAN message and it's limited by the panda firmware and by
+ openpilot to a value between -255 and 255. In addition, the vehicle EPS unit will fault for
+ commands outside the values of -409 and 409. A steering torque rate limit is enforced by the panda firmware and by
+ openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
+ 0.85s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's
+ torque exceeds 50 units in the opposite dicrection to ensure limited applied torque against the
+ driver's will.
+
+Chrysler/Jeep/Fiat (Lateral only)
+------
+
+ - While the system is engaged, steer commands are subject to the same limits used by
+ the stock system.
+
+ - Steering torque is controlled through the 0x292 CAN message and it's limited by the panda firmware and by
+ openpilot to a value between -261 and 261. In addition, the vehicle EPS unit will fault for
+ commands outside these limits. A steering torque rate limit is enforced by the panda firmware and by
+ openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
+ 0.87s. Commanded steering torque is limited by the panda firmware and by openpilot to be no more than 80
+ units above the actual EPS generated motor torque to ensure limited differences between
+ commanded and actual torques.
+
+
+**Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or
not fully meeting the above requirements.
diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk
index aa70618563..b4b5e9377b 100644
--- a/apk/ai.comma.plus.offroad.apk
+++ b/apk/ai.comma.plus.offroad.apk
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:94b4a6bc1f7fc720b9f638268ff2345368164efd8a4710616b3e9ad5fd473fcf
-size 18388501
+oid sha256:e03ff9d6482c19f14ce565d64aab24433847208106e5a5e31947f5307e510ee5
+size 18376966
diff --git a/cereal/car.capnp b/cereal/car.capnp
index fbcbb10651..8fb3128676 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -72,6 +72,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
calibrationProgress @47;
lowBattery @48;
invalidGiraffeHonda @49;
+ vehicleModelInvalid @50;
}
}
@@ -317,6 +318,7 @@ struct CarParams {
hyundai @8;
chrysler @9;
tesla @10;
+ subaru @11;
}
# things about the car in the manual
diff --git a/cereal/log.capnp b/cereal/log.capnp
index eb6ea7632d..3f79deefe5 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -419,7 +419,7 @@ struct Live100Data {
alertType @44 :Text;
alertSound @45 :Text;
awarenessStatus @26 :Float32;
- angleOffset @27 :Float32;
+ angleModelBias @27 :Float32;
gpsPlannerActive @40 :Bool;
engageable @41 :Bool; # can OP be engaged?
driverMonitoringOn @43 :Bool;
@@ -582,6 +582,9 @@ struct Plan {
vCurvature @21 :Float32;
decelForTurn @22 :Bool;
mapValid @25 :Bool;
+ radarValid @28 :Bool;
+
+ processingDelay @29 :Float32;
struct GpsTrajectory {
@@ -608,8 +611,12 @@ struct PathPlan {
rPoly @6 :List(Float32);
rProb @7 :Float32;
- angleSteers @8 :Float32;
+ angleSteers @8 :Float32; # deg
+ rateSteers @13 :Float32; # deg/s
valid @9 :Bool;
+ paramsValid @10 :Bool;
+ modelValid @12 :Bool;
+ angleOffset @11 :Float32;
}
struct LiveLocationData {
@@ -1602,6 +1609,9 @@ struct LiveParametersData {
valid @0 :Bool;
gyroBias @1 :Float32;
angleOffset @2 :Float32;
+ angleOffsetAverage @3 :Float32;
+ stiffnessFactor @4 :Float32;
+ steerRatio @5 :Float32;
}
struct LiveMapData {
diff --git a/common/ffi_wrapper.py b/common/ffi_wrapper.py
index 65b790a803..fd6b2bf7b3 100644
--- a/common/ffi_wrapper.py
+++ b/common/ffi_wrapper.py
@@ -6,7 +6,11 @@ from cffi import FFI
TMPDIR = "/tmp/ccache"
-def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=[]):
+
+def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=None):
+ if libraries is None:
+ libraries = []
+
cache = name + "_" + hashlib.sha1(c_code).hexdigest()
try:
os.mkdir(tmpdir)
@@ -28,7 +32,11 @@ def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=[]):
return mod.ffi, mod.lib
-def compile_code(name, c_code, c_header, directory, cflags="", libraries=[]):
+
+def compile_code(name, c_code, c_header, directory, cflags="", libraries=None):
+ if libraries is None:
+ libraries = []
+
ffibuilder = FFI()
ffibuilder.set_source(name, c_code, source_extension='.cpp', libraries=libraries)
ffibuilder.cdef(c_header)
@@ -36,6 +44,7 @@ def compile_code(name, c_code, c_header, directory, cflags="", libraries=[]):
os.environ['CFLAGS'] = cflags
ffibuilder.compile(verbose=True, debug=False, tmpdir=directory)
+
def wrap_compiled(name, directory):
sys.path.append(directory)
mod = __import__(name)
diff --git a/common/params.py b/common/params.py
index de0e7a1419..30ebaa0609 100755
--- a/common/params.py
+++ b/common/params.py
@@ -29,6 +29,7 @@ import fcntl
import tempfile
from enum import Enum
+
def mkdirs_exists_ok(path):
try:
os.makedirs(path)
@@ -36,52 +37,47 @@ def mkdirs_exists_ok(path):
if not os.path.isdir(path):
raise
+
class TxType(Enum):
PERSISTENT = 1
CLEAR_ON_MANAGER_START = 2
CLEAR_ON_CAR_START = 3
+
class UnknownKeyName(Exception):
pass
+
keys = {
-# written: manager
-# read: loggerd, uploaderd, offroad
- "DongleId": TxType.PERSISTENT,
"AccessToken": TxType.PERSISTENT,
- "Version": TxType.PERSISTENT,
- "TrainingVersion": TxType.PERSISTENT,
- "GitCommit": TxType.PERSISTENT,
+ "CalibrationParams": TxType.PERSISTENT,
+ "CarParams": TxType.CLEAR_ON_CAR_START,
+ "CompletedTrainingVersion": TxType.PERSISTENT,
+ "ControlsParams": TxType.PERSISTENT,
+ "DoUninstall": TxType.CLEAR_ON_MANAGER_START,
+ "DongleId": TxType.PERSISTENT,
"GitBranch": TxType.PERSISTENT,
+ "GitCommit": TxType.PERSISTENT,
"GitRemote": TxType.PERSISTENT,
-# written: baseui
-# read: ui, controls
- "IsMetric": TxType.PERSISTENT,
- "IsFcwEnabled": TxType.PERSISTENT,
"HasAcceptedTerms": TxType.PERSISTENT,
- "CompletedTrainingVersion": TxType.PERSISTENT,
- "IsUploadVideoOverCellularEnabled": TxType.PERSISTENT,
"IsDriverMonitoringEnabled": TxType.PERSISTENT,
+ "IsFcwEnabled": TxType.PERSISTENT,
"IsGeofenceEnabled": TxType.PERSISTENT,
- "SpeedLimitOffset": TxType.PERSISTENT,
-# written: visiond
-# read: visiond, controlsd
- "CalibrationParams": TxType.PERSISTENT,
- "ControlsParams": TxType.PERSISTENT,
-# written: controlsd
-# read: radard
- "CarParams": TxType.CLEAR_ON_CAR_START,
-
- "Passive": TxType.PERSISTENT,
- "DoUninstall": TxType.CLEAR_ON_MANAGER_START,
- "ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
+ "IsMetric": TxType.PERSISTENT,
"IsUpdateAvailable": TxType.PERSISTENT,
- "LongitudinalControl": TxType.PERSISTENT,
+ "IsUploadVideoOverCellularEnabled": TxType.PERSISTENT,
"LimitSetSpeed": TxType.PERSISTENT,
-
+ "LiveParameters": TxType.PERSISTENT,
+ "LongitudinalControl": TxType.PERSISTENT,
+ "Passive": TxType.PERSISTENT,
"RecordFront": TxType.PERSISTENT,
+ "ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
+ "SpeedLimitOffset": TxType.PERSISTENT,
+ "TrainingVersion": TxType.PERSISTENT,
+ "Version": TxType.PERSISTENT,
}
+
def fsync_dir(path):
fd = os.open(path, os.O_RDONLY)
try:
diff --git a/common/sympy_helpers.py b/common/sympy_helpers.py
new file mode 100644
index 0000000000..879eb71bbf
--- /dev/null
+++ b/common/sympy_helpers.py
@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+import sympy as sp
+import numpy as np
+
+def cross(x):
+ ret = sp.Matrix(np.zeros((3,3)))
+ ret[0,1], ret[0,2] = -x[2], x[1]
+ ret[1,0], ret[1,2] = x[2], -x[0]
+ ret[2,0], ret[2,1] = -x[1], x[0]
+ return ret
+
+def euler_rotate(roll, pitch, yaw):
+ # make symbolic rotation matrix from eulers
+ matrix_roll = sp.Matrix([[1, 0, 0],
+ [0, sp.cos(roll), -sp.sin(roll)],
+ [0, sp.sin(roll), sp.cos(roll)]])
+ matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
+ [0, 1, 0],
+ [-sp.sin(pitch), 0, sp.cos(pitch)]])
+ matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
+ [sp.sin(yaw), sp.cos(yaw), 0],
+ [0, 0, 1]])
+ return matrix_yaw*matrix_pitch*matrix_roll
+
+def quat_rotate(q0, q1, q2, q3):
+ # make symbolic rotation matrix from quat
+ return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2)],
+ [2*(q1*q2 - q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 + q0*q1)],
+ [2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
+
+def quat_matrix_l(p):
+ return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
+ [p[1], p[0], -p[3], p[2]],
+ [p[2], p[3], p[0], -p[1]],
+ [p[3], -p[2], p[1], p[0]]])
+
+def quat_matrix_r(p):
+ return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
+ [p[1], p[0], p[3], -p[2]],
+ [p[2], -p[3], p[0], p[1]],
+ [p[3], p[2], -p[1], p[0]]])
+
+
+def sympy_into_c(sympy_functions):
+ from sympy.utilities import codegen
+ routines = []
+ for name, expr, args in sympy_functions:
+ r = codegen.make_routine(name, expr, language="C99")
+
+ # argument ordering input to sympy is broken with function with output arguments
+ nargs = []
+ # reorder the input arguments
+ for aa in args:
+ if aa is None:
+ nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1,1]))
+ continue
+ found = False
+ for a in r.arguments:
+ if str(aa.name) == str(a.name):
+ nargs.append(a)
+ found = True
+ break
+ if not found:
+ # [1,1] is a hack for Matrices
+ nargs.append(codegen.InputArgument(aa, dimensions=[1,1]))
+ # add the output arguments
+ for a in r.arguments:
+ if type(a) == codegen.OutputArgument:
+ nargs.append(a)
+
+ #assert len(r.arguments) == len(args)+1
+ r.arguments = nargs
+
+ # add routine to list
+ routines.append(r)
+
+ [(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
+ c_code = '\n'.join(filter(lambda x: len(x) > 0 and x[0] != '#', c_code.split("\n")))
+ c_header = '\n'.join(filter(lambda x: len(x) > 0 and x[0] != '#', c_header.split("\n")))
+
+ return c_header, c_code
diff --git a/common/transformations/camera.py b/common/transformations/camera.py
index 348152d799..42432a7bbc 100644
--- a/common/transformations/camera.py
+++ b/common/transformations/camera.py
@@ -112,6 +112,7 @@ def img_from_device(pt_device):
return pt_img.reshape(input_shape)[:,:2]
+#TODO please use generic img transform below
def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
size = img.shape[:2]
rot = orient.rot_from_euler(eulers)
@@ -138,3 +139,36 @@ def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
img_warped = cv2.warpPerspective(img, M, size[::-1])
return img_warped[H_border: size[0] - H_border,
W_border: size[1] - W_border]
+
+
+def transform_img(base_img,
+ augment_trans=np.array([0,0,0]),
+ augment_eulers=np.array([0,0,0]),
+ from_intr=eon_intrinsics,
+ to_intr=eon_intrinsics,
+ calib_rot_view=None,
+ output_size=None):
+ cy = from_intr[1,2]
+ size = base_img.shape[:2]
+ if not output_size:
+ output_size = size[::-1]
+ h = 1.22
+ quadrangle = np.array([[0, cy + 20],
+ [size[1]-1, cy + 20],
+ [0, size[0]-1],
+ [size[1]-1, size[0]-1]], dtype=np.float32)
+ quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1))))
+ quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1],
+ h*np.ones(4),
+ h/quadrangle_norm[:,1]))
+ rot = orient.rot_from_euler(augment_eulers)
+ if calib_rot_view is not None:
+ rot = calib_rot_view.dot(rot)
+ to_extrinsics = np.hstack((rot.T, -augment_trans[:,None]))
+ to_KE = to_intr.dot(to_extrinsics)
+ warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1)))))
+ warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2],
+ warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32)
+ M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32))
+ augmented_rgb = cv2.warpPerspective(base_img, M, output_size, borderMode=cv2.BORDER_REPLICATE)
+ return augmented_rgb
diff --git a/common/transformations/model.py b/common/transformations/model.py
index 424bd6158e..2d41d9cebe 100644
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@@ -31,6 +31,18 @@ model_intrinsics = np.array(
[ 0. , 0. , 1.]])
+# MED model
+MEDMODEL_INPUT_SIZE = (640, 240)
+MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
+MEDMODEL_CY = 47.6
+
+medmodel_zoom = 1.
+medmodel_intrinsics = np.array(
+ [[ eon_focal_length / medmodel_zoom, 0. , 0.5 * MEDMODEL_INPUT_SIZE[0]],
+ [ 0. , eon_focal_length / medmodel_zoom, MEDMODEL_CY],
+ [ 0. , 0. , 1.]])
+
+
# BIG model
BIGMODEL_INPUT_SIZE = (864, 288)
@@ -57,6 +69,9 @@ model_frame_from_road_frame = np.dot(model_intrinsics,
bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
get_view_frame_from_road_frame(0, 0, 0, model_height))
+medmodel_frame_from_road_frame = np.dot(medmodel_intrinsics,
+ get_view_frame_from_road_frame(0, 0, 0, model_height))
+
model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics))
# 'camera from model camera'
@@ -110,3 +125,17 @@ def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
camera_frame_from_bigmodel_frame = np.dot(camera_frame_from_ground, ground_from_bigmodel_frame)
return camera_frame_from_bigmodel_frame
+
+
+def get_model_frame(snu_full, camera_frame_from_model_frame, size):
+ idxs = camera_frame_from_model_frame.dot(np.column_stack([np.tile(np.arange(size[0]), size[1]),
+ np.tile(np.arange(size[1]), (size[0],1)).T.flatten(),
+ np.ones(size[0] * size[1])]).T).T.astype(int)
+ calib_flat = snu_full[idxs[:,1], idxs[:,0]]
+ if len(snu_full.shape) == 3:
+ calib = calib_flat.reshape((size[1], size[0], 3))
+ elif len(snu_full.shape) == 2:
+ calib = calib_flat.reshape((size[1], size[0]))
+ else:
+ raise ValueError("shape of input img is weird")
+ return calib
diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc
index ca9ebeb205..ef7913b392 100644
--- a/models/monitoring_model.dlc
+++ b/models/monitoring_model.dlc
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:782e58e2fcbcbb39011011aa72c884ab3aa0cef22c434282df17ce5d64f2cba1
-size 428100
+oid sha256:0e880365b63e8d48bf8bee797dc96b483682a6b0b14e968e937c9109377b926e
+size 427951
diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt
index 8cadf936eb..48b2ddb311 100644
--- a/requirements_openpilot.txt
+++ b/requirements_openpilot.txt
@@ -15,7 +15,7 @@ cffi==1.11.5
contextlib2==0.5.4
crc16==0.1.1
crcmod==1.7
-cryptography==2.1.4
+cryptography==1.4
cycler==0.10.0
decorator==4.0.10
docopt==0.6.2
@@ -24,6 +24,7 @@ evdev==0.6.1
fastcluster==1.1.20
filterpy==1.2.4
ipaddress==1.0.16
+json-rpc==1.12.1
libusb1==1.5.0
lmdb==0.92
mpmath==1.0.0
@@ -31,7 +32,7 @@ nose==1.3.7
numpy==1.11.1
pause==0.1.2
py==1.4.31
-pyOpenSSL==17.5.0
+pyOpenSSL==16.0.0
pyasn1-modules==0.0.8
pyasn1==0.1.9
pycapnp==0.6.3
@@ -64,3 +65,4 @@ sympy==1.1.1
tqdm==4.23.1
ujson==1.35
v4l2==0.2
+websocket_client==0.55.0
diff --git a/selfdrive/athena/__init__.py b/selfdrive/athena/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
new file mode 100755
index 0000000000..f6f9ef231a
--- /dev/null
+++ b/selfdrive/athena/athenad.py
@@ -0,0 +1,128 @@
+#!/usr/bin/env python2.7
+import json
+import os
+import random
+import time
+import threading
+import traceback
+import zmq
+import Queue
+from jsonrpc import JSONRPCResponseManager, dispatcher
+from websocket import create_connection, WebSocketTimeoutException
+
+import selfdrive.crash as crash
+import selfdrive.messaging as messaging
+from common.params import Params
+from selfdrive.services import service_list
+from selfdrive.swaglog import cloudlog
+from selfdrive.version import version, dirty
+
+ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
+HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4)
+
+dispatcher["echo"] = lambda s: s
+payload_queue = Queue.Queue()
+response_queue = Queue.Queue()
+
+def handle_long_poll(ws):
+ end_event = threading.Event()
+
+ threads = [
+ threading.Thread(target=ws_recv, args=(ws, end_event)),
+ threading.Thread(target=ws_send, args=(ws, end_event))
+ ] + [
+ threading.Thread(target=jsonrpc_handler, args=(end_event,))
+ for x in xrange(HANDLER_THREADS)
+ ]
+
+ map(lambda thread: thread.start(), threads)
+ try:
+ while not end_event.is_set():
+ time.sleep(0.1)
+ except (KeyboardInterrupt, SystemExit):
+ end_event.set()
+ raise
+ finally:
+ for i, thread in enumerate(threads):
+ thread.join()
+
+def jsonrpc_handler(end_event):
+ while not end_event.is_set():
+ try:
+ data = payload_queue.get(timeout=1)
+ response = JSONRPCResponseManager.handle(data, dispatcher)
+ response_queue.put_nowait(response)
+ except Queue.Empty:
+ pass
+ except Exception as e:
+ cloudlog.exception("athena jsonrpc handler failed")
+ traceback.print_exc()
+ response_queue.put_nowait(json.dumps({"error": str(e)}))
+
+# security: user should be able to request any message from their car
+# TODO: add service to, for example, start visiond and take a picture
+@dispatcher.add_method
+def getMessage(service=None, timeout=1000):
+ context = zmq.Context()
+ if service is None or service not in service_list:
+ raise Exception("invalid service")
+ socket = messaging.sub_sock(context, service_list[service].port)
+ socket.setsockopt(zmq.RCVTIMEO, timeout)
+ ret = messaging.recv_one(socket)
+ return ret.to_dict()
+
+def ws_recv(ws, end_event):
+ while not end_event.is_set():
+ try:
+ data = ws.recv()
+ payload_queue.put_nowait(data)
+ except WebSocketTimeoutException:
+ pass
+ except Exception:
+ traceback.print_exc()
+ end_event.set()
+
+def ws_send(ws, end_event):
+ while not end_event.is_set():
+ try:
+ response = response_queue.get(timeout=1)
+ ws.send(response.json)
+ except Queue.Empty:
+ pass
+ except Exception:
+ traceback.print_exc()
+ end_event.set()
+
+def backoff(retries):
+ return random.randrange(0, min(128, int(2 ** retries)))
+
+def main(gctx=None):
+ params = Params()
+ dongle_id = params.get("DongleId")
+ access_token = params.get("AccessToken")
+ ws_uri = ATHENA_HOST + "/ws/" + dongle_id
+
+ crash.bind_user(id=dongle_id)
+ crash.bind_extra(version=version, dirty=dirty, is_eon=True)
+ crash.install()
+
+ conn_retries = 0
+ while 1:
+ try:
+ print("connecting to %s" % ws_uri)
+ ws = create_connection(ws_uri,
+ cookie="jwt=" + access_token,
+ enable_multithread=True)
+ ws.settimeout(1)
+ conn_retries = 0
+ handle_long_poll(ws)
+ except (KeyboardInterrupt, SystemExit):
+ break
+ except Exception:
+ conn_retries += 1
+ traceback.print_exc()
+
+ time.sleep(backoff(conn_retries))
+
+if __name__ == "__main__":
+ main()
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 74b1f4c5de..dd89d4d6d4 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -42,6 +42,7 @@
#define SAFETY_HYUNDAI 7
#define SAFETY_TESLA 8
#define SAFETY_CHRYSLER 9
+#define SAFETY_SUBARU 10
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
@@ -124,6 +125,9 @@ void *safety_setter_thread(void *s) {
case (int)cereal::CarParams::SafetyModels::CHRYSLER:
safety_setting = SAFETY_CHRYSLER;
break;
+ case (int)cereal::CarParams::SafetyModels::SUBARU:
+ safety_setting = SAFETY_SUBARU;
+ break;
default:
LOGE("unknown safety model: %d", safety_model);
}
@@ -331,6 +335,11 @@ void can_send(void *s) {
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot();
+ if (nanos_since_boot() - event.getLogMonoTime() > 1e9) {
+ //Older than 1 second. Dont send.
+ zmq_msg_close(&msg);
+ return;
+ }
int msg_count = event.getCan().size();
uint32_t *send = (uint32_t*)malloc(msg_count*0x10);
diff --git a/selfdrive/can/common.h b/selfdrive/can/common.h
index f8d07ae681..4f09722843 100644
--- a/selfdrive/can/common.h
+++ b/selfdrive/can/common.h
@@ -10,6 +10,7 @@
unsigned int honda_checksum(unsigned int address, uint64_t d, int l);
unsigned int toyota_checksum(unsigned int address, uint64_t d, int l);
+unsigned int pedal_checksum(unsigned int address, uint64_t d, int l);
struct SignalPackValue {
const char* name;
@@ -41,6 +42,8 @@ enum SignalType {
HONDA_CHECKSUM,
HONDA_COUNTER,
TOYOTA_CHECKSUM,
+ PEDAL_CHECKSUM,
+ PEDAL_COUNTER,
};
struct Signal {
diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc
index 2d4fb9570a..776403b22f 100644
--- a/selfdrive/can/dbc_template.cc
+++ b/selfdrive/can/dbc_template.cc
@@ -27,6 +27,10 @@ const Signal sigs_{{address}}[] = {
.type = SignalType::HONDA_COUNTER,
{% elif checksum_type == "toyota" and sig.name == "CHECKSUM" %}
.type = SignalType::TOYOTA_CHECKSUM,
+ {% elif address in [512, 513] and sig.name == "CHECKSUM_PEDAL" %}
+ .type = SignalType::PEDAL_CHECKSUM,
+ {% elif address in [512, 513] and sig.name == "COUNTER_PEDAL" %}
+ .type = SignalType::PEDAL_COUNTER,
{% else %}
.type = SignalType::DEFAULT,
{% endif %}
diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py
index 09f5e5121d..cf712cf634 100644
--- a/selfdrive/can/libdbc_py.py
+++ b/selfdrive/can/libdbc_py.py
@@ -39,6 +39,8 @@ typedef enum {
HONDA_CHECKSUM,
HONDA_COUNTER,
TOYOTA_CHECKSUM,
+ PEDAL_CHECKSUM,
+ PEDAL_COUNTER,
} SignalType;
typedef struct {
diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc
index 7bb4ef26f7..b6845b47db 100644
--- a/selfdrive/can/parser.cc
+++ b/selfdrive/can/parser.cc
@@ -51,6 +51,30 @@ unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) {
return s & 0xFF;
}
+unsigned int pedal_checksum(unsigned int address, uint64_t d, int l) {
+ uint8_t crc = 0xFF;
+ uint8_t poly = 0xD5; // standard crc8
+
+ d >>= ((8-l)*8); // remove padding
+ d >>= 8; // remove checksum
+
+ uint8_t *dat = (uint8_t *)&d;
+
+ int i, j;
+ for (i = 0; i < l - 1; i++) {
+ crc ^= dat[i];
+ for (j = 0; j < 8; j++) {
+ if ((crc & 0x80) != 0) {
+ crc = (uint8_t)((crc << 1) ^ poly);
+ }
+ else {
+ crc <<= 1;
+ }
+ }
+ }
+ return crc;
+}
+
namespace {
uint64_t read_u64_be(const uint8_t* v) {
@@ -113,16 +137,23 @@ struct MessageState {
return false;
}
} else if (sig.type == SignalType::HONDA_COUNTER) {
- if (!honda_update_counter(tmp)) {
+ if (!update_counter_generic(tmp, sig.b2)) {
return false;
}
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
- // INFO("CHECKSUM %d %d %018llX - %lld vs %d\n", address, size, dat, tmp, toyota_checksum(address, dat, size));
-
if (toyota_checksum(address, dat, size) != tmp) {
INFO("%X CHECKSUM FAIL\n", address);
return false;
}
+ } else if (sig.type == SignalType::PEDAL_CHECKSUM) {
+ if (pedal_checksum(address, dat, size) != tmp) {
+ INFO("%X PEDAL CHECKSUM FAIL\n", address);
+ return false;
+ }
+ } else if (sig.type == SignalType::PEDAL_COUNTER) {
+ if (!update_counter_generic(tmp, sig.b2)) {
+ return false;
+ }
}
vals[i] = tmp * sig.factor + sig.offset;
@@ -134,10 +165,10 @@ struct MessageState {
}
- bool honda_update_counter(int64_t v) {
+ bool update_counter_generic(int64_t v, int cnt_size) {
uint8_t old_counter = counter;
counter = v;
- if (((old_counter+1) & 3) != v) {
+ if (((old_counter+1) & ((1 << cnt_size) -1)) != v) {
counter_fail += 1;
if (counter_fail > 1) {
INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py
index 165673cea5..8960953eda 100644
--- a/selfdrive/can/parser.py
+++ b/selfdrive/can/parser.py
@@ -4,8 +4,12 @@ import numbers
from selfdrive.can.libdbc_py import libdbc, ffi
+
class CANParser(object):
- def __init__(self, dbc_name, signals, checks=[], bus=0, sendcan=False, tcp_addr="127.0.0.1"):
+ def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1"):
+ if checks is None:
+ checks = []
+
self.can_valid = True
self.vl = defaultdict(dict)
self.ts = defaultdict(dict)
diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py
index 331e0a3130..810947c8ef 100755
--- a/selfdrive/can/process_dbc.py
+++ b/selfdrive/can/process_dbc.py
@@ -50,7 +50,11 @@ for address, msg_name, msg_size, sigs in msgs:
sys.exit("COUNTER is not 2 bits longs %s" % msg_name)
if sig.start_bit % 8 != 5:
sys.exit("COUNTER starts at wrong bit %s" % msg_name)
-
+ if address in [0x200, 0x201]:
+ if sig.name == "COUNTER_PEDAL" and sig.size != 4:
+ sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name)
+ if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
+ sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name)
# Fail on duplicate message names
c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index f5c30921a0..255b20806c 100644
--- a/selfdrive/car/__init__.py
+++ b/selfdrive/car/__init__.py
@@ -45,3 +45,39 @@ def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torq
min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP))
return int(round(apply_torque))
+
+
+def crc8_pedal(data):
+ crc = 0xFF # standard init value
+ poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1
+ size = len(data)
+ for i in range(size-1, -1, -1):
+ crc ^= data[i]
+ for j in range(8):
+ if ((crc & 0x80) != 0):
+ crc = ((crc << 1) ^ poly) & 0xFF
+ else:
+ crc <<= 1
+ return crc
+
+
+def create_gas_command(packer, gas_amount, idx):
+ # Common gas pedal msg generator
+ enable = gas_amount > 0.001
+
+ values = {
+ "ENABLE": enable,
+ "COUNTER_PEDAL": idx & 0xF,
+ }
+
+ if enable:
+ values["GAS_COMMAND"] = gas_amount * 255.
+ values["GAS_COMMAND2"] = gas_amount * 255.
+
+ dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2]
+
+ dat = [ord(i) for i in dat]
+ checksum = crc8_pedal(dat[:-1])
+ values["CHECKSUM_PEDAL"] = checksum
+
+ return packer.make_can_msg("GAS_COMMAND", 0, values)
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index 72f602d9a8..a6344027df 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -111,17 +111,17 @@ class CarState(object):
self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
- self.v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
+ v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
# Kalman filter
- if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
- self.v_ego_raw = self.v_wheel
- v_ego_x = self.v_ego_kf.update(self.v_wheel)
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
- self.standstill = not self.v_wheel > 0.001
+ self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE']
diff --git a/selfdrive/car/chrysler/chryslercan_test.py b/selfdrive/car/chrysler/chryslercan_test.py
index 85a8496612..2edfff9f2a 100644
--- a/selfdrive/car/chrysler/chryslercan_test.py
+++ b/selfdrive/car/chrysler/chryslercan_test.py
@@ -1,6 +1,5 @@
import chryslercan
from values import CAR
-from carcontroller import CarController
from selfdrive.can.packer import CANPacker
from cereal import car
diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py
index 703ba6a51c..6e3a24b8fd 100644
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@@ -34,7 +34,7 @@ FINGERPRINTS = {
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770:8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8},
# Based on 0607d2516fc2148f|2019-02-13--23-03-16
{
- 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8
}
],
CAR.JEEP_CHEROKEE: [
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index c860cfdee4..5a8f1f142c 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -65,17 +65,17 @@ class CarState(object):
self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
- self.v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
+ v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
# Kalman filter
- if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
- self.v_ego_raw = self.v_wheel
- v_ego_x = self.v_ego_kf.update(self.v_wheel)
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
- self.standstill = not self.v_wheel > 0.001
+ self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index d5fa975de5..ae97a38b9c 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -79,10 +79,13 @@ class CarState(object):
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
- speed_estimate = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
+ v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
- self.v_ego_raw = speed_estimate
- v_ego_x = self.v_ego_kf.update(speed_estimate)
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = [[v_wheel], [0.0]]
+
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index f87f56e494..08c0235fd4 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -3,6 +3,7 @@ from common.realtime import sec_since_boot
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip
+from selfdrive.car import create_gas_command
from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import AH, CruiseButtons, CAR
from selfdrive.can.packer import CANPacker
@@ -170,7 +171,7 @@ class CarController(object):
else:
# Send gas and brake commands.
if (frame % 2) == 0:
- idx = (frame / 2) % 4
+ idx = frame / 2
pump_on, self.last_pump_ts = brake_pump_hysteresys(apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx))
@@ -179,6 +180,6 @@ class CarController(object):
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
- can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx))
+ can_sends.append(create_gas_command(self.packer, apply_gas, idx))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 00aa358d8a..f4c7cade8c 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -215,15 +215,15 @@ class CarState(object):
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
- self.v_wheel = (self.v_wheel_fl+self.v_wheel_fr+self.v_wheel_rl+self.v_wheel_rr)/4.
+ v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr)/4.
# blend in transmission speed at low speed, since it has more low speed accuracy
- self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
+ self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
- self.v_weight * self.v_wheel
+ self.v_weight * v_wheel
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = [[speed], [0.0]]
+ self.v_ego_kf.x = [[speed], [0.0]]
self.v_ego_raw = speed
v_ego_x = self.v_ego_kf.update(speed)
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index 3d92d24a7c..435360d65f 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -41,18 +41,6 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx)
-def create_gas_command(packer, gas_amount, idx):
- enable = gas_amount > 0.001
-
- values = {"ENABLE": enable}
-
- if enable:
- values["GAS_COMMAND"] = gas_amount * 255.
- values["GAS_COMMAND2"] = gas_amount * 255.
-
- return packer.make_can_msg("GAS_COMMAND", 0, values, idx)
-
-
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx):
values = {
"STEER_TORQUE": apply_steer if lkas_active else 0,
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 3b423ae04f..bd3e63740c 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -167,22 +167,22 @@ class CarState(object):
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
- self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
+ v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
- self.low_speed_lockout = self.v_wheel < 1.0
+ self.low_speed_lockout = v_wheel < 1.0
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
- if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
- self.v_ego_raw = self.v_wheel
- v_ego_x = self.v_ego_kf.update(self.v_wheel)
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
- self.standstill = not self.v_wheel > 0.1
+ self.standstill = not v_wheel > 0.1
self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index a854eebbc3..d7fc2bd394 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -87,7 +87,6 @@ class CarInterface(object):
ret.minSteerSpeed = 0.
elif candidate == CAR.KIA_SORENTO:
ret.steerKf = 0.00005
- ret.steerRateCost = 0.5
ret.mass = 1985 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
@@ -96,7 +95,6 @@ class CarInterface(object):
ret.minSteerSpeed = 0.
elif candidate == CAR.ELANTRA:
ret.steerKf = 0.00006
- ret.steerRateCost = 0.5
ret.mass = 1275 + std_cargo
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
@@ -106,7 +104,6 @@ class CarInterface(object):
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.GENESIS:
ret.steerKf = 0.00005
- ret.steerRateCost = 0.5
ret.mass = 2060 + std_cargo
ret.wheelbase = 3.01
ret.steerRatio = 16.5
@@ -123,7 +120,6 @@ class CarInterface(object):
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_STINGER:
ret.steerKf = 0.00005
- ret.steerRateCost = 0.5
ret.mass = 1825 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
diff --git a/selfdrive/car/subaru/__init__.py b/selfdrive/car/subaru/__init__.py
new file mode 100644
index 0000000000..8b13789179
--- /dev/null
+++ b/selfdrive/car/subaru/__init__.py
@@ -0,0 +1 @@
+
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
new file mode 100644
index 0000000000..e12f5f957f
--- /dev/null
+++ b/selfdrive/car/subaru/carstate.py
@@ -0,0 +1,104 @@
+import numpy as np
+from common.kalman.simple_kalman import KF1D
+from selfdrive.config import Conversions as CV
+from selfdrive.can.parser import CANParser
+from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
+
+def get_powertrain_can_parser(CP):
+ # this function generates lists for signal, messages and initial values
+ signals = [
+ # sig_name, sig_address, default
+ ("Steer_Torque_Sensor", "Steering_Torque", 0),
+ ("Steering_Angle", "Steering_Torque", 0),
+ ("Cruise_On", "CruiseControl", 0),
+ ("Cruise_Activated", "CruiseControl", 0),
+ ("Brake_Pedal", "Brake_Pedal", 0),
+ ("Throttle_Pedal", "Throttle", 0),
+ ("LEFT_BLINKER", "Dashlights", 0),
+ ("RIGHT_BLINKER", "Dashlights", 0),
+ ("SEATBELT_FL", "Dashlights", 0),
+ ("FL", "Wheel_Speeds", 0),
+ ("FR", "Wheel_Speeds", 0),
+ ("RL", "Wheel_Speeds", 0),
+ ("RR", "Wheel_Speeds", 0),
+ ("DOOR_OPEN_FR", "BodyInfo", 1),
+ ("DOOR_OPEN_FL", "BodyInfo", 1),
+ ("DOOR_OPEN_RR", "BodyInfo", 1),
+ ("DOOR_OPEN_RL", "BodyInfo", 1),
+ ]
+
+ checks = [
+ # sig_address, frequency
+ ("Dashlights", 10),
+ ("CruiseControl", 20),
+ ("Wheel_Speeds", 50),
+ ("Steering_Torque", 100),
+ ("BodyInfo", 10),
+ ]
+
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+
+class CarState(object):
+ def __init__(self, CP):
+ # initialize can parser
+ self.CP = CP
+
+ self.car_fingerprint = CP.carFingerprint
+ self.left_blinker_on = False
+ self.prev_left_blinker_on = False
+ self.right_blinker_on = False
+ self.prev_right_blinker_on = False
+ self.steer_torque_driver = 0
+ self.steer_not_allowed = False
+ self.main_on = False
+
+ # vEgo kalman filter
+ dt = 0.01
+ self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
+ A=np.matrix([[1., dt], [0., 1.]]),
+ C=np.matrix([1., 0.]),
+ K=np.matrix([[0.12287673], [0.29666309]]))
+ self.v_ego = 0.
+
+ def update(self, cp):
+
+ self.can_valid = True
+ self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal']
+ self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal']
+ self.user_gas_pressed = self.pedal_gas > 0
+ self.brake_pressed = self.brake_pressure > 0
+ self.brake_lights = bool(self.brake_pressed)
+
+ self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS
+ self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS
+ self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS
+ self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
+
+ v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
+ # Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
+
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
+
+ self.v_ego = float(v_ego_x[0])
+ self.a_ego = float(v_ego_x[1])
+ self.standstill = self.v_ego_raw < 0.01
+
+ self.prev_left_blinker_on = self.left_blinker_on
+ self.prev_right_blinker_on = self.right_blinker_on
+ self.left_blinker_on = cp.vl["Dashlights"]['LEFT_BLINKER'] == 1
+ self.right_blinker_on = cp.vl["Dashlights"]['RIGHT_BLINKER'] == 1
+ self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
+ self.steer_torque_driver = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
+ self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated']
+ self.main_on = cp.vl["CruiseControl"]['Cruise_On']
+ self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint]
+ self.angle_steers = cp.vl["Steering_Torque"]['Steering_Angle']
+ self.door_open = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'],
+ cp.vl["BodyInfo"]['DOOR_OPEN_RL'],
+ cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
+ cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
+
+
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
new file mode 100644
index 0000000000..93a1ac62b2
--- /dev/null
+++ b/selfdrive/car/subaru/interface.py
@@ -0,0 +1,205 @@
+#!/usr/bin/env python
+from cereal import car
+from common.realtime import sec_since_boot
+from selfdrive.config import Conversions as CV
+from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.car.subaru.values import CAR
+from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser
+
+try:
+ from selfdrive.car.subaru.carcontroller import CarController
+except ImportError:
+ CarController = None
+
+
+class CarInterface(object):
+ def __init__(self, CP, sendcan=None):
+ self.CP = CP
+
+ self.frame = 0
+ self.can_invalid_count = 0
+ self.acc_active_prev = 0
+
+ # *** init the major players ***
+ self.CS = CarState(CP)
+ self.VM = VehicleModel(CP)
+ self.pt_cp = get_powertrain_can_parser(CP)
+
+ # sending if read only is False
+ if sendcan is not None:
+ self.sendcan = sendcan
+ self.CC = CarController(CP.carFingerprint)
+
+ @staticmethod
+ def compute_gb(accel, speed):
+ return float(accel) / 4.0
+
+ @staticmethod
+ def calc_accel_override(a_ego, a_target, v_ego, v_target):
+ return 1.0
+
+ @staticmethod
+ def get_params(candidate, fingerprint):
+ ret = car.CarParams.new_message()
+
+ ret.carName = "subaru"
+ ret.carFingerprint = candidate
+ ret.safetyModel = car.CarParams.SafetyModels.subaru
+
+ ret.enableCruise = False
+ ret.steerLimitAlert = True
+ ret.enableCamera = True
+
+ std_cargo = 136
+ ret.steerRateCost = 0.7
+
+ if candidate in [CAR.IMPREZA]:
+ ret.mass = 1568 + std_cargo
+ ret.wheelbase = 2.67
+ ret.centerToFront = ret.wheelbase * 0.5
+ ret.steerRatio = 15
+ tire_stiffness_factor = 1.0
+ ret.steerActuatorDelay = 0.4 # end-to-end angle controller
+ ret.steerKf = 0.00005
+ ret.steerKiBP, ret.steerKpBP = [[0., 20.], [0., 20.]]
+ ret.steerKpV, ret.steerKiV = [[0.2, 0.3], [0.02, 0.03]]
+ ret.steerMaxBP = [0.] # m/s
+ ret.steerMaxV = [1.]
+
+ ret.steerControlType = car.CarParams.SteerControlType.torque
+ ret.steerRatioRear = 0.
+ # testing tuning
+
+ # No long control in subaru
+ ret.gasMaxBP = [0.]
+ ret.gasMaxV = [0.]
+ ret.brakeMaxBP = [0.]
+ ret.brakeMaxV = [0.]
+ ret.longPidDeadzoneBP = [0.]
+ ret.longPidDeadzoneV = [0.]
+ ret.longitudinalKpBP = [0.]
+ ret.longitudinalKpV = [0.]
+ ret.longitudinalKiBP = [0.]
+ ret.longitudinalKiV = [0.]
+
+ # end from gm
+
+ # hardcoding honda civic 2016 touring params so they can be used to
+ # scale unknown params for other cars
+ mass_civic = 2923./2.205 + std_cargo
+ wheelbase_civic = 2.70
+ centerToFront_civic = wheelbase_civic * 0.4
+ centerToRear_civic = wheelbase_civic - centerToFront_civic
+ rotationalInertia_civic = 2500
+ tireStiffnessFront_civic = 192150
+ tireStiffnessRear_civic = 202500
+ centerToRear = ret.wheelbase - ret.centerToFront
+
+ # TODO: get actual value, for now starting with reasonable value for
+ # civic and scaling by mass and wheelbase
+ ret.rotationalInertia = rotationalInertia_civic * \
+ ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
+
+ # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
+ # mass and CG position, so all cars will have approximately similar dyn behaviors
+ ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \
+ ret.mass / mass_civic * \
+ (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
+ ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \
+ ret.mass / mass_civic * \
+ (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
+
+ return ret
+
+ # returns a car.CarState
+ def update(self, c):
+
+ self.pt_cp.update(int(sec_since_boot() * 1e9), False)
+ self.CS.update(self.pt_cp)
+
+ # create message
+ ret = car.CarState.new_message()
+
+ # speeds
+ ret.vEgo = self.CS.v_ego
+ ret.aEgo = self.CS.a_ego
+ ret.vEgoRaw = self.CS.v_ego_raw
+ ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
+ ret.standstill = self.CS.standstill
+ ret.wheelSpeeds.fl = self.CS.v_wheel_fl
+ ret.wheelSpeeds.fr = self.CS.v_wheel_fr
+ ret.wheelSpeeds.rl = self.CS.v_wheel_rl
+ ret.wheelSpeeds.rr = self.CS.v_wheel_rr
+
+ # steering wheel
+ ret.steeringAngle = self.CS.angle_steers
+
+ # torque and user override. Driver awareness
+ # timer resets when the user uses the steering wheel.
+ ret.steeringPressed = self.CS.steer_override
+ ret.steeringTorque = self.CS.steer_torque_driver
+
+ # cruise state
+ ret.cruiseState.available = bool(self.CS.main_on)
+ ret.leftBlinker = self.CS.left_blinker_on
+ ret.rightBlinker = self.CS.right_blinker_on
+ ret.seatbeltUnlatched = self.CS.seatbelt_unlatched
+
+ buttonEvents = []
+
+ # blinkers
+ if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'leftBlinker'
+ be.pressed = self.CS.left_blinker_on
+ buttonEvents.append(be)
+
+ if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'rightBlinker'
+ be.pressed = self.CS.right_blinker_on
+ buttonEvents.append(be)
+
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'accelCruise'
+ buttonEvents.append(be)
+
+
+ events = []
+ if not self.CS.can_valid:
+ self.can_invalid_count += 1
+ if self.can_invalid_count >= 5:
+ events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+ else:
+ self.can_invalid_count = 0
+
+ if ret.seatbeltUnlatched:
+ events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+
+ if self.CS.acc_active and not self.acc_active_prev:
+ events.append(create_event('pcmEnable', [ET.ENABLE]))
+ if not self.CS.acc_active:
+ events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
+
+ ## handle button presses
+ #for b in ret.buttonEvents:
+ # # do enable on both accel and decel buttons
+ # if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+ # events.append(create_event('buttonEnable', [ET.ENABLE]))
+ # # do disable on button down
+ # if b.type == "cancel" and b.pressed:
+ # events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
+
+ ret.events = events
+
+ # update previous brake/gas pressed
+ self.acc_active_prev = self.CS.acc_active
+
+
+ # cast to reader so it can't be modified
+ return ret.as_reader()
+
+ def apply(self, c):
+ self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators)
+ self.frame += 1
diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py
new file mode 100644
index 0000000000..96159fd87d
--- /dev/null
+++ b/selfdrive/car/subaru/radar_interface.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+from cereal import car
+import time
+
+
+class RadarInterface(object):
+ def __init__(self, CP):
+ # radar
+ self.pts = {}
+ self.delay = 0.1
+
+ def update(self):
+
+ ret = car.RadarState.new_message()
+ time.sleep(0.05) # radard runs on RI updates
+
+ return ret
+
+if __name__ == "__main__":
+ RI = RadarInterface(None)
+ while 1:
+ ret = RI.update()
+ print(chr(27) + "[2J")
+ print ret
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
new file mode 100644
index 0000000000..18b8929564
--- /dev/null
+++ b/selfdrive/car/subaru/values.py
@@ -0,0 +1,18 @@
+from selfdrive.car import dbc_dict
+
+class CAR:
+ IMPREZA = "SUBARU IMPREZA LIMITED 2019"
+
+FINGERPRINTS = {
+ CAR.IMPREZA: [{
+ 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
+ }],
+}
+
+STEER_THRESHOLD = {
+ CAR.IMPREZA: 80,
+}
+
+DBC = {
+ CAR.IMPREZA: dbc_dict('subaru_global_2017', None),
+}
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8c1ed11929..cdd1bde51b 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -2,10 +2,11 @@ from cereal import car
from common.numpy_fast import clip, interp
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car import apply_toyota_steer_torque_limits
+from selfdrive.car import create_gas_command
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
create_steer_command, create_ui_command, \
create_ipas_steer_command, create_accel_command, \
- create_fcw_command, create_gas_command
+ create_fcw_command
from selfdrive.car.toyota.values import ECU, STATIC_MSGS
from selfdrive.can.packer import CANPacker
@@ -222,10 +223,10 @@ class CarController(object):
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
- if CS.CP.enableGasInterceptor:
+ if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
- can_sends.append(create_gas_command(self.packer, apply_gas))
+ can_sends.append(create_gas_command(self.packer, apply_gas, frame/2))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index bb3b3f0521..02debc47bf 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -62,7 +62,7 @@ def get_can_parser(CP):
if CP.carFingerprint == CAR.PRIUS:
signals += [("STATE", "AUTOPARK_STATUS", 0)]
-
+
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
@@ -129,17 +129,17 @@ class CarState(object):
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
- self.v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
+ v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
# Kalman filter
- if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
- self.v_ego_raw = self.v_wheel
- v_ego_x = self.v_ego_kf.update(self.v_wheel)
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
- self.standstill = not self.v_wheel > 0.001
+ self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index ae257940ae..720c0ef60a 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -78,18 +78,6 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
}
return packer.make_can_msg("ACC_CONTROL", 0, values)
-def create_gas_command(packer, gas_amount):
- """Creates a CAN message for the Pedal DBC GAS_COMMAND."""
- enable = gas_amount > 0.001
-
- values = {"ENABLE": enable}
-
- if enable:
- values["GAS_COMMAND"] = gas_amount * 255.
- values["GAS_COMMAND2"] = gas_amount * 255.
-
- return packer.make_can_msg("GAS_COMMAND", 0, values)
-
def create_fcw_command(packer, fcw):
values = {
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index ec127c941d..2f3f44448a 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -89,23 +89,12 @@ FINGERPRINTS = {
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, 512: 6, 513: 6, 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
- },
- # Prius Prime
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 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, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 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, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 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
- },
- # Taiwanese Prius Prime
- {
+ # with ipas
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 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, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 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
}],
#Corolla w/ added Pedal Support (512L and 513L)
CAR.COROLLA: [{
- 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 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: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 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, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- # Corolla LE 2017 w/ added Pedal Support (512L and 513L)
- {
- 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 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: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
+ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 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: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 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, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
}],
CAR.LEXUS_RXH: [{
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: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
@@ -133,6 +122,10 @@ FINGERPRINTS = {
#LE and LE with Blindspot Monitor
{
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, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+ },
+ #SL
+ {
+ 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, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 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, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.HIGHLANDER: [{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 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, 1014: 8, 1017: 8, 1020: 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, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 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, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index b1deb21165..b1cb55d122 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.5.9-release"
+#define COMMA_VERSION "0.5.10-release"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 7a17556ec3..b5d699bdaa 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -11,7 +11,7 @@ import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.car.car_helpers import get_car
-from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \
+from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \
get_events, \
create_event, \
EventTypes as ET, \
@@ -202,7 +202,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
- driver_status, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc):
+ driver_status, LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
@@ -241,7 +241,7 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
# Run angle offset learner at 20 Hz
if rk.frame % 5 == 2:
- angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
+ angle_model_bias = learn_angle_model_bias(active, CS.vEgo, angle_model_bias,
path_plan.cPoly, path_plan.cProb, CS.steeringAngle,
CS.steeringPressed)
@@ -277,12 +277,12 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
AM.process_alerts(sec_since_boot())
- return actuators, v_cruise_kph, driver_status, angle_offset, v_acc_sol, a_acc_sol
+ return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol
def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, AM, driver_status,
- LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc):
+ LaC, LoC, angle_model_bias, passive, start_time, params, v_acc, a_acc):
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
@@ -353,7 +353,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(plan.jerkFactor),
- "angleOffset": float(angle_offset),
+ "angleModelBias": float(angle_model_bias),
"gpsPlannerActive": plan.gpsPlannerActive,
"vCurvature": plan.vCurvature,
"decelForTurn": plan.decelForTurn,
@@ -378,7 +378,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
carcontrol.send(cc_send.to_bytes())
if (rk.frame % 36000) == 0: # update angle offset every 6 minutes
- params.put("ControlsParams", json.dumps({'angle_offset': angle_offset}))
+ params.put("ControlsParams", json.dumps({'angle_model_bias': angle_model_bias}))
return CC
@@ -461,12 +461,15 @@ def controlsd_thread(gctx=None, rate=100):
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
controls_params = params.get("ControlsParams")
+
# Read angle offset from previous drive
+ angle_model_bias = 0.
if controls_params is not None:
- controls_params = json.loads(controls_params)
- angle_offset = controls_params['angle_offset']
- else:
- angle_offset = 0.
+ try:
+ controls_params = json.loads(controls_params)
+ angle_model_bias = controls_params['angle_model_bias']
+ except (ValueError, KeyError):
+ pass
prof = Profiler(False) # off by default
@@ -485,6 +488,8 @@ def controlsd_thread(gctx=None, rate=100):
plan_age = (start_time - plan.logMonoTime) / 1e9
if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+ if not path_plan.pathPlan.paramsValid:
+ events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
events += list(plan.plan.events)
# Only allow engagement with brake pressed when stopped behind another stopped car
@@ -498,16 +503,16 @@ def controlsd_thread(gctx=None, rate=100):
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
- actuators, v_cruise_kph, driver_status, angle_offset, v_acc, a_acc = \
+ actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc = \
state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status,
- LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
+ LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
- live100, AM, driver_status, LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc)
+ live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, params, v_acc, a_acc)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz
diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py
index cc9b19d5d4..17caf7fbf6 100644
--- a/selfdrive/controls/lib/alerts.py
+++ b/selfdrive/controls/lib/alerts.py
@@ -624,4 +624,11 @@ ALERTS = [
"Set 0111 for openpilot. 1011 for stock",
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+ Alert(
+ "vehicleModelInvalid",
+ "Vehicle Parameter Identification Failed",
+ "",
+ AlertStatus.normal, AlertSize.small,
+ Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1),
]
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 9d78c96674..15d5a202f7 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -55,7 +55,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
-def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, angle_steers, steer_override):
+def learn_angle_model_bias(lateral_control, v_ego, angle_model_bias, c_poly, c_prob, angle_steers, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
# while being in the middle of the lane
min_offset = -5. # deg
@@ -69,10 +69,10 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, ang
# only learn if lateral control is active and if driver is not overriding:
if lateral_control and not steer_override:
- angle_offset += c_poly[3] * alpha_v
- angle_offset = clip(angle_offset, min_offset, max_offset)
+ angle_model_bias += c_poly[3] * alpha_v
+ angle_model_bias = clip(angle_model_bias, min_offset, max_offset)
- return angle_offset
+ return angle_model_bias
def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py
new file mode 100644
index 0000000000..f93a72cfcc
--- /dev/null
+++ b/selfdrive/controls/lib/fcw.py
@@ -0,0 +1,71 @@
+import numpy as np
+from collections import defaultdict
+
+from common.numpy_fast import interp
+
+_FCW_A_ACT_V = [-3., -2.]
+_FCW_A_ACT_BP = [0., 30.]
+
+
+class FCWChecker(object):
+ def __init__(self):
+ self.reset_lead(0.0)
+
+ def reset_lead(self, cur_time):
+ self.last_fcw_a = 0.0
+ self.v_lead_max = 0.0
+ self.lead_seen_t = cur_time
+ self.last_fcw_time = 0.0
+ self.last_min_a = 0.0
+
+ self.counters = defaultdict(lambda: 0)
+
+ @staticmethod
+ def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
+ max_ttc = 5.0
+
+ v_rel = v_ego - v_lead
+ a_rel = a_ego - a_lead
+
+ # assuming that closing gap ARel comes from lead vehicle decel,
+ # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
+ # This helps underweighting ARel when v_lead is close to zero.
+ t_decel = 2.
+ a_rel = np.minimum(a_rel, v_lead / t_decel)
+
+ # delta of the quadratic equation to solve for ttc
+ delta = v_rel**2 + 2 * x_lead * a_rel
+
+ # assign an arbitrary high ttc value if there is no solution to ttc
+ if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1):
+ ttc = max_ttc
+ else:
+ ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
+ return ttc
+
+ def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
+ mpc_solution_a = list(mpc_solution[0].a_ego)
+ self.last_min_a = min(mpc_solution_a)
+ self.v_lead_max = max(self.v_lead_max, v_lead)
+
+ if (fcw_lead > 0.99):
+ ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
+ self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0
+ self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
+ self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
+ self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
+ self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
+ self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
+ self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
+ self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
+
+ a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
+ a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
+
+ fcw_allowed = all(c >= 10 for c in self.counters.values())
+ if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
+ self.last_fcw_time = cur_time
+ self.last_fcw_a = self.last_min_a
+ return True
+
+ return False
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
index cc06fd40d7..6949438708 100644
--- a/selfdrive/controls/lib/latcontrol.py
+++ b/selfdrive/controls/lib/latcontrol.py
@@ -37,6 +37,8 @@ class LatControl(object):
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
+ # TODO: feedforward something based on path_plan.rateSteers
+ steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
index f4cee7f2d0..e5adf02cf4 100644
--- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
@@ -26,6 +26,7 @@ typedef struct {
double y[N+1];
double psi[N+1];
double delta[N+1];
+ double rate[N+1];
double cost;
} log_t;
@@ -103,7 +104,7 @@ int run_mpc(state_t * x0, log_t * solution,
acado_preparationStep();
acado_feedbackStep();
-
+
/* printf("lat its: %d\n", acado_getNWSR()); // n iterations
printf("Objective: %.6f\n", acado_getObjective()); // solution cost */
@@ -112,6 +113,7 @@ int run_mpc(state_t * x0, log_t * solution,
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
+ solution->rate[i] = acadoVariables.u[i];
}
solution->cost = acado_getObjective();
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o
index 0e8eb49134..cd461aa4eb 100644
--- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o
+++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:1d469e8cd75122d5996338da166919f3dda15c1ef72f5ce3e33c46096a168228
-size 2328
+oid sha256:b0ccf356c299a12f40fd99fa61361812e5fedff6f1836c8afad98b7b2f3a2a2a
+size 2456
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc.so b/selfdrive/controls/lib/lateral_mpc/libmpc.so
index 0d1a787c00..977124333c 100755
--- a/selfdrive/controls/lib/lateral_mpc/libmpc.so
+++ b/selfdrive/controls/lib/lateral_mpc/libmpc.so
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:0cc8148b7105e645cd232ac0c088c4324a545328eb64781939762214ddf14f90
+oid sha256:849733e32eecd68112c4ab305c013025dbfa41ec3f13c54fbb9368388d835260
size 532192
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
index 1537d2f6c0..3af46c304e 100644
--- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
@@ -18,6 +18,7 @@ typedef struct {
double y[21];
double psi[21];
double delta[21];
+ double rate[21];
double cost;
} log_t;
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
new file mode 100644
index 0000000000..19fb706deb
--- /dev/null
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -0,0 +1,120 @@
+import numpy as np
+
+import selfdrive.messaging as messaging
+from selfdrive.swaglog import cloudlog
+from common.realtime import sec_since_boot
+from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
+from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
+
+
+class LongitudinalMpc(object):
+ def __init__(self, mpc_id, live_longitudinal_mpc):
+ self.live_longitudinal_mpc = live_longitudinal_mpc
+ self.mpc_id = mpc_id
+
+ self.setup_mpc()
+ self.v_mpc = 0.0
+ self.v_mpc_future = 0.0
+ self.a_mpc = 0.0
+ self.v_cruise = 0.0
+ self.prev_lead_status = False
+ self.prev_lead_x = 0.0
+ self.new_lead = False
+
+ self.last_cloudlog_t = 0.0
+
+ def send_mpc_solution(self, qp_iterations, calculation_time):
+ qp_iterations = max(0, qp_iterations)
+ dat = messaging.new_message()
+ dat.init('liveLongitudinalMpc')
+ dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
+ dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
+ dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
+ dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
+ dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
+ dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
+ dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
+ dat.liveLongitudinalMpc.qpIterations = qp_iterations
+ dat.liveLongitudinalMpc.mpcId = self.mpc_id
+ dat.liveLongitudinalMpc.calculationTime = calculation_time
+ self.live_longitudinal_mpc.send(dat.to_bytes())
+
+ def setup_mpc(self):
+ ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
+ self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+ MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+
+ self.mpc_solution = ffi.new("log_t *")
+ self.cur_state = ffi.new("state_t *")
+ self.cur_state[0].v_ego = 0
+ self.cur_state[0].a_ego = 0
+ self.a_lead_tau = _LEAD_ACCEL_TAU
+
+ def set_cur_state(self, v, a):
+ self.cur_state[0].v_ego = v
+ self.cur_state[0].a_ego = a
+
+ def update(self, CS, lead, v_cruise_setpoint):
+ v_ego = CS.carState.vEgo
+
+ # Setup current mpc state
+ self.cur_state[0].x_ego = 0.0
+
+ if lead is not None and lead.status:
+ x_lead = lead.dRel
+ v_lead = max(0.0, lead.vLead)
+ a_lead = lead.aLeadK
+
+ if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
+ v_lead = 0.0
+ a_lead = 0.0
+
+ self.a_lead_tau = lead.aLeadTau
+ self.new_lead = False
+ if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
+ self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
+ self.new_lead = True
+
+ self.prev_lead_status = True
+ self.prev_lead_x = x_lead
+ self.cur_state[0].x_l = x_lead
+ self.cur_state[0].v_l = v_lead
+ else:
+ self.prev_lead_status = False
+ # Fake a fast lead car, so mpc keeps running
+ self.cur_state[0].x_l = 50.0
+ self.cur_state[0].v_l = v_ego + 10.0
+ a_lead = 0.0
+ self.a_lead_tau = _LEAD_ACCEL_TAU
+
+ # Calculate mpc
+ t = sec_since_boot()
+ n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
+ duration = int((sec_since_boot() - t) * 1e9)
+ self.send_mpc_solution(n_its, duration)
+
+ # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
+ self.v_mpc = self.mpc_solution[0].v_ego[1]
+ self.a_mpc = self.mpc_solution[0].a_ego[1]
+ self.v_mpc_future = self.mpc_solution[0].v_ego[10]
+
+ # Reset if NaN or goes through lead car
+ dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego))
+ crashing = min(dls) < -50.0
+ nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego)))
+ backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01
+
+ if ((backwards or crashing) and self.prev_lead_status) or nans:
+ if t > self.last_cloudlog_t + 5.0:
+ self.last_cloudlog_t = t
+ cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
+ self.mpc_id, backwards, crashing, nans))
+
+ self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+ MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+ self.cur_state[0].v_ego = v_ego
+ self.cur_state[0].a_ego = 0.0
+ self.v_mpc = v_ego
+ self.a_mpc = CS.carState.aEgo
+ self.prev_lead_status = False
diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp
index f5c95394d8..1f0e28d005 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp
+++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp
@@ -18,30 +18,21 @@ int main( )
DifferentialEquation f;
DifferentialState x_ego, v_ego, a_ego;
- DifferentialState x_l, v_l, t;
-
- OnlineData lambda, a_l_0;
+ OnlineData x_l, v_l;
Control j_ego;
auto desired = 4.0 + RW(v_ego, v_l);
auto d_l = x_l - x_ego;
- // Directly calculate a_l to prevent instabilites due to discretization
- auto a_l = a_l_0 * exp(-lambda * t * t / 2);
-
// Equations of motion
f << dot(x_ego) == v_ego;
f << dot(v_ego) == a_ego;
f << dot(a_ego) == j_ego;
- f << dot(x_l) == v_l;
- f << dot(v_l) == a_l;
- f << dot(t) == 1;
-
// Running cost
Function h;
- h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
+ h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
h << (d_l - desired) / (0.05 * v_ego + 0.5);
h << a_ego * (0.1 * v_ego + 1.0);
h << j_ego * (0.1 * v_ego + 1.0);
@@ -51,7 +42,7 @@ int main( )
// Terminal cost
Function hN;
- hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
+ hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
hN << (d_l - desired) / (0.05 * v_ego + 0.5);
hN << a_ego * (0.1 * v_ego + 1.0);
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o
index cad75619ea..8b1e489c3e 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:81c0f18ac2f84fa44a2afca0206173dd866605a0b7dbe60fd2640734d54ce3b9
-size 5480
+oid sha256:453ffa5ad5c533e6d6065167fb1a674ed9196e2ade87df4a55d163ff178eccf7
+size 5376
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
index 8ad8163992..a7e8651b34 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:6d5761f29b51905c42910fdf9e7b5744f09f132c69caa8ac6e96e57ac1f17e6b
-size 8832
+oid sha256:c50ba5b1353617c7a18d17f1417c4036d8e5c41db79f82cb9a9b4a19032e6cc6
+size 8752
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
index 5c6ab1c17e..1703fea50d 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:26a4622ed83e6315cf34d55e46a3bb4af8db1d135f9c54c4e10dbd4d09a47dda
-size 34770
+oid sha256:221b739b59f5a314fc832a29a7fcf49da960012acc22aed82e66331f950ac5f8
+size 11486
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o
index f9761abb3e..f3f43e77d5 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:b6f5e9a2fc5f5765ce2f53c16d6ccbd1142cd514ab6acee0625a12fef8f11fd9
-size 8584
+oid sha256:1a5e0651c2c8c650b59421bb4c4beea5c3b666bbd1c6e440eb2b62e2a64119bb
+size 3560
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
index 0a2e0e68cd..4b72f3f959 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:7aaa8e6766705d1cd22c65d418e10a4c4b54864809cc9ad5e5de09c98e72cfd0
+oid sha256:c4099e03a411a516b406ef62cf0d923fb9447b376ae7dcbebe680a29f6c217d9
size 1948
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
index 8e91a24fd2..1b0c8bf3f2 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:383c2fa9974e0802dd86773aed20e068b39e0e5c505e6a641b30332f99f875b7
+oid sha256:3cc91e06813e2f18c87f0f2c798eb76a4e81e12c1027892a6c2b7d3451b03d54
size 1821
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o
index 174b8555dc..839a64035d 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:21349c8535aa0c186edcf75fbf815822dfddbedc3f5e090c8bcd416dea5a1ced
+oid sha256:d8648ab44187e2be8e7cdb128249dfac75338d98d0d93ca9878ea4a4b3f2ad3c
size 2992
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
index fec5c00837..64bc6db8df 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:c00034f1aa41c2f00e93c7d748cf5f663435360563107b6898441aac661a9bf2
-size 448071
+oid sha256:a58b02a90b3776ce308ab77094ebd8cdd23e788e1501a4c050862e65e827b185
+size 349038
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
index 9ce8e68540..c1cf63df7c 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:6a618ebaa304fe2776942918ea1f007a44eb63afd5b5e2946082034073358c2d
-size 307280
+oid sha256:16e942a5676e23d49e01b17ff664a55bddffa9e7d054caf4f6dbb03d16ec2e9e
+size 274768
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o
index d4f8b32d1a..7c03ce5dfe 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:eb9f45801304976cb37dcfbcff9681244bf78e97284d8b8da7264871a827783f
+oid sha256:a955478920067e11d3c1d40f96323bfb943196d66954911dfc62c5f673ecab06
size 8048
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o
index 46f1450f1a..f1dbf45635 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:0285ad4d884bf0bdcd3fa692ddc0814ec12b73ec0b0433b18b9072192fd503e0
+oid sha256:81feeda7638a908cd175f97addf62d16c4f47d3e38b020a0069838f3502ceff0
size 8112
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o
index f4529e083e..868cffebbc 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:df47bdf5dba35ac627a72e0c58e7d1648a0759d17b852a3347cac5e183e4b046
+oid sha256:57289f47ce5d35735edc673351e496e32ec606028512ecd785b9ea4309005629
size 2944
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
index 89b887be73..27891b56ed 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:6f9c880eb9d53f6b86acc90166150bc96ad25c1554b3dc61e449217e8faabc0a
+oid sha256:d1df0d4b3379b6daebd902add7e38a8f53b90c3b7c25b1d8855fd44a043fe650
size 4488
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o
index 1f38a483e5..13522ae13f 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:5a3b55e323c19172587d0e58c619a1e4d2c95711473b071bff29c0d32dad442d
+oid sha256:3ac57f4a4c5410bf29c12a32418e3b0ccd020d0362cc4a5316a915482cb4c99d
size 4496
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o
index b090b63b5b..2dec436cf0 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:acefa3c955a443246265289085a7fb9c096c5310bc456a8ccda785d94852e4ff
-size 72928
+oid sha256:67f333a6f1d18e9372b0c9eb8588cf4d3db87d3f731ff56497fbcecd79294400
+size 72200
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o
index b4b18a06f2..5027584212 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:728d61fd9f2e72f9fca96e51ae58b23308469625f8e97a29918493e79e9b37b9
-size 37832
+oid sha256:250c5e3626bd753796225bca9d031751a9f21e73e7b7e7fc8c4821b59af96298
+size 37592
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o
index d8d7575783..37ccdf7bf4 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:f6b12c121ee23318322af7882c07d37dd298068898160a8c1975fd5e9fb6d79f
+oid sha256:ec3bcbb2a42f595b00d2b7ed13f08077c1c759366bf83fb553e397f5091dc60c
size 3856
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
index b1e1a5af9b..c2d92939b6 100755
--- a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
+++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:79a968b415bc198402eac3b630b3087c1bc2b5a62883b9e39c8c43469ee9f388
-size 446096
+oid sha256:6c30e9afb75af10d72d0ea5e30b06c0d5310cce9312a441a2d260eb82d66f5ea
+size 405136
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
index 36b6f8b7e0..354a4a493e 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
@@ -32,6 +32,7 @@ def _get_libmpc(mpc_id):
double j_ego[21];
double x_l[21];
double v_l[21];
+ double a_l[21];
double t[21];
double cost;
} log_t;
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
index c0b43e53c9..e903ccc758 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
@@ -29,6 +29,7 @@ typedef struct {
double j_ego[N+1];
double x_l[N+1];
double v_l[N+1];
+ double a_l[N+1];
double t[N+1];
double cost;
} log_t;
@@ -68,17 +69,19 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j
void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){
int i;
- double x_ego = 0.0;
- double a_ego = 0.0;
double x_l = x_l_0;
double v_l = v_l_0;
double a_l = a_l_0;
+ double x_ego = 0.0;
+ double a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
- if (v_ego > v_l){
- a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
+ if (a_ego > 0){
+ a_ego = 0.0;
}
+
+
double dt = 0.2;
double t = 0.;
@@ -87,51 +90,66 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0
dt = 0.6;
}
- // Directly calculate a_l to prevent instabilites due to discretization
- a_l = a_l_0 * exp(-l * t * t / 2);
-
- /* printf("x_e: %.2f\t v_e: %.2f\t a_e: %.2f\t", x_ego, v_ego, a_ego); */
- /* printf("x_l: %.2f\t v_l: %.2f\t a_l: %.2f\n", x_l, v_l, a_l); */
-
+ /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_ego, v_ego, a_l); */
acadoVariables.x[i*NX] = x_ego;
acadoVariables.x[i*NX+1] = v_ego;
acadoVariables.x[i*NX+2] = a_ego;
- acadoVariables.x[i*NX+3] = x_l;
- acadoVariables.x[i*NX+4] = v_l;
- acadoVariables.x[i*NX+5] = t;
-
- x_ego += v_ego * dt;
v_ego += a_ego * dt;
- x_l += v_l * dt;
- v_l += a_l * dt;
- t += dt;
-
if (v_ego <= 0.0) {
v_ego = 0.0;
a_ego = 0.0;
}
+
+ x_ego += v_ego * dt;
+ t += dt;
}
+
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0;
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0;
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
}
int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
+ // Calculate lead vehicle predictions
int i;
+ double t = 0.;
+ double dt = 0.2;
+ double x_l = x0->x_l;
+ double v_l = x0->v_l;
+ double a_l = a_l_0;
+
+ /* printf("t\tx_l\t_v_l\t_al\n"); */
+ for (i = 0; i < N + 1; ++i){
+ if (i > 4){
+ dt = 0.6;
+ }
+
+ /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_l, v_l, a_l); */
+
+ acadoVariables.od[i*NOD] = x_l;
+ acadoVariables.od[i*NOD+1] = v_l;
- for (i = 0; i <= NOD * N; i+= NOD){
- acadoVariables.od[i] = l;
- acadoVariables.od[i+1] = a_l_0;
+ solution->x_l[i] = x_l;
+ solution->v_l[i] = v_l;
+ solution->a_l[i] = a_l;
+ solution->t[i] = t;
+
+ a_l = a_l_0 * exp(-l * t * t / 2);
+ x_l += v_l * dt;
+ v_l += a_l * dt;
+ if (v_l < 0.0){
+ a_l = 0.0;
+ v_l = 0.0;
+ }
+
+ t += dt;
}
acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
- acadoVariables.x[3] = acadoVariables.x0[3] = x0->x_l;
- acadoVariables.x[4] = acadoVariables.x0[4] = x0->v_l;
- acadoVariables.x[5] = acadoVariables.x0[5] = 0.0;
acado_preparationStep();
acado_feedbackStep();
@@ -140,10 +158,6 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
solution->x_ego[i] = acadoVariables.x[i*NX];
solution->v_ego[i] = acadoVariables.x[i*NX+1];
solution->a_ego[i] = acadoVariables.x[i*NX+2];
- solution->x_l[i] = acadoVariables.x[i*NX+3];
- solution->v_l[i] = acadoVariables.x[i*NX+4];
- solution->t[i] = acadoVariables.x[i*NX+5];
-
solution->j_ego[i] = acadoVariables.u[i];
}
solution->cost = acado_getObjective();
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o
index 6f62bdad86..db80688516 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o
+++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
-oid sha256:b9352e718fb6a33b154e5ff749c65051dde02cc5bcf8ccf2f8a44c2bed85e8cc
-size 3976
+oid sha256:3b76bbb87fa3725ded4f4372174c3c7e4e5a58b86b950cd8867222f4cf3c375b
+size 3136
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index 55fd2afaec..0161d2f946 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -46,15 +46,18 @@ class PathPlanner(object):
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
- def update(self, CP, VM, CS, md, live100):
+ def update(self, CP, VM, CS, md, live100, live_parameters):
v_ego = CS.carState.vEgo
angle_steers = CS.carState.steeringAngle
active = live100.live100.active
- angle_offset = live100.live100.angleOffset
+
+ angle_offset_bias = live100.live100.angleModelBias + live_parameters.liveParameters.angleOffsetAverage
+
self.MP.update(v_ego, md)
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
+ VM.update_params(live_parameters.liveParameters.stiffnessFactor, live_parameters.liveParameters.steerRatio)
curvature_factor = VM.curvature_factor(v_ego)
l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly))
@@ -62,7 +65,7 @@ class PathPlanner(object):
p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly))
# account for actuation delay
- self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)
+ self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.sR, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@@ -72,19 +75,22 @@ class PathPlanner(object):
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
+ rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
else:
- delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio
+ delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR
+ rate_desired = 0.0
self.cur_state[0].delta = delta_desired
- self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset)
+ self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias)
+
# Check for infeasable MPC solution
mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
- self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio
+ self.cur_state[0].delta = math.radians(angle_steers) / VM.sR
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
@@ -108,7 +114,10 @@ class PathPlanner(object):
plan_send.pathPlan.rPoly = map(float, r_poly)
plan_send.pathPlan.rProb = float(self.MP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
+ plan_send.pathPlan.rateSteers = float(rate_desired)
+ plan_send.pathPlan.angleOffset = float(live_parameters.liveParameters.angleOffsetAverage)
plan_send.pathPlan.valid = bool(plan_valid)
+ plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid)
self.plan.send(plan_send.to_bytes())
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index e0a53e9973..c26711f57e 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -2,19 +2,18 @@
import zmq
import math
import numpy as np
-from collections import defaultdict
from common.params import Params
-from common.realtime import sec_since_boot
from common.numpy_fast import interp
+
import selfdrive.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
-from selfdrive.controls.lib.drive_helpers import create_event, MPC_COST_LONG, EventTypes as ET
-from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
+from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.speed_smoother import speed_smoother
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
-from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+from selfdrive.controls.lib.fcw import FCWChecker
+from selfdrive.controls.lib.long_mpc import LongitudinalMpc
NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS
@@ -37,9 +36,6 @@ _A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.]
_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
_A_TOTAL_MAX_BP = [0., 20., 40.]
-_FCW_A_ACT_V = [-3., -2.]
-_FCW_A_ACT_BP = [0., 30.]
-
def calc_cruise_accel_limits(v_ego, following):
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
@@ -65,182 +61,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
return a_target
-class FCWChecker(object):
- def __init__(self):
- self.reset_lead(0.0)
-
- def reset_lead(self, cur_time):
- self.last_fcw_a = 0.0
- self.v_lead_max = 0.0
- self.lead_seen_t = cur_time
- self.last_fcw_time = 0.0
- self.last_min_a = 0.0
-
- self.counters = defaultdict(lambda: 0)
-
- @staticmethod
- def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
- max_ttc = 5.0
-
- v_rel = v_ego - v_lead
- a_rel = a_ego - a_lead
-
- # assuming that closing gap ARel comes from lead vehicle decel,
- # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
- # This helps underweighting ARel when v_lead is close to zero.
- t_decel = 2.
- a_rel = np.minimum(a_rel, v_lead / t_decel)
-
- # delta of the quadratic equation to solve for ttc
- delta = v_rel**2 + 2 * x_lead * a_rel
-
- # assign an arbitrary high ttc value if there is no solution to ttc
- if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1):
- ttc = max_ttc
- else:
- ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
- return ttc
-
- def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
- mpc_solution_a = list(mpc_solution[0].a_ego)
- self.last_min_a = min(mpc_solution_a)
- self.v_lead_max = max(self.v_lead_max, v_lead)
-
- if (fcw_lead > 0.99):
- ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
- self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0
- self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
- self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
- self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
- self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
- self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
- self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
- self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
-
- a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
- a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
-
- fcw_allowed = all(c >= 10 for c in self.counters.values())
- if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
- self.last_fcw_time = cur_time
- self.last_fcw_a = self.last_min_a
- return True
-
- return False
-
-
-class LongitudinalMpc(object):
- def __init__(self, mpc_id, live_longitudinal_mpc):
- self.live_longitudinal_mpc = live_longitudinal_mpc
- self.mpc_id = mpc_id
-
- self.setup_mpc()
- self.v_mpc = 0.0
- self.v_mpc_future = 0.0
- self.a_mpc = 0.0
- self.v_cruise = 0.0
- self.prev_lead_status = False
- self.prev_lead_x = 0.0
- self.new_lead = False
-
- self.last_cloudlog_t = 0.0
-
- def send_mpc_solution(self, qp_iterations, calculation_time):
- qp_iterations = max(0, qp_iterations)
- dat = messaging.new_message()
- dat.init('liveLongitudinalMpc')
- dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
- dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
- dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
- dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
- dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
- dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
- dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
- dat.liveLongitudinalMpc.qpIterations = qp_iterations
- dat.liveLongitudinalMpc.mpcId = self.mpc_id
- dat.liveLongitudinalMpc.calculationTime = calculation_time
- self.live_longitudinal_mpc.send(dat.to_bytes())
-
- def setup_mpc(self):
- ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
- self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
- MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-
- self.mpc_solution = ffi.new("log_t *")
- self.cur_state = ffi.new("state_t *")
- self.cur_state[0].v_ego = 0
- self.cur_state[0].a_ego = 0
- self.a_lead_tau = _LEAD_ACCEL_TAU
-
- def set_cur_state(self, v, a):
- self.cur_state[0].v_ego = v
- self.cur_state[0].a_ego = a
-
- def update(self, CS, lead, v_cruise_setpoint):
- v_ego = CS.carState.vEgo
-
- # Setup current mpc state
- self.cur_state[0].x_ego = 0.0
-
- if lead is not None and lead.status:
- x_lead = lead.dRel
- v_lead = max(0.0, lead.vLead)
- a_lead = lead.aLeadK
-
- if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
- v_lead = 0.0
- a_lead = 0.0
-
- self.a_lead_tau = max(lead.aLeadTau, (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2))
- self.new_lead = False
- if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
- self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
- self.new_lead = True
-
- self.prev_lead_status = True
- self.prev_lead_x = x_lead
- self.cur_state[0].x_l = x_lead
- self.cur_state[0].v_l = v_lead
- else:
- self.prev_lead_status = False
- # Fake a fast lead car, so mpc keeps running
- self.cur_state[0].x_l = 50.0
- self.cur_state[0].v_l = v_ego + 10.0
- a_lead = 0.0
- self.a_lead_tau = _LEAD_ACCEL_TAU
-
- # Calculate mpc
- t = sec_since_boot()
- n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
- duration = int((sec_since_boot() - t) * 1e9)
- self.send_mpc_solution(n_its, duration)
-
- # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
- self.v_mpc = self.mpc_solution[0].v_ego[1]
- self.a_mpc = self.mpc_solution[0].a_ego[1]
- self.v_mpc_future = self.mpc_solution[0].v_ego[10]
-
- # Reset if NaN or goes through lead car
- dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego))
- crashing = min(dls) < -50.0
- nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego)))
- backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01
-
- if ((backwards or crashing) and self.prev_lead_status) or nans:
- if t > self.last_cloudlog_t + 5.0:
- self.last_cloudlog_t = t
- cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
- self.mpc_id, backwards, crashing, nans))
-
- self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
- MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
- self.cur_state[0].v_ego = v_ego
- self.cur_state[0].a_ego = 0.0
- self.v_mpc = v_ego
- self.a_mpc = CS.carState.aEgo
- self.prev_lead_status = False
-
-
class Planner(object):
def __init__(self, CP, fcw_enabled):
context = zmq.Context()
@@ -250,8 +70,6 @@ class Planner(object):
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port)
- self.radar_errors = []
-
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
@@ -264,19 +82,11 @@ class Planner(object):
self.v_cruise = 0.0
self.a_cruise = 0.0
- self.lead_1 = None
- self.lead_2 = None
-
self.longitudinalPlanSource = 'cruise'
- self.fcw = False
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.params = Params()
- self.v_curvature = NO_CURVATURE_SPEED
- self.v_speedlimit = NO_CURVATURE_SPEED
- self.decel_for_turn = False
- self.map_valid = False
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
@@ -313,19 +123,15 @@ class Planner(object):
force_slow_decel = live100.live100.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
- self.last_md_ts = md.logMonoTime
-
- self.radar_errors = list(live20.live20.radarErrors)
-
- self.lead_1 = live20.live20.leadOne
- self.lead_2 = live20.live20.leadTwo
+ lead_1 = live20.live20.leadOne
+ lead_2 = live20.live20.leadTwo
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
- following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
+ following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
- self.v_speedlimit = NO_CURVATURE_SPEED
- self.v_curvature = NO_CURVATURE_SPEED
- self.map_valid = live_map_data.liveMapData.mapValid
+ v_speedlimit = NO_CURVATURE_SPEED
+ v_curvature = NO_CURVATURE_SPEED
+ map_valid = live_map_data.liveMapData.mapValid
# Speed limit and curvature
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
@@ -333,16 +139,16 @@ class Planner(object):
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
- self.v_speedlimit = speed_limit + offset
+ v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
- self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
+ v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
- self.decel_for_turn = bool(self.v_curvature < min([v_cruise_setpoint, self.v_speedlimit, v_ego + 1.]))
- v_cruise_setpoint = min([v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
+ decel_for_turn = bool(v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
+ v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
# Calculate speed for normal cruise control
if enabled:
@@ -356,9 +162,9 @@ class Planner(object):
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# Change accel limits based on time remaining to turn
- if self.decel_for_turn:
+ if decel_for_turn:
time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.))
- required_decel = min(0, (self.v_curvature - self.v_cruise) / time_to_turn)
+ required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
@@ -383,8 +189,8 @@ class Planner(object):
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
- self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
- self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
+ self.mpc1.update(CS, lead_1, v_cruise_setpoint)
+ self.mpc2.update(CS, lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
@@ -393,11 +199,11 @@ class Planner(object):
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
- self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
- self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
- self.lead_1.yRel, self.lead_1.vLat,
- self.lead_1.fcw, blinkers) and not CS.carState.brakePressed
- if self.fcw:
+ fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
+ lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
+ lead_1.yRel, lead_1.vLat,
+ lead_1.fcw, blinkers) and not CS.carState.brakePressed
+ if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
@@ -409,8 +215,12 @@ class Planner(object):
# TODO: Move all these events to controlsd. This has nothing to do with planning
events = []
if model_dead:
- events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
- if 'fault' in self.radar_errors:
+ events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+
+ radar_errors = list(live20.live20.radarErrors)
+ if 'commIssue' in radar_errors:
+ events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+ if 'fault' in radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
plan_send.plan.events = events
@@ -428,12 +238,12 @@ class Planner(object):
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
- plan_send.plan.vCurvature = self.v_curvature
- plan_send.plan.decelForTurn = self.decel_for_turn
- plan_send.plan.mapValid = self.map_valid
+ plan_send.plan.vCurvature = v_curvature
+ plan_send.plan.decelForTurn = decel_for_turn
+ plan_send.plan.mapValid = map_valid
# Send out fcw
- fcw = self.fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
+ fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py
index 95e3ef3783..1dc77bf375 100755
--- a/selfdrive/controls/lib/vehicle_model.py
+++ b/selfdrive/controls/lib/vehicle_model.py
@@ -102,11 +102,18 @@ class VehicleModel(object):
self.l = CP.wheelbase
self.aF = CP.centerToFront
self.aR = CP.wheelbase - CP.centerToFront
- self.cF = CP.tireStiffnessFront
- self.cR = CP.tireStiffnessRear
- self.sR = CP.steerRatio
self.chi = CP.steerRatioRear
+ self.cF_orig = CP.tireStiffnessFront
+ self.cR_orig = CP.tireStiffnessRear
+ self.update_params(1.0, CP.steerRatio)
+
+ def update_params(self, stiffness_factor, steer_ratio):
+ """Update the vehicle model with a new stiffness factor and steer ratio"""
+ self.cF = stiffness_factor * self.cF_orig
+ self.cR = stiffness_factor * self.cR_orig
+ self.sR = steer_ratio
+
def steady_state_sol(self, sa, u):
"""Returns the steady state solution.
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index c6f9040b20..2a95375408 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -33,6 +33,7 @@ def plannerd_thread():
live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller)
model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller)
+ live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller)
car_state = messaging.new_message()
car_state.init('carState')
@@ -45,15 +46,23 @@ def plannerd_thread():
live_map_data = messaging.new_message()
live_map_data.init('liveMapData')
+ live_parameters = messaging.new_message()
+ live_parameters.init('liveParameters')
+ live_parameters.liveParameters.valid = True
+ live_parameters.liveParameters.steerRatio = CP.steerRatio
+ live_parameters.liveParameters.stiffnessFactor = 1.0
+
while True:
for socket, event in poller.poll():
if socket is live100_sock:
live100 = messaging.recv_one(socket)
elif socket is car_state_sock:
car_state = messaging.recv_one(socket)
+ elif socket is live_parameters_sock:
+ live_parameters = messaging.recv_one(socket)
elif socket is model_sock:
model = messaging.recv_one(socket)
- PP.update(CP, VM, car_state, model, live100)
+ PP.update(CP, VM, car_state, model, live100, live_parameters)
elif socket is live_map_data_sock:
live_map_data = messaging.recv_one(socket)
elif socket is live20_sock:
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 3952a913ac..4d44c96b9d 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -3,8 +3,9 @@ import zmq
import numpy as np
import numpy.matlib
import importlib
-from collections import defaultdict
+from collections import defaultdict, deque
from fastcluster import linkage_vector
+
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
@@ -65,6 +66,14 @@ def radard_thread(gctx=None):
poller = zmq.Poller()
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
+ live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller)
+
+ # Default parameters
+ live_parameters = messaging.new_message()
+ live_parameters.init('liveParameters')
+ live_parameters.liveParameters.valid = True
+ live_parameters.liveParameters.steerRatio = CP.steerRatio
+ live_parameters.liveParameters.stiffnessFactor = 1.0
MP = ModelParser()
RI = RadarInterface(CP)
@@ -95,7 +104,8 @@ def radard_thread(gctx=None):
# v_ego
v_ego = None
- v_ego_array = np.zeros([2, v_len])
+ v_ego_hist_t = deque(maxlen=v_len)
+ v_ego_hist_v = deque(maxlen=v_len)
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
@@ -115,6 +125,9 @@ def radard_thread(gctx=None):
l100 = messaging.recv_one(socket)
elif socket is model:
md = messaging.recv_one(socket)
+ elif socket is live_parameters_sock:
+ live_parameters = messaging.recv_one(socket)
+ VM.update_params(live_parameters.liveParameters.stiffnessFactor, live_parameters.liveParameters.steerRatio)
if l100 is not None:
active = l100.live100.active
@@ -122,8 +135,8 @@ def radard_thread(gctx=None):
steer_angle = l100.live100.angleSteers
steer_override = l100.live100.steerOverride
- v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
- v_ego_array = v_ego_array[:, 1:]
+ v_ego_hist_v.append(v_ego)
+ v_ego_hist_t.append(float(rk.frame)/rate)
last_l100_ts = l100.logMonoTime
@@ -165,7 +178,7 @@ def radard_thread(gctx=None):
path_y = np.polyval(MP.d_poly, path_x)
else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset
- path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
+ path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=live_parameters.liveParameters.angleOffsetAverage)[0]
# *** remove missing points from meta data ***
for ids in tracks.keys():
@@ -181,7 +194,8 @@ def radard_thread(gctx=None):
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(rk.frame)/rate
- v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
+ v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_hist_t, v_ego_hist_v)
+
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
diff --git a/selfdrive/crash.py b/selfdrive/crash.py
index 20003ee9fa..94bdfd125b 100644
--- a/selfdrive/crash.py
+++ b/selfdrive/crash.py
@@ -1,6 +1,7 @@
"""Install exception handler for process crash."""
import os
import sys
+import threading
from selfdrive.version import version, dirty
from selfdrive.swaglog import cloudlog
@@ -38,3 +39,25 @@ else:
capture_exception(exc_info=exc_info)
__excepthook__(*exc_info)
sys.excepthook = handle_exception
+
+ """
+ Workaround for `sys.excepthook` thread bug from:
+ http://bugs.python.org/issue1230540
+ Call once from the main thread before creating any threads.
+ Source: https://stackoverflow.com/a/31622038
+ """
+ init_original = threading.Thread.__init__
+
+ def init(self, *args, **kwargs):
+ init_original(self, *args, **kwargs)
+ run_original = self.run
+
+ def run_with_except_hook(*args2, **kwargs2):
+ try:
+ run_original(*args2, **kwargs2)
+ except Exception:
+ sys.excepthook(*sys.exc_info())
+
+ self.run = run_with_except_hook
+
+ threading.Thread.__init__ = init
diff --git a/selfdrive/locationd/kalman/__init__.py b/selfdrive/locationd/kalman/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/selfdrive/locationd/kalman/ekf_c.c b/selfdrive/locationd/kalman/ekf_c.c
new file mode 100644
index 0000000000..1331036de3
--- /dev/null
+++ b/selfdrive/locationd/kalman/ekf_c.c
@@ -0,0 +1,124 @@
+#include
+#include
+
+typedef Eigen::Matrix DDM;
+typedef Eigen::Matrix EEM;
+typedef Eigen::Matrix DEM;
+
+void predict(double *in_x, double *in_P, double *in_Q, double dt) {
+ typedef Eigen::Matrix RRM;
+
+ double nx[DIM] = {0};
+ double in_F[EDIM*EDIM] = {0};
+
+ // functions from sympy
+ f_fun(in_x, dt, nx);
+ F_fun(in_x, dt, in_F);
+
+
+ EEM F(in_F);
+ EEM P(in_P);
+ EEM Q(in_Q);
+
+ RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
+ P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
+ P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
+ P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
+
+ P = P + dt*Q;
+
+ // copy out state
+ memcpy(in_x, nx, DIM * sizeof(double));
+ memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+}
+
+// note: extra_args dim only correct when null space projecting
+// otherwise 1
+template
+void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
+ typedef Eigen::Matrix ZZM;
+ typedef Eigen::Matrix ZDM;
+ typedef Eigen::Matrix ZEM;
+ typedef Eigen::Matrix XEM;
+ typedef Eigen::Matrix EZM;
+ typedef Eigen::Matrix X1M;
+ typedef Eigen::Matrix XXM;
+
+ double in_hx[ZDIM] = {0};
+ double in_H[ZDIM * DIM] = {0};
+ double in_H_mod[EDIM * DIM] = {0};
+ double delta_x[EDIM] = {0};
+ double x_new[DIM] = {0};
+
+
+ // state x, P
+ Eigen::Matrix z(in_z);
+ EEM P(in_P);
+ ZZM pre_R(in_R);
+
+ // functions from sympy
+ h_fun(in_x, in_ea, in_hx);
+ H_fun(in_x, in_ea, in_H);
+ ZDM pre_H(in_H);
+
+ // get y (y = z - hx)
+ Eigen::Matrix pre_y(in_hx); pre_y = z - pre_y;
+ X1M y; XXM H; XXM R;
+ if (Hea_fun){
+ typedef Eigen::Matrix ZAM;
+ double in_Hea[ZDIM * EADIM] = {0};
+ Hea_fun(in_x, in_ea, in_Hea);
+ ZAM Hea(in_Hea);
+ XXM A = Hea.transpose().fullPivLu().kernel();
+
+
+ y = A.transpose() * pre_y;
+ H = A.transpose() * pre_H;
+ R = A.transpose() * pre_R * A;
+ } else {
+ y = pre_y;
+ H = pre_H;
+ R = pre_R;
+ }
+ // get modified H
+ H_mod_fun(in_x, in_H_mod);
+ DEM H_mod(in_H_mod);
+ XEM H_err = H * H_mod;
+
+ // Do mahalobis distance test
+ if (MAHA_TEST){
+ XXM a = (H_err * P * H_err.transpose() + R).inverse();
+ double maha_dist = y.transpose() * a * y;
+ if (maha_dist > MAHA_THRESHOLD){
+ R = 1.0e16 * R;
+ }
+ }
+
+ // Outlier resilient weighting
+ double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
+
+ // kalman gains and I_KH
+ XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
+ XEM KT = S.fullPivLu().solve(H_err * P.transpose());
+ //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
+ //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
+ //std::cout << "Here is the matrix rot:\n" << K << std::endl;
+ EEM I_KH = Eigen::Matrix::Identity() - (KT.transpose() * H_err);
+
+ // update state by injecting dx
+ Eigen::Matrix dx(delta_x);
+ dx = (KT.transpose() * y);
+ memcpy(delta_x, dx.data(), EDIM * sizeof(double));
+ err_fun(in_x, delta_x, x_new);
+ Eigen::Matrix x(x_new);
+
+ // update cov
+ P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
+
+ // copy out state
+ memcpy(in_x, x.data(), DIM * sizeof(double));
+ memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+ memcpy(in_z, y.data(), y.rows() * sizeof(double));
+}
+
+
diff --git a/selfdrive/locationd/kalman/ekf_sym.py b/selfdrive/locationd/kalman/ekf_sym.py
new file mode 100644
index 0000000000..2dfefe92d3
--- /dev/null
+++ b/selfdrive/locationd/kalman/ekf_sym.py
@@ -0,0 +1,564 @@
+import os
+from bisect import bisect_right
+import sympy as sp
+import numpy as np
+from numpy import dot
+from common.ffi_wrapper import compile_code, wrap_compiled
+from common.sympy_helpers import sympy_into_c
+import scipy
+from scipy.stats import chi2
+
+
+EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
+
+def solve(a, b):
+ if a.shape[0] == 1 and a.shape[1] == 1:
+ #assert np.allclose(b/a[0][0], np.linalg.solve(a, b))
+ return b/a[0][0]
+ else:
+ return np.linalg.solve(a, b)
+
+def null(H, eps=1e-12):
+ from scipy import linalg
+ u, s, vh = linalg.svd(H)
+ padding = max(0,np.shape(H)[1]-np.shape(s)[0])
+ null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0)
+ null_space = scipy.compress(null_mask, vh, axis=0)
+ return scipy.transpose(null_space)
+
+def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]):
+ # optional state transition matrix, H modifier
+ # and err_function if an error-state kalman filter (ESKF)
+ # is desired. Best described in "Quaternion kinematics
+ # for the error-state Kalman filter" by Joan Sola
+
+ if eskf_params:
+ err_eqs = eskf_params[0]
+ inv_err_eqs = eskf_params[1]
+ H_mod_sym = eskf_params[2]
+ f_err_sym = eskf_params[3]
+ x_err_sym = eskf_params[4]
+ else:
+ nom_x = sp.MatrixSymbol('nom_x',dim_x,1)
+ true_x = sp.MatrixSymbol('true_x',dim_x,1)
+ delta_x = sp.MatrixSymbol('delta_x',dim_x,1)
+ err_function_sym = sp.Matrix(nom_x + delta_x)
+ inv_err_function_sym = sp.Matrix(true_x - nom_x)
+ err_eqs = [err_function_sym, nom_x, delta_x]
+ inv_err_eqs = [inv_err_function_sym, nom_x, true_x]
+
+ H_mod_sym = sp.Matrix(np.eye(dim_x))
+ f_err_sym = f_sym
+ x_err_sym = x_sym
+
+ # This configures the multi-state augmentation
+ # needed for EKF-SLAM with MSCKF (Mourikis et al 2007)
+ if msckf_params:
+ msckf = True
+ dim_main = msckf_params[0] # size of the main state
+ dim_augment = msckf_params[1] # size of one augment state chunk
+ dim_main_err = msckf_params[2]
+ dim_augment_err = msckf_params[3]
+ N = msckf_params[4]
+ feature_track_kinds = msckf_params[5]
+ assert dim_main + dim_augment*N == dim_x
+ assert dim_main_err + dim_augment_err*N == dim_err
+ else:
+ msckf = False
+ dim_main = dim_x
+ dim_augment = 0
+ dim_main_err = dim_err
+ dim_augment_err = 0
+ N = 0
+
+ # linearize with jacobians
+ F_sym = f_err_sym.jacobian(x_err_sym)
+ for sym in x_err_sym:
+ F_sym = F_sym.subs(sym, 0)
+ for i in xrange(len(obs_eqs)):
+ obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym))
+ if msckf and obs_eqs[i][1] in feature_track_kinds:
+ obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2]))
+ else:
+ obs_eqs[i].append(None)
+
+ # collect sympy functions
+ sympy_functions = []
+
+ # error functions
+ sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]]))
+ sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]]))
+
+ # H modifier for ESKF updates
+ sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym]))
+
+ # state propagation function
+ sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym]))
+ sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym]))
+
+ # observation functions
+ for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
+ sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym]))
+ sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym]))
+ if msckf and kind in feature_track_kinds:
+ sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym]))
+
+ # Generate and wrap all th c code
+ header, code = sympy_into_c(sympy_functions)
+ extra_header = "#define DIM %d\n" % dim_x
+ extra_header += "#define EDIM %d\n" % dim_err
+ extra_header += "#define MEDIM %d\n" % dim_main_err
+ extra_header += "typedef void (*Hfun)(double *, double *, double *);\n"
+
+ extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);"
+
+ extra_post = ""
+
+ for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
+ if msckf and kind in feature_track_kinds:
+ He_str = 'He_%d' % kind
+ # ea_dim = ea_sym.shape[0]
+ else:
+ He_str = 'NULL'
+ # ea_dim = 1 # not really dim of ea but makes c function work
+ maha_thresh = chi2.ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
+ maha_test = kind in maha_test_kinds
+ extra_post += """
+ void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
+ update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d);
+ }
+ """ % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind)
+ extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh)
+ extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind
+
+ code += "\n" + extra_header
+ code += "\n" + open(os.path.join(EXTERNAL_PATH, "ekf_c.c")).read()
+ code += "\n" + extra_post
+ header += "\n" + extra_header
+ compile_code(name, code, header, EXTERNAL_PATH)
+
+class EKF_sym(object):
+ def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
+ N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]):
+ '''
+ Generates process function and all
+ observation functions for the kalman
+ filter.
+ '''
+ if N > 0:
+ self.msckf = True
+ else:
+ self.msckf = False
+ self.N = N
+ self.dim_augment = dim_augment
+ self.dim_augment_err = dim_augment_err
+ self.dim_main = dim_main
+ self.dim_main_err = dim_main_err
+
+ # state
+ x_initial = x_initial.reshape((-1, 1))
+ self.dim_x = x_initial.shape[0]
+ self.dim_err = P_initial.shape[0]
+ assert dim_main + dim_augment*N == self.dim_x
+ assert dim_main_err + dim_augment_err*N == self.dim_err
+
+ # kinds that should get mahalanobis distance
+ # tested for outlier rejection
+ self.maha_test_kinds = maha_test_kinds
+
+ # process noise
+ self.Q = Q
+
+ # rewind stuff
+ self.rewind_t = []
+ self.rewind_states = []
+ self.rewind_obscache = []
+ self.init_state(x_initial, P_initial, None)
+
+ ffi, lib = wrap_compiled(name, EXTERNAL_PATH)
+ kinds, self.feature_track_kinds = [], []
+ for func in dir(lib):
+ if func[:2] == 'h_':
+ kinds.append(int(func[2:]))
+ if func[:3] == 'He_':
+ self.feature_track_kinds.append(int(func[3:]))
+
+ # wrap all the sympy functions
+ def wrap_1lists(name):
+ func = eval("lib.%s" % name, {"lib":lib})
+ def ret(lst1, out):
+ func(ffi.cast("double *", lst1.ctypes.data),
+ ffi.cast("double *", out.ctypes.data))
+ return ret
+ def wrap_2lists(name):
+ func = eval("lib.%s" % name, {"lib":lib})
+ def ret(lst1, lst2, out):
+ func(ffi.cast("double *", lst1.ctypes.data),
+ ffi.cast("double *", lst2.ctypes.data),
+ ffi.cast("double *", out.ctypes.data))
+ return ret
+ def wrap_1list_1float(name):
+ func = eval("lib.%s" % name, {"lib":lib})
+ def ret(lst1, fl, out):
+ func(ffi.cast("double *", lst1.ctypes.data),
+ ffi.cast("double", fl),
+ ffi.cast("double *", out.ctypes.data))
+ return ret
+
+ self.f = wrap_1list_1float("f_fun")
+ self.F = wrap_1list_1float("F_fun")
+
+ self.err_function = wrap_2lists("err_fun")
+ self.inv_err_function = wrap_2lists("inv_err_fun")
+ self.H_mod = wrap_1lists("H_mod_fun")
+
+ self.hs, self.Hs, self.Hes = {}, {}, {}
+ for kind in kinds:
+ self.hs[kind] = wrap_2lists("h_%d" % kind)
+ self.Hs[kind] = wrap_2lists("H_%d" % kind)
+ if self.msckf and kind in self.feature_track_kinds:
+ self.Hes[kind] = wrap_2lists("He_%d" % kind)
+
+ # wrap the C++ predict function
+ def _predict_blas(x, P, dt):
+ lib.predict(ffi.cast("double *", x.ctypes.data),
+ ffi.cast("double *", P.ctypes.data),
+ ffi.cast("double *", self.Q.ctypes.data),
+ ffi.cast("double", dt))
+ return x, P
+
+ # wrap the C++ update function
+ def fun_wrapper(f, kind):
+ f = eval("lib.%s" % f, {"lib": lib})
+ def _update_inner_blas(x, P, z, R, extra_args):
+ f(ffi.cast("double *", x.ctypes.data),
+ ffi.cast("double *", P.ctypes.data),
+ ffi.cast("double *", z.ctypes.data),
+ ffi.cast("double *", R.ctypes.data),
+ ffi.cast("double *", extra_args.ctypes.data))
+ if self.msckf and kind in self.feature_track_kinds:
+ y = z[:-len(extra_args)]
+ else:
+ y = z
+ return x, P, y
+ return _update_inner_blas
+
+ self._updates = {}
+ for kind in kinds:
+ self._updates[kind] = fun_wrapper("update_%d" % kind, kind)
+
+ def _update_blas(x, P, kind, z, R, extra_args=[]):
+ return self._updates[kind](x, P, z, R, extra_args)
+
+ # assign the functions
+ self._predict = _predict_blas
+ #self._predict = self._predict_python
+ self._update = _update_blas
+ #self._update = self._update_python
+
+
+ def init_state(self, state, covs, filter_time):
+ self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
+ self.P = np.array(covs).astype(np.float64)
+ self.filter_time = filter_time
+ self.augment_times = [0]*self.N
+ self.rewind_obscache = []
+ self.rewind_t = []
+ self.rewind_states = []
+
+ def augment(self):
+ # TODO this is not a generalized way of doing
+ # this and implies that the augmented states
+ # are simply the first (dim_augment_state)
+ # elements of the main state.
+ assert self.msckf
+ d1 = self.dim_main
+ d2 = self.dim_main_err
+ d3 = self.dim_augment
+ d4 = self.dim_augment_err
+ # push through augmented states
+ self.x[d1:-d3] = self.x[d1+d3:]
+ self.x[-d3:] = self.x[:d3]
+ assert self.x.shape == (self.dim_x, 1)
+ # push through augmented covs
+ assert self.P.shape == (self.dim_err, self.dim_err)
+ P_reduced = self.P
+ P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=1)
+ P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=0)
+ assert P_reduced.shape == (self.dim_err -d4, self.dim_err -d4)
+ to_mult = np.zeros((self.dim_err, self.dim_err - d4))
+ to_mult[:-d4,:] = np.eye(self.dim_err - d4)
+ to_mult[-d4:,:d4] = np.eye(d4)
+ self.P = to_mult.dot(P_reduced.dot(to_mult.T))
+ self.augment_times = self.augment_times[1:]
+ self.augment_times.append(self.filter_time)
+ assert self.P.shape == (self.dim_err, self.dim_err)
+
+ def state(self):
+ return np.array(self.x).flatten()
+
+ def covs(self):
+ return self.P
+
+ def rewind(self, t):
+ # find where we are rewinding to
+ idx = bisect_right(self.rewind_t, t)
+ assert self.rewind_t[idx-1] <= t
+ assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called
+
+ # set the state to the time right before that
+ self.filter_time = self.rewind_t[idx-1]
+ self.x[:] = self.rewind_states[idx-1][0]
+ self.P[:] = self.rewind_states[idx-1][1]
+
+ # return the observations we rewound over for fast forwarding
+ ret = self.rewind_obscache[idx:]
+
+ # throw away the old future
+ # TODO: is this making a copy?
+ self.rewind_t = self.rewind_t[:idx]
+ self.rewind_states = self.rewind_states[:idx]
+ self.rewind_obscache = self.rewind_obscache[:idx]
+
+ return ret
+
+ def checkpoint(self, obs):
+ # push to rewinder
+ self.rewind_t.append(self.filter_time)
+ self.rewind_states.append((np.copy(self.x), np.copy(self.P)))
+ self.rewind_obscache.append(obs)
+
+ # only keep a certain number around
+ REWIND_TO_KEEP = 512
+ self.rewind_t = self.rewind_t[-REWIND_TO_KEEP:]
+ self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:]
+ self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:]
+
+ def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False):
+ # TODO handle rewinding at this level"
+
+ # rewind
+ if t < self.filter_time:
+ if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] -1.0:
+ print "observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time)
+ return None
+ rewound = self.rewind(t)
+ else:
+ rewound = []
+
+ ret = self._predict_and_update_batch(t, kind, z, R, extra_args, augment)
+
+ # optional fast forward
+ for r in rewound:
+ self._predict_and_update_batch(*r)
+
+ return ret
+
+ def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False):
+ """The main kalman filter function
+ Predicts the state and then updates a batch of observations
+
+ dim_x: dimensionality of the state space
+ dim_z: dimensionality of the observation and depends on kind
+ n: number of observations
+
+ Args:
+ t (float): Time of observation
+ kind (int): Type of observation
+ z (vec [n,dim_z]): Measurements
+ R (mat [n,dim_z, dim_z]): Measurement Noise
+ extra_args (list, [n]): Values used in H computations
+ """
+ # initialize time
+ if self.filter_time is None:
+ self.filter_time = t
+
+ # predict
+ dt = t - self.filter_time
+ assert dt >= 0
+ self.x, self.P = self._predict(self.x, self.P, dt)
+ self.filter_time = t
+ xk_km1, Pk_km1 = np.copy(self.x).flatten(), np.copy(self.P)
+
+ # update batch
+ y = []
+ for i in xrange(len(z)):
+ # these are from the user, so we canonicalize them
+ z_i = np.array(z[i], dtype=np.float64, order='F')
+ R_i = np.array(R[i], dtype=np.float64, order='F')
+ extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
+ # update
+ self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
+ y.append(y_i)
+ xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)
+
+ if augment:
+ self.augment()
+
+ # checkpoint
+ self.checkpoint((t, kind, z, R, extra_args))
+
+ return xk_km1, xk_k, Pk_km1, Pk_k, t, kind, y, z, extra_args
+
+ def _predict_python(self, x, P, dt):
+ x_new = np.zeros(x.shape, dtype=np.float64)
+ self.f(x, dt, x_new)
+
+ F = np.zeros(P.shape, dtype=np.float64)
+ self.F(x, dt, F)
+
+ if not self.msckf:
+ P = dot(dot(F, P), F.T)
+ else:
+ # Update the predicted state covariance:
+ # Pk+1|k = |F*Pii*FT + Q*dt F*Pij |
+ # |PijT*FT Pjj |
+ # Where F is the jacobian of the main state
+ # predict function, Pii is the main state's
+ # covariance and Q its process noise. Pij
+ # is the covariance between the augmented
+ # states and the main state.
+ #
+ d2 = self.dim_main_err # known at compile time
+ F_curr = F[:d2, :d2]
+ P[:d2, :d2] = (F_curr.dot(P[:d2, :d2])).dot(F_curr.T)
+ P[:d2, d2:] = F_curr.dot(P[:d2, d2:])
+ P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T)
+
+ P += dt*self.Q
+ return x_new, P
+
+ def _update_python(self, x, P, kind, z, R, extra_args=[]):
+ # init vars
+ z = z.reshape((-1, 1))
+ h = np.zeros(z.shape, dtype=np.float64)
+ H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
+
+ # C functions
+ self.hs[kind](x, extra_args, h)
+ self.Hs[kind](x, extra_args, H)
+
+ # y is the "loss"
+ y = z - h
+
+ # *** same above this line ***
+
+ if self.msckf and kind in self.Hes:
+ # Do some algebraic magic to decorrelate
+ He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64)
+ self.Hes[kind](x, extra_args, He)
+
+ # TODO: Don't call a function here, do projection locally
+ A = null(He.T)
+
+ y = A.T.dot(y)
+ H = A.T.dot(H)
+ R = A.T.dot(R.dot(A))
+
+ # TODO If nullspace isn't the dimension we want
+ if A.shape[1] + He.shape[1] != A.shape[0]:
+ print 'Warning: null space projection failed, measurement ignored'
+ return x, P, np.zeros(A.shape[0] - He.shape[1])
+
+ # if using eskf
+ H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
+ self.H_mod(x, H_mod)
+ H = H.dot(H_mod)
+
+ # Do mahalobis distance test
+ # currently just runs on msckf observations
+ # could run on anything if needed
+ if self.msckf and kind in self.maha_test_kinds:
+ a = np.linalg.inv(H.dot(P).dot(H.T) + R)
+ maha_dist = y.T.dot(a.dot(y))
+ if maha_dist > chi2.ppf(0.95, y.shape[0]):
+ R = 10e16*R
+
+ # *** same below this line ***
+
+ # Outlier resilient weighting as described in:
+ # "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..."
+ weight = 1 #(1.5)/(1 + np.sum(y**2)/np.sum(R))
+
+ S = dot(dot(H, P), H.T) + R/weight
+ K = solve(S, dot(H, P.T)).T
+ I_KH = np.eye(P.shape[0]) - dot(K, H)
+
+ # update actual state
+ delta_x = dot(K, y)
+ P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
+
+ # inject observed error into state
+ x_new = np.zeros(x.shape, dtype=np.float64)
+ self.err_function(x, delta_x, x_new)
+ return x_new, P, y.flatten()
+
+ def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):
+ # init vars
+ z = z.reshape((-1, 1))
+ h = np.zeros(z.shape, dtype=np.float64)
+ H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
+
+ # C functions
+ self.hs[kind](x, extra_args, h)
+ self.Hs[kind](x, extra_args, H)
+
+ # y is the "loss"
+ y = z - h
+
+ # if using eskf
+ H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
+ self.H_mod(x, H_mod)
+ H = H.dot(H_mod)
+
+ a = np.linalg.inv(H.dot(P).dot(H.T) + R)
+ maha_dist = y.T.dot(a.dot(y))
+ if maha_dist > chi2.ppf(maha_thresh, y.shape[0]):
+ return False
+ else:
+ return True
+
+
+
+
+ def rts_smooth(self, estimates, norm_quats=False):
+ '''
+ Returns rts smoothed results of
+ kalman filter estimates
+
+ If the kalman state is augmented with
+ old states only the main state is smoothed
+ '''
+ xk_n = estimates[-1][0]
+ Pk_n = estimates[-1][2]
+ Fk_1 = np.zeros(Pk_n.shape, dtype=np.float64)
+
+ states_smoothed = [xk_n]
+ covs_smoothed = [Pk_n]
+ for k in xrange(len(estimates) - 2, -1, -1):
+ xk1_n = xk_n
+ if norm_quats:
+ xk1_n[3:7] /= np.linalg.norm(xk1_n[3:7])
+ Pk1_n = Pk_n
+
+ xk1_k, _, Pk1_k, _, t2, _, _, _, _ = estimates[k + 1]
+ _, xk_k, _, Pk_k, t1, _, _, _, _ = estimates[k]
+ dt = t2 - t1
+ self.F(xk_k, dt, Fk_1)
+
+ d1 = self.dim_main
+ d2 = self.dim_main_err
+ Ck = np.linalg.solve(Pk1_k[:d2,:d2], Fk_1[:d2,:d2].dot(Pk_k[:d2,:d2].T)).T
+ xk_n = xk_k
+ delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64)
+ self.inv_err_function(xk1_k, xk1_n, delta_x)
+ delta_x[:d2] = Ck.dot(delta_x[:d2])
+ x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64)
+ self.err_function(xk_k, delta_x, x_new)
+ xk_n[:d1] = x_new[:d1,0]
+ Pk_n = Pk_k
+ Pk_n[:d2,:d2] = Pk_k[:d2,:d2] + Ck.dot(Pk1_n[:d2,:d2] - Pk1_k[:d2,:d2]).dot(Ck.T)
+ states_smoothed.append(xk_n)
+ covs_smoothed.append(Pk_n)
+
+ return np.flipud(np.vstack(states_smoothed)), np.stack(covs_smoothed, 0)[::-1]
diff --git a/selfdrive/locationd/kalman/kalman_helpers.py b/selfdrive/locationd/kalman/kalman_helpers.py
new file mode 100644
index 0000000000..b15f1542e1
--- /dev/null
+++ b/selfdrive/locationd/kalman/kalman_helpers.py
@@ -0,0 +1,165 @@
+import numpy as np
+import os
+from bisect import bisect
+from tqdm import tqdm
+
+
+class ObservationKind(object):
+ UNKNOWN = 0
+ NO_OBSERVATION = 1
+ GPS_NED = 2
+ ODOMETRIC_SPEED = 3
+ PHONE_GYRO = 4
+ GPS_VEL = 5
+ PSEUDORANGE_GPS = 6
+ PSEUDORANGE_RATE_GPS = 7
+ SPEED = 8
+ NO_ROT = 9
+ PHONE_ACCEL = 10
+ ORB_POINT = 11
+ ECEF_POS = 12
+ CAMERA_ODO_TRANSLATION = 13
+ CAMERA_ODO_ROTATION = 14
+ ORB_FEATURES = 15
+ MSCKF_TEST = 16
+ FEATURE_TRACK_TEST = 17
+ LANE_PT = 18
+ IMU_FRAME = 19
+ PSEUDORANGE_GLONASS = 20
+ PSEUDORANGE_RATE_GLONASS = 21
+ PSEUDORANGE = 22
+ PSEUDORANGE_RATE = 23
+
+ names = ['Unknown',
+ 'No observation',
+ 'GPS NED',
+ 'Odometric speed',
+ 'Phone gyro',
+ 'GPS velocity',
+ 'GPS pseudorange',
+ 'GPS pseudorange rate',
+ 'Speed',
+ 'No rotation',
+ 'Phone acceleration',
+ 'ORB point',
+ 'ECEF pos',
+ 'camera odometric translation',
+ 'camera odometric rotation',
+ 'ORB features',
+ 'MSCKF test',
+ 'Feature track test',
+ 'Lane ecef point',
+ 'imu frame eulers',
+ 'GLONASS pseudorange',
+ 'GLONASS pseudorange rate']
+
+ @classmethod
+ def to_string(cls, kind):
+ return cls.names[kind]
+
+
+
+SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
+ ObservationKind.PSEUDORANGE_RATE_GPS,
+ ObservationKind.PSEUDORANGE_GLONASS,
+ ObservationKind.PSEUDORANGE_RATE_GLONASS]
+
+
+def run_car_ekf_offline(kf, observations_by_kind):
+ from laika.raw_gnss import GNSSMeasurement
+ observations = []
+ # create list of observations with element format: [kind, time, data]
+ for kind in observations_by_kind:
+ for t, data in zip(observations_by_kind[kind][0], observations_by_kind[kind][1]):
+ observations.append([t, kind, data])
+ observations.sort(key=lambda obs: obs[0])
+
+ times, estimates = run_observations_through_filter(kf, observations)
+
+ forward_states = np.stack(e[1] for e in estimates)
+ forward_covs = np.stack(e[3] for e in estimates)
+ smoothed_states, smoothed_covs = kf.rts_smooth(estimates)
+
+ observations_dict = {}
+ # TODO assuming observations and estimates
+ # are same length may not work with VO
+ for e in estimates:
+ t = e[4]
+ kind = str(int(e[5]))
+ res = e[6]
+ z = e[7]
+ ea = e[8]
+ if len(z) == 0:
+ continue
+ if kind not in observations_dict:
+ observations_dict[kind] = {}
+ observations_dict[kind]['t'] = np.array(len(z)*[t])
+ observations_dict[kind]['z'] = np.array(z)
+ observations_dict[kind]['ea'] = np.array(ea)
+ observations_dict[kind]['residual'] = np.array(res)
+ else:
+ observations_dict[kind]['t'] = np.append(observations_dict[kind]['t'], np.array(len(z)*[t]))
+ observations_dict[kind]['z'] = np.vstack((observations_dict[kind]['z'], np.array(z)))
+ observations_dict[kind]['ea'] = np.vstack((observations_dict[kind]['ea'], np.array(ea)))
+ observations_dict[kind]['residual'] = np.vstack((observations_dict[kind]['residual'], np.array(res)))
+
+ # add svIds to gnss data
+ for kind in map(str, SAT_OBS):
+ if int(kind) in observations_by_kind and kind in observations_dict:
+ observations_dict[kind]['svIds'] = np.array([])
+ observations_dict[kind]['CNO'] = np.array([])
+ observations_dict[kind]['std'] = np.array([])
+ for obs in observations_by_kind[int(kind)][1]:
+ observations_dict[kind]['svIds'] = np.append(observations_dict[kind]['svIds'],
+ np.array([obs[:,GNSSMeasurement.PRN]]))
+ observations_dict[kind]['std'] = np.append(observations_dict[kind]['std'],
+ np.array([obs[:,GNSSMeasurement.PR_STD]]))
+ return smoothed_states, smoothed_covs, forward_states, forward_covs, times, observations_dict
+
+
+def run_observations_through_filter(kf, observations, filter_time=None):
+ estimates = []
+
+ for obs in tqdm(observations):
+ t = obs[0]
+ kind = obs[1]
+ data = obs[2]
+ estimates.append(kf.predict_and_observe(t, kind, data))
+ times = [x[4] for x in estimates]
+ return times, estimates
+
+
+def save_residuals_plot(obs, save_path, data_name):
+ import matplotlib.pyplot as plt
+ import mpld3
+ fig = plt.figure(figsize=(10,20))
+ fig.suptitle('Residuals of ' + data_name, fontsize=24)
+ n = len(obs.keys())
+ start_times = [obs[kind]['t'][0] for kind in obs]
+ start_time = min(start_times)
+ xlims = [start_time + 3, start_time + 60]
+
+ for i, kind in enumerate(obs):
+ ax = fig.add_subplot(n, 1, i+1)
+ ax.set_xlim(xlims)
+ t = obs[kind]['t']
+ res = obs[kind]['residual']
+ start_idx = bisect(t, xlims[0])
+ if len(res) == start_idx:
+ continue
+ ylim = max(np.linalg.norm(res[start_idx:], axis=1))
+ ax.set_ylim([-ylim, ylim])
+ if int(kind) in SAT_OBS:
+ svIds = obs[kind]['svIds']
+ for svId in set(svIds):
+ svId_idx = (svIds == svId)
+ t = obs[kind]['t'][svId_idx]
+ res = obs[kind]['residual'][svId_idx]
+ ax.plot(t, res, label='SV ' + str(int(svId)))
+ ax.legend(loc='right')
+ else:
+ ax.plot(t, res)
+ plt.title('Residual of kind ' + ObservationKind.to_string(int(kind)), fontsize=20)
+ plt.tight_layout()
+ os.makedirs(save_path)
+ mpld3.save_html(fig, save_path + 'residuals_plot.html')
diff --git a/selfdrive/locationd/kalman/loc_local_kf.py b/selfdrive/locationd/kalman/loc_local_kf.py
new file mode 100755
index 0000000000..df6d41c77e
--- /dev/null
+++ b/selfdrive/locationd/kalman/loc_local_kf.py
@@ -0,0 +1,128 @@
+#!/usr/bin/env python
+import numpy as np
+import loc_local_model
+
+from kalman_helpers import ObservationKind
+from ekf_sym import EKF_sym
+
+
+
+class States(object):
+ VELOCITY = slice(0,3) # device frame velocity in m/s
+ ANGULAR_VELOCITY = slice(3, 6) # roll, pitch and yaw rates in device frame in radians/s
+ GYRO_BIAS = slice(6, 9) # roll, pitch and yaw biases
+ ODO_SCALE = slice(9, 10) # odometer scale
+ ACCELERATION = slice(10, 13) # Acceleration in device frame in m/s**2
+
+
+class LocLocalKalman(object):
+ def __init__(self):
+ x_initial = np.array([0, 0, 0,
+ 0, 0, 0,
+ 0, 0, 0,
+ 1,
+ 0, 0, 0])
+
+ # state covariance
+ P_initial = np.diag([10**2, 10**2, 10**2,
+ 1**2, 1**2, 1**2,
+ 0.05**2, 0.05**2, 0.05**2,
+ 0.02**2,
+ 1**2, 1**2, 1**2])
+
+ # process noise
+ Q = np.diag([0.0**2, 0.0**2, 0.0**2,
+ .01**2, .01**2, .01**2,
+ (0.005/100)**2, (0.005/100)**2, (0.005/100)**2,
+ (0.02/100)**2,
+ 3**2, 3**2, 3**2])
+
+ self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
+ ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2])}
+
+ # MSCKF stuff
+ self.dim_state = len(x_initial)
+ self.dim_main = self.dim_state
+
+ name = 'loc_local'
+ loc_local_model.gen_model(name, self.dim_state)
+
+ # init filter
+ self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_main, self.dim_main)
+
+ @property
+ def x(self):
+ return self.filter.state()
+
+ @property
+ def t(self):
+ return self.filter.filter_time
+
+ @property
+ def P(self):
+ return self.filter.covs()
+
+ def predict(self, t):
+ if self.t:
+ # Does NOT modify filter state
+ return self.filter._predict(self.x, self.P, t - self.t)[0]
+ else:
+ raise RuntimeError("Request predict on filter with uninitialized time")
+
+ def rts_smooth(self, estimates):
+ return self.filter.rts_smooth(estimates, norm_quats=True)
+
+
+ def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
+ if covs_diag is not None:
+ P = np.diag(covs_diag)
+ elif covs is not None:
+ P = covs
+ else:
+ P = self.filter.covs()
+ self.filter.init_state(state, P, filter_time)
+
+ def predict_and_observe(self, t, kind, data):
+ if len(data) > 0:
+ data = np.atleast_2d(data)
+ if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
+ r = self.predict_and_update_odo_trans(data, t, kind)
+ elif kind == ObservationKind.CAMERA_ODO_ROTATION:
+ r = self.predict_and_update_odo_rot(data, t, kind)
+ elif kind == ObservationKind.ODOMETRIC_SPEED:
+ r = self.predict_and_update_odo_speed(data, t, kind)
+ else:
+ r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
+ return r
+
+ def get_R(self, kind, n):
+ obs_noise = self.obs_noise[kind]
+ dim = obs_noise.shape[0]
+ R = np.zeros((n, dim, dim))
+ for i in xrange(n):
+ R[i,:,:] = obs_noise
+ return R
+
+ def predict_and_update_odo_speed(self, speed, t, kind):
+ z = np.array(speed)
+ R = np.zeros((len(speed), 1, 1))
+ for i, _ in enumerate(z):
+ R[i,:,:] = np.diag([0.2**2])
+ return self.filter.predict_and_update_batch(t, kind, z, R)
+
+ def predict_and_update_odo_trans(self, trans, t, kind):
+ z = trans[:,:3]
+ R = np.zeros((len(trans), 3, 3))
+ for i, _ in enumerate(z):
+ R[i,:,:] = np.diag(trans[i,3:]**2)
+ return self.filter.predict_and_update_batch(t, kind, z, R)
+
+ def predict_and_update_odo_rot(self, rot, t, kind):
+ z = rot[:,:3]
+ R = np.zeros((len(rot), 3, 3))
+ for i, _ in enumerate(z):
+ R[i,:,:] = np.diag(rot[i,3:]**2)
+ return self.filter.predict_and_update_batch(t, kind, z, R)
+
+if __name__ == "__main__":
+ LocLocalKalman()
diff --git a/selfdrive/locationd/kalman/loc_local_model.py b/selfdrive/locationd/kalman/loc_local_model.py
new file mode 100644
index 0000000000..2d69cac024
--- /dev/null
+++ b/selfdrive/locationd/kalman/loc_local_model.py
@@ -0,0 +1,80 @@
+import numpy as np
+import sympy as sp
+import os
+
+from kalman_helpers import ObservationKind
+from ekf_sym import gen_code
+
+
+def gen_model(name, dim_state):
+
+ # check if rebuild is needed
+ try:
+ dir_path = os.path.dirname(__file__)
+ deps = [dir_path + '/' + 'ekf_c.c',
+ dir_path + '/' + 'ekf_sym.py',
+ dir_path + '/' + 'loc_local_model.py',
+ dir_path + '/' + 'loc_local_kf.py']
+
+ outs = [dir_path + '/' + name + '.o',
+ dir_path + '/' + name + '.so',
+ dir_path + '/' + name + '.cpp']
+ out_times = map(os.path.getmtime, outs)
+ dep_times = map(os.path.getmtime, deps)
+ rebuild = os.getenv("REBUILD", False)
+ if min(out_times) > max(dep_times) and not rebuild:
+ return
+ map(os.remove, outs)
+ except OSError:
+ pass
+
+ # make functions and jacobians with sympy
+ # state variables
+ state_sym = sp.MatrixSymbol('state', dim_state, 1)
+ state = sp.Matrix(state_sym)
+ v = state[0:3,:]
+ omega = state[3:6,:]
+ vroll, vpitch, vyaw = omega
+ vx, vy, vz = v
+ roll_bias, pitch_bias, yaw_bias = state[6:9,:]
+ odo_scale = state[9,:]
+ accel = state[10:13,:]
+
+ dt = sp.Symbol('dt')
+
+ # Time derivative of the state as a function of state
+ state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+ state_dot[:3,:] = accel
+
+ # Basic descretization, 1st order intergrator
+ # Can be pretty bad if dt is big
+ f_sym = sp.Matrix(state + dt*state_dot)
+
+ #
+ # Observation functions
+ #
+
+ # extra args
+ #imu_rot = euler_rotate(*imu_angles)
+ #h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
+ # vpitch + pitch_bias,
+ # vyaw + yaw_bias])
+ h_gyro_sym = sp.Matrix([vroll + roll_bias,
+ vpitch + pitch_bias,
+ vyaw + yaw_bias])
+
+ speed = vx**2 + vy**2 + vz**2
+ h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
+
+ h_relative_motion = sp.Matrix(v)
+ h_phone_rot_sym = sp.Matrix([vroll,
+ vpitch,
+ vyaw])
+
+
+ obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
+ [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+ [h_phone_rot_sym, ObservationKind.NO_ROT, None],
+ [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
+ [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None]]
+ gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
diff --git a/selfdrive/locationd/locationd_local.py b/selfdrive/locationd/locationd_local.py
new file mode 100755
index 0000000000..0b1af2f4c1
--- /dev/null
+++ b/selfdrive/locationd/locationd_local.py
@@ -0,0 +1,274 @@
+#!/usr/bin/env python
+import os
+import zmq
+import math
+import json
+
+os.environ["OMP_NUM_THREADS"] = "1"
+import numpy as np
+from bisect import bisect_right
+
+from cereal import car
+from common.params import Params
+from common.numpy_fast import clip
+import selfdrive.messaging as messaging
+from selfdrive.swaglog import cloudlog
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.services import service_list
+from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman
+from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
+
+DEBUG = False
+kf = LocLocalKalman() # Make sure that model is generated on import time
+
+MAX_ANGLE_OFFSET = math.radians(10.)
+MAX_ANGLE_OFFSET_TH = math.radians(9.)
+MIN_STIFFNESS = 0.5
+MAX_STIFFNESS = 2.0
+MIN_SR = 0.5
+MAX_SR = 2.0
+MIN_SR_TH = 0.55
+MAX_SR_TH = 1.9
+
+LEARNING_RATE = 3
+
+class Localizer(object):
+ def __init__(self, disabled_logs=None, dog=None):
+ self.kf = LocLocalKalman()
+ self.reset_kalman()
+
+ self.max_age = .2 # seconds
+ self.calibration_valid = False
+
+ if disabled_logs is None:
+ self.disabled_logs = list()
+ else:
+ self.disabled_logs = disabled_logs
+
+ def reset_kalman(self):
+ self.filter_time = None
+ self.observation_buffer = []
+ self.converter = None
+ self.speed_counter = 0
+ self.sensor_counter = 0
+
+ def liveLocationMsg(self, time):
+ fix = messaging.log.KalmanOdometry.new_message()
+
+ predicted_state = self.kf.x
+ fix.trans = [float(predicted_state[0]), float(predicted_state[1]), float(predicted_state[2])]
+ fix.rot = [float(predicted_state[3]), float(predicted_state[4]), float(predicted_state[5])]
+
+ return fix
+
+ def update_kalman(self, time, kind, meas):
+ idx = bisect_right([x[0] for x in self.observation_buffer], time)
+ self.observation_buffer.insert(idx, (time, kind, meas))
+ while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
+ self.kf.predict_and_observe(*self.observation_buffer.pop(0))
+
+ def handle_cam_odo(self, log, current_time):
+ self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot,
+ log.cameraOdometry.rotStd]))
+ self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
+ log.cameraOdometry.transStd]))
+
+ def handle_car_state(self, log, current_time):
+ self.speed_counter += 1
+ if self.speed_counter % 5 == 0:
+ self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.carState.vEgo]))
+
+ def handle_sensors(self, log, current_time):
+ for sensor_reading in log.sensorEvents:
+ # TODO does not yet account for double sensor readings in the log
+ if sensor_reading.type == 4:
+ self.sensor_counter += 1
+ if self.sensor_counter % LEARNING_RATE == 0:
+ self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]])
+
+ def handle_log(self, log):
+ current_time = 1e-9 * log.logMonoTime
+ typ = log.which
+ if typ in self.disabled_logs:
+ return
+ if typ == "sensorEvents":
+ self.handle_sensors(log, current_time)
+ elif typ == "carState":
+ self.handle_car_state(log, current_time)
+ elif typ == "cameraOdometry":
+ self.handle_cam_odo(log, current_time)
+
+
+class ParamsLearner(object):
+ def __init__(self, VM, angle_offset=0., stiffness_factor=1.0, steer_ratio=None, learning_rate=1.0):
+ self.VM = VM
+
+ self.ao = math.radians(angle_offset)
+ self.slow_ao = math.radians(angle_offset)
+ self.x = stiffness_factor
+ self.sR = VM.sR if steer_ratio is None else steer_ratio
+ self.MIN_SR = MIN_SR * self.VM.sR
+ self.MAX_SR = MAX_SR * self.VM.sR
+ self.MIN_SR_TH = MIN_SR_TH * self.VM.sR
+ self.MAX_SR_TH = MAX_SR_TH * self.VM.sR
+
+ self.alpha1 = 0.01 * learning_rate
+ self.alpha2 = 0.00025 * learning_rate
+ self.alpha3 = 0.1 * learning_rate
+ self.alpha4 = 1.0 * learning_rate
+
+ def get_values(self):
+ return {
+ 'angleOffsetAverage': math.degrees(self.slow_ao),
+ 'stiffnessFactor': self.x,
+ 'steerRatio': self.sR,
+ }
+
+ def update(self, psi, u, sa):
+ cF0 = self.VM.cF
+ cR0 = self.VM.cR
+ aR = self.VM.aR
+ aF = self.VM.aF
+ l = self.VM.l
+ m = self.VM.m
+
+ x = self.x
+ ao = self.ao
+ sR = self.sR
+
+ # Gradient descent: learn angle offset, tire stiffness and steer ratio.
+ if u > 10.0 and abs(math.degrees(sa)) < 15.:
+ self.ao -= self.alpha1 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
+
+ ao = self.slow_ao
+ self.slow_ao -= self.alpha2 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
+
+ self.x -= self.alpha3 * -2.0*cF0*cR0*l*m*u**3*(ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**3)
+
+ self.sR -= self.alpha4 * -2.0*cF0*cR0*l*u*x*(ao - sa)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**3*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
+
+ if DEBUG:
+ # s1 = "Measured yaw rate % .6f" % psi
+ # ao = 0.
+ # s2 = "Uncompensated yaw % .6f" % (1.0*u*(-ao + sa)/(l*sR*(1 - m*u**2*(aF*cF0*x - aR*cR0*x)/(cF0*cR0*l**2*x**2))))
+ # instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa
+ s4 = "Instant AO: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao))
+ s5 = "Stiffnes: % .3f x" % self.x
+ print s4, s5
+
+
+ self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
+ self.slow_ao = clip(self.slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
+ self.x = clip(self.x, MIN_STIFFNESS, MAX_STIFFNESS)
+ self.sR = clip(self.sR, self.MIN_SR, self.MAX_SR)
+
+ # don't check stiffness for validity, as it can change quickly if sR is off
+ valid = abs(self.slow_ao) < MAX_ANGLE_OFFSET_TH and \
+ self.sR > self.MIN_SR_TH and self.sR < self.MAX_SR_TH
+
+ return valid
+
+
+def locationd_thread(gctx, addr, disabled_logs):
+ ctx = zmq.Context()
+ poller = zmq.Poller()
+
+ car_state_socket = messaging.sub_sock(ctx, service_list['carState'].port, poller, addr=addr, conflate=True)
+ sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True)
+ camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True)
+
+ kalman_odometry_socket = messaging.pub_sock(ctx, service_list['kalmanOdometry'].port)
+ live_parameters_socket = messaging.pub_sock(ctx, service_list['liveParameters'].port)
+
+ params_reader = Params()
+ cloudlog.info("Parameter learner is waiting for CarParams")
+ CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
+ VM = VehicleModel(CP)
+ cloudlog.info("Parameter learner got CarParams: %s" % CP.carFingerprint)
+
+ params = params_reader.get("LiveParameters")
+
+ # Check if car model matches
+ if params is not None:
+ params = json.loads(params)
+ if params.get('carFingerprint', None) != CP.carFingerprint:
+ cloudlog.info("Parameter learner found parameters for wrong car.")
+ params = None
+
+ if params is None:
+ params = {
+ 'carFingerprint': CP.carFingerprint,
+ 'angleOffsetAverage': 0.0,
+ 'stiffnessFactor': 1.0,
+ 'steerRatio': VM.sR,
+ }
+ cloudlog.info("Parameter learner resetting to default values")
+
+ cloudlog.info("Parameter starting with: %s" % str(params))
+ localizer = Localizer(disabled_logs=disabled_logs)
+
+ learner = ParamsLearner(VM,
+ angle_offset=params['angleOffsetAverage'],
+ stiffness_factor=params['stiffnessFactor'],
+ steer_ratio=params['steerRatio'],
+ learning_rate=LEARNING_RATE)
+
+ i = 0
+ while True:
+ for socket, event in poller.poll(timeout=1000):
+ log = messaging.recv_one(socket)
+ localizer.handle_log(log)
+
+ if socket is car_state_socket:
+ if not localizer.kf.t:
+ continue
+
+ if i % LEARNING_RATE == 0:
+ # carState is not updating the Kalman Filter, so update KF manually
+ localizer.kf.predict(1e-9 * log.logMonoTime)
+
+ predicted_state = localizer.kf.x
+ yaw_rate = -float(predicted_state[5])
+
+ steering_angle = math.radians(log.carState.steeringAngle)
+ params_valid = learner.update(yaw_rate, log.carState.vEgo, steering_angle)
+
+ params = messaging.new_message()
+ params.init('liveParameters')
+ params.liveParameters.valid = bool(params_valid)
+ params.liveParameters.angleOffset = float(math.degrees(learner.ao))
+ params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao))
+ params.liveParameters.stiffnessFactor = float(learner.x)
+ params.liveParameters.steerRatio = float(learner.sR)
+ live_parameters_socket.send(params.to_bytes())
+
+ if i % 6000 == 0: # once a minute
+ params = learner.get_values()
+ params['carFingerprint'] = CP.carFingerprint
+ params_reader.put("LiveParameters", json.dumps(params))
+
+ i += 1
+ elif socket is camera_odometry_socket:
+ msg = messaging.new_message()
+ msg.init('kalmanOdometry')
+ msg.logMonoTime = log.logMonoTime
+ msg.kalmanOdometry = localizer.liveLocationMsg(log.logMonoTime * 1e-9)
+ kalman_odometry_socket.send(msg.to_bytes())
+ elif socket is sensor_events_socket:
+ pass
+
+
+def main(gctx=None, addr="127.0.0.1"):
+ IN_CAR = os.getenv("IN_CAR", False)
+ disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
+
+ # No speed for now
+ disabled_logs.append('carState')
+ if IN_CAR:
+ addr = "192.168.5.11"
+
+ locationd_thread(gctx, addr, disabled_logs)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/selfdrive/locationd/ublox.py b/selfdrive/locationd/ublox.py
index 06f174fba0..70e2d2fa0d 100644
--- a/selfdrive/locationd/ublox.py
+++ b/selfdrive/locationd/ublox.py
@@ -207,10 +207,13 @@ class UBloxDescriptor:
def __init__(self,
name,
msg_format,
- fields=[],
+ fields=None,
count_field=None,
format2=None,
fields2=None):
+ if fields is None:
+ fields = []
+
self.name = name
self.msg_format = msg_format
self.fields = fields
@@ -465,7 +468,7 @@ msg_types = {
(CLASS_RXM, MSG_RXM_SFRB):
UBloxDescriptor('RXM_SFRB', '