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