mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-05 21:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.5.10 release

Browse Source 
old-commit-hash: f74a201edc
This commit is contained in:
Vehicle Researcher
2019-03-26 01:09:18 -07:00
parent 1bbcc51a36
commit 30f7a33535
102 changed files with 2765 additions and 968 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2 -1
View File
@@ -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
.travis.yml
+3 -1
View File
@@ -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'
README.md
+2 -3
View File
@@ -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 | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph | Custom<sup>9</sup>|
| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph | Custom<sup>9</sup>|
| Tesla | Model S 2012-13 | All | Yes | Not yet | Not applicable | 0mph | Custom<sup>8</sup>|
[[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266). <br />
[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246) <br />
<sup>9</sup>Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla) <br />
<sup>8</sup>Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla) <br />
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.
RELEASES.md
+13 -1
View File
@@ -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)
========================
SAFETY.md
+31 -2
View File
@@ -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.
apk/ai.comma.plus.offroad.apk
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:94b4a6bc1f7fc720b9f638268ff2345368164efd8a4710616b3e9ad5fd473fcf
size 18388501
oid sha256:e03ff9d6482c19f14ce565d64aab24433847208106e5a5e31947f5307e510ee5
size 18376966
cereal/car.capnp
+2
View File
@@ -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
cereal/log.capnp
+12 -2
View File
@@ -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 {
common/ffi_wrapper.py
+11 -2
View File
@@ -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)
common/params.py
+24 -28
View File
@@ -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,
"GitBranch": 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,
"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,
"CompletedTrainingVersion": TxType.PERSISTENT,
"ControlsParams": TxType.PERSISTENT,
"DoUninstall": TxType.CLEAR_ON_MANAGER_START,
"ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
"DongleId": TxType.PERSISTENT,
"GitBranch": TxType.PERSISTENT,
"GitCommit": TxType.PERSISTENT,
"GitRemote": TxType.PERSISTENT,
"HasAcceptedTerms": TxType.PERSISTENT,
"IsDriverMonitoringEnabled": TxType.PERSISTENT,
"IsFcwEnabled": TxType.PERSISTENT,
"IsGeofenceEnabled": TxType.PERSISTENT,
"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:
common/sympy_helpers.py
+81
View File
@@ -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
common/transformations/camera.py
+34
View File
@@ -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
common/transformations/model.py
+29
View File
@@ -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
models/monitoring_model.dlc
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:782e58e2fcbcbb39011011aa72c884ab3aa0cef22c434282df17ce5d64f2cba1
size 428100
oid sha256:0e880365b63e8d48bf8bee797dc96b483682a6b0b14e968e937c9109377b926e
size 427951
requirements_openpilot.txt
+4 -2
View File
@@ -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
selfdrive/athena/__init__.py
View File
selfdrive/athena/athenad.py
Executable
+128
View File
@@ -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()
selfdrive/boardd/boardd.cc
+9
View File
@@ -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<cereal::Event>();
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);
selfdrive/can/common.h
+3
View File
@@ -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 {
selfdrive/can/dbc_template.cc
+4
View File
@@ -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 %}
selfdrive/can/libdbc_py.py
+2
View File
@@ -39,6 +39,8 @@ typedef enum {
HONDA_CHECKSUM,
HONDA_COUNTER,
TOYOTA_CHECKSUM,
PEDAL_CHECKSUM,
PEDAL_COUNTER,
} SignalType;
typedef struct {
selfdrive/can/parser.cc
+36 -5
View File
@@ -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);
selfdrive/can/parser.py
+5 -1
View File
@@ -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)
selfdrive/can/process_dbc.py
+5 -1
View File
@@ -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])
selfdrive/car/__init__.py
+36
View File
@@ -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)
selfdrive/car/chrysler/carstate.py
+6 -6
View File
@@ -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']
selfdrive/car/chrysler/chryslercan_test.py
-1
View File
@@ -1,6 +1,5 @@
import chryslercan
from values import CAR
from carcontroller import CarController
from selfdrive.can.packer import CANPacker
from cereal import car
selfdrive/car/chrysler/values.py
+1 -1
View File
@@ -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: [
selfdrive/car/ford/carstate.py
+6 -6
View File
@@ -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
selfdrive/car/gm/carstate.py
+6 -3
View File
@@ -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])
selfdrive/car/honda/carcontroller.py
+3 -2
View File
@@ -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())
selfdrive/car/honda/carstate.py
+4 -4
View File
@@ -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)
selfdrive/car/honda/hondacan.py
-12
View File
@@ -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,
selfdrive/car/hyundai/carstate.py
+7 -7
View File
@@ -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']
selfdrive/car/hyundai/interface.py
-4
View File
@@ -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
selfdrive/car/subaru/__init__.py
+1
View File
@@ -0,0 +1 @@
selfdrive/car/subaru/carstate.py
+104
View File
@@ -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']])
selfdrive/car/subaru/interface.py
+205
View File
@@ -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
selfdrive/car/subaru/radar_interface.py
+24
View File
@@ -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
selfdrive/car/subaru/values.py
+18
View File
@@ -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),
}
selfdrive/car/toyota/carcontroller.py
+4 -3
View File
@@ -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:
selfdrive/car/toyota/carstate.py
+7 -7
View File
@@ -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']
selfdrive/car/toyota/toyotacan.py
-12
View File
@@ -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 = {
selfdrive/car/toyota/values.py
+6 -13
View File
@@ -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
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.5.9-release"
#define COMMA_VERSION "0.5.10-release"
selfdrive/controls/controlsd.py
+19 -14
View File
@@ -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
selfdrive/controls/lib/alerts.py
+7
View File
@@ -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),
]
selfdrive/controls/lib/drive_helpers.py
+4 -4
View File
@@ -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):
selfdrive/controls/lib/fcw.py
+71
View File
@@ -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
selfdrive/controls/lib/latcontrol.py
+2
View File
@@ -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,
selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+3 -1
View File
@@ -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();
selfdrive/controls/lib/lateral_mpc/lateral_mpc.o
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:1d469e8cd75122d5996338da166919f3dda15c1ef72f5ce3e33c46096a168228
size 2328
oid sha256:b0ccf356c299a12f40fd99fa61361812e5fedff6f1836c8afad98b7b2f3a2a2a
size 2456
selfdrive/controls/lib/lateral_mpc/libmpc.so
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0cc8148b7105e645cd232ac0c088c4324a545328eb64781939762214ddf14f90
oid sha256:849733e32eecd68112c4ab305c013025dbfa41ec3f13c54fbb9368388d835260
size 532192
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+1
View File
@@ -18,6 +18,7 @@ typedef struct {
double y[21];
double psi[21];
double delta[21];
double rate[21];
double cost;
} log_t;
selfdrive/controls/lib/long_mpc.py
+120
View File
@@ -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
selfdrive/controls/lib/longitudinal_mpc/generator.cpp
+3 -12
View File
@@ -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);
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:81c0f18ac2f84fa44a2afca0206173dd866605a0b7dbe60fd2640734d54ce3b9
size 5480
oid sha256:453ffa5ad5c533e6d6065167fb1a674ed9196e2ade87df4a55d163ff178eccf7
size 5376
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6d5761f29b51905c42910fdf9e7b5744f09f132c69caa8ac6e96e57ac1f17e6b
size 8832
oid sha256:c50ba5b1353617c7a18d17f1417c4036d8e5c41db79f82cb9a9b4a19032e6cc6
size 8752
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:26a4622ed83e6315cf34d55e46a3bb4af8db1d135f9c54c4e10dbd4d09a47dda
size 34770
oid sha256:221b739b59f5a314fc832a29a7fcf49da960012acc22aed82e66331f950ac5f8
size 11486
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:b6f5e9a2fc5f5765ce2f53c16d6ccbd1142cd514ab6acee0625a12fef8f11fd9
size 8584
oid sha256:1a5e0651c2c8c650b59421bb4c4beea5c3b666bbd1c6e440eb2b62e2a64119bb
size 3560
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:7aaa8e6766705d1cd22c65d418e10a4c4b54864809cc9ad5e5de09c98e72cfd0
oid sha256:c4099e03a411a516b406ef62cf0d923fb9447b376ae7dcbebe680a29f6c217d9
size 1948
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:383c2fa9974e0802dd86773aed20e068b39e0e5c505e6a641b30332f99f875b7
oid sha256:3cc91e06813e2f18c87f0f2c798eb76a4e81e12c1027892a6c2b7d3451b03d54
size 1821
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:21349c8535aa0c186edcf75fbf815822dfddbedc3f5e090c8bcd416dea5a1ced
oid sha256:d8648ab44187e2be8e7cdb128249dfac75338d98d0d93ca9878ea4a4b3f2ad3c
size 2992
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:c00034f1aa41c2f00e93c7d748cf5f663435360563107b6898441aac661a9bf2
size 448071
oid sha256:a58b02a90b3776ce308ab77094ebd8cdd23e788e1501a4c050862e65e827b185
size 349038
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6a618ebaa304fe2776942918ea1f007a44eb63afd5b5e2946082034073358c2d
size 307280
oid sha256:16e942a5676e23d49e01b17ff664a55bddffa9e7d054caf4f6dbb03d16ec2e9e
size 274768
selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:eb9f45801304976cb37dcfbcff9681244bf78e97284d8b8da7264871a827783f
oid sha256:a955478920067e11d3c1d40f96323bfb943196d66954911dfc62c5f673ecab06
size 8048
selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0285ad4d884bf0bdcd3fa692ddc0814ec12b73ec0b0433b18b9072192fd503e0
oid sha256:81feeda7638a908cd175f97addf62d16c4f47d3e38b020a0069838f3502ceff0
size 8112
selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:df47bdf5dba35ac627a72e0c58e7d1648a0759d17b852a3347cac5e183e4b046
oid sha256:57289f47ce5d35735edc673351e496e32ec606028512ecd785b9ea4309005629
size 2944
selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6f9c880eb9d53f6b86acc90166150bc96ad25c1554b3dc61e449217e8faabc0a
oid sha256:d1df0d4b3379b6daebd902add7e38a8f53b90c3b7c25b1d8855fd44a043fe650
size 4488
selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:5a3b55e323c19172587d0e58c619a1e4d2c95711473b071bff29c0d32dad442d
oid sha256:3ac57f4a4c5410bf29c12a32418e3b0ccd020d0362cc4a5316a915482cb4c99d
size 4496
selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:acefa3c955a443246265289085a7fb9c096c5310bc456a8ccda785d94852e4ff
size 72928
oid sha256:67f333a6f1d18e9372b0c9eb8588cf4d3db87d3f731ff56497fbcecd79294400
size 72200
selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:728d61fd9f2e72f9fca96e51ae58b23308469625f8e97a29918493e79e9b37b9
size 37832
oid sha256:250c5e3626bd753796225bca9d031751a9f21e73e7b7e7fc8c4821b59af96298
size 37592
selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f6b12c121ee23318322af7882c07d37dd298068898160a8c1975fd5e9fb6d79f
oid sha256:ec3bcbb2a42f595b00d2b7ed13f08077c1c759366bf83fb553e397f5091dc60c
size 3856
selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:79a968b415bc198402eac3b630b3087c1bc2b5a62883b9e39c8c43469ee9f388
size 446096
oid sha256:6c30e9afb75af10d72d0ea5e30b06c0d5310cce9312a441a2d260eb82d66f5ea
size 405136
selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
+1
View File
@@ -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;
selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
+43 -29
View File
@@ -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;
for (i = 0; i <= NOD * N; i+= NOD){
acadoVariables.od[i] = l;
acadoVariables.od[i+1] = 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;
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();
selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:b9352e718fb6a33b154e5ff749c65051dde02cc5bcf8ccf2f8a44c2bed85e8cc
size 3976
oid sha256:3b76bbb87fa3725ded4f4372174c3c7e4e5a58b86b950cd8867222f4cf3c375b
size 3136
selfdrive/controls/lib/pathplanner.py
+15 -6
View File
@@ -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())
selfdrive/controls/lib/planner.py
+33 -223
View File
@@ -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())
selfdrive/controls/lib/vehicle_model.py
+10 -3
View File
@@ -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.
selfdrive/controls/plannerd.py
+10 -1
View File
@@ -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:
selfdrive/controls/radard.py
+20 -6
View File
@@ -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))
selfdrive/crash.py
+23
View File
@@ -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
selfdrive/locationd/kalman/__init__.py
View File
selfdrive/locationd/kalman/ekf_c.c
+124
View File
@@ -0,0 +1,124 @@
#include <eigen3/Eigen/Dense>
#include <iostream>
typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
void predict(double *in_x, double *in_P, double *in_Q, double dt) {
typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> 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 <int ZDIM, int EADIM, bool MAHA_TEST>
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<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
typedef Eigen::Matrix<double, ZDIM, EDIM, Eigen::RowMajor> ZEM;
typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> 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<double, ZDIM, 1> 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<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
X1M y; XXM H; XXM R;
if (Hea_fun){
typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> 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<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
// update state by injecting dx
Eigen::Matrix<double, EDIM, 1> 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<double, DIM, 1> 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));
}
selfdrive/locationd/kalman/ekf_sym.py
+564
View File
@@ -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]
selfdrive/locationd/kalman/kalman_helpers.py
+165
View File
@@ -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')
selfdrive/locationd/kalman/loc_local_kf.py
Executable
+128
View File
@@ -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()
selfdrive/locationd/kalman/loc_local_model.py
+80
View File
@@ -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)
selfdrive/locationd/locationd_local.py
Executable
+274
View File
@@ -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()
selfdrive/locationd/ublox.py
+6 -3
View File
@@ -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', '<BB10I', ['chn', 'svid', 'dwrd[10]']),
(CLASS_RXM, MSG_RXM_SFRBX):
UBloxDescriptor('RXM_SFRBX', '<8B', ['gnssId', 'svid', 'reserved1', 'freqId', 'numWords',
UBloxDescriptor('RXM_SFRBX', '<8B', ['gnssId', 'svid', 'reserved1', 'freqId', 'numWords',
'reserved2', 'version', 'reserved3'], 'numWords', 'I', ['dwrd']),
(CLASS_AID, MSG_AID_ALM):
UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
@@ -704,7 +707,7 @@ class UBlox:
time.sleep(0.1)
self.panda.set_esp_power(1)
time.sleep(0.5)
# can't set above 9600 now...
self.baudrate = 9600
self.dev = PandaSerial(self.panda, 1, self.baudrate)
selfdrive/loggerd/loggerd
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f4ce9c8b78e58c57b0f033354f1411811457b9e401c16fe5c5b130aa9fe86d3a
oid sha256:4e0f935ee35f4856cfc315209da7c0d869fe518c00740ccd3646c108ce4a7745
size 1630952
selfdrive/manager.py
+4
View File
@@ -102,10 +102,12 @@ managed_processes = {
"pandad": "selfdrive.pandad",
"ui": ("selfdrive/ui", ["./start.sh"]),
"calibrationd": "selfdrive.locationd.calibrationd",
"locationd": "selfdrive.locationd.locationd_local",
"visiond": ("selfdrive/visiond", ["./visiond"]),
"sensord": ("selfdrive/sensord", ["./sensord"]),
"gpsd": ("selfdrive/sensord", ["./gpsd"]),
"updated": "selfdrive.updated",
"athena": "selfdrive.athena.athenad",
}
android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame")
@@ -128,6 +130,7 @@ persistent_processes = [
'ui',
'gpsd',
'updated',
'athena'
]
car_started_processes = [
@@ -137,6 +140,7 @@ car_started_processes = [
'sensord',
'radard',
'calibrationd',
'locationd',
'visiond',
'proclogd',
'ubloxd',
selfdrive/mapd/default_speeds.json
+1
View File
@@ -29,6 +29,7 @@
"DK:rural": "80",
"DK:motorway": "130",
"DE:living_street": "7",
"DE:residential": "30",
"DE:urban": "50",
"DE:rural": "100",
"DE:trunk": "none",
selfdrive/mapd/default_speeds_by_region.json
-454
View File
@@ -1,454 +0,0 @@
{
"CA": {
"Default": [
{
"speed": "100",
"tags": {
"highway": "motorway"
}
},
{
"speed": "80",
"tags": {
"highway": "trunk"
}
},
{
"speed": "80",
"tags": {
"highway": "primary"
}
},
{
"speed": "50",
"tags": {
"highway": "secondary"
}
},
{
"speed": "50",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "80",
"tags": {
"highway": "unclassified"
}
},
{
"speed": "40",
"tags": {
"highway": "residential"
}
},
{
"speed": "40",
"tags": {
"highway": "service"
}
},
{
"speed": "90",
"tags": {
"highway": "motorway_link"
}
},
{
"speed": "80",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "80",
"tags": {
"highway": "primary_link"
}
},
{
"speed": "50",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "50",
"tags": {
"highway": "tertiary_link"
}
},
{
"speed": "20",
"tags": {
"highway": "living_street"
}
}
]
},
"DE": {
"Default": [
{
"speed": "none",
"tags": {
"highway": "motorway"
}
},
{
"speed": "10",
"tags": {
"highway": "living_street"
}
},
{
"speed": "100",
"tags": {
"zone:traffic": "DE:rural"
}
},
{
"speed": "50",
"tags": {
"zone:traffic": "DE:urban"
}
},
{
"speed": "30",
"tags": {
"bicycle_road": "yes"
}
}
]
},
"AU": {
"Default": [
{
"speed": "100",
"tags": {
"highway": "motorway"
}
},
{
"speed": "80",
"tags": {
"highway": "trunk"
}
},
{
"speed": "80",
"tags": {
"highway": "primary"
}
},
{
"speed": "50",
"tags": {
"highway": "secondary"
}
},
{
"speed": "50",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "80",
"tags": {
"highway": "unclassified"
}
},
{
"speed": "50",
"tags": {
"highway": "residential"
}
},
{
"speed": "40",
"tags": {
"highway": "service"
}
},
{
"speed": "90",
"tags": {
"highway": "motorway_link"
}
},
{
"speed": "80",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "80",
"tags": {
"highway": "primary_link"
}
},
{
"speed": "50",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "50",
"tags": {
"highway": "tertiary_link"
}
},
{
"speed": "30",
"tags": {
"highway": "living_street"
}
}
]
},
"US": {
"South Dakota": [
{
"speed": "80 mph",
"tags": {
"highway": "motorway"
}
},
{
"speed": "70 mph",
"tags": {
"highway": "trunk"
}
},
{
"speed": "65 mph",
"tags": {
"highway": "primary"
}
},
{
"speed": "70 mph",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "65 mph",
"tags": {
"highway": "primary_link"
}
}
],
"Wisconsin": [
{
"speed": "65 mph",
"tags": {
"highway": "trunk"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "unclassified"
}
},
{
"speed": "65 mph",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "tertiary_link"
}
}
],
"Default": [
{
"speed": "65 mph",
"tags": {
"highway": "motorway"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "trunk"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "primary"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "secondary"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "unclassified"
}
},
{
"speed": "25 mph",
"tags": {
"highway": "residential"
}
},
{
"speed": "25 mph",
"tags": {
"highway": "service"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "motorway_link"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "primary_link"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "tertiary_link"
}
},
{
"speed": "15 mph",
"tags": {
"highway": "living_street"
}
}
],
"Michigan": [
{
"speed": "70 mph",
"tags": {
"highway": "motorway"
}
}
],
"Oregon": [
{
"speed": "55 mph",
"tags": {
"highway": "motorway"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "secondary"
}
},
{
"speed": "30 mph",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "15 mph",
"tags": {
"highway": "service"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "30 mph",
"tags": {
"highway": "tertiary_link"
}
}
],
"New York": [
{
"speed": "45 mph",
"tags": {
"highway": "primary"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "secondary"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "30 mph",
"tags": {
"highway": "residential"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "primary_link"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "tertiary_link"
}
}
]
}
}
selfdrive/mapd/default_speeds_generator.py
+12 -6
View File
@@ -1,9 +1,9 @@
#!/usr/bin/env python
import json
OUTPUT_FILENAME = "default_speeds_by_region.json"
DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json"
def main():
def main(filename = DEFAULT_OUTPUT_FILENAME):
countries = []
"""
@@ -137,27 +137,33 @@ def main():
""" Default rules """
DE.add_rule({"highway": "motorway"}, "none")
DE.add_rule({"highway": "living_street"}, "10")
DE.add_rule({"highway": "residential"}, "30")
DE.add_rule({"zone:traffic": "DE:rural"}, "100")
DE.add_rule({"zone:traffic": "DE:urban"}, "50")
DE.add_rule({"zone:maxspeed": "DE:30"}, "30")
DE.add_rule({"zone:maxspeed": "DE:urban"}, "50")
DE.add_rule({"zone:maxspeed": "DE:rural"}, "100")
DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none")
DE.add_rule({"bicycle_road": "yes"}, "30")
""" --- DO NOT MODIFY CODE BELOW THIS LINE --- """
""" --- ADD YOUR COUNTRY OR STATE ABOVE --- """
# Final step
write_json(countries)
write_json(countries, filename)
def write_json(countries):
def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME):
out_dict = {}
for country in countries:
out_dict.update(country.jsonify())
json_string = json.dumps(out_dict, indent=2)
with open(OUTPUT_FILENAME, "wb") as f:
with open(filename, "wb") as f:
f.write(json_string)
class Region(object):
ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road"]
ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"]
ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"]
def __init__(self, name):
self.name = name
selfdrive/mapd/mapd.py
+6 -27
View File
@@ -1,12 +1,13 @@
#!/usr/bin/env python
# Add phonelibs openblas to LD_LIBRARY_PATH if import fails
from common.basedir import BASEDIR
try:
from scipy import spatial
except ImportError as e:
import os
import sys
from common.basedir import BASEDIR
openblas_path = os.path.join(BASEDIR, "phonelibs/openblas/")
os.environ['LD_LIBRARY_PATH'] += ':' + openblas_path
@@ -15,6 +16,10 @@ except ImportError as e:
args.extend(sys.argv)
os.execv(sys.executable, args)
DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json"
import default_speeds_generator
default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE)
import os
import sys
import time
@@ -45,31 +50,6 @@ last_query_result = None
last_query_pos = None
cache_valid = False
def setup_thread_excepthook():
"""
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
def build_way_query(lat, lon, radius=50):
"""Builds a query to find all highways within a given radius around a point"""
pos = " (around:%f,%f,%f)" % (radius, lat, lon)
@@ -301,7 +281,6 @@ def main(gctx=None):
crash.bind_extra(version=version, dirty=dirty, is_eon=True)
crash.install()
setup_thread_excepthook()
main_thread = threading.Thread(target=mapsd_thread)
main_thread.daemon = True
main_thread.start()
selfdrive/sensord/gpsd
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a912e6346d7da8591acc72b5aa2d2382cb815d44f8567c656097ac180fe846b6
oid sha256:b07f5055891b22f181023c132917416ebb8385201ffd172c5f9584ff8e8ff47b
size 1171544
selfdrive/sensord/sensord
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:09b15277bb78a39ef3734b4a4773e8fed6431541395385ea49b78fbb1c3f37c4
oid sha256:b8a9e5e8d9e3ae4349b846f3f621aa0d67a114867ac0bc7c3ab891c425a56c2b
size 1159016
selfdrive/test/plant/plant.py
+8
View File
@@ -11,6 +11,7 @@ from common.realtime import Ratekeeper
from selfdrive.config import Conversions as CV
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.car import crc8_pedal
from selfdrive.car.honda.hondacan import fix
from selfdrive.car.honda.values import CAR
from selfdrive.car.honda.carstate import get_can_signals
@@ -284,12 +285,19 @@ class Plant(object):
if "COUNTER" in honda.get_signals(msg):
msg_struct["COUNTER"] = self.rk.frame % 4
if "COUNTER_PEDAL" in honda.get_signals(msg):
msg_struct["COUNTER_PEDAL"] = self.rk.frame % 0xf
msg = honda.lookup_msg_id(msg)
msg_data = honda.encode(msg, msg_struct)
if "CHECKSUM" in honda.get_signals(msg):
msg_data = fix(msg_data, msg)
if "CHECKSUM_PEDAL" in honda.get_signals(msg):
msg_struct["CHECKSUM_PEDAL"] = crc8_pedal([ord(i) for i in msg_data][:-1])
msg_data = honda.encode(msg, msg_struct)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message

Some files were not shown because too many files have changed in this diff Show More