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

openpilot v0.5.10 release

Browse Source
This commit is contained in:
Vehicle Researcher
2019-03-26 01:09:18 -07:00
parent be5c2aef3a
commit f74a201edc
102 changed files with 6394 additions and 5433 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
BIN
View File
Binary file not shown.
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
BIN
View File
Binary file not shown.
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
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/libmpc.so
BIN
View File
Binary file not shown.
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
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
+64 -67
View File
@@ -70,7 +70,7 @@ extern "C"
/** Number of control variables. */
#define ACADO_NU 1
/** Number of differential variables. */
#define ACADO_NX 6
#define ACADO_NX 3
/** Number of algebraic variables. */
#define ACADO_NXA 0
/** Number of differential derivative variables. */
@@ -80,7 +80,7 @@ extern "C"
/** Number of references/measurements on the last (N + 1)st node. */
#define ACADO_NYN 3
/** Total number of QP optimization variables. */
#define ACADO_QP_NV 26
#define ACADO_QP_NV 23
/** Number of Runge-Kutta stages per integration step. */
#define ACADO_RK_NSTAGES 4
/** Providing interface for arrival cost. */
@@ -102,11 +102,11 @@ extern "C"
typedef struct ACADOvariables_
{
int dummy;
/** Matrix of size: 21 x 6 (row major format)
/** Matrix of size: 21 x 3 (row major format)
*
* Matrix containing 21 differential variable vectors.
*/
real_t x[ 126 ];
real_t x[ 63 ];
/** Column vector of size: 20
*
@@ -138,11 +138,11 @@ real_t W[ 320 ];
/** Matrix of size: 3 x 3 (row major format) */
real_t WN[ 9 ];
/** Column vector of size: 6
/** Column vector of size: 3
*
* Current state feedback vector.
*/
real_t x0[ 6 ];
real_t x0[ 3 ];
} ACADOvariables;
@@ -155,22 +155,19 @@ real_t x0[ 6 ];
*/
typedef struct ACADOworkspace_
{
/** Column vector of size: 10 */
real_t rhs_aux[ 10 ];
real_t rk_ttt;
/** Row vector of size: 51 */
real_t rk_xxx[ 51 ];
/** Row vector of size: 18 */
real_t rk_xxx[ 18 ];
/** Matrix of size: 4 x 48 (row major format) */
real_t rk_kkk[ 192 ];
/** Matrix of size: 4 x 15 (row major format) */
real_t rk_kkk[ 60 ];
/** Row vector of size: 51 */
real_t state[ 51 ];
/** Row vector of size: 18 */
real_t state[ 18 ];
/** Column vector of size: 120 */
real_t d[ 120 ];
/** Column vector of size: 60 */
real_t d[ 60 ];
/** Column vector of size: 80 */
real_t Dy[ 80 ];
@@ -178,26 +175,26 @@ real_t Dy[ 80 ];
/** Column vector of size: 3 */
real_t DyN[ 3 ];
/** Matrix of size: 120 x 6 (row major format) */
real_t evGx[ 720 ];
/** Matrix of size: 60 x 3 (row major format) */
real_t evGx[ 180 ];
/** Column vector of size: 120 */
real_t evGu[ 120 ];
/** Column vector of size: 60 */
real_t evGu[ 60 ];
/** Column vector of size: 30 */
real_t objAuxVar[ 30 ];
/** Column vector of size: 15 */
real_t objAuxVar[ 15 ];
/** Row vector of size: 9 */
real_t objValueIn[ 9 ];
/** Row vector of size: 6 */
real_t objValueIn[ 6 ];
/** Row vector of size: 32 */
real_t objValueOut[ 32 ];
/** Row vector of size: 20 */
real_t objValueOut[ 20 ];
/** Matrix of size: 120 x 6 (row major format) */
real_t Q1[ 720 ];
/** Matrix of size: 60 x 3 (row major format) */
real_t Q1[ 180 ];
/** Matrix of size: 120 x 4 (row major format) */
real_t Q2[ 480 ];
/** Matrix of size: 60 x 4 (row major format) */
real_t Q2[ 240 ];
/** Column vector of size: 20 */
real_t R1[ 20 ];
@@ -205,53 +202,53 @@ real_t R1[ 20 ];
/** Matrix of size: 20 x 4 (row major format) */
real_t R2[ 80 ];
/** Column vector of size: 120 */
real_t S1[ 120 ];
/** Column vector of size: 60 */
real_t S1[ 60 ];
/** Matrix of size: 6 x 6 (row major format) */
real_t QN1[ 36 ];
/** Matrix of size: 3 x 3 (row major format) */
real_t QN1[ 9 ];
/** Matrix of size: 6 x 3 (row major format) */
real_t QN2[ 18 ];
/** Matrix of size: 3 x 3 (row major format) */
real_t QN2[ 9 ];
/** Column vector of size: 6 */
real_t Dx0[ 6 ];
/** Column vector of size: 3 */
real_t Dx0[ 3 ];
/** Matrix of size: 6 x 6 (row major format) */
real_t T[ 36 ];
/** Matrix of size: 3 x 3 (row major format) */
real_t T[ 9 ];
/** Column vector of size: 1260 */
real_t E[ 1260 ];
/** Column vector of size: 630 */
real_t E[ 630 ];
/** Column vector of size: 1260 */
real_t QE[ 1260 ];
/** Column vector of size: 630 */
real_t QE[ 630 ];
/** Matrix of size: 120 x 6 (row major format) */
real_t QGx[ 720 ];
/** Matrix of size: 60 x 3 (row major format) */
real_t QGx[ 180 ];
/** Column vector of size: 120 */
real_t Qd[ 120 ];
/** Column vector of size: 60 */
real_t Qd[ 60 ];
/** Column vector of size: 126 */
real_t QDy[ 126 ];
/** Column vector of size: 63 */
real_t QDy[ 63 ];
/** Matrix of size: 20 x 6 (row major format) */
real_t H10[ 120 ];
/** Matrix of size: 20 x 3 (row major format) */
real_t H10[ 60 ];
/** Matrix of size: 26 x 26 (row major format) */
real_t H[ 676 ];
/** Matrix of size: 23 x 23 (row major format) */
real_t H[ 529 ];
/** Matrix of size: 20 x 26 (row major format) */
real_t A[ 520 ];
/** Matrix of size: 20 x 23 (row major format) */
real_t A[ 460 ];
/** Column vector of size: 26 */
real_t g[ 26 ];
/** Column vector of size: 23 */
real_t g[ 23 ];
/** Column vector of size: 26 */
real_t lb[ 26 ];
/** Column vector of size: 23 */
real_t lb[ 23 ];
/** Column vector of size: 26 */
real_t ub[ 26 ];
/** Column vector of size: 23 */
real_t ub[ 23 ];
/** Column vector of size: 20 */
real_t lbA[ 20 ];
@@ -259,11 +256,11 @@ real_t lbA[ 20 ];
/** Column vector of size: 20 */
real_t ubA[ 20 ];
/** Column vector of size: 26 */
real_t x[ 26 ];
/** Column vector of size: 23 */
real_t x[ 23 ];
/** Column vector of size: 46 */
real_t y[ 46 ];
/** Column vector of size: 43 */
real_t y[ 43 ];
} ACADOworkspace;
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
+71 -314
View File
@@ -23,72 +23,24 @@
void acado_rhs_forw(const real_t* in, real_t* out)
{
const real_t* xd = in;
const real_t* u = in + 48;
const real_t* od = in + 49;
/* Vector of auxiliary variables; number of elements: 10. */
real_t* a = acadoWorkspace.rhs_aux;
/* Compute intermediate quantities: */
a[0] = (exp((((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[5])/(real_t)(2.0000000000000000e+00))));
a[1] = ((real_t)(1.0000000000000000e+00)/(real_t)(2.0000000000000000e+00));
a[2] = (exp((((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[5])/(real_t)(2.0000000000000000e+00))));
a[3] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[36])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[36]))*a[1])*a[2]);
a[4] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[37])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[37]))*a[1])*a[2]);
a[5] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[38])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[38]))*a[1])*a[2]);
a[6] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[39])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[39]))*a[1])*a[2]);
a[7] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[40])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[40]))*a[1])*a[2]);
a[8] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[41])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[41]))*a[1])*a[2]);
a[9] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[47])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[47]))*a[1])*a[2]);
const real_t* u = in + 15;
/* Compute outputs: */
out[0] = xd[1];
out[1] = xd[2];
out[2] = u[0];
out[3] = xd[4];
out[4] = (od[1]*a[0]);
out[5] = (real_t)(1.0000000000000000e+00);
out[6] = xd[12];
out[7] = xd[13];
out[8] = xd[14];
out[9] = xd[15];
out[10] = xd[16];
out[11] = xd[17];
out[12] = xd[18];
out[13] = xd[19];
out[14] = xd[20];
out[15] = xd[21];
out[16] = xd[22];
out[17] = xd[23];
out[18] = (real_t)(0.0000000000000000e+00);
out[19] = (real_t)(0.0000000000000000e+00);
out[20] = (real_t)(0.0000000000000000e+00);
out[21] = (real_t)(0.0000000000000000e+00);
out[22] = (real_t)(0.0000000000000000e+00);
out[23] = (real_t)(0.0000000000000000e+00);
out[24] = xd[30];
out[25] = xd[31];
out[26] = xd[32];
out[27] = xd[33];
out[28] = xd[34];
out[29] = xd[35];
out[30] = (od[1]*a[3]);
out[31] = (od[1]*a[4]);
out[32] = (od[1]*a[5]);
out[33] = (od[1]*a[6]);
out[34] = (od[1]*a[7]);
out[35] = (od[1]*a[8]);
out[36] = (real_t)(0.0000000000000000e+00);
out[37] = (real_t)(0.0000000000000000e+00);
out[38] = (real_t)(0.0000000000000000e+00);
out[39] = (real_t)(0.0000000000000000e+00);
out[40] = (real_t)(0.0000000000000000e+00);
out[41] = (real_t)(0.0000000000000000e+00);
out[42] = xd[43];
out[43] = xd[44];
out[44] = (real_t)(1.0000000000000000e+00);
out[45] = xd[46];
out[46] = (od[1]*a[9]);
out[47] = (real_t)(0.0000000000000000e+00);
out[3] = xd[6];
out[4] = xd[7];
out[5] = xd[8];
out[6] = xd[9];
out[7] = xd[10];
out[8] = xd[11];
out[9] = (real_t)(0.0000000000000000e+00);
out[10] = (real_t)(0.0000000000000000e+00);
out[11] = (real_t)(0.0000000000000000e+00);
out[12] = xd[13];
out[13] = xd[14];
out[14] = (real_t)(1.0000000000000000e+00);
}
/* Fixed step size:0.2 */
@@ -100,51 +52,21 @@ int run1;
int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
int numInts = numSteps[rk_index];
acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
rk_eta[6] = 1.0000000000000000e+00;
rk_eta[7] = 0.0000000000000000e+00;
rk_eta[3] = 1.0000000000000000e+00;
rk_eta[4] = 0.0000000000000000e+00;
rk_eta[5] = 0.0000000000000000e+00;
rk_eta[6] = 0.0000000000000000e+00;
rk_eta[7] = 1.0000000000000000e+00;
rk_eta[8] = 0.0000000000000000e+00;
rk_eta[9] = 0.0000000000000000e+00;
rk_eta[10] = 0.0000000000000000e+00;
rk_eta[11] = 0.0000000000000000e+00;
rk_eta[11] = 1.0000000000000000e+00;
rk_eta[12] = 0.0000000000000000e+00;
rk_eta[13] = 1.0000000000000000e+00;
rk_eta[13] = 0.0000000000000000e+00;
rk_eta[14] = 0.0000000000000000e+00;
rk_eta[15] = 0.0000000000000000e+00;
rk_eta[16] = 0.0000000000000000e+00;
rk_eta[17] = 0.0000000000000000e+00;
rk_eta[18] = 0.0000000000000000e+00;
rk_eta[19] = 0.0000000000000000e+00;
rk_eta[20] = 1.0000000000000000e+00;
rk_eta[21] = 0.0000000000000000e+00;
rk_eta[22] = 0.0000000000000000e+00;
rk_eta[23] = 0.0000000000000000e+00;
rk_eta[24] = 0.0000000000000000e+00;
rk_eta[25] = 0.0000000000000000e+00;
rk_eta[26] = 0.0000000000000000e+00;
rk_eta[27] = 1.0000000000000000e+00;
rk_eta[28] = 0.0000000000000000e+00;
rk_eta[29] = 0.0000000000000000e+00;
rk_eta[30] = 0.0000000000000000e+00;
rk_eta[31] = 0.0000000000000000e+00;
rk_eta[32] = 0.0000000000000000e+00;
rk_eta[33] = 0.0000000000000000e+00;
rk_eta[34] = 1.0000000000000000e+00;
rk_eta[35] = 0.0000000000000000e+00;
rk_eta[36] = 0.0000000000000000e+00;
rk_eta[37] = 0.0000000000000000e+00;
rk_eta[38] = 0.0000000000000000e+00;
rk_eta[39] = 0.0000000000000000e+00;
rk_eta[40] = 0.0000000000000000e+00;
rk_eta[41] = 1.0000000000000000e+00;
rk_eta[42] = 0.0000000000000000e+00;
rk_eta[43] = 0.0000000000000000e+00;
rk_eta[44] = 0.0000000000000000e+00;
rk_eta[45] = 0.0000000000000000e+00;
rk_eta[46] = 0.0000000000000000e+00;
rk_eta[47] = 0.0000000000000000e+00;
acadoWorkspace.rk_xxx[48] = rk_eta[48];
acadoWorkspace.rk_xxx[49] = rk_eta[49];
acadoWorkspace.rk_xxx[50] = rk_eta[50];
acadoWorkspace.rk_xxx[15] = rk_eta[15];
acadoWorkspace.rk_xxx[16] = rk_eta[16];
acadoWorkspace.rk_xxx[17] = rk_eta[17];
for (run1 = 0; run1 < 1; ++run1)
{
@@ -164,39 +86,6 @@ acadoWorkspace.rk_xxx[11] = + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
@@ -213,186 +102,54 @@ acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_k
acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[30] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[31] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[32] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[33] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[34] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[35] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[36] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[37] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[38] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[39] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[40] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[41] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[42] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[43] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[44] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[45] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[46] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[47] + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[72] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[73] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[74] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[75] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[76] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[77] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[78] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[79] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[80] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[81] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[82] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[83] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[84] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[85] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[86] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[87] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[88] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[89] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[90] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[91] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[92] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[93] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[94] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[95] + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 96 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[143] + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 144 ]) );
rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[144];
rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[145];
rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[146];
rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[147];
rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[148];
rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[149];
rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[150];
rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[151];
rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[152];
rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[153];
rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[154];
rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[155];
rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[156];
rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[157];
rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[158];
rk_eta[15] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[159];
rk_eta[16] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[160];
rk_eta[17] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[161];
rk_eta[18] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[162];
rk_eta[19] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[163];
rk_eta[20] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[164];
rk_eta[21] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[165];
rk_eta[22] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[166];
rk_eta[23] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[167];
rk_eta[24] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[168];
rk_eta[25] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[169];
rk_eta[26] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[170];
rk_eta[27] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[171];
rk_eta[28] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[172];
rk_eta[29] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[173];
rk_eta[30] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[174];
rk_eta[31] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[175];
rk_eta[32] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[176];
rk_eta[33] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[177];
rk_eta[34] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[178];
rk_eta[35] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[179];
rk_eta[36] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[180];
rk_eta[37] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[181];
rk_eta[38] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[182];
rk_eta[39] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[183];
rk_eta[40] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[184];
rk_eta[41] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[185];
rk_eta[42] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[186];
rk_eta[43] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[187];
rk_eta[44] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[188];
rk_eta[45] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[189];
rk_eta[46] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[190];
rk_eta[47] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[191];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 15 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[14];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 30 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[30] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[31] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[32] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[33] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[34] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[35] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[36] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[37] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[38] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[39] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[40] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[41] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[42] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[43] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[44] + rk_eta[14];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 45 ]) );
rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45];
rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46];
rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47];
rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[48];
rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[49];
rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[50];
rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[51];
rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[52];
rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[53];
rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[54];
rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[55];
rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[56];
rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[57];
rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[58];
rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[59];
acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
}
}
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
+1 -1
View File
@@ -40,7 +40,7 @@ int acado_solve( void )
{
acado_nWSR = QPOASES_NWSRMAX;
QProblem qp(26, 20);
QProblem qp(23, 20);
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
+1 -1
View File
@@ -37,7 +37,7 @@
*/
/** Maximum number of optimization variables. */
#define QPOASES_NVMAX 26
#define QPOASES_NVMAX 23
/** Maximum number of constraints. */
#define QPOASES_NCMAX 20
/** Maximum number of working set recalculations. */
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
+3531 -4121
View File
File diff suppressed because one or more lines are too long
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
BIN
View File
Binary file not shown.
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
BIN
View File
Binary file not shown.
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
BIN
View File
Binary file not shown.
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
BIN
View File
Binary file not shown.
selfdrive/sensord/sensord
BIN
View File
Binary file not shown.
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