openpilot v0.8.0 release

CONTRIBUTING.md

@@ -28,7 +28,7 @@ Code is automatically checked for style by Github Actions as part of the automat
 
 We've released a [Model Port guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for porting to Toyota/Lexus models.
 
-If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty. 
+If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty.
 
 ## Pull Requests
 

README.md

@@ -66,7 +66,7 @@ Supported Cars
 | ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
 | Acura     | ILX 2016-18                   | AcuraWatch Plus   | openpilot        | 25mph<sup>1</sup>  | 25mph             |
 | Acura     | RDX 2016-18                   | AcuraWatch Plus   | openpilot        | 25mph<sup>1</sup>  | 12mph             |
-| Acura     | RDX 2020                      | AcuraWatch        | Stock            | 0mph               | 3mph              |
+| Acura     | RDX 2020                      | All               | Stock            | 0mph               | 3mph              |
 | Honda     | Accord 2018-20                | All               | Stock            | 0mph               | 3mph              |
 | Honda     | Accord Hybrid 2018-20         | All               | Stock            | 0mph               | 3mph              |
 | Honda     | Civic Hatchback 2017-19       | Honda Sensing     | Stock            | 0mph               | 12mph             |
@@ -78,6 +78,7 @@ Supported Cars
 | Honda     | Fit 2018-19                   | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
 | Honda     | HR-V 2019                     | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
 | Honda     | Insight 2019-20               | All               | Stock            | 0mph               | 3mph              |
+| Honda     | Inspire 2018                  | All               | Stock            | 0mph               | 3mph              |
 | Honda     | Odyssey 2018-20               | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 0mph              |
 | Honda     | Passport 2019                 | All               | openpilot        | 25mph<sup>1</sup>  | 12mph             |
 | Honda     | Pilot 2016-19                 | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
@@ -85,10 +86,11 @@ Supported Cars
 | Hyundai   | Palisade 2020                 | All               | Stock            | 0mph               | 0mph         |
 | Hyundai   | Sonata 2020                   | All               | Stock            | 0mph               | 0mph              |
 | Lexus     | CT Hybrid 2017-18             | LSS               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Lexus     | ES 2019                       | All               | openpilot        | 0mph               | 0mph              |
+| Lexus     | ES 2019-20                    | All               | openpilot        | 0mph               | 0mph              |
 | Lexus     | ES Hybrid 2019                | All               | openpilot        | 0mph               | 0mph              |
 | Lexus     | IS 2017-2019                  | All               | Stock            | 22mph              | 0mph              |
 | Lexus     | IS Hybrid 2017                | All               | Stock            | 0mph               | 0mph              |
+| Lexus     | NX 2018                       | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Lexus     | NX Hybrid 2018                | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Lexus     | RX 2016-18                    | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Lexus     | RX 2020                       | All               | openpilot        | 0mph               | 0mph              |
@@ -96,11 +98,11 @@ Supported Cars
 | Lexus     | RX Hybrid 2020                | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Avalon 2016-18                | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
 | Toyota    | Camry 2018-20                 | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
-| Toyota    | Camry Hybrid 2018-19          | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
+| Toyota    | Camry Hybrid 2018-20          | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
 | Toyota    | C-HR 2017-19                  | All               | Stock            | 0mph               | 0mph              |
 | Toyota    | C-HR Hybrid 2017-19           | All               | Stock            | 0mph               | 0mph              |
 | Toyota    | Corolla 2017-19               | All               | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
-| Toyota    | Corolla 2020                  | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Corolla 2020-21               | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Corolla Hatchback 2019-20     | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Corolla Hybrid 2020-21        | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Highlander 2017-19            | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
@@ -108,11 +110,12 @@ Supported Cars
 | Toyota    | Highlander Hybrid 2017-19     | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Toyota    | Highlander Hybrid 2020        | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Prius 2016-20                 | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Toyota    | Prius 2021                    | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Prius Prime 2017-20           | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Toyota    | Rav4 2016-18                  | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
 | Toyota    | Rav4 2019-21                  | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Rav4 Hybrid 2016-18           | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Toyota    | Rav4 Hybrid 2019-20           | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Rav4 Hybrid 2019-21           | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Sienna 2018-20                | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
 
 <sup>1</sup>[Comma Pedal](https://github.com/commaai/openpilot/wiki/comma-pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).*** <br />
@@ -136,7 +139,7 @@ Community Maintained Cars and Features
 | Genesis   | G70 2018                      | All               | Stock            | 0mph               | 0mph         |
 | Genesis   | G80 2018                      | All               | Stock            | 0mph               | 0mph         |
 | Genesis   | G90 2018                      | All               | Stock            | 0mph               | 0mph         |
-| GMC       | Acadia Denali 2018<sup>2</sup>| Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
+| GMC       | Acadia 2018<sup>1</sup>       | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
 | Holden    | Astra 2017<sup>1</sup>        | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
 | Hyundai   | Elantra 2017-19               | SCC + LKAS        | Stock            | 19mph              | 34mph        |
 | Hyundai   | Genesis 2015-16               | SCC + LKAS        | Stock            | 19mph              | 37mph        |
@@ -149,6 +152,7 @@ Community Maintained Cars and Features
 | Jeep      | Grand Cherokee 2016-18        | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
 | Jeep      | Grand Cherokee 2019-20        | Adaptive Cruise   | Stock            | 0mph               | 39mph        |
 | Kia       | Forte 2018-19                 | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Niro EV 2020                  | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Kia       | Optima 2017                   | SCC + LKAS        | Stock            | 0mph               | 32mph        |
 | Kia       | Optima 2019                   | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Kia       | Sorento 2018                  | SCC + LKAS        | Stock            | 0mph               | 0mph         |
@@ -162,8 +166,7 @@ Community Maintained Cars and Features
 | Subaru    | Impreza 2017-19               | EyeSight          | Stock            | 0mph               | 0mph         |
 | Volkswagen| Golf 2015-19                  | Driver Assistance | Stock            | 0mph               | 0mph         |
 
-<sup>1</sup>Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built giraffe](https://github.com/commaai/openpilot/wiki/GM). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
+<sup>1</sup>Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
-<sup>2</sup>Requires a custom connector for the developer [car harness](https://comma.ai/shop/products/car-harness) <br />
 
 Although it's not upstream, there's a community of people getting openpilot to run on Tesla's [here](https://tinkla.us/)
 
@@ -239,7 +242,6 @@ Many factors can impact the performance of openpilot DM, causing it to be unable
 * Low light conditions, such as driving at night or in dark tunnels.
 * Bright light (due to oncoming headlights, direct sunlight, etc.).
 * The driver's face is partially or completely outside field of view of the driver facing camera.
-* Right hand driving vehicles.
 * The driver facing camera is obstructed, covered, or damaged.
 
 The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A driver should not rely on openpilot DM to assess their level of attention.
@@ -269,7 +271,7 @@ Safety and Testing
 
 Testing on PC
 ------
-For simplified development and experimentation, openpilot can be run in the CARLA driving simulator, which allows you to develop openpilot without a car. The whole setup should only take a few minutes. 
+For simplified development and experimentation, openpilot can be run in the CARLA driving simulator, which allows you to develop openpilot without a car. The whole setup should only take a few minutes.
 
 Steps:
 1) Start the CARLA server on first terminal

RELEASES.md

@@ -1,3 +1,15 @@
+Version 0.8.0 (2020-11-30)
+========================
+ * New driving model: fully 3D and improved cut-in detection
+ * UI draws 2 road edges, 4 lanelines and paths in 3D
+ * Major fixes to cut-in detection for openpilot longitudinal
+ * Grey panda is no longer supported, upgrade to comma two or black panda
+ * Lexus NX 2018 support thanks to matt12eagles!
+ * Kia Niro EV 2020 support thanks to nickn17!
+ * Toyota Prius 2021 support thanks to rav4kumar!
+ * Improved lane positioning with uncertain lanelines, wide lanes and exits
+ * Improved lateral control for Prius and Subaru
+
 Version 0.7.10 (2020-10-29)
 ========================
  * Grey panda is deprecated, upgrade to comma two or black panda

SConstruct

@@ -5,6 +5,8 @@ import shutil
 import subprocess
 import sys
 import platform
+import numpy as np
+from sysconfig import get_paths
 
 TICI = os.path.isfile('/TICI')
 Decider('MD5-timestamp')
@@ -128,6 +130,10 @@ else:
 # change pythonpath to this
 lenv["PYTHONPATH"] = Dir("#").path
 
+#Get the path for Python.h for cython linking
+python_path = get_paths()['include']
+numpy_path = np.get_include()
+
 env = Environment(
   ENV=lenv,
   CCFLAGS=[
@@ -159,6 +165,7 @@ env = Environment(
     "#phonelibs/linux/include",
     "#phonelibs/snpe/include",
     "#phonelibs/nanovg",
+    "#selfdrive/boardd",
     "#selfdrive/common",
     "#selfdrive/camerad",
     "#selfdrive/camerad/include",
@@ -181,50 +188,14 @@ env = Environment(
   CXXFLAGS=["-std=c++1z"] + cxxflags,
   LIBPATH=libpath + [
     "#cereal",
+    "#selfdrive/boardd",
     "#selfdrive/common",
     "#phonelibs",
-  ]
+  ],
+  CYTHONCFILESUFFIX=".cpp",
+  tools=["default", "cython"]
 )
 
-qt_env = None
-if arch in ["x86_64", "Darwin", "larch64"]:
-  qt_env = env.Clone()
-
-  if arch == "Darwin":
-    qt_env['QTDIR'] = "/usr/local/opt/qt"
-    QT_BASE = "/usr/local/opt/qt/"
-    qt_dirs = [
-      QT_BASE + "include/",
-      QT_BASE + "include/QtWidgets",
-      QT_BASE + "include/QtGui",
-      QT_BASE + "include/QtCore",
-      QT_BASE + "include/QtDBus",
-      QT_BASE + "include/QtMultimedia",
-    ]
-    qt_env["LINKFLAGS"] += ["-F" + QT_BASE + "lib"]
-  else:
-    qt_env['QTDIR'] = "/usr"
-    qt_dirs = [
-      f"/usr/include/{real_arch}-linux-gnu/qt5",
-      f"/usr/include/{real_arch}-linux-gnu/qt5/QtWidgets",
-      f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui",
-      f"/usr/include/{real_arch}-linux-gnu/qt5/QtCore",
-      f"/usr/include/{real_arch}-linux-gnu/qt5/QtDBus",
-      f"/usr/include/{real_arch}-linux-gnu/qt5/QtMultimedia",
-      f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.5.1/QtGui",
-    ]
-
-  qt_env.Tool('qt')
-  qt_env['CPPPATH'] += qt_dirs
-  qt_flags = [
-    "-D_REENTRANT",
-    "-DQT_NO_DEBUG",
-    "-DQT_WIDGETS_LIB",
-    "-DQT_GUI_LIB",
-    "-DQT_CORE_LIB"
-  ]
-  qt_env['CXXFLAGS'] += qt_flags
-
 if os.environ.get('SCONS_CACHE'):
   cache_dir = '/tmp/scons_cache'
 
@@ -238,6 +209,9 @@ if os.environ.get('SCONS_CACHE'):
       if not os.path.isdir(cache_dir_branch) and os.path.isdir(cache_dir):
         shutil.copytree(cache_dir, cache_dir_branch)
       cache_dir = cache_dir_branch
+  elif TICI:
+    cache_dir = '/data/scons_cache'
+
   CacheDir(cache_dir)
 
 node_interval = 5
@@ -261,9 +235,28 @@ def abspath(x):
     # rpath works elsewhere
     return x[0].path.rsplit("/", 1)[1][:-3]
 
+#Cython build enviroment
+envCython = env.Clone()
+envCython["CPPPATH"] += [python_path, numpy_path]
+envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-deprecated-declarations"]
+
+python_libs = []
+if arch == "Darwin":
+  envCython["LINKFLAGS"]=["-bundle", "-undefined", "dynamic_lookup"]
+elif arch == "aarch64":
+  envCython["LINKFLAGS"]=["-shared"]
+
+  python_libs.append(os.path.basename(python_path))
+else:
+  envCython["LINKFLAGS"]=["-pthread", "-shared"]
+
+envCython["LIBS"] = python_libs
+
+Export('envCython')
+
 # still needed for apks
 zmq = 'zmq'
-Export('env', 'qt_env', 'arch', 'zmq', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY')
+Export('env', 'arch', 'real_arch', 'zmq', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY')
 
 # cereal and messaging are shared with the system
 SConscript(['cereal/SConscript'])
@@ -316,6 +309,5 @@ SConscript(['selfdrive/ui/SConscript'])
 if arch != "Darwin":
   SConscript(['selfdrive/logcatd/SConscript'])
 
-
 if arch == "x86_64":
   SConscript(['tools/lib/index_log/SConscript'])

cereal/SConscript

@@ -2,6 +2,7 @@ Import('env', 'arch', 'zmq', 'cython_dependencies')
 
 import shutil
 
+cereal_dir = Dir('.')
 gen_dir = Dir('gen')
 messaging_dir = Dir('messaging')
 
@@ -9,12 +10,12 @@ messaging_dir = Dir('messaging')
 env.Command(["gen/c/include/c++.capnp.h", "gen/c/include/java.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS")
 env.Command(['gen/cpp/car.capnp.c++', 'gen/cpp/log.capnp.c++', 'gen/cpp/car.capnp.h', 'gen/cpp/log.capnp.h'],
             ['car.capnp', 'log.capnp'],
-            'capnpc $SOURCES --src-prefix=cereal -o c++:' + gen_dir.path + '/cpp/')
+            f"capnpc --src-prefix={cereal_dir.path} $SOURCES -o c++:{gen_dir.path}/cpp/")
 
 if shutil.which('capnpc-java'):
   env.Command(['gen/java/Car.java', 'gen/java/Log.java'],
               ['car.capnp', 'log.capnp'],
-              'capnpc $SOURCES --src-prefix=cereal -o java:' + gen_dir.path + '/java/')
+              f"capnpc $SOURCES --src-prefix={cereal_dir.path} -o java:{gen_dir.path}/java/")
 
 # TODO: remove non shared cereal and messaging
 cereal_objects = env.SharedObject([

cereal/car.capnp

@@ -73,7 +73,6 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     preLaneChangeLeft @57;
     preLaneChangeRight @58;
     laneChange @59;
-    invalidGiraffeToyota @60;
     internetConnectivityNeeded @61;
     communityFeatureDisallowed @62;
     lowMemory @63;
@@ -94,14 +93,13 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     startupMaster @78;
     fcw @79;
     steerSaturated @80;
-    whitePandaUnsupported @81;
-    startupGreyPanda @82;
     belowEngageSpeed @84;
     noGps @85;
     wrongCruiseMode @87;
     modeldLagging @89;
     deviceFalling @90;
     fanMalfunction @91;
+    cameraMalfunction @92;
 
     gasUnavailableDEPRECATED @3;
     dataNeededDEPRECATED @16;
@@ -112,9 +110,13 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     driverMonitorOffDEPRECATED @42;
     calibrationProgressDEPRECATED @47;
     invalidGiraffeHondaDEPRECATED @49;
+    invalidGiraffeToyotaDEPRECATED @60;
+    whitePandaUnsupportedDEPRECATED @81;
+    startupGreyPandaDEPRECATED @82;
     canErrorPersistentDEPRECATED @83;
     focusRecoverActiveDEPRECATED @86;
     neosUpdateRequiredDEPRECATED @88;
+    modelLagWarningDEPRECATED @93;
   }
 }
 

cereal/log.capnp

@@ -628,6 +628,8 @@ struct ModelData {
   frameAge @12 :UInt32;
   frameDropPerc @13 :Float32;
   timestampEof @9 :UInt64;
+  modelExecutionTime @14 :Float32;
+  rawPred @15 :Data;
 
   path @1 :PathData;
   leftLane @2 :PathData;
@@ -694,6 +696,8 @@ struct ModelDataV2 {
   frameAge @1 :UInt32;
   frameDropPerc @2 :Float32;
   timestampEof @3 :UInt64;
+  modelExecutionTime @15 :Float32;
+  rawPred @16 :Data;
 
   position @4 :XYZTData;
   orientation @5 :XYZTData;
@@ -701,7 +705,9 @@ struct ModelDataV2 {
   orientationRate @7 :XYZTData;
   laneLines @8 :List(XYZTData);
   laneLineProbs @9 :List(Float32);
+  laneLineStds @13 :List(Float32);
   roadEdges @10 :List(XYZTData);
+  roadEdgeStds @14 :List(Float32);
   leads @11 :List(LeadDataV2);
 
   meta @12 :MetaData;
@@ -754,6 +760,8 @@ struct EncodeIndex {
   segmentId @4 :UInt32;
   # index into camera file in segment in encode order
   segmentIdEncode @5 :UInt32;
+  timestampSof @6 :UInt64;
+  timestampEof @7 :UInt64;
 
   enum Type {
     bigBoxLossless @0;   # rcamera.mkv
@@ -1943,6 +1951,9 @@ struct OrbKeyFrame {
 
 struct DriverState {
   frameId @0 :UInt32;
+  modelExecutionTime @14 :Float32;
+  rawPred @15 :Data;
+
   descriptorDEPRECATED @1 :List(Float32);
   stdDEPRECATED @2 :Float32;
   faceOrientation @3 :List(Float32);
@@ -2119,11 +2130,13 @@ struct Event {
     thumbnail @66: Thumbnail;
     carEvents @68: List(Car.CarEvent);
     carParams @69: Car.CarParams;
-    frontFrame @70: FrameData;
+    frontFrame @70: FrameData; # driver facing camera
     dMonitoringState @71: DMonitoringState;
     liveLocationKalman @72 :LiveLocationKalman;
     sentinel @73 :Sentinel;
     wideFrame @74: FrameData;
     modelV2 @75 :ModelDataV2;
+    frontEncodeIdx @76 :EncodeIndex; # driver facing camera
+    wideEncodeIdx @77 :EncodeIndex;
   }
 }

common/SConscript

@@ -1,14 +1,4 @@
-Import('env', 'cython_dependencies')
+Import('envCython')
 
-# Build cython clock module
+envCython.Program('clock.so', 'clock.pyx')
-env.Command(['common_pyx.so', 'clock.cpp'],
+envCython.Program('params_pyx.so', 'params_pyx.pyx')
-            cython_dependencies + ['common_pyx_setup.py', 'clock.pyx'],
-            "cd common && python3 common_pyx_setup.py build_ext --inplace")
-
-# Build cython params module
-env.Command(['params_pyx.so', 'params_pyx.cpp'],
-            cython_dependencies + [
-                'params_pyx_setup.py', 'params_pyx.pyx', 'params_pxd.pxd',
-                '#selfdrive/common/params.cc', '#selfdrive/common/params.h',
-                '#selfdrive/common/util.c', '#selfdrive/common/util.h'],
-            "cd common && python3 params_pyx_setup.py build_ext --inplace")

common/clock.pyx

@@ -1,3 +1,4 @@
+# distutils: language = c++
 # cython: language_level = 3
 from posix.time cimport clock_gettime, timespec, CLOCK_MONOTONIC_RAW, clockid_t
 

common/common_pyx_setup.py

@@ -1,20 +0,0 @@
-from distutils.core import Extension, setup  # pylint: disable=import-error,no-name-in-module
-from Cython.Build import cythonize
-
-from common.cython_hacks import BuildExtWithoutPlatformSuffix
-
-sourcefiles = ['clock.pyx']
-extra_compile_args = ["-std=c++1z"]
-
-setup(name='common',
-      cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
-      ext_modules=cythonize(
-        Extension(
-          "common_pyx",
-          language="c++",
-          sources=sourcefiles,
-          extra_compile_args=extra_compile_args,
-        ),
-        nthreads=4,
-      ),
-)

common/kalman/SConscript

@@ -1,6 +1,3 @@
-Import('env', 'cython_dependencies')
+Import('envCython')
-
-env.Command(['simple_kalman_impl.so'],
-            cython_dependencies + ['simple_kalman_impl.pyx', 'simple_kalman_impl.pxd', 'simple_kalman_setup.py'],
-            "cd common/kalman && python3 simple_kalman_setup.py build_ext --inplace")
 
+envCython.Program('simple_kalman_impl.so', 'simple_kalman_impl.pyx')

common/kalman/simple_kalman_impl.pyx

@@ -1,3 +1,4 @@
+# distutils: language = c++
 # cython: language_level=3
 
 cdef class KF1D:

common/kalman/simple_kalman_setup.py

@@ -1,10 +0,0 @@
-from distutils.core import Extension, setup
-
-from Cython.Build import cythonize
-
-from common.cython_hacks import BuildExtWithoutPlatformSuffix
-
-setup(name='Simple Kalman Implementation',
-      cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
-      ext_modules=cythonize(Extension("simple_kalman_impl",
-                                      ["simple_kalman_impl.pyx"])))

common/kalman/tests/test_simple_kalman.py

@@ -82,3 +82,6 @@ kf = KF1D(x0=[[x0_0], [x1_0]],
     kf_speed = timeit.timeit("kf.update(1234)", setup=setup, number=10000)
     kf_old_speed = timeit.timeit("kf_old.update(1234)", setup=setup, number=10000)
     self.assertTrue(kf_speed < kf_old_speed / 4)
+
+if __name__ == "__main__":
+  unittest.main()

common/params_pyx.pyx

@@ -2,7 +2,7 @@
 # cython: language_level = 3
 from libcpp cimport bool
 from libcpp.string cimport string
-from params_pxd cimport Params as c_Params
+from common.params_pxd cimport Params as c_Params
 
 import os
 import threading
@@ -69,6 +69,7 @@ keys = {
   b"Offroad_IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START],
   b"Offroad_NeosUpdate": [TxType.CLEAR_ON_MANAGER_START],
   b"Offroad_UpdateFailed": [TxType.CLEAR_ON_MANAGER_START],
+  b"Offroad_HardwareUnsupported": [TxType.CLEAR_ON_MANAGER_START],
 }
 
 def ensure_bytes(v):

common/params_pyx_setup.py

@@ -1,33 +0,0 @@
-import os
-import subprocess
-from distutils.core import Extension, setup
-from Cython.Build import cythonize
-
-from common.cython_hacks import BuildExtWithoutPlatformSuffix
-from common.basedir import BASEDIR
-from common.hardware import TICI
-
-ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()  # pylint: disable=unexpected-keyword-arg
-
-sourcefiles = ['params_pyx.pyx']
-extra_compile_args = ["-std=c++11"]
-
-if ARCH == "aarch64":
-  if TICI:
-    extra_compile_args += ["-DQCOM2"]
-  else:
-    extra_compile_args += ["-DQCOM"]
-
-
-setup(name='common',
-      cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
-      ext_modules=cythonize(
-        Extension(
-          "params_pyx",
-          language="c++",
-          sources=sourcefiles,
-          include_dirs=[BASEDIR, os.path.join(BASEDIR, 'selfdrive')],
-          extra_compile_args=extra_compile_args
-        )
-      )
-)

common/realtime.py

@@ -5,7 +5,7 @@ import time
 import multiprocessing
 
 from common.hardware import PC
-from common.common_pyx import sec_since_boot  # pylint: disable=no-name-in-module, import-error
+from common.clock import sec_since_boot  # pylint: disable=no-name-in-module, import-error
 
 
 # time step for each process

common/spinner.py

@@ -4,17 +4,14 @@ from common.basedir import BASEDIR
 
 
 class Spinner():
-  def __init__(self, noop=False):
+  def __init__(self):
-    # spinner is currently only implemented for android
+    try:
-    self.spinner_proc = None
+      self.spinner_proc = subprocess.Popen(["./spinner"],
-    if not noop:
+                                           stdin=subprocess.PIPE,
-      try:
+                                           cwd=os.path.join(BASEDIR, "selfdrive", "ui"),
-        self.spinner_proc = subprocess.Popen(["./spinner"],
+                                           close_fds=True)
-                                             stdin=subprocess.PIPE,
+    except OSError:
-                                             cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
+      self.spinner_proc = None
-                                             close_fds=True)
-      except OSError:
-        self.spinner_proc = None
 
   def __enter__(self):
     return self

common/text_window.py

@@ -6,17 +6,14 @@ from common.basedir import BASEDIR
 
 
 class TextWindow:
-  def __init__(self, s, noop=False):
+  def __init__(self, text):
-    # text window is only implemented for android currently
+    try:
-    self.text_proc = None
+      self.text_proc = subprocess.Popen(["./text", text],
-    if not noop:
+                                        stdin=subprocess.PIPE,
-      try:
+                                        cwd=os.path.join(BASEDIR, "selfdrive", "ui"),
-        self.text_proc = subprocess.Popen(["./text", s],
+                                        close_fds=True)
-                                          stdin=subprocess.PIPE,
+    except OSError:
-                                          cwd=os.path.join(BASEDIR, "selfdrive", "ui", "text"),
+      self.text_proc = None
-                                          close_fds=True)
-      except OSError:
-        self.text_proc = None
 
   def get_status(self):
     if self.text_proc is not None:

common/transformations/SConscript

@@ -1,8 +1,3 @@
-Import('env', 'cython_dependencies')
+Import('envCython')
 
-d = Dir('.')
+envCython.Program('transformations.so', 'transformations.pyx')
-
-env.Command(['transformations.so'],
-            cython_dependencies + ['transformations.pxd', 'transformations.pyx',
-             'coordinates.cc', 'orientation.cc', 'coordinates.hpp', 'orientation.hpp'],
-            'cd ' + d.path + ' && python3 setup.py build_ext --inplace')

common/transformations/camera.py

@@ -1,29 +1,67 @@
 import numpy as np
-import common.transformations.orientation as orient
 
-FULL_FRAME_SIZE = (1164, 874)
+import common.transformations.orientation as orient
-W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
+from common.hardware import TICI
-eon_focal_length = FOCAL = 910.0
+
+## -- hardcoded hardware params --
+eon_f_focal_length = 910.0
+eon_d_focal_length = 860.0
+leon_d_focal_length = 650.0
+tici_f_focal_length = 2648.0
+tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame
+
+eon_f_frame_size = (1164, 874)
+eon_d_frame_size = (1152, 864)
+leon_d_frame_size = (816, 612)
+tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208)
 
 # aka 'K' aka camera_frame_from_view_frame
-eon_intrinsics = np.array([
+eon_fcam_intrinsics = np.array([
-  [FOCAL,   0.,   W/2.],
+  [eon_f_focal_length,  0.0,  float(eon_f_frame_size[0])/2],
-  [  0.,  FOCAL,  H/2.],
+  [0.0,  eon_f_focal_length,  float(eon_f_frame_size[1])/2],
-  [  0.,    0.,     1.]])
+  [0.0,  0.0,                                          1.0]])
-
+eon_intrinsics = eon_fcam_intrinsics # xx
 
 leon_dcam_intrinsics = np.array([
-  [650,   0,   816//2],
+  [leon_d_focal_length,  0.0,  float(leon_d_frame_size[0])/2],
-  [  0,  650,  612//2],
+  [0.0,  leon_d_focal_length,  float(leon_d_frame_size[1])/2],
-  [  0,    0,     1]])
+  [0.0,  0.0,                                            1.0]])
 
 eon_dcam_intrinsics = np.array([
-  [860,   0,   1152//2],
+  [eon_d_focal_length,  0.0,  float(eon_d_frame_size[0])/2],
-  [  0,  860,  864//2],
+  [0.0,  eon_d_focal_length,  float(eon_d_frame_size[1])/2],
-  [  0,    0,     1]])
+  [0.0,  0.0,                                          1.0]])
+
+tici_fcam_intrinsics = np.array([
+  [tici_f_focal_length,  0.0,  float(tici_f_frame_size[0])/2],
+  [0.0,  tici_f_focal_length,  float(tici_f_frame_size[1])/2],
+  [0.0,  0.0,                                            1.0]])
+
+tici_dcam_intrinsics = np.array([
+  [tici_d_focal_length,  0.0,  float(tici_d_frame_size[0])/2],
+  [0.0,  tici_d_focal_length,  float(tici_d_frame_size[1])/2],
+  [0.0,  0.0,                                            1.0]])
+
+tici_ecam_intrinsics = tici_dcam_intrinsics
 
 # aka 'K_inv' aka view_frame_from_camera_frame
-eon_intrinsics_inv = np.linalg.inv(eon_intrinsics)
+eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics)
+eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx
+
+tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics)
+tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics)
+
+
+if not TICI:
+  FULL_FRAME_SIZE = eon_f_frame_size
+  FOCAL = eon_f_focal_length
+  fcam_intrinsics = eon_fcam_intrinsics
+else:
+  FULL_FRAME_SIZE = tici_f_frame_size
+  FOCAL = tici_f_focal_length
+  fcam_intrinsics = tici_fcam_intrinsics
+
+W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
 
 
 # device/mesh : x->forward, y-> right, z->down
@@ -69,9 +107,9 @@ def vp_from_ke(m):
   return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0])
 
 
-def vp_from_rpy(rpy):
+def vp_from_rpy(rpy, intrinsics=fcam_intrinsics):
   e = get_view_frame_from_road_frame(rpy[0], rpy[1], rpy[2], 1.22)
-  ke = np.dot(eon_intrinsics, e)
+  ke = np.dot(intrinsics, e)
   return vp_from_ke(ke)
 
 
@@ -81,7 +119,7 @@ def roll_from_ke(m):
                     -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
 
 
-def normalize(img_pts, intrinsics=eon_intrinsics):
+def normalize(img_pts, intrinsics=fcam_intrinsics):
   # normalizes image coordinates
   # accepts single pt or array of pts
   intrinsics_inv = np.linalg.inv(intrinsics)
@@ -94,7 +132,7 @@ def normalize(img_pts, intrinsics=eon_intrinsics):
   return img_pts_normalized[:, :2].reshape(input_shape)
 
 
-def denormalize(img_pts, intrinsics=eon_intrinsics):
+def denormalize(img_pts, intrinsics=fcam_intrinsics, width=W, height=H):
   # denormalizes image coordinates
   # accepts single pt or array of pts
   img_pts = np.array(img_pts)
@@ -102,9 +140,9 @@ def denormalize(img_pts, intrinsics=eon_intrinsics):
   img_pts = np.atleast_2d(img_pts)
   img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1))))
   img_pts_denormalized = img_pts.dot(intrinsics.T)
-  img_pts_denormalized[img_pts_denormalized[:, 0] > W] = np.nan
+  img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan
   img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan
-  img_pts_denormalized[img_pts_denormalized[:, 1] > H] = np.nan
+  img_pts_denormalized[img_pts_denormalized[:, 1] > height] = np.nan
   img_pts_denormalized[img_pts_denormalized[:, 1] < 0] = np.nan
   return img_pts_denormalized[:, :2].reshape(input_shape)
 
@@ -137,18 +175,10 @@ def img_from_device(pt_device):
   return pt_img.reshape(input_shape)[:, :2]
 
 
-def get_camera_frame_from_calib_frame(camera_frame_from_road_frame):
+def get_camera_frame_from_calib_frame(camera_frame_from_road_frame, intrinsics=fcam_intrinsics):
   camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
-  calib_frame_from_ground = np.dot(eon_intrinsics,
+  calib_frame_from_ground = np.dot(intrinsics,
                                      get_view_frame_from_road_frame(0, 0, 0, 1.22))[:, (0, 1, 3)]
   ground_from_calib_frame = np.linalg.inv(calib_frame_from_ground)
   camera_frame_from_calib_frame = np.dot(camera_frame_from_ground, ground_from_calib_frame)
   return camera_frame_from_calib_frame
-
-
-def pretransform_from_calib(calib):
-  roll, pitch, yaw, height = calib
-  view_frame_from_road_frame = get_view_frame_from_road_frame(roll, pitch, yaw, height)
-  camera_frame_from_road_frame = np.dot(eon_intrinsics, view_frame_from_road_frame)
-  camera_frame_from_calib_frame = get_camera_frame_from_calib_frame(camera_frame_from_road_frame)
-  return np.linalg.inv(camera_frame_from_calib_frame)

common/transformations/model.py

@@ -1,34 +1,33 @@
 import numpy as np
 
-from common.transformations.camera import (FULL_FRAME_SIZE, eon_focal_length,
+from common.transformations.camera import (FULL_FRAME_SIZE,
+                                           FOCAL,
                                            get_view_frame_from_road_frame,
                                            get_view_frame_from_calib_frame,
                                            vp_from_ke)
 
 # segnet
-
 SEGNET_SIZE = (512, 384)
 
-segnet_frame_from_camera_frame = np.array([
+def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=FULL_FRAME_SIZE):
-  [float(SEGNET_SIZE[0])/FULL_FRAME_SIZE[0],    0.,          ],
+  return np.array([[float(segnet_size[0]) / full_frame_size[0],  0.0],
-  [     0.,          float(SEGNET_SIZE[1])/FULL_FRAME_SIZE[1]]])
+                   [0.0,  float(segnet_size[1]) / full_frame_size[1]]])
-
+segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame() # xx
 
 # model
-
 MODEL_INPUT_SIZE = (320, 160)
 MODEL_YUV_SIZE = (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1] * 3 // 2)
-MODEL_CX = MODEL_INPUT_SIZE[0]/2.
+MODEL_CX = MODEL_INPUT_SIZE[0] / 2.
 MODEL_CY = 21.
 
-model_zoom = 1.25
+model_fl = 728.0
 model_height = 1.22
 
 # canonical model transform
-model_intrinsics = np.array(
+model_intrinsics = np.array([
-  [[ eon_focal_length / model_zoom,    0. ,  MODEL_CX],
+  [model_fl,  0.0,  MODEL_CX],
-   [   0. ,  eon_focal_length / model_zoom,  MODEL_CY],
+  [0.0,  model_fl,  MODEL_CY],
-   [   0. ,                            0. ,   1.]])
+  [0.0,  0.0,            1.0]])
 
 
 # MED model
@@ -36,34 +35,45 @@ MEDMODEL_INPUT_SIZE = (512, 256)
 MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
 MEDMODEL_CY = 47.6
 
-medmodel_zoom = 1.
+medmodel_fl = 910.0
-medmodel_intrinsics = np.array(
+medmodel_intrinsics = np.array([
-  [[ eon_focal_length / medmodel_zoom,    0. ,  0.5 * MEDMODEL_INPUT_SIZE[0]],
+  [medmodel_fl,  0.0,  0.5 * MEDMODEL_INPUT_SIZE[0]],
-   [   0. ,  eon_focal_length / medmodel_zoom,  MEDMODEL_CY],
+  [0.0,  medmodel_fl,                   MEDMODEL_CY],
-   [   0. ,                            0. ,   1.]])
+  [0.0,  0.0,                                   1.0]])
+
 
 # CAL model
 CALMODEL_INPUT_SIZE = (512, 256)
 CALMODEL_YUV_SIZE = (CALMODEL_INPUT_SIZE[0], CALMODEL_INPUT_SIZE[1] * 3 // 2)
 CALMODEL_CY = 47.6
 
-calmodel_zoom = 1.5
+calmodel_fl = 606.7
-calmodel_intrinsics = np.array(
+calmodel_intrinsics = np.array([
-  [[ eon_focal_length / calmodel_zoom,    0. ,  0.5 * CALMODEL_INPUT_SIZE[0]],
+  [calmodel_fl,  0.0,  0.5 * CALMODEL_INPUT_SIZE[0]],
-   [   0. ,  eon_focal_length / calmodel_zoom,  CALMODEL_CY],
+  [0.0,  calmodel_fl,                   CALMODEL_CY],
-   [   0. ,                            0. ,   1.]])
+  [0.0,  0.0,                                   1.0]])
 
 
 # BIG model
-
 BIGMODEL_INPUT_SIZE = (1024, 512)
 BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2)
 
-bigmodel_zoom = 1.
+bigmodel_fl = 910.0
-bigmodel_intrinsics = np.array(
+bigmodel_intrinsics = np.array([
-  [[ eon_focal_length / bigmodel_zoom,    0. , 0.5 * BIGMODEL_INPUT_SIZE[0]],
+  [bigmodel_fl,  0.0,  0.5 * BIGMODEL_INPUT_SIZE[0]],
-   [   0. ,  eon_focal_length / bigmodel_zoom,  256+MEDMODEL_CY],
+  [0.0,  bigmodel_fl,             256 + MEDMODEL_CY],
-   [   0. ,                            0. ,   1.]])
+  [0.0,  0.0,                                   1.0]])
+
+
+# SBIG model (big model with the size of small model)
+SBIGMODEL_INPUT_SIZE = (512, 256)
+SBIGMODEL_YUV_SIZE = (SBIGMODEL_INPUT_SIZE[0], SBIGMODEL_INPUT_SIZE[1] * 3 // 2)
+
+sbigmodel_fl = 455.0
+sbigmodel_intrinsics = np.array([
+  [sbigmodel_fl,  0.0,  0.5 * SBIGMODEL_INPUT_SIZE[0]],
+  [0.0,  sbigmodel_fl,      0.5 * (256 + MEDMODEL_CY)],
+  [0.0,  0.0,                                     1.0]])
 
 model_frame_from_road_frame = np.dot(model_intrinsics,
   get_view_frame_from_road_frame(0, 0, 0, model_height))
@@ -80,20 +90,21 @@ medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
 model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics))
 medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
 
+
 # 'camera from model camera'
 def get_model_height_transform(camera_frame_from_road_frame, height):
   camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([
-      [1, 0, 0],
+    [1, 0, 0],
-      [0, 1, 0],
+    [0, 1, 0],
-      [0, 0, 0],
+    [0, 0, 0],
-      [0, 0, 1],
+    [0, 0, 1],
   ]))
 
   camera_frame_from_road_high = np.dot(camera_frame_from_road_frame, np.array([
-      [1, 0, 0],
+    [1, 0, 0],
-      [0, 1, 0],
+    [0, 1, 0],
-      [0, 0, height - model_height],
+    [0, 0, height - model_height],
-      [0, 0, 1],
+    [0, 0, 1],
   ]))
 
   road_high_from_camera_frame = np.linalg.inv(camera_frame_from_road_high)
@@ -104,13 +115,14 @@ def get_model_height_transform(camera_frame_from_road_frame, height):
 
 # camera_frame_from_model_frame aka 'warp matrix'
 # was: calibration.h/CalibrationTransform
-def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height):
+def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height, camera_fl=FOCAL):
   vp = vp_from_ke(camera_frame_from_road_frame)
 
+  model_zoom = camera_fl / model_fl
   model_camera_from_model_frame = np.array([
-    [model_zoom,         0., vp[0] - MODEL_CX * model_zoom],
+    [model_zoom,  0.0,  vp[0] - MODEL_CX * model_zoom],
-    [        0., model_zoom, vp[1] - MODEL_CY * model_zoom],
+    [0.0,  model_zoom,  vp[1] - MODEL_CY * model_zoom],
-    [        0.,         0.,                            1.],
+    [0.0,  0.0,                                   1.0],
   ])
 
   # This function is super slow, so skip it if height is very close to canonical

common/transformations/setup.py

@@ -1,20 +0,0 @@
-import numpy
-
-from Cython.Build import cythonize
-from distutils.core import Extension, setup  # pylint: disable=import-error,no-name-in-module
-from common.cython_hacks import BuildExtWithoutPlatformSuffix
-
-setup(
-  name='Cython transformations wrapper',
-  cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
-  ext_modules=cythonize(
-    Extension(
-      "transformations",
-      sources=["transformations.pyx"],
-      language="c++",
-      extra_compile_args=["-std=c++1z", "-Wno-cpp"],
-      include_dirs=[numpy.get_include()],
-    ),
-    nthreads=4,
-  )
-)

common/transformations/transformations.pyx

@@ -1,18 +1,20 @@
-from transformations cimport Matrix3, Vector3, Quaternion
+# distutils: language = c++
-from transformations cimport ECEF, NED, Geodetic
+# cython: language_level = 3
+from common.transformations.transformations cimport Matrix3, Vector3, Quaternion
+from common.transformations.transformations cimport ECEF, NED, Geodetic
 
-from transformations cimport euler2quat as euler2quat_c
+from common.transformations.transformations cimport euler2quat as euler2quat_c
-from transformations cimport quat2euler as quat2euler_c
+from common.transformations.transformations cimport quat2euler as quat2euler_c
-from transformations cimport quat2rot as quat2rot_c
+from common.transformations.transformations cimport quat2rot as quat2rot_c
-from transformations cimport rot2quat as rot2quat_c
+from common.transformations.transformations cimport rot2quat as rot2quat_c
-from transformations cimport euler2rot as euler2rot_c
+from common.transformations.transformations cimport euler2rot as euler2rot_c
-from transformations cimport rot2euler as rot2euler_c
+from common.transformations.transformations cimport rot2euler as rot2euler_c
-from transformations cimport rot_matrix as rot_matrix_c
+from common.transformations.transformations cimport rot_matrix as rot_matrix_c
-from transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
+from common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
-from transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
+from common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
-from transformations cimport geodetic2ecef as geodetic2ecef_c
+from common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c
-from transformations cimport ecef2geodetic as ecef2geodetic_c
+from common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c
-from transformations cimport LocalCoord_c
+from common.transformations.transformations cimport LocalCoord_c
 
 
 import cython

launch_chffrplus.sh

@@ -8,6 +8,11 @@ source "$BASEDIR/launch_env.sh"
 
 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
 
+function tici_init {
+  sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor'
+  sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu4/governor'
+}
+
 function two_init {
   # Restrict Android and other system processes to the first two cores
   echo 0-1 > /dev/cpuset/background/cpus
@@ -19,15 +24,32 @@ function two_init {
   # openpilot gets all the cores
   echo 0-3 > /dev/cpuset/app/cpus
 
+  # set up governors
+  # +50mW offroad, +500mW onroad for 30% more RAM bandwidth
+  echo "performance" > /sys/class/devfreq/soc:qcom,cpubw/governor
+  echo 1056000 > /sys/class/devfreq/soc:qcom,m4m/max_freq
+  echo "performance" > /sys/class/devfreq/soc:qcom,m4m/governor
+
+  # unclear if these help, but they don't seem to hurt
+  echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor
+  echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu2/governor
+
+  # GPU
+  echo "performance" > /sys/class/devfreq/b00000.qcom,kgsl-3d0/governor
+
+  # /sys/class/devfreq/soc:qcom,mincpubw is the only one left at "powersave"
+  # it seems to gain nothing but a wasted 500mW
+
   # Collect RIL and other possibly long-running I/O interrupts onto CPU 1
   echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio)
   echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage)
   echo 1 > /proc/irq/35/smp_affinity_list # wifi (wlan_pci)
+  echo 1 > /proc/irq/6/smp_affinity_list  # MDSS
+
   # USB traffic needs realtime handling on cpu 3
   [ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list # USB for LeEco
   [ -d "/proc/irq/736" ] && echo 3 > /proc/irq/736/smp_affinity_list # USB for OP3T
 
-
   # Check for NEOS update
   if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then
     if [ -f "$DIR/scripts/continue.sh" ]; then
@@ -109,6 +131,10 @@ function launch {
     two_init
   fi
 
+  if [ -f /TICI ]; then
+    tici_init
+  fi
+
   # handle pythonpath
   ln -sfn $(pwd) /data/pythonpath
   export PYTHONPATH="$PWD"

opendbc/can/common.cc

@@ -97,18 +97,17 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
   // a magic variable padding byte tacked onto the end of the payload.
   // https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf
 
-  uint8_t *dat = (uint8_t *)&d;
   uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR
 
   // CRC the payload first, skipping over the first byte where the CRC lives.
   for (int i = 1; i < l; i++) {
-    crc ^= dat[i];
+    crc ^= (d >> (i*8)) & 0xFF;
     crc = crc8_lut_8h2f[crc];
   }
 
   // Look up and apply the magic final CRC padding byte, which permutes by CAN
   // address, and additionally (for SOME addresses) by the message counter.
-  uint8_t counter = dat[1] & 0x0F;
+  uint8_t counter = ((d >> 8) & 0xFF) & 0x0F;
   switch(address) {
     case 0x86:  // LWI_01 Steering Angle
       crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter];
@@ -176,11 +175,9 @@ unsigned int pedal_checksum(uint64_t d, int l) {
   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];
+    crc ^= (d >> (i*8)) & 0xFF;
     for (j = 0; j < 8; j++) {
       if ((crc & 0x80) != 0) {
         crc = (uint8_t)((crc << 1) ^ poly);

opendbc/lexus_nx300_2018_pt_generated.dbc

@@ -0,0 +1,404 @@
+CM_ "AUTOGENERATED FILE, DO NOT EDIT"
+
+
+CM_ "Imported file _comma.dbc starts here"
+BO_ 359 STEERING_IPAS_COMMA: 8 IPAS
+ SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX
+ SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX
+ SG_ SET_ME_X10 : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX
+ SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+
+CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant";
+
+ BO_ 512 GAS_COMMAND: 6 EON
+  SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR
+  SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR
+  SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR
+  SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR
+  SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" INTERCEPTOR
+
+ BO_ 513 GAS_SENSOR: 6 INTERCEPTOR
+  SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON
+  SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON
+  SG_ STATE : 39|4@0+ (1,0) [0|15] "" EON
+  SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" EON
+  SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" EON
+
+ VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ;
+
+
+CM_ "Imported file _toyota_2017.dbc starts here"
+VERSION ""
+
+
+NS_ :
+    NS_DESC_
+    CM_
+    BA_DEF_
+    BA_
+    VAL_
+    CAT_DEF_
+    CAT_
+    FILTER
+    BA_DEF_DEF_
+    EV_DATA_
+    ENVVAR_DATA_
+    SGTYPE_
+    SGTYPE_VAL_
+    BA_DEF_SGTYPE_
+    BA_SGTYPE_
+    SIG_TYPE_REF_
+    VAL_TABLE_
+    SIG_GROUP_
+    SIG_VALTYPE_
+    SIGTYPE_VALTYPE_
+    BO_TX_BU_
+    BA_DEF_REL_
+    BA_REL_
+    BA_DEF_DEF_REL_
+    BU_SG_REL_
+    BU_EV_REL_
+    BU_BO_REL_
+    SG_MUL_VAL_
+
+BS_:
+
+BU_: XXX DSU HCU EPS IPAS CGW
+
+BO_ 36 KINEMATICS: 8 XXX
+ SG_ ACCEL_Y : 33|10@0+ (0.03589,-18.375) [0|65535] "m/s^2" XXX
+ SG_ YAW_RATE : 1|10@0+ (0.244,-125) [0|65535] "deg/sec" XXX
+ SG_ STEERING_TORQUE : 17|10@0+ (1,-512) [0|65535] "" XXX
+
+BO_ 37 STEER_ANGLE_SENSOR: 8 XXX
+ SG_ STEER_ANGLE : 3|12@0- (1.5,0) [-500|500] "deg" XXX
+ SG_ STEER_FRACTION : 39|4@0- (0.1,0) [-0.7|0.7] "deg" XXX
+ SG_ STEER_RATE : 35|12@0- (1,0) [-2000|2000] "deg/s" XXX
+
+BO_ 166 BRAKE: 8 XXX
+ SG_ BRAKE_AMOUNT : 7|8@0+ (1,0) [0|255] "" XXX
+ SG_ BRAKE_PEDAL : 23|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 170 WHEEL_SPEEDS: 8 XXX
+ SG_ WHEEL_SPEED_FR : 7|16@0+ (0.01,-67.67) [0|250] "kph" XXX
+ SG_ WHEEL_SPEED_FL : 23|16@0+ (0.01,-67.67) [0|250] "kph" XXX
+ SG_ WHEEL_SPEED_RR : 39|16@0+ (0.01,-67.67) [0|250] "kph" XXX
+ SG_ WHEEL_SPEED_RL : 55|16@0+ (0.01,-67.67) [0|250] "kph" XXX
+
+BO_ 180 SPEED: 8 XXX
+ SG_ ENCODER : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ SPEED : 47|16@0+ (0.01,0) [0|250] "kph" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 353 DSU_SPEED: 7 XXX
+ SG_ FORWARD_SPEED : 15|16@0- (0.00390625,-30) [0|255] "kph" XXX
+
+BO_ 452 ENGINE_RPM: 8 CGW
+ SG_ RPM : 7|16@0- (0.78125,0) [0|0] "rpm" SCS
+
+BO_ 466 PCM_CRUISE: 8 XXX
+ SG_ GAS_RELEASED : 4|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_ACTIVE : 5|1@0+ (1,0) [0|1] "" XXX
+ SG_ STANDSTILL_ON : 12|1@0+ (1,0) [0|1] "" XXX
+ SG_ ACCEL_NET : 23|16@0- (0.001,0) [-20|20] "m/s2" XXX
+ SG_ CRUISE_STATE : 55|4@0+ (1,0) [0|15] "" XXX
+ SG_ CANCEL_REQ : 49|1@1+ (1,0) [0|1] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 467 PCM_CRUISE_2: 8 XXX
+ SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX
+ SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "kph" XXX
+ SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "kph" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 552 ACCELEROMETER: 8 XXX
+ SG_ ACCEL_Z : 22|15@0- (1,0) [0|32767] "" XXX
+ SG_ ACCEL_X : 6|15@0- (0.001,0) [-20|20] "m/s2" XXX
+
+BO_ 560 BRAKE_MODULE2: 7 XXX
+ SG_ BRAKE_PRESSED : 26|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 614 STEERING_IPAS: 8 IPAS
+ SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX
+ SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX
+ SG_ SET_ME_X10 : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX
+ SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 643 PRE_COLLISION: 7 DSU
+ SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X00 : 15|8@0+ (1,0) [0|255] "" XXX
+ SG_ FORCE : 23|16@0- (2,0) [0|255] "N" XXX
+ SG_ SET_ME_X002 : 33|8@0+ (1,0) [0|3] "" XXX
+ SG_ BRAKE_STATUS : 39|3@0+ (1,0) [0|255] "" XXX
+ SG_ STATE : 36|3@0+ (1,0) [0|255] "" XXX
+ SG_ SET_ME_X003 : 40|1@0+ (1,0) [0|1] "" XXX
+ SG_ PRECOLLISION_ACTIVE : 41|1@0+ (1,0) [0|255] "" XXX
+ SG_ CHECKSUM : 55|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 740 STEERING_LKA: 5 XXX
+ SG_ LKA_STATE : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ STEER_REQUEST : 0|1@0+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 6|6@0+ (1,0) [0|63] "" XXX
+ SG_ SET_ME_1 : 7|1@0+ (1,0) [0|1] "" XXX
+ SG_ STEER_TORQUE_CMD : 15|16@0- (1,0) [0|65535] "" XXX
+ SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 742 LEAD_INFO: 8 DSU
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" HCU
+ SG_ LEAD_REL_SPEED : 23|12@0- (0.025,0) [-100|100] "m/s" HCU
+ SG_ LEAD_LONG_DIST : 7|13@0+ (0.05,0) [0|300] "m" HCU
+
+BO_ 835 ACC_CONTROL: 8 DSU
+ SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
+ SG_ SET_ME_X01 : 23|2@0+ (1,0) [0|3] "" HCU
+ SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
+ SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
+ SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
+ SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
+ SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
+
+BO_ 836 PRE_COLLISION_2: 8 DSU
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
+
+BO_ 869 DSU_CRUISE : 7 DSU
+ SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX
+ SG_ SET_BTN : 2|1@0+ (1,0) [0|0] "" XXX
+ SG_ CANCEL_BTN : 1|1@0+ (1,0) [0|0] "" XXX
+ SG_ MAIN_ON : 0|1@0+ (1,0) [0|0] "" XXX
+ SG_ SET_SPEED : 15|8@0+ (1,0) [0|0] "km/h" XXX
+ SG_ CRUISE_REQUEST : 31|8@0+ (100,-12800) [0|0] "N" XXX
+ SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|0] "m" XXX
+
+BO_ 921 PCM_CRUISE_SM: 8 XXX
+ SG_ MAIN_ON : 4|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRUISE_CONTROL_STATE : 11|4@0+ (1,0) [0|15] "" XXX
+ SG_ DISTANCE_LINES : 14|2@0+ (1,0) [0|3] "" XXX
+ SG_ UI_SET_SPEED : 31|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 951 ESP_CONTROL: 8 ESP
+ SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
+ SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
+ SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 1041 ACC_HUD: 8 DSU
+ SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+
+BO_ 1042 LKAS_HUD: 8 XXX
+ SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
+ SG_ RIGHT_LINE : 3|2@0+ (1,0) [0|3] "" XXX
+ SG_ LEFT_LINE : 5|2@0+ (1,0) [0|3] "" XXX
+ SG_ SET_ME_X01 : 7|2@0+ (1,0) [0|3] "" XXX
+ SG_ SET_ME_X01_2 : 11|2@0+ (1,0) [0|3] "" XXX
+ SG_ LDA_ALERT : 9|2@0+ (1,0) [0|3] "" XXX
+ SG_ TWO_BEEPS : 12|1@0+ (1,0) [0|1] "" XXX
+ SG_ ADJUSTING_CAMERA : 13|1@0+ (1,0) [0|1] "" XXX
+ SG_ LDA_MALFUNCTION : 15|1@0+ (1,0) [0|1] "" XXX
+ SG_ REPEATED_BEEPS : 32|1@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X0C : 23|8@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X2C : 47|8@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X38 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_X02 : 63|8@0+ (1,0) [0|1] "" XXX
+
+BO_ 1043 TIME : 8 CGW
+ SG_ YEAR : 7|8@0+ (1,0) [0|0] "year" XXX
+ SG_ MONTH : 15|8@0+ (1,0) [0|0] "month" XXX
+ SG_ DAY : 23|8@0+ (1,0) [0|0] "day" XXX
+ SG_ HOUR : 31|8@0+ (1,0) [0|0] "hour" XXX
+ SG_ MINUTE : 39|8@0+ (1,0) [0|0] "minute" XXX
+ SG_ GMT_DIFF : 55|1@0+ (1,0) [0|0] "" XXX
+ SG_ GMTDIFF_HOURS : 54|4@0+ (1,0) [0|0] "hours" XXX
+ SG_ GMTDIFF_MINUTES : 50|6@0+ (1,0) [0|0] "minutes" XXX
+ SG_ SUMMER : 60|1@0+ (1,0) [0|0] "" XXX
+
+BO_ 1408 VIN_PART_1: 8 CGW
+ SG_ VIN_1 : 7|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_2 : 15|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_3 : 23|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_4 : 31|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_5 : 39|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_6 : 47|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_7 : 55|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_8 : 63|8@0+ (1,0) [0|0] "" XXX
+
+BO_ 1409 VIN_PART_2: 8 CGW
+ SG_ VIN_9 : 7|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_10 : 15|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_11 : 23|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_12 : 31|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_13 : 39|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_14 : 47|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_15 : 55|8@0+ (1,0) [0|0] "" XXX
+ SG_ VIN_16 : 63|8@0+ (1,0) [0|0] "" XXX
+
+BO_ 1410 VIN_PART_3: 8 CGW
+ SG_ VIN_17 : 7|8@0+ (1,0) [0|0] "" XXX
+
+BO_ 1553 UI_SETTING: 8 XXX
+ SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX
+
+BO_ 1556 STEERING_LEVERS: 8 XXX
+ SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
+
+BO_ 1568 SEATS_DOORS: 8 XXX
+ SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_FL : 45|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_RL : 42|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_RR : 43|1@0+ (1,0) [0|1] "" XXX
+ SG_ DOOR_OPEN_FR : 44|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 1570 LIGHT_STALK: 8 SCM
+ SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 1161 RSA1: 8 FCM
+ SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
+ SG_ TSGNGRY1 : 12|3@0+ (1,0) [0|0] "" XXX
+ SG_ TSGNHLT1 : 9|2@0+ (1,0) [0|0] "" XXX
+ SG_ SPDVAL1 : 23|8@0+ (1,0) [0|0] "kph" XXX
+ SG_ SPLSGN1 : 31|4@0+ (1,0) [0|0] "" XXX
+ SG_ SPLSGN2 : 27|4@0+ (1,0) [0|0] "" XXX
+ SG_ TSGN2 : 39|8@0+ (1,0) [0|0] "" XXX
+ SG_ TSGNGRY2 : 44|3@0+ (1,0) [0|0] "" XXX
+ SG_ TSGNHLT2 : 41|2@0+ (1,0) [0|0] "" XXX
+ SG_ SPDVAL2 : 55|8@0+ (1,0) [0|0] "" XXX
+ SG_ BZRRQ_P : 63|2@0+ (1,0) [0|0] "" XXX
+ SG_ BZRRQ_A : 61|2@0+ (1,0) [0|0] "" XXX
+ SG_ SYNCID1 : 59|4@0+ (1,0) [0|0] "" XXX
+
+BO_ 1162 RSA2: 8 FCM
+ SG_ TSGN3 : 7|8@0+ (1,0) [0|0] "" XXX
+ SG_ TSGNGRY3 : 12|3@0+ (1,0) [0|0] "" XXX
+ SG_ TSGNHLT3 : 9|2@0+ (1,0) [0|0] "" XXX
+ SG_ SPLSGN3 : 31|4@0+ (1,0) [0|0] "" XXX
+ SG_ SPLSGN4 : 27|4@0+ (1,0) [0|0] "" XXX
+ SG_ TSGN4 : 39|8@0+ (1,0) [0|0] "" XXX
+ SG_ TSGNGRY4 : 44|3@0+ (1,0) [0|0] "" XXX
+ SG_ TSGNHLT4 : 41|2@0+ (1,0) [0|0] "" XXX
+ SG_ DPSGNREQ : 54|1@0+ (1,0) [0|0] "" XXX
+ SG_ SGNNUMP : 53|3@0+ (1,0) [0|0] "" XXX
+ SG_ SGNNUMA : 50|3@0+ (1,0) [0|0] "" XXX
+ SG_ SPDUNT : 63|2@0+ (1,0) [0|0] "" XXX
+ SG_ TSRWMSG : 61|2@0+ (1,0) [0|0] "" XXX
+ SG_ SYNCID2 : 59|4@0+ (1,0) [0|0] "" XXX
+
+BO_ 1163 RSA3: 8 FCM
+ SG_ TSREQPD : 7|1@0+ (1,0) [0|0] "" XXX
+ SG_ TSRMSW : 6|1@0+ (1,0) [0|0] "" XXX
+ SG_ OTSGNNTM : 5|2@0+ (1,0) [0|0] "" XXX
+ SG_ NTLVLSPD : 3|2@0+ (1,0) [0|0] "" XXX
+ SG_ OVSPNTM : 1|2@0+ (1,0) [0|0] "" XXX
+ SG_ OVSPVALL : 11|4@0+ (1,-5) [0|0] "" XXX
+ SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
+ SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
+ SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
+
+CM_ SG_ 36 ACCEL_Y "unit is tbd";
+CM_ SG_ 36 YAW_RATE "verify";
+CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
+CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
+CM_ SG_ 37 STEER_RATE "factor is tbd";
+CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
+CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
+CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
+CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
+CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
+CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value";
+CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active";
+CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
+CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
+CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
+CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
+CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
+CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
+CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
+CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
+CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
+CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz";
+CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"
+CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0";
+CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz";
+CM_ SG_ 1163 TSREQPD "always 1";
+CM_ SG_ 1163 TSRMSW "always 1";
+CM_ SG_ 1163 OTSGNNTM "always 3";
+CM_ SG_ 1163 NTLVLSPD "always 3";
+CM_ SG_ 1163 OVSPNTM "always 3";
+CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds";
+CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds";
+CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds";
+CM_ SG_ 1163 TSRSPU "always 1";
+
+VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off";
+VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok";
+VAL_ 614 STATE 3 "enabled" 1 "disabled";
+VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
+VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
+VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
+VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
+VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
+VAL_ 1042 LEFT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
+VAL_ 1553 UNITS 1 "km" 2 "miles";
+VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
+VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
+VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
+VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
+VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
+VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
+
+
+CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
+
+CM_ "lexus_nx300_2018_pt.dbc starts here"
+
+
+
+BO_ 550 BRAKE_MODULE: 8 XXX
+ SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
+ SG_ BRAKE_POSITION : 16|9@0+ (1,0) [0|511] "" XXX
+ SG_ BRAKE_PRESSED : 37|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 705 GAS_PEDAL: 8 XXX
+ SG_ GAS_RELEASED : 3|1@0+ (1,0) [0|1] "" XXX
+ SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX
+
+BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
+ SG_ STEER_TORQUE_EPS : 47|16@0- (0.73,0) [-20000|20000] "" XXX
+ SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
+ SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
+
+BO_ 610 EPS_STATUS: 5 EPS
+ SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
+ SG_ LKA_STATE : 31|7@0+ (1,0) [0|127] "" XXX
+ SG_ TYPE : 24|1@0+ (1,0) [0|1] "" XXX
+ SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 956 GEAR_PACKET: 8 XXX
+ SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX
+ SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX
+ SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX
+
+CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force";
+CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8";
+CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered";
+CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others";
+VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled";
+VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby";
+VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P";
+VAL_ 956 SPORT_ON 0 "off" 1 "on";
+VAL_ 956 ECON_ON 0 "off" 1 "on";

opendbc/toyota_nodsu_pt_generated.dbc

@@ -424,14 +424,6 @@ BO_ 956 GEAR_PACKET: 8 XXX
  SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX
  SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX
 
-BO_ 1653 Date_Time: 8 XXX
- SG_ Year : 23|8@0+ (1,0) [0|255] "" XXX
- SG_ Month : 31|8@0+ (1,0) [0|255] "" XXX
- SG_ Day : 39|8@0+ (1,0) [0|255] "" XXX
- SG_ Hour : 47|8@0+ (1,0) [0|255] "" XXX
- SG_ Minute : 55|8@0+ (1,0) [0|255] "" XXX
- SG_ Second : 63|8@0+ (1,0) [0|255] "" XXX
-
 CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force";
 CM_ SG_ 548 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8";
 CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others";

panda/board/boards/uno.h

@@ -72,11 +72,9 @@ void uno_set_usb_power_mode(uint8_t mode) {
   bool valid = false;
   switch (mode) {
     case USB_POWER_CLIENT:
-      uno_set_phone_power(false);
       valid = true;
       break;
     case USB_POWER_CDP:
-      uno_set_phone_power(true);
       uno_bootkick();
       valid = true;
       break;

release/build_release2.sh

@@ -35,7 +35,9 @@ else
   git checkout --orphan release2-staging
 fi
 
-VERSION=$(cat selfdrive/common/version.h | awk -F\" '{print $2}')
+VERSION=$(cat selfdrive/common/version.h | awk -F[\"-]  '{print $2}')
+echo "#define COMMA_VERSION \"$VERSION-release\"" > selfdrive/common/version.h
+
 git commit -m "openpilot v$VERSION"
 
 # Build signed panda firmware

selfdrive/boardd/SConscript

@@ -1,8 +1,6 @@
-Import('env', 'common', 'cereal', 'messaging', 'cython_dependencies')
+Import('env', 'envCython', 'common', 'cereal', 'messaging')
 
 env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
 env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
 
-env.Command(['boardd_api_impl.so', 'boardd_api_impl.cpp'],
+envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
-            cython_dependencies + ['libcan_list_to_can_capnp.a', 'boardd_api_impl.pyx', 'boardd_setup.py'],
-            "cd selfdrive/boardd && python3 boardd_setup.py build_ext --inplace")

selfdrive/boardd/boardd.cc

@@ -56,7 +56,9 @@ struct tm get_time(){
 }
 
 bool time_valid(struct tm sys_time){
-  return 1900 + sys_time.tm_year >= 2019;
+  int year = 1900 + sys_time.tm_year;
+  int month = 1 + sys_time.tm_mon;
+  return (year > 2020) || (year == 2020 && month >= 10);
 }
 
 void safety_setter_thread() {
@@ -107,6 +109,8 @@ void safety_setter_thread() {
   cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
   cereal::CarParams::SafetyModel safety_model = car_params.getSafetyModel();
 
+  panda->set_unsafe_mode(0);  // see safety_declarations.h for allowed values
+
   auto safety_param = car_params.getSafetyParam();
   LOGW("setting safety model: %d with param %d", (int)safety_model, safety_param);
 

selfdrive/boardd/boardd_setup.py

@@ -1,22 +0,0 @@
-from distutils.core import Extension, setup
-from Cython.Build import cythonize
-
-from common.cython_hacks import BuildExtWithoutPlatformSuffix
-
-libraries = ['can_list_to_can_capnp', 'capnp', 'kj']
-
-setup(name='Boardd API Implementation',
-      cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
-      ext_modules=cythonize(
-        Extension(
-          "boardd_api_impl",
-          libraries=libraries,
-          library_dirs=[
-            './',
-          ],
-          sources=['boardd_api_impl.pyx'],
-          language="c++",
-          extra_compile_args=["-std=c++1z", "-Wno-nullability-completeness"],
-        )
-      )
-)

selfdrive/boardd/panda.cc

@@ -188,6 +188,10 @@ void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int sa
   usb_write(0xdc, (uint16_t)safety_model, safety_param);
 }
 
+void Panda::set_unsafe_mode(uint16_t unsafe_mode) {
+  usb_write(0xdf, unsafe_mode, 0);
+}
+
 cereal::HealthData::HwType Panda::get_hw_type() {
   unsigned char hw_query[1] = {0};
 
@@ -279,7 +283,6 @@ const char* Panda::get_serial(){
 
   delete[] serial_buf;
   return NULL;
-
 }
 
 void Panda::set_power_saving(bool power_saving){

selfdrive/boardd/panda.h

@@ -63,6 +63,7 @@ class Panda {
   // Panda functionality
   cereal::HealthData::HwType get_hw_type();
   void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0);
+  void set_unsafe_mode(uint16_t unsafe_mode);
   void set_rtc(struct tm sys_time);
   struct tm get_rtc();
   void set_fan_speed(uint16_t fan_speed);

selfdrive/camerad/SConscript

@@ -11,10 +11,6 @@ if arch == "aarch64":
 elif arch == "larch64":
   libs += ['atomic']
   cameras = ['cameras/camera_qcom2.cc']
-  # no screen
-  # env = env.Clone()
-  # env.Append(CXXFLAGS = '-DNOSCREEN')
-  # env.Append(CFLAGS = '-DNOSCREEN')
 else:
   if USE_WEBCAM:
     libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio']
@@ -28,8 +24,9 @@ else:
 
   if arch == "Darwin":
     del libs[libs.index('OpenCL')]
+    del libs[libs.index(gpucommon)][gpucommon.index('GL')]
     env = env.Clone()
-    env['FRAMEWORKS'] = ['OpenCL']
+    env['FRAMEWORKS'] = ['OpenCL', 'OpenGL']
 
 env.SharedLibrary('snapshot/visionipc',
   ["#selfdrive/common/visionipc.c", "#selfdrive/common/ipc.c"])

selfdrive/camerad/cameras/camera_common.cc

@@ -24,6 +24,12 @@
 #include "common/util.h"
 #include "imgproc/utils.h"
 
+const int env_xmin = getenv("XMIN") ? atoi(getenv("XMIN")) : 0;
+const int env_xmax = getenv("XMAX") ? atoi(getenv("XMAX")) : -1;
+const int env_ymin = getenv("YMIN") ? atoi(getenv("YMIN")) : 0;
+const int env_ymax = getenv("YMAX") ? atoi(getenv("YMAX")) : -1;
+const int env_scale = getenv("SCALE") ? atoi(getenv("SCALE")) : 1;
+
 static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b) {
   char args[4096];
   snprintf(args, sizeof(args),
@@ -164,9 +170,8 @@ bool CameraBuf::acquire() {
     }
     assert(clSetKernelArg(krnl_debayer, 2, sizeof(float), &digital_gain) == 0);
     const size_t debayer_work_size = rgb_height;  // doesn't divide evenly, is this okay?
-    const size_t debayer_local_work_size = 128;
     assert(clEnqueueNDRangeKernel(q, krnl_debayer, 1, NULL,
-                                  &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event) == 0);
+                                  &debayer_work_size, NULL, 0, 0, &debayer_event) == 0);
 #endif
   } else {
     assert(cur_rgb_buf->len >= frame_size);
@@ -208,7 +213,6 @@ void CameraBuf::stop() {
 
 void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt) {
   framed.setFrameId(frame_data.frame_id);
-  framed.setEncodeId(cnt);
   framed.setTimestampEof(frame_data.timestamp_eof);
   framed.setFrameLength(frame_data.frame_length);
   framed.setIntegLines(frame_data.integ_lines);
@@ -220,6 +224,27 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
   framed.setGainFrac(frame_data.gain_frac);
 }
 
+void fill_frame_image(cereal::FrameData::Builder &framed, uint8_t *dat, int w, int h, int stride) {
+  if (dat != nullptr) {
+    int scale = env_scale;
+    int x_min = env_xmin; int y_min = env_ymin; int x_max = w-1; int y_max = h-1;
+    if (env_xmax != -1) x_max = env_xmax;
+    if (env_ymax != -1) y_max = env_ymax;
+    int new_width = (x_max - x_min + 1) / scale;
+    int new_height = (y_max - y_min + 1) / scale;
+    uint8_t *resized_dat = new uint8_t[new_width*new_height*3];
+
+    int goff = x_min*3 + y_min*stride;
+    for (int r=0;r<new_height;r++) {
+      for (int c=0;c<new_width;c++) {
+        memcpy(&resized_dat[(r*new_width+c)*3], &dat[goff+r*stride*scale+c*3*scale], 3*sizeof(uint8_t));
+      }
+    }
+    framed.setImage(kj::arrayPtr((const uint8_t*)resized_dat, new_width*new_height*3));
+    delete[] resized_dat;
+  }
+}
+
 void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) {
   const CameraBuf *b = &c->buf;
 
@@ -283,21 +308,14 @@ void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) {
   }
 }
 
-void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, bool front, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) {
+void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) {
   const CameraBuf *b = &c->buf;
 
   uint32_t lum_binning[256] = {0};
   for (int y = y_start; y < y_end; y += y_skip) {
     for (int x = x_start; x < x_end; x += x_skip) {
-      if (!front) {
+      uint8_t lum = pix_ptr[(y * b->yuv_width) + x];
-        uint8_t lum = pix_ptr[(y * b->yuv_width) + x];
+      lum_binning[lum]++;
-        lum_binning[lum]++;
-      } else {
-        // TODO: should get rid of RGB here
-        const uint8_t *pix = &pix_ptr[y * b->rgb_stride + x * 3];
-        unsigned int lum = (unsigned int)(pix[0] + pix[1] + pix[2]);
-        lum_binning[std::min(lum / 3, 255u)]++;
-      }
     }
   }
 
@@ -403,12 +421,15 @@ void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, i
     y_end = 1148;
     skip = 4;
 #endif
-    set_exposure_target(c, (const uint8_t *)b->cur_rgb_buf->addr, 1, x_start, x_end, 2, y_start, y_end, skip);
+    set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, x_start, x_end, 2, y_start, y_end, skip);
   }
 
   MessageBuilder msg;
   auto framed = msg.initEvent().initFrontFrame();
   framed.setFrameType(cereal::FrameData::FrameType::FRONT);
   fill_frame_data(framed, b->cur_frame_data, cnt);
+  if (env_send_front) {
+    fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride);
+  }
   pm->send("frontFrame", msg);
 }

selfdrive/camerad/cameras/camera_common.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <stdlib.h>
 #include <stdbool.h>
 #include <stdint.h>
 #include <memory>
@@ -34,6 +35,10 @@
 #define LOG_CAMERA_ID_QCAMERA 3
 #define LOG_CAMERA_ID_MAX 4
 
+const bool env_send_front = getenv("SEND_FRONT") != NULL;
+const bool env_send_rear = getenv("SEND_REAR") != NULL;
+const bool env_send_wide = getenv("SEND_WIDE") != NULL;
+
 typedef struct CameraInfo {
   const char* name;
   int frame_width, frame_height;
@@ -58,6 +63,7 @@ typedef struct LogCameraInfo {
 
 typedef struct FrameMetadata {
   uint32_t frame_id;
+  uint64_t timestamp_sof; // only set on tici
   uint64_t timestamp_eof;
   unsigned int frame_length;
   unsigned int integ_lines;
@@ -128,8 +134,9 @@ public:
 typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt);
 
 void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt);
+void fill_frame_image(cereal::FrameData::Builder &framed, uint8_t *dat, int w, int h, int stride);
 void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr);
-void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, bool front, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
+void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
 std::thread start_process_thread(MultiCameraState *cameras, const char *tname,
                                     CameraState *cs, int priority, process_thread_cb callback);
 void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt);

selfdrive/camerad/cameras/camera_qcom.cc

@@ -1,4 +1,3 @@
-#include <stdlib.h>
 #include <stdio.h>
 #include <stdbool.h>
 #include <assert.h>
@@ -333,7 +332,8 @@ void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
   s->rear.device = s->device;
   s->front.device = s->device;
 
-  s->sm = new SubMaster({"driverState", "sensorEvents"});
+  s->sm_front = new SubMaster({"driverState"});
+  s->sm_rear = new SubMaster({"sensorEvents"});
   s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"});
 
   int err;
@@ -1399,7 +1399,7 @@ static void camera_open(CameraState *s, bool rear) {
       err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data);
       LOG("ois init settings: %d", err);
     } else {
-      // leeco actuator
+      // leeco actuator (DW9800W H-Bridge Driver IC)
       // from sniff
       s->infinity_dac = 364;
 
@@ -1407,6 +1407,7 @@ static void camera_open(CameraState *s, bool rear) {
         {
           .reg_write_type = MSM_ACTUATOR_WRITE_DAC,
           .hw_mask = 0,
+          // MSB here at address 3
           .reg_addr = 3,
           .hw_shift = 0,
           .data_type = 9,
@@ -1417,11 +1418,14 @@ static void camera_open(CameraState *s, bool rear) {
       };
 
       struct reg_settings_t actuator_init_settings[] = {
-        { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 },
+        { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 },   // PD = power down
-        { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 },
+        { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 },   // 0 = power up
-        { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 },
+        { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 },   // RING = SAC mode
-        { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 },
+        { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 },  // 0x40 = SAC3 mode
         { .reg_addr=7, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=113, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 },
+        // 0x71 = DIV1 | DIV0 | SACT0 -- Tvib x 1/4 (quarter)
+        // SAC Tvib = 6.3 ms + 0.1 ms = 6.4 ms / 4 = 1.6 ms
+        // LSC 1-step = 252 + 1*4 = 256 ms / 4 = 64 ms
       };
 
       struct region_params_t region_params[] = {
@@ -2042,7 +2046,7 @@ static void* ops_thread(void* arg) {
 }
 
 void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) {
-  common_camera_process_front(s->sm, s->pm, c, cnt);
+  common_camera_process_front(s->sm_front, s->pm, c, cnt);
 }
 
 // called by processing_thread
@@ -2051,11 +2055,11 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
   // cache rgb roi and write to cl
 
   // gz compensation
-  s->sm->update(0);
+  s->sm_rear->update(0);
-  if (s->sm->updated("sensorEvents")) {
+  if (s->sm_rear->updated("sensorEvents")) {
     float vals[3] = {0.0};
     bool got_accel = false;
-    auto sensor_events = (*(s->sm))["sensorEvents"].getSensorEvents();
+    auto sensor_events = (*(s->sm_rear))["sensorEvents"].getSensorEvents();
     for (auto sensor_event : sensor_events) {
       if (sensor_event.which() == cereal::SensorEventData::ACCELERATION) {
         auto v = sensor_event.getAcceleration().getV();
@@ -2136,6 +2140,9 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
     MessageBuilder msg;
     auto framed = msg.initEvent().initFrame();
     fill_frame_data(framed, b->cur_frame_data, cnt);
+    if (env_send_rear) {
+      fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride);
+    }
     framed.setFocusVal(kj::ArrayPtr<const int16_t>(&s->rear.focus[0], NUM_FOCUS));
     framed.setFocusConf(kj::ArrayPtr<const uint8_t>(&s->rear.confidence[0], NUM_FOCUS));
     framed.setSharpnessScore(kj::ArrayPtr<const uint16_t>(&s->lapres[0], ARRAYSIZE(s->lapres)));
@@ -2154,7 +2161,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
   const int exposure_height = 314;
   const int skip = 1;
   if (cnt % 3 == 0) {
-    set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, 0, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip);
+    set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip);
   }
 }
 
@@ -2287,6 +2294,7 @@ void cameras_close(MultiCameraState *s) {
 
   clReleaseProgram(s->prg_rgb_laplacian);
   clReleaseKernel(s->krnl_rgb_laplacian);
-  delete s->sm;
+  delete s->sm_front;
+  delete s->sm_rear;
   delete s->pm;
 }

selfdrive/camerad/cameras/camera_qcom.h

@@ -142,7 +142,8 @@ typedef struct MultiCameraState {
   CameraState rear;
   CameraState front;
 
-  SubMaster *sm;
+  SubMaster *sm_front;
+  SubMaster *sm_rear;
   PubMaster *pm;
 
 } MultiCameraState;

selfdrive/camerad/main.cc

@@ -205,6 +205,7 @@ void* visionserver_client_thread(void* arg) {
             stream_i == VISION_STREAM_YUV_WIDE) {
           CameraBuf *b = get_camerabuf_by_type(s, (VisionStreamType)stream_i);
           rep.d.stream_acq.extra.frame_id = b->yuv_metas[idx].frame_id;
+          rep.d.stream_acq.extra.timestamp_sof = b->yuv_metas[idx].timestamp_sof;
           rep.d.stream_acq.extra.timestamp_eof = b->yuv_metas[idx].timestamp_eof;
         }
         vipc_send(fd, &rep);

selfdrive/car/car_helpers.py

@@ -9,12 +9,11 @@ from selfdrive.swaglog import cloudlog
 import cereal.messaging as messaging
 from selfdrive.car import gen_empty_fingerprint
 
-from cereal import car, log
+from cereal import car
 EventName = car.CarEvent.EventName
-HwType = log.HealthData.HwType
 
 
-def get_startup_event(car_recognized, controller_available, hw_type):
+def get_startup_event(car_recognized, controller_available):
   if comma_remote and tested_branch:
     event = EventName.startup
   else:
@@ -24,8 +23,6 @@ def get_startup_event(car_recognized, controller_available, hw_type):
     event = EventName.startupNoCar
   elif car_recognized and not controller_available:
     event = EventName.startupNoControl
-  elif hw_type == HwType.greyPanda:
-    event = EventName.startupGreyPanda
   return event
 
 
@@ -84,11 +81,11 @@ def only_toyota_left(candidate_cars):
 
 
 # **** for use live only ****
-def fingerprint(logcan, sendcan, has_relay):
+def fingerprint(logcan, sendcan):
   fixed_fingerprint = os.environ.get('FINGERPRINT', "")
   skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
 
-  if has_relay and not fixed_fingerprint and not skip_fw_query:
+  if not fixed_fingerprint and not skip_fw_query:
     # Vin query only reliably works thorugh OBDII
     bus = 1
 
@@ -144,8 +141,7 @@ def fingerprint(logcan, sendcan, has_relay):
       # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
       if only_toyota_left(candidate_cars[b]):
         frame_fingerprint = 100  # 1s
-      if len(candidate_cars[b]) == 1:
+      if len(candidate_cars[b]) == 1 and frame > frame_fingerprint:
-        if frame > frame_fingerprint:
           # fingerprint done
           car_fingerprint = candidate_cars[b][0]
 
@@ -171,15 +167,15 @@ def fingerprint(logcan, sendcan, has_relay):
   return car_fingerprint, finger, vin, car_fw, source
 
 
-def get_car(logcan, sendcan, has_relay=False):
+def get_car(logcan, sendcan):
-  candidate, fingerprints, vin, car_fw, source = fingerprint(logcan, sendcan, has_relay)
+  candidate, fingerprints, vin, car_fw, source = fingerprint(logcan, sendcan)
 
   if candidate is None:
     cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
     candidate = "mock"
 
   CarInterface, CarController, CarState = interfaces[candidate]
-  car_params = CarInterface.get_params(candidate, fingerprints, has_relay, car_fw)
+  car_params = CarInterface.get_params(candidate, fingerprints, car_fw)
   car_params.carVin = vin
   car_params.carFw = car_fw
   car_params.fingerprintSource = source

selfdrive/car/chrysler/interface.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from cereal import car
-from selfdrive.car.chrysler.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS
+from selfdrive.car.chrysler.values import CAR
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 
 
@@ -11,11 +11,11 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 3.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=None, has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=None, car_fw=None):
     if fingerprint is None:
       fingerprint = gen_empty_fingerprint()
 
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "chrysler"
     ret.safetyModel = car.CarParams.SafetyModel.chrysler
 
@@ -52,8 +52,7 @@ class CarInterface(CarInterfaceBase):
     # mass and CG position, so all cars will have approximately similar dyn behaviors
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
 
-    ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
+    ret.enableCamera = True
-    print("ECU Camera Simulated: {0}".format(ret.enableCamera))
 
     return ret
 

selfdrive/car/chrysler/values.py

@@ -91,8 +91,3 @@ DBC = {
 }
 
 STEER_THRESHOLD = 120
-
-
-ECU_FINGERPRINT = {
-  Ecu.fwdCamera: [0x292],  # lkas cmd
-}

selfdrive/car/ford/interface.py

@@ -2,8 +2,8 @@
 from cereal import car
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
-from selfdrive.car.ford.values import MAX_ANGLE, Ecu, ECU_FINGERPRINT, FINGERPRINTS
+from selfdrive.car.ford.values import MAX_ANGLE
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 
 
@@ -14,8 +14,8 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 3.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "ford"
     ret.safetyModel = car.CarParams.SafetyModel.ford
     ret.dashcamOnly = True
@@ -43,7 +43,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.steerControlType = car.CarParams.SteerControlType.angle
 
-    ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
+    ret.enableCamera = True
     cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
 
     return ret

selfdrive/car/ford/values.py

@@ -15,10 +15,6 @@ FINGERPRINTS = {
   }],
 }
 
-ECU_FINGERPRINT = {
-  Ecu.fwdCamera: [970, 973, 984]
-}
-
 DBC = {
   CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'),
 }

selfdrive/car/fw_versions.py

@@ -127,8 +127,8 @@ def match_fw_to_car(fw_versions):
       if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER] and found_version is None:
         continue
 
-      # TODO: COROLLA_TSS2 engine can show on two different addresses
+      # TODO: on some toyota, the engine can show on two different addresses
-      if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR] and found_version is None:
+      if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS] and found_version is None:
         continue
 
       # ignore non essential ecus

selfdrive/car/gm/interface.py

@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.config import Conversions as CV
-from selfdrive.car.gm.values import CAR, Ecu, ECU_FINGERPRINT, CruiseButtons, \
+from selfdrive.car.gm.values import CAR, CruiseButtons, \
-                                    AccState, FINGERPRINTS
+                                    AccState
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -16,8 +16,8 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 4.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "gm"
     ret.safetyModel = car.CarParams.SafetyModel.gm
     ret.enableCruise = False  # stock cruise control is kept off
@@ -29,7 +29,7 @@ class CarInterface(CarInterfaceBase):
     # Presence of a camera on the object bus is ok.
     # Have to go to read_only if ASCM is online (ACC-enabled cars),
     # or camera is on powertrain bus (LKA cars without ACC).
-    ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
+    ret.enableCamera = True
     ret.openpilotLongitudinalControl = ret.enableCamera
     tire_stiffness_factor = 0.444  # not optimized yet
 

selfdrive/car/gm/values.py

@@ -70,10 +70,6 @@ FINGERPRINTS = {
 
 STEER_THRESHOLD = 1.0
 
-ECU_FINGERPRINT = {
-  Ecu.fwdCamera: [384, 715]  # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
-}
-
 DBC = {
   CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
   CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),

selfdrive/car/honda/carcontroller.py

@@ -145,22 +145,22 @@ class CarController():
     # Send steering command.
     idx = frame % 4
     can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
-      lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl))
+      lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
 
     # Send dashboard UI commands.
     if (frame % 10) == 0:
       idx = (frame//10) % 4
-      can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl, CS.stock_hud))
+      can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud))
 
     if not CS.CP.openpilotLongitudinalControl:
       if (frame % 2) == 0:
         idx = frame // 2
-        can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack))
+        can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx))
       # If using stock ACC, spam cancel command to kill gas when OP disengages.
       if pcm_cancel_cmd:
-        can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
+        can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint))
       elif CS.out.cruiseState.standstill:
-        can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
+        can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint))
 
     else:
       # Send gas and brake commands.
@@ -174,7 +174,7 @@ class CarController():
           apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
           pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
           can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
-            pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake))
+            pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.stock_brake))
           self.apply_brake_last = apply_brake
 
           if CS.CP.enableGasInterceptor:

selfdrive/car/honda/carstate.py

@@ -336,7 +336,7 @@ class CarState(CarStateBase):
   @staticmethod
   def get_can_parser(CP):
     signals, checks = get_can_signals(CP)
-    bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0
+    bus_pt = 1 if CP.carFingerprint in HONDA_BOSCH else 0
     return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt)
 
   @staticmethod
@@ -361,8 +361,7 @@ class CarState(CarStateBase):
     if CP.carFingerprint in [CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]:
       checks = [(0x194, 100)]
 
-    bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2
+    return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
-    return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
 
   @staticmethod
   def get_body_can_parser(CP):

selfdrive/car/honda/hondacan.py

@@ -7,24 +7,19 @@ from selfdrive.car.honda.values import HONDA_BOSCH
 # 2 = ACC-CAN - camera side
 # 3 = F-CAN A - OBDII port
 
-# CAN bus layout with giraffe
+def get_pt_bus(car_fingerprint):
-# 0 = F-CAN B - powertrain
+  return 1 if car_fingerprint in HONDA_BOSCH else 0
-# 1 = ACC-CAN - camera side
-# 2 = ACC-CAN - radar side
-
-def get_pt_bus(car_fingerprint, has_relay):
-  return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0
 
 
-def get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled=False):
+def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False):
   if radar_disabled:
     # when radar is disabled, steering commands are sent directly to powertrain bus
-    return get_pt_bus(car_fingerprint, has_relay)
+    return get_pt_bus(car_fingerprint)
   # normally steering commands are sent to radar, which forwards them to powertrain bus
-  return 2 if car_fingerprint in HONDA_BOSCH and not has_relay else 0
+  return 0
 
 
-def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay, stock_brake):
+def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, stock_brake):
   # TODO: do we loose pressure if we keep pump off for long?
   brakelights = apply_brake > 0
   brake_rq = apply_brake > 0
@@ -45,13 +40,13 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
     "AEB_REQ_2": 0,
     "AEB_STATUS": 0,
   }
-  bus = get_pt_bus(car_fingerprint, has_relay)
+  bus = get_pt_bus(car_fingerprint)
   return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
 
 
-def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, car_fingerprint, has_relay):
+def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, car_fingerprint):
   commands = []
-  bus = get_pt_bus(car_fingerprint, has_relay)
+  bus = get_pt_bus(car_fingerprint)
 
   control_on = 5 if enabled else 0
   # no gas = -30000
@@ -84,31 +79,31 @@ def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, ca
 
   return commands
 
-def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay, radar_disabled):
+def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, radar_disabled):
   values = {
     "STEER_TORQUE": apply_steer if lkas_active else 0,
     "STEER_TORQUE_REQUEST": lkas_active,
   }
-  bus = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled)
+  bus = get_lkas_cmd_bus(car_fingerprint, radar_disabled)
   return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
 
 
-def create_bosch_supplemental_1(packer, car_fingerprint, idx, has_relay):
+def create_bosch_supplemental_1(packer, car_fingerprint, idx):
   # non-active params
   values = {
     "SET_ME_X04": 0x04,
     "SET_ME_X80": 0x80,
     "SET_ME_X10": 0x10,
   }
-  bus = get_lkas_cmd_bus(car_fingerprint, has_relay)
+  bus = get_lkas_cmd_bus(car_fingerprint)
   return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values, idx)
 
 
-def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, openpilot_longitudinal_control, stock_hud):
+def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, openpilot_longitudinal_control, stock_hud):
   commands = []
-  bus_pt = get_pt_bus(car_fingerprint, has_relay)
+  bus_pt = get_pt_bus(car_fingerprint)
   radar_disabled = car_fingerprint in HONDA_BOSCH and openpilot_longitudinal_control
-  bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled)
+  bus_lkas = get_lkas_cmd_bus(car_fingerprint, radar_disabled)
 
   if openpilot_longitudinal_control:
     if car_fingerprint in HONDA_BOSCH:
@@ -158,10 +153,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
   return commands
 
 
-def spam_buttons_command(packer, button_val, idx, car_fingerprint, has_relay):
+def spam_buttons_command(packer, button_val, idx, car_fingerprint):
   values = {
     'CRUISE_BUTTONS': button_val,
     'CRUISE_SETTING': 0,
   }
-  bus = get_pt_bus(car_fingerprint, has_relay)
+  bus = get_pt_bus(car_fingerprint)
   return packer.make_can_msg("SCM_BUTTONS", bus, values, idx)

selfdrive/car/honda/interface.py

@@ -6,8 +6,8 @@ from common.realtime import DT_CTRL
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.events import ET
-from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, Ecu, ECU_FINGERPRINT, FINGERPRINTS
+from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH
-from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -119,19 +119,18 @@ class CarInterface(CarInterfaceBase):
     return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):  # pylint: disable=dangerous-default-value
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # pylint: disable=dangerous-default-value
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "honda"
 
     if candidate in HONDA_BOSCH:
-      ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness if has_relay else car.CarParams.SafetyModel.hondaBoschGiraffe
+      ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness
-      rdr_bus = 0 if has_relay else 2
+      ret.enableCamera = True
-      ret.enableCamera = is_ecu_disconnected(fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
       ret.radarOffCan = True
       ret.openpilotLongitudinalControl = False
     else:
       ret.safetyModel = car.CarParams.SafetyModel.hondaNidec
-      ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
+      ret.enableCamera = True
       ret.enableGasInterceptor = 0x201 in fingerprint[0]
       ret.openpilotLongitudinalControl = ret.enableCamera
 
@@ -253,7 +252,7 @@ class CarInterface(CarInterfaceBase):
         # stock request output values:    0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400
         # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800
         ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
-        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]]
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]]
       else:
         ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]

selfdrive/car/honda/values.py

@@ -231,6 +231,7 @@ FW_VERSIONS = {
       b'37805-6A0-A850\x00\x00',
       b'37805-6A0-C540\x00\x00',
       b'37805-6A1-H650\x00\x00',
+      b'37805-6M4-B730\x00\x00',
     ],
     (Ecu.transmission, 0x18da1ef1, None): [
       b'28101-6A7-A220\x00\x00',
@@ -239,6 +240,7 @@ FW_VERSIONS = {
       b'28101-6A7-A330\x00\x00',
       b'28101-6A7-A510\x00\x00',
       b'28101-6A9-H140\x00\x00',
+      b'28101-6A9-H420\x00\x00',
     ],
     (Ecu.gateway, 0x18daeff1, None): [
       b'38897-TVA-A230\x00\x00',
@@ -249,6 +251,7 @@ FW_VERSIONS = {
       b'46114-TVA-A080\x00\x00',
       b'46114-TVA-A120\x00\x00',
       b'46114-TVE-H550\x00\x00',
+      b'46114-TVE-H560\x00\x00',
     ],
     (Ecu.combinationMeter, 0x18da60f1, None): [
       b'78109-TVA-A010\x00\x00',
@@ -259,6 +262,7 @@ FW_VERSIONS = {
       b'78109-TVA-C010\x00\x00',
       b'78109-TVE-H610\x00\x00',
       b'78109-TWA-A210\x00\x00',
+      b'78109-TBX-H310\x00\x00',
     ],
     (Ecu.hud, 0x18da61f1, None): [
       b'78209-TVA-A010\x00\x00',
@@ -266,10 +270,12 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x18dab5f1, None): [
       b'36161-TVA-A060\x00\x00',
       b'36161-TVE-H050\x00\x00',
+      b'36161-TBX-H130\x00\x00',
     ],
     (Ecu.srs, 0x18da53f1, None): [
       b'77959-TVA-A460\x00\x00',
       b'77959-TVA-H230\x00\x00',
+      b'77959-TBX-H230\x00\x00',
     ],
     (Ecu.vsa, 0x18da28f1, None): [
       b'57114-TVA-B050\x00\x00',
@@ -281,12 +287,14 @@ FW_VERSIONS = {
       b'36802-TVA-A160\x00\x00',
       b'36802-TVA-A170\x00\x00',
       b'36802-TVE-H070\x00\x00',
+      b'36802-TBX-H140\x00\x00',
     ],
     (Ecu.eps, 0x18da30f1, None): [
       b'39990-TVA-A140\x00\x00',
       b'39990-TVA-A150\x00\x00',  # Are these two different steerRatio?
       b'39990-TVA-A160\x00\x00',  # Sport, Sport 2.0T and Touring 2.0T have different ratios
       b'39990-TVE-H130\x00\x00',
+      b'39990-TBX-H120\x00\x00',
     ],
   },
   CAR.ACCORDH: {
@@ -936,11 +944,23 @@ FW_VERSIONS = {
     ],
   },
   CAR.HRV: {
-    (Ecu.gateway, 0x18daeff1, None): [b'38897-T7A-A010\x00\x00'],
+    (Ecu.gateway, 0x18daeff1, None): [
-    (Ecu.eps, 0x18da30f1, None): [b'39990-THX-A020\x00\x00'],
+      b'38897-T7A-A010\x00\x00',
-    (Ecu.fwdRadar, 0x18dab0f1, None): [b'36161-T7A-A240\x00\x00'],
+    ],
-    (Ecu.srs, 0x18da53f1, None): [b'77959-T7A-A230\x00\x00'],
+    (Ecu.eps, 0x18da30f1, None): [
-    (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-THX-A210\x00\x00'],
+      b'39990-THX-A020\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x18dab0f1, None): [
+      b'36161-T7A-A140\x00\x00',
+      b'36161-T7A-A240\x00\x00',
+    ],
+    (Ecu.srs, 0x18da53f1, None): [
+      b'77959-T7A-A230\x00\x00',
+    ],
+    (Ecu.combinationMeter, 0x18da60f1, None): [
+      b'78109-THX-A110\x00\x00',
+      b'78109-THX-A210\x00\x00',
+    ],
   },
 }
 
@@ -1016,10 +1036,4 @@ SPEED_FACTOR = {
   CAR.INSIGHT: 1.,
 }
 
-# msgs sent for steering controller by camera module on can 0.
-# those messages are mutually exclusive on CRV and non-CRV cars
-ECU_FINGERPRINT = {
-  Ecu.fwdCamera: [0xE4, 0x194],   # steer torque cmd
-}
-
 HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G])

selfdrive/car/hyundai/carcontroller.py

@@ -15,10 +15,7 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
   # initialize to no line visible
   sys_state = 1
   if left_lane and right_lane or sys_warning:  # HUD alert only display when LKAS status is active
-    if enabled or sys_warning:
+    sys_state = 3 if enabled or sys_warning else 4
-      sys_state = 3
-    else:
-      sys_state = 4
   elif left_lane:
     sys_state = 5
   elif right_lane:
@@ -37,14 +34,14 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
+    self.p = SteerLimitParams(CP)
+    self.packer = CANPacker(dbc_name)
+
     self.apply_steer_last = 0
     self.car_fingerprint = CP.carFingerprint
-    self.packer = CANPacker(dbc_name)
     self.steer_rate_limited = False
     self.last_resume_frame = 0
 
-    self.p = SteerLimitParams(CP)
-
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
              left_lane, right_lane, left_lane_depart, right_lane_depart):
     # Steering Torque
@@ -64,7 +61,7 @@ class CarController():
 
     self.apply_steer_last = apply_steer
 
-    sys_warning, sys_state, left_lane_warning, right_lane_warning =\
+    sys_warning, sys_state, left_lane_warning, right_lane_warning = \
       process_hud_alert(enabled, self.car_fingerprint, visual_alert,
                         left_lane, right_lane, left_lane_depart, right_lane_depart)
 
@@ -83,7 +80,7 @@ class CarController():
         self.last_resume_frame = frame
 
     # 20 Hz LFA MFA message
-    if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ]:
+    if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV]:
       can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
 
     return can_sends

selfdrive/car/hyundai/hyundaican.py

@@ -15,11 +15,9 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
   values["CF_Lkas_LdwsRHWarning"] = right_lane_depart
   values["CR_Lkas_StrToqReq"] = apply_steer
   values["CF_Lkas_ActToi"] = steer_req
-  values["CF_Lkas_ToiFlt"] = 0
   values["CF_Lkas_MsgCount"] = frame % 0x10
-  values["CF_Lkas_Chksum"] = 0
 
-  if car_fingerprint in [CAR.SONATA, CAR.PALISADE]:
+  if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV]:
     values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
     values["CF_Lkas_LdwsOpt_USM"] = 2
 

selfdrive/car/hyundai/interface.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.config import Conversions as CV
-from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS
+from selfdrive.car.hyundai.values import CAR
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 
 class CarInterface(CarInterfaceBase):
@@ -12,8 +12,8 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 3.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):  # pylint: disable=dangerous-default-value
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # pylint: disable=dangerous-default-value
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     ret.carName = "hyundai"
     ret.safetyModel = car.CarParams.SafetyModel.hyundai
@@ -58,13 +58,6 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 13.75 * 1.15
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
-    elif candidate == CAR.KIA_SORENTO:
-      ret.lateralTuning.pid.kf = 0.00005
-      ret.mass = 1985. + STD_CARGO_KG
-      ret.wheelbase = 2.78
-      ret.steerRatio = 14.4 * 1.1   # 10% higher at the center seems reasonable
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
     elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
       ret.lateralTuning.pid.kf = 0.00006
       ret.mass = 1275. + STD_CARGO_KG
@@ -79,44 +72,12 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 2060. + STD_CARGO_KG
       ret.wheelbase = 3.01
       ret.steerRatio = 16.5
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+      ret.lateralTuning.init('indi')
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
+      ret.lateralTuning.indi.innerLoopGain = 3.5
+      ret.lateralTuning.indi.outerLoopGain = 2.0
+      ret.lateralTuning.indi.timeConstant = 1.4
+      ret.lateralTuning.indi.actuatorEffectiveness = 2.3
       ret.minSteerSpeed = 60 * CV.KPH_TO_MS
-    elif candidate == CAR.GENESIS_G70: 
-      ret.lateralTuning.pid.kf = 0.00005 
-      ret.mass = 1640. + STD_CARGO_KG 
-      ret.wheelbase = 2.84 
-      ret.steerRatio = 16.5 
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
-    elif candidate == CAR.GENESIS_G80:
-      ret.lateralTuning.pid.kf = 0.00005
-      ret.mass = 2060. + STD_CARGO_KG
-      ret.wheelbase = 3.01
-      ret.steerRatio = 16.5
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
-    elif candidate == CAR.GENESIS_G90:
-      ret.mass = 2200
-      ret.wheelbase = 3.15
-      ret.steerRatio = 12.069
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
-    elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
-      ret.lateralTuning.pid.kf = 0.00005
-      ret.mass = 3558. * CV.LB_TO_KG
-      ret.wheelbase = 2.80
-      ret.steerRatio = 13.75
-      tire_stiffness_factor = 0.5
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
-    elif candidate == CAR.KIA_STINGER:
-      ret.lateralTuning.pid.kf = 0.00005
-      ret.mass = 1825. + STD_CARGO_KG
-      ret.wheelbase = 2.78
-      ret.steerRatio = 14.4 * 1.15   # 15% higher at the center seems reasonable
-      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
     elif candidate == CAR.KONA:
       ret.lateralTuning.pid.kf = 0.00005
       ret.mass = 1275. + STD_CARGO_KG
@@ -142,6 +103,47 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
       ret.minSteerSpeed = 32 * CV.MPH_TO_MS
+    elif candidate == CAR.VELOSTER:
+      ret.lateralTuning.pid.kf = 0.00005
+      ret.mass = 3558. * CV.LB_TO_KG
+      ret.wheelbase = 2.80
+      ret.steerRatio = 13.75 * 1.15
+      tire_stiffness_factor = 0.5
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+
+    # Kia
+    elif candidate == CAR.KIA_SORENTO:
+      ret.lateralTuning.pid.kf = 0.00005
+      ret.mass = 1985. + STD_CARGO_KG
+      ret.wheelbase = 2.78
+      ret.steerRatio = 14.4 * 1.1   # 10% higher at the center seems reasonable
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+    elif candidate == CAR.KIA_NIRO_EV:
+      ret.lateralTuning.pid.kf = 0.00006
+      ret.mass = 1737. + STD_CARGO_KG
+      ret.wheelbase = 2.7
+      ret.steerRatio = 13.73  # Spec
+      tire_stiffness_factor = 0.385
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+    elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
+      ret.lateralTuning.pid.kf = 0.00005
+      ret.mass = 3558. * CV.LB_TO_KG
+      ret.wheelbase = 2.80
+      ret.steerRatio = 13.75
+      tire_stiffness_factor = 0.5
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+    elif candidate == CAR.KIA_STINGER:
+      ret.lateralTuning.pid.kf = 0.00005
+      ret.mass = 1825. + STD_CARGO_KG
+      ret.wheelbase = 2.78
+      ret.steerRatio = 14.4 * 1.15   # 15% higher at the center seems reasonable
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+
     elif candidate == CAR.KIA_FORTE:
       ret.lateralTuning.pid.kf = 0.00005
       ret.mass = 3558. * CV.LB_TO_KG
@@ -150,18 +152,36 @@ class CarInterface(CarInterfaceBase):
       tire_stiffness_factor = 0.5
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
-    elif candidate == CAR.VELOSTER:
+
+    # Genesis
+    elif candidate == CAR.GENESIS_G70:
+      ret.lateralTuning.init('indi')
+      ret.lateralTuning.indi.innerLoopGain = 2.5
+      ret.lateralTuning.indi.outerLoopGain = 3.5
+      ret.lateralTuning.indi.timeConstant = 1.4
+      ret.lateralTuning.indi.actuatorEffectiveness = 1.8
+      ret.steerActuatorDelay = 0.1
+      ret.mass = 1640.0 + STD_CARGO_KG
+      ret.wheelbase = 2.84
+      ret.steerRatio = 13.56
+    elif candidate == CAR.GENESIS_G80:
       ret.lateralTuning.pid.kf = 0.00005
-      ret.mass = 3558. * CV.LB_TO_KG
+      ret.mass = 2060. + STD_CARGO_KG
-      ret.wheelbase = 2.80
+      ret.wheelbase = 3.01
-      ret.steerRatio = 13.75 * 1.15
+      ret.steerRatio = 16.5
-      tire_stiffness_factor = 0.5
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]  
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
+    elif candidate == CAR.GENESIS_G90:
+      ret.mass = 2200
+      ret.wheelbase = 3.15
+      ret.steerRatio = 12.069
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
+
 
     # these cars require a special panda safety mode due to missing counters and checksums in the messages
-    if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019, 
+    if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019,
-                     CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70]:
+                     CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80]:
       ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy
 
     ret.centerToFront = ret.wheelbase * 0.4
@@ -175,7 +195,7 @@ class CarInterface(CarInterfaceBase):
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
 
-    ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
+    ret.enableCamera = True
 
     return ret
 

selfdrive/car/hyundai/values.py

@@ -7,7 +7,7 @@ Ecu = car.CarParams.Ecu
 # Steer torque limits
 class SteerLimitParams:
   def __init__(self, CP):
-    if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE]:
+    if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70]:
       self.STEER_MAX = 384
     else:
       self.STEER_MAX = 255
@@ -19,19 +19,12 @@ class SteerLimitParams:
 
 
 class CAR:
+  # Hyundai
   ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017"
   ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
-  GENESIS_G70 = "GENESIS G70 2018"
-  GENESIS_G80 = "GENESIS G80 2017"
-  GENESIS_G90 = "GENESIS G90 2017"
   HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016"
   IONIQ = "HYUNDAI IONIQ ELECTRIC PREMIUM SE 2020"
   IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019"
-  KIA_FORTE = "KIA FORTE E 2018"
-  KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016"
-  KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
-  KIA_SORENTO = "KIA SORENTO GT LINE 2018"
-  KIA_STINGER = "KIA STINGER GT2 2018"
   KONA = "HYUNDAI KONA 2020"
   KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
   SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019"
@@ -40,6 +33,19 @@ class CAR:
   PALISADE = "HYUNDAI PALISADE 2020"
   VELOSTER = "HYUNDAI VELOSTER 2019"
 
+  # Kia
+  KIA_FORTE = "KIA FORTE E 2018"
+  KIA_NIRO_EV = "KIA NIRO EV 2020"
+  KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016"
+  KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
+  KIA_SORENTO = "KIA SORENTO GT LINE 2018"
+  KIA_STINGER = "KIA STINGER GT2 2018"
+
+  # Genesis
+  GENESIS_G70 = "GENESIS G70 2018"
+  GENESIS_G80 = "GENESIS G80 2017"
+  GENESIS_G90 = "GENESIS G90 2017"
+
 
 class Buttons:
   NONE = 0
@@ -91,7 +97,7 @@ FINGERPRINTS = {
     {66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8},
   ],
   CAR.KIA_OPTIMA: [{
-      64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
+    64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8, 1371: 8, 1397: 8, 1961: 8
   }],
   CAR.KIA_SORENTO: [{
     67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1
@@ -127,7 +133,10 @@ FINGERPRINTS = {
     67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832 : 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1170: 8, 1173: 8, 1186: 2, 1191: 2, 1193: 8, 1265: 4,1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1378: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8
   }],
   CAR.KONA_EV: [{
-    127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8
+    127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 1157: 4, 1193: 8, 1379: 8, 1988: 8, 1996: 8
+  }],
+  CAR.KIA_NIRO_EV: [{
+    127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 516: 8, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1193: 8, 1225: 8, 1260: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8
   }],
   CAR.KIA_FORTE: [{
     67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1427: 6, 1456: 4, 1470: 8
@@ -146,35 +155,38 @@ FINGERPRINTS = {
   }]
 }
 
-ECU_FINGERPRINT = {
-  Ecu.fwdCamera: [832, 1156, 1191, 1342]
-}
-
 # Don't use these fingerprints for fingerprinting, they are still used for ECU detection
 IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.GENESIS_G70, CAR.KONA]
 
 FW_VERSIONS = {
   CAR.SONATA: {
     (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00DN8_ SCC FHCUP      1.00 1.01 99110-L1000         ',
       b'\xf1\x00DN8_ SCC FHCUP      1.00 1.00 99110-L0000         ',
       b'\xf1\x00DN8_ SCC F-CU-      1.00 1.00 99110-L0000         ',
     ],
     (Ecu.esp, 0x7d1, None): [
+      b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300\xf1\xa01.02',
+      b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100',
       b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04',
     ],
     (Ecu.engine, 0x7e0, None): [
+      b'HM6M2_0a0_BD0',
       b'\xf1\x87391162M003\xf1\xa0000F',
       b'\xf1\x87391162M003\xf1\xa00240',
     ],
     (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103\xf1\xa01.03',
       b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101\xf1\xa01.01',
       b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101\xf1\xa01.01',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00DN8 MFC  AT KOR LHD 1.00 1.02 99211-L1000 190422',
       b'\xf1\x00DN8 MFC  AT USA LHD 1.00 1.00 99211-L0000 190716',
       b'\xf1\x00DN8 MFC  AT USA LHD 1.00 1.01 99211-L0000 191016',
     ],
     (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\xf1\x00bcsh8p54  U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
     ],
   },
@@ -235,11 +247,13 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [b'\x01TJS-JNU06F200H0A', ],
     (Ecu.eps, 0x7d4, None): [b'\xf1\x00JSL MDPS C 1.00 1.03 56340-J3000 8308', ],
     (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00JS  LKAS AT USA LHD 1.00 1.02 95740-J3000 K32', ],
-    (Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80', ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80',
+      b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\x00\x00\x00\x00',
+    ],
   },
   CAR.GENESIS_G70: {
     (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00IK__ SCC F-CUP      1.00 1.02 96400-G9100         \xf1\xa01.02', ],
-    (Ecu.esp, 0x7d1, None): [b'\xf1\x00\x00\x00\x00\x00\x00\x00', ],
     (Ecu.engine, 0x7e0, None): [b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', ],
     (Ecu.eps, 0x7d4, None): [b'\xf1\x00IK  MDPS R 1.00 1.06 57700-G9420 4I4VL106', ],
     (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00IK  MFC  AT USA LHD 1.00 1.01 95740-G9000 170920', ],
@@ -253,6 +267,30 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ],
     (Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', ],
   },
+  CAR.KONA_EV: {
+    (Ecu.esp, 0x7D1, None): [b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000\xf1\xa01.05', ],
+    (Ecu.fwdCamera, 0x7C4, None): [b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', ],
+    (Ecu.eps, 0x7D4, None): [b'\xf1\x00OS  MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', ],
+    (Ecu.fwdRadar, 0x7D0, None): [b'\xf1\x00OSev SCC F-CUP      1.00 1.01 99110-K4000         \xf1\xa01.01', ],
+  },
+  CAR.KIA_NIRO_EV: {
+    (Ecu.fwdRadar, 0x7D0, None): [
+      b'\xf1\x00DEev SCC F-CUP      1.00 1.03 96400-Q4100         \xf1\xa01.03',
+      b'\xf1\x00DEev SCC F-CUP      1.00 1.00 99110-Q4000         \xf1\xa01.00',
+    ],
+    (Ecu.esp, 0x7D1, None): [
+      b'\xf1\xa01.06',
+      b'\xf1\xa01.07',
+    ],
+    (Ecu.eps, 0x7D4, None): [
+      b'\xf1\x00DE  MDPS C 1.00 1.05 56310Q4000\x00 4DEEC105',
+      b'\xf1\x00DE  MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105',
+    ],
+    (Ecu.fwdCamera, 0x7C4, None): [
+      b'\xf1\x00DEE MFC  AT USA LHD 1.00 1.03 95740-Q4000 180821',
+      b'\xf1\x00DEE MFC  AT EUR LHD 1.00 1.00 99211-Q4000 191211',
+    ],
+  },
   CAR.KIA_OPTIMA: {
     (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00JF__ SCC F-CUP      1.00 1.00 96400-D4110         '],
     (Ecu.esp, 0x7d1, None): [b'\xf1\x00JF ESC \v 11 \x18\x030 58920-D5180',],
@@ -272,15 +310,15 @@ FEATURES = {
   # which message has the gear
   "use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
   "use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_2019, CAR.VELOSTER]),
-  "use_elect_gears": set([CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ]),
+  "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ]),
 
   # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
-  "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_FORTE, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]),
+  "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]),
 
   "use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA]),
 }
 
-EV_HYBRID = set([CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV])
+EV_HYBRID = set([CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV])
 
 DBC = {
   CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
@@ -292,6 +330,7 @@ DBC = {
   CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', None),
   CAR.IONIQ: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None),

selfdrive/car/interfaces.py

@@ -42,15 +42,15 @@ class CarInterfaceBase():
     raise NotImplementedError
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     raise NotImplementedError
 
   # returns a set of default params to avoid repetition in car specific params
   @staticmethod
-  def get_std_params(candidate, fingerprint, has_relay):
+  def get_std_params(candidate, fingerprint):
     ret = car.CarParams.new_message()
     ret.carFingerprint = candidate
-    ret.isPandaBlack = has_relay
+    ret.isPandaBlack = True # TODO: deprecate this field
 
     # standard ALC params
     ret.steerControlType = car.CarParams.SteerControlType.torque

selfdrive/car/mazda/interface.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.config import Conversions as CV
-from selfdrive.car.mazda.values import CAR, LKAS_LIMITS, FINGERPRINTS, ECU_FINGERPRINT, Ecu
+from selfdrive.car.mazda.values import CAR, LKAS_LIMITS
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -15,8 +15,8 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 4.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     ret.carName = "mazda"
     ret.safetyModel = car.CarParams.SafetyModel.mazda
@@ -67,7 +67,7 @@ class CarInterface(CarInterfaceBase):
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
 
-    ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
+    ret.enableCamera = True
 
     return ret
 

selfdrive/car/mazda/values.py

@@ -71,10 +71,6 @@ FINGERPRINTS = {
   ],
 }
 
-ECU_FINGERPRINT = {
-  Ecu.fwdCamera: [579],   # steer torque cmd
-}
-
 
 DBC = {
   CAR.CX5: dbc_dict('mazda_2017', None),

selfdrive/car/mock/interface.py

@@ -33,8 +33,8 @@ class CarInterface(CarInterfaceBase):
     return accel
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "mock"
     ret.safetyModel = car.CarParams.SafetyModel.noOutput
     ret.mass = 1700.

selfdrive/car/nissan/interface.py

@@ -14,9 +14,9 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 4.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
 
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "nissan"
     ret.safetyModel = car.CarParams.SafetyModel.nissan
 

selfdrive/car/subaru/interface.py

@@ -11,8 +11,8 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 4.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     ret.carName = "subaru"
     ret.radarOffCan = True
@@ -24,11 +24,8 @@ class CarInterface(CarInterfaceBase):
 
     # Subaru port is a community feature, since we don't own one to test
     ret.communityFeature = True
-
     ret.dashcamOnly = candidate in PREGLOBAL_CARS
 
-    # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches)
-    # was never released
     ret.enableCamera = True
 
     ret.steerRateCost = 0.7

selfdrive/car/subaru/values.py

@@ -78,10 +78,6 @@ STEER_THRESHOLD = {
   CAR.OUTBACK_PREGLOBAL_2018: 75,
 }
 
-ECU_FINGERPRINT = {
-  Ecu.fwdCamera: [290, 356],   # steer torque cmd
-}
-
 DBC = {
   CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None),
   CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None),

selfdrive/car/tests/test_car_interfaces.py

@@ -25,36 +25,35 @@ class TestCarInterfaces(unittest.TestCase):
 
       car_fw = []
 
-      for has_relay in [True, False]:
+      car_params = CarInterface.get_params(car_name, fingerprints, car_fw)
-        car_params = CarInterface.get_params(car_name, fingerprints, has_relay, car_fw)
+      car_interface = CarInterface(car_params, CarController, CarState)
-        car_interface = CarInterface(car_params, CarController, CarState)
+      assert car_params
-        assert car_params
+      assert car_interface
-        assert car_interface
 
-        self.assertGreater(car_params.mass, 1)
+      self.assertGreater(car_params.mass, 1)
-        self.assertGreater(car_params.steerRateCost, 1e-3)
+      self.assertGreater(car_params.steerRateCost, 1e-3)
 
-        tuning = car_params.lateralTuning.which()
+      tuning = car_params.lateralTuning.which()
-        if tuning == 'pid':
+      if tuning == 'pid':
-          self.assertTrue(len(car_params.lateralTuning.pid.kpV))
+        self.assertTrue(len(car_params.lateralTuning.pid.kpV))
-        elif tuning == 'lqr':
+      elif tuning == 'lqr':
-          self.assertTrue(len(car_params.lateralTuning.lqr.a))
+        self.assertTrue(len(car_params.lateralTuning.lqr.a))
-        elif tuning == 'indi':
+      elif tuning == 'indi':
-          self.assertGreater(car_params.lateralTuning.indi.outerLoopGain, 1e-3)
+        self.assertGreater(car_params.lateralTuning.indi.outerLoopGain, 1e-3)
 
-        # Run car interface
+      # Run car interface
-        CC = car.CarControl.new_message()
+      CC = car.CarControl.new_message()
-        for _ in range(10):
+      for _ in range(10):
-          car_interface.update(CC, [])
+        car_interface.update(CC, [])
-          car_interface.apply(CC)
+        car_interface.apply(CC)
-          car_interface.apply(CC)
+        car_interface.apply(CC)
 
-        CC = car.CarControl.new_message()
+      CC = car.CarControl.new_message()
-        CC.enabled = True
+      CC.enabled = True
-        for _ in range(10):
+      for _ in range(10):
-          car_interface.update(CC, [])
+        car_interface.update(CC, [])
-          car_interface.apply(CC)
+        car_interface.apply(CC)
-          car_interface.apply(CC)
+        car_interface.apply(CC)
 
       # Test radar interface
       RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % car_params.carName).RadarInterface

selfdrive/car/toyota/interface.py

@@ -14,8 +14,8 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 3.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):  # pylint: disable=dangerous-default-value
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # pylint: disable=dangerous-default-value
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     ret.carName = "toyota"
     ret.safetyModel = car.CarParams.SafetyModel.toyota
@@ -243,7 +243,7 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
       ret.lateralTuning.pid.kf = 0.00007
 
-    elif candidate == CAR.LEXUS_NXH:
+    elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX]:
       stop_and_go = True
       ret.safetyParam = 73
       ret.wheelbase = 2.66
@@ -253,6 +253,16 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
       ret.lateralTuning.pid.kf = 0.00006
 
+    elif candidate == CAR.PRIUS_TSS2:
+      stop_and_go = True
+      ret.safetyParam = 73
+      ret.wheelbase = 2.70002 #from toyota online sepc.
+      ret.steerRatio = 13.4   # True steerRation from older prius
+      tire_stiffness_factor = 0.6371   # hand-tune
+      ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.15]]
+      ret.lateralTuning.pid.kf = 0.00007818594
+
     ret.steerRateCost = 1.
     ret.centerToFront = ret.wheelbase * 0.44
 
@@ -265,7 +275,7 @@ class CarInterface(CarInterfaceBase):
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
 
-    ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
+    ret.enableCamera = True
     # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it
     smartDsu = 0x2FF in fingerprint[0]
     # In TSS2 cars the camera does long control
@@ -317,8 +327,6 @@ class CarInterface(CarInterfaceBase):
     # events
     events = self.create_common_events(ret)
 
-    if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera and not self.CP.isPandaBlack:
-      events.add(EventName.invalidGiraffeToyota)
     if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl:
       events.add(EventName.lowSpeedLockout)
     if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl:

selfdrive/car/toyota/values.py

@@ -13,6 +13,7 @@ class SteerLimitParams:
 
 class CAR:
   PRIUS = "TOYOTA PRIUS 2017"
+  PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021"
   RAV4H = "TOYOTA RAV4 HYBRID 2017"
   RAV4 = "TOYOTA RAV4 2017"
   COROLLA = "TOYOTA COROLLA 2017"
@@ -39,28 +40,28 @@ class CAR:
   LEXUS_CTH = "LEXUS CT 200H 2018"
   RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
   LEXUS_NXH = "LEXUS NX300H 2018"
-
+  LEXUS_NX = "LEXUS NX300 2018"
 
 # addr: (ecu, cars, bus, 1/freq*100, vl)
 STATIC_MSGS = [
-  (0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1,   3, b'\xf4\x01\x90\x83\x00\x37'),
+  (0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1,   3, b'\xf4\x01\x90\x83\x00\x37'),
   (0x128, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1,   3, b'\x03\x00\x20\x00\x00\x52'),
-  (0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1,   2, b'\x00\x00\x00\x46'),
+  (0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1,   2, b'\x00\x00\x00\x46'),
-  (0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1,   7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
+  (0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1,   7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
-  (0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1,   7, b'\x00\x1e\x00\x00\x00\x80\x07'),
+  (0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1,   7, b'\x00\x1e\x00\x00\x00\x80\x07'),
   (0X161, Ecu.dsu, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH), 1,  7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
-  (0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0,   3, b'\x00\x00\x00\x00\x00\x00\x8c'),
+  (0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0,   3, b'\x00\x00\x00\x00\x00\x00\x8c'),
   (0x2E6, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,   3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
   (0x2E7, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,   3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
   (0x33E, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,  20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
-  (0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0,   5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
+  (0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0,   5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
-  (0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.HIGHLANDERH), 0,  20, b'\x00\x00\x00\x80\x03\x00\x08'),
+  (0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0,  20, b'\x00\x00\x00\x80\x03\x00\x08'),
   (0x365, Ecu.dsu, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0,  20, b'\x00\x00\x00\x80\xfc\x00\x08'),
-  (0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.HIGHLANDERH), 0,  20, b'\x00\x00\x4d\x82\x40\x02\x00'),
+  (0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0,  20, b'\x00\x00\x4d\x82\x40\x02\x00'),
   (0x366, Ecu.dsu, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0,  20, b'\x00\x72\x07\xff\x09\xfe\x00'),
   (0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
   (0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1,  100, b'\x00\x00\x01\x79'),
-  (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
+  (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
 ]
 
 ECU_FINGERPRINT = {
@@ -265,11 +266,17 @@ FINGERPRINTS = {
   }],
   CAR.LEXUS_NXH: [{
     36: 8, 37: 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, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 800: 8, 810: 2, 812: 3, 818: 8, 822: 8, 824: 8, 835: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 889: 8, 891: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 987: 8, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+  }],
+  CAR.LEXUS_NX: [{
+    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 658: 8, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 810: 2, 812: 3, 818: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+  }],
+  CAR.PRIUS_TSS2: [{
+    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 765: 8, 800: 8, 810: 2, 814: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 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, 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, 1172: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1593: 8, 1595: 8, 1649: 8, 1653: 8, 1654: 8, 1655: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   }]
 }
 
 # Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection
-IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2]
+IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, CAR.LEXUS_NX]
 
 FW_VERSIONS = {
   CAR.AVALON: {
@@ -300,6 +307,7 @@ FW_VERSIONS = {
       b'\x018966306Q3100\x00\x00\x00\x00',
       b'\x018966306Q4000\x00\x00\x00\x00',
       b'\x018966306Q4100\x00\x00\x00\x00',
+      b'\x018966306Q4200\x00\x00\x00\x00',
       b'\x018966333P3100\x00\x00\x00\x00',
       b'\x018966333P3200\x00\x00\x00\x00',
       b'\x018966333P4200\x00\x00\x00\x00',
@@ -309,6 +317,7 @@ FW_VERSIONS = {
       b'\x018966333P4700\x00\x00\x00\x00',
       b'\x018966333Q6000\x00\x00\x00\x00',
       b'\x018966333Q6200\x00\x00\x00\x00',
+      b'\x018966333Q6300\x00\x00\x00\x00',
       b'\x018966333W6000\x00\x00\x00\x00',
     ],
     (Ecu.dsu, 0x791, None): [
@@ -317,6 +326,7 @@ FW_VERSIONS = {
       b'8821F0603300    ',
       b'8821F0607200    ',
       b'8821F0608000    ',
+      b'8821F0608200    ',
       b'8821F0609100    ',
     ],
     (Ecu.esp, 0x7b0, None): [
@@ -330,6 +340,7 @@ FW_VERSIONS = {
       b'8965B33540\x00\x00\x00\x00\x00\x00',
       b'8965B33542\x00\x00\x00\x00\x00\x00',
       b'8965B33580\x00\x00\x00\x00\x00\x00',
+      b'8965B33581\x00\x00\x00\x00\x00\x00',
       b'8965B33621\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [  # Same as 0x791
@@ -338,6 +349,7 @@ FW_VERSIONS = {
       b'8821F0603300    ',
       b'8821F0607200    ',
       b'8821F0608000    ',
+      b'8821F0608200    ',
       b'8821F0609100    ',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
@@ -554,8 +566,10 @@ FW_VERSIONS = {
       b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
       b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
       b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
+      b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
+      b'\x018965B1255000\x00\x00\x00\x00',
       b'8965B12361\x00\x00\x00\x00\x00\x00',
       b'\x018965B12350\x00\x00\x00\x00\x00\x00',
       b'\x018965B12470\x00\x00\x00\x00\x00\x00',
@@ -680,6 +694,7 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\x0230E40000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x0230EA2000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x0230EA2100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'8821F4702100\x00\x00\x00\x00',
@@ -731,23 +746,33 @@ FW_VERSIONS = {
     (Ecu.engine, 0x700, None): [
       b'\x018966353M7100\x00\x00\x00\x00',
       b'\x018966353Q2300\x00\x00\x00\x00',
+      b'\x018966353R1100\x00\x00\x00\x00',
       b'\x018966353R8100\x00\x00\x00\x00',
     ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\x02353P7000\x00\x00\x00\x00\x00\x00\x00\x00530J5000\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
     (Ecu.esp, 0x7b0, None): [
       b'F152653330\x00\x00\x00\x00\x00\x00',
+      b'F152653301\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.dsu, 0x791, None): [
       b'881515306400\x00\x00\x00\x00',
       b'881515306500\x00\x00\x00\x00',
+      b'881515306200\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B53271\x00\x00\x00\x00\x00\x00',
+      b'8965B53280\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'8821F4702300\x00\x00\x00\x00',
+      b'8821F4702100\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'8646F5301300\x00\x00\x00\x00',
       b'8646F5301400\x00\x00\x00\x00',
+      b'8646F5301200\x00\x00\x00\x00',
     ],
   },
   CAR.PRIUS: {
@@ -923,9 +948,11 @@ FW_VERSIONS = {
       b'\x01896634A19100\x00\x00\x00\x00',
       b'\x01896634A20000\x00\x00\x00\x00',
       b'\x01896634A22000\x00\x00\x00\x00',
+      b'\x01896634A45000\x00\x00\x00\x00',
       b'\x01896634A46000\x00\x00\x00\x00',
       b'\x01F152642551\x00\x00\x00\x00\x00\x00',
       b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
+      b'\x028966342V1000\x00\x00\x00\x00897CF1202001\x00\x00\x00\x00',
       b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
       b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
     ],
@@ -943,6 +970,7 @@ FW_VERSIONS = {
     (Ecu.eps, 0x7a1, None): [
       b'8965B42170\x00\x00\x00\x00\x00\x00',
       b'8965B42171\x00\x00\x00\x00\x00\x00',
+      b'8965B42180\x00\x00\x00\x00\x00\x00',
       b'8965B42181\x00\x00\x00\x00\x00\x00',
       b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00',
       b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00',
@@ -963,6 +991,7 @@ FW_VERSIONS = {
   },
   CAR.RAV4H_TSS2: {
     (Ecu.engine, 0x700, None): [
+      b'\x01896634A15000\x00\x00\x00\x00',
       b'\x018966342M5000\x00\x00\x00\x00',
       b'\x018966342X6000\x00\x00\x00\x00',
       b'\x018966342W8000\x00\x00\x00\x00',
@@ -978,7 +1007,9 @@ FW_VERSIONS = {
       b'F152642531\x00\x00\x00\x00\x00\x00',
       b'F152642532\x00\x00\x00\x00\x00\x00',
       b'F152642521\x00\x00\x00\x00\x00\x00',
+      b'F152642540\x00\x00\x00\x00\x00\x00',
       b'F152642541\x00\x00\x00\x00\x00\x00',
+      b'F152642542\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
       b'8965B42170\x00\x00\x00\x00\x00\x00',
@@ -1068,9 +1099,30 @@ FW_VERSIONS = {
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
+      b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
     ],
   },
+  CAR.LEXUS_NX: {
+    (Ecu.engine, 0x700, None): [
+      b'\x01896637851000\x00\x00\x00\x00',
+    ],
+    (Ecu.esp, 0x7b0, None): [
+      b'F152678140\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.dsu, 0x791, None): [
+      b'881517803100\x00\x00\x00\x00',
+    ],
+    (Ecu.eps, 0x7a1, None): [
+      b'8965B78060\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x750, 0xf): [
+      b'8821F4702300\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'8646F7801100\x00\x00\x00\x00',
+    ],
+  },
   CAR.LEXUS_NXH: {
     (Ecu.engine, 0x7e0, None): [
       b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -1219,6 +1271,23 @@ FW_VERSIONS = {
       b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
     ],
   },
+  CAR.PRIUS_TSS2: {
+    (Ecu.engine, 0x700, None): [
+      b'\x038966347C1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00',
+    ],
+    (Ecu.esp, 0x7b0, None): [
+      b'F152647520\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.eps, 0x7a1, None): [
+      b'8965B47070\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x750, 0xf): [
+      b'\x018821F3301400\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'\x028646F4707000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
+    ],
+  },
 }
 
 STEER_THRESHOLD = 100
@@ -1251,8 +1320,10 @@ DBC = {
   CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'),
   CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
   CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'),
+  CAR.LEXUS_NX: dbc_dict('lexus_nx300_2018_pt_generated', 'toyota_adas'),
+  CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
 }
 
 NO_DSU_CAR = set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2])
-TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2])
+TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2])
-NO_STOP_TIMER_CAR = set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2])  # no resume button press required
+NO_STOP_TIMER_CAR = set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2])  # no resume button press required

selfdrive/car/volkswagen/interface.py

@@ -18,8 +18,8 @@ class CarInterface(CarInterfaceBase):
     return float(accel) / 4.0
 
   @staticmethod
-  def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
+  def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
-    ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     # VW port is a community feature, since we don't own one to test
     ret.communityFeature = True

selfdrive/common/clutil.c

@@ -39,7 +39,7 @@ void clu_init(void) {
 cl_device_id cl_get_device_id(cl_device_type device_type) {
   bool opencl_platform_found = false;
   cl_device_id device_id = NULL;
-  
+
   cl_uint num_platforms = 0;
   int err = clGetPlatformIDs(0, NULL, &num_platforms);
   assert(err == 0);
@@ -66,7 +66,7 @@ cl_device_id cl_get_device_id(cl_device_type device_type) {
     break;
   }
   free(platform_ids);
-  
+
   if (!opencl_platform_found) {
     printf("No valid openCL platform found\n");
     assert(opencl_platform_found);

selfdrive/common/i2c.cc

@@ -12,7 +12,10 @@
 
 #ifdef QCOM2
 // TODO: decide if we want to isntall libi2c-dev everywhere
-#include <linux/i2c-dev.h>
+extern "C" {
+  #include <linux/i2c-dev.h>
+  #include <i2c/smbus.h>
+}
 
 I2CBus::I2CBus(uint8_t bus_id){
   char bus_name[20];

selfdrive/common/mat.h

@@ -1,5 +1,4 @@
-#ifndef COMMON_MAT_H
+#pragma once
-#define COMMON_MAT_H
 
 typedef struct vec3 {
 	float v[3];
@@ -17,7 +16,7 @@ typedef struct mat4 {
   float v[4*4];
 } mat4;
 
-static inline mat3 matmul3(const mat3 a, const mat3 b) {
+static inline mat3 matmul3(const mat3 &a, const mat3 &b) {
   mat3 ret = {{0.0}};
   for (int r=0; r<3; r++) {
     for (int c=0; c<3; c++) {
@@ -31,7 +30,7 @@ static inline mat3 matmul3(const mat3 a, const mat3 b) {
   return ret;
 }
 
-static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
+static inline vec3 matvecmul3(const mat3 &a, const vec3 &b) {
   vec3 ret = {{0.0}};
   for (int r=0; r<3; r++) {
     for (int c=0; c<3; c++) {
@@ -41,7 +40,7 @@ static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
   return ret;
 }
 
-static inline mat4 matmul(const mat4 a, const mat4 b) {
+static inline mat4 matmul(const mat4 &a, const mat4 &b) {
   mat4 ret = {{0.0}};
   for (int r=0; r<4; r++) {
     for (int c=0; c<4; c++) {
@@ -55,7 +54,7 @@ static inline mat4 matmul(const mat4 a, const mat4 b) {
   return ret;
 }
 
-static inline vec4 matvecmul(const mat4 a, const vec4 b) {
+static inline vec4 matvecmul(const mat4 &a, const vec4 &b) {
   vec4 ret = {{0.0}};
   for (int r=0; r<4; r++) {
     for (int c=0; c<4; c++) {
@@ -67,7 +66,7 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) {
 
 // scales the input and output space of a transformation matrix
 // that assumes pixel-center origin.
-static inline mat3 transform_scale_buffer(const mat3 in, float s) {
+static inline mat3 transform_scale_buffer(const mat3 &in, float s) {
   // in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
 
   mat3 transform_out = {{
@@ -84,5 +83,3 @@ static inline mat3 transform_scale_buffer(const mat3 in, float s) {
 
   return matmul3(transform_in, matmul3(in, transform_out));
 }
-
-#endif

selfdrive/common/modeldata.h

@@ -1,6 +1,9 @@
 #pragma once
 
 #define MODEL_PATH_DISTANCE 192
+#define TRAJECTORY_SIZE 33
+#define MIN_DRAW_DISTANCE 10.0
+#define MAX_DRAW_DISTANCE 100.0
 #define POLYFIT_DEGREE 4
 #define SPEED_PERCENTILES 10
 #define DESIRE_PRED_SIZE 32

selfdrive/common/params.cc

@@ -49,30 +49,17 @@ void params_sig_handler(int signal) {
 }
 
 static int fsync_dir(const char* path){
-  int result = 0;
   int fd = open(path, O_RDONLY, 0755);
-
   if (fd < 0){
-    result = -1;
+    return -1;
-    goto cleanup;
-  }
-
-  result = fsync(fd);
-  if (result < 0) {
-    goto cleanup;
-  }
-
- cleanup:
-  int result_close = 0;
-  if (fd >= 0){
-    result_close = close(fd);
   }
 
+  int result = fsync(fd);
+  int result_close = close(fd);
   if (result_close < 0) {
-    return result_close;
+    result = result_close;
-  } else {
-    return result;
   }
+  return result;
 }
 
 static int mkdir_p(std::string path) {
@@ -237,10 +224,6 @@ cleanup:
 }
 
 int Params::delete_db_value(std::string key) {
-  return delete_db_value(key.c_str());
-}
-
-int Params::delete_db_value(const char* key) {
   int lock_fd = -1;
   int result;
   std::string path;
@@ -256,7 +239,7 @@ int Params::delete_db_value(const char* key) {
   }
 
   // Delete value.
-  path = params_path + "/d/" + std::string(key);
+  path = params_path + "/d/" + key;
   result = remove(path.c_str());
   if (result != 0) {
     result = ERR_NO_VALUE;

selfdrive/common/params.h

@@ -33,7 +33,6 @@ public:
   // Delete a value from the params database.
   // Inputs are the same as read_db_value, without value and value_sz.
   int delete_db_value(std::string key);
-  int delete_db_value(const char* key);
 
   // Reads a value from the params database, blocking until successful.
   // Inputs are the same as read_db_value.

selfdrive/common/spinner.h

@@ -1,14 +0,0 @@
-#ifndef COMMON_SPINNER_H
-#define COMMON_SPINNER_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-int spin(int argc, char** argv);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.7.10-release"
+#define COMMA_VERSION "0.8.0-d56e04c0-2020-11-24T21:53:22"

selfdrive/common/visionipc.h

@@ -57,6 +57,7 @@ typedef struct VisionStreamBufs {
 typedef struct VIPCBufExtra {
   // only for yuv
   uint32_t frame_id;
+  uint64_t timestamp_sof;
   uint64_t timestamp_eof;
 } VIPCBufExtra;
 

selfdrive/controls/controlsd.py

@@ -52,7 +52,7 @@ class Controls:
 
     self.sm = sm
     if self.sm is None:
-      self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration',
+      self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'frontFrame',
                                      'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
 
     self.can_sock = can_sock
@@ -61,12 +61,10 @@ class Controls:
       self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
 
     # wait for one health and one CAN packet
-    hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
-    has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
     print("Waiting for CAN messages...")
     get_one_can(self.can_sock)
 
-    self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)
+    self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
 
     # read params
     params = Params()
@@ -131,7 +129,7 @@ class Controls:
     self.sm['dMonitoringState'].awarenessStatus = 1.
     self.sm['dMonitoringState'].faceDetected = False
 
-    self.startup_event = get_startup_event(car_recognized, controller_available, hw_type)
+    self.startup_event = get_startup_event(car_recognized, controller_available)
 
     if not sounds_available:
       self.events.add(EventName.soundsUnavailable, static=True)
@@ -141,8 +139,6 @@ class Controls:
       self.events.add(EventName.communityFeatureDisallowed, static=True)
     if not car_recognized:
       self.events.add(EventName.carUnrecognized, static=True)
-    if hw_type == HwType.whitePanda:
-      self.events.add(EventName.whitePandaUnsupported, static=True)
 
     # controlsd is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)
@@ -205,7 +201,8 @@ class Controls:
 
     if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL):
       self.events.add(EventName.canError)
-    if self.mismatch_counter >= 200:
+    if (self.sm['health'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \
+       self.mismatch_counter >= 200:
       self.events.add(EventName.controlsMismatch)
     if not self.sm.alive['plan'] and self.sm.alive['pathPlan']:
       # only plan not being received: radar not communicating
@@ -235,8 +232,11 @@ class Controls:
       self.events.add(EventName.relayMalfunction)
     if self.sm['plan'].fcw:
       self.events.add(EventName.fcw)
-    if self.sm['model'].frameDropPerc > 1 and (not SIMULATION):
+    if not self.sm.alive['frontFrame'] and (self.sm.frame > 5 / DT_CTRL) and not SIMULATION:
-        self.events.add(EventName.modeldLagging)
+      self.events.add(EventName.cameraMalfunction)
+
+    if self.sm['model'].frameDropPerc > 20 and not SIMULATION:
+      self.events.add(EventName.modeldLagging)
 
     # Only allow engagement with brake pressed when stopped behind another stopped car
     if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
@@ -456,7 +456,7 @@ class Controls:
       self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
 
     force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \
-                    (self.state == State.softDisabling)
+                  (self.state == State.softDisabling)
 
     steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD
 

selfdrive/controls/lib/alerts_offroad.json

@@ -36,5 +36,9 @@
   "Offroad_NeosUpdate": {
     "text": "An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install.",
     "severity": 0
+  },
+  "Offroad_HardwareUnsupported": {
+    "text": "White and grey panda are unsupported. Upgrade to comma two or black panda.",
+    "severity": 0
   }
 }

selfdrive/controls/lib/events.py

@@ -252,31 +252,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
       Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
   },
 
-  EventName.startupGreyPanda: {
-    ET.PERMANENT: Alert(
-      "WARNING: Grey panda is deprecated",
-      "Upgrade to comma two or black panda",
-      AlertStatus.userPrompt, AlertSize.mid,
-      Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
-  },
-
-  EventName.invalidGiraffeToyota: {
-    ET.PERMANENT: Alert(
-      "Unsupported Giraffe Configuration",
-      "Visit comma.ai/tg",
-      AlertStatus.normal, AlertSize.mid,
-      Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
-  },
-
-  EventName.whitePandaUnsupported: {
-    ET.PERMANENT: Alert(
-      "White Panda No Longer Supported",
-      "Upgrade to comma two or black panda",
-      AlertStatus.normal, AlertSize.mid,
-      Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
-    ET.NO_ENTRY: NoEntryAlert("Unsupported Hardware"),
-  },
-
   EventName.invalidLkasSetting: {
     ET.PERMANENT: Alert(
       "Stock LKAS is turned on",
@@ -480,6 +455,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
     ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Contact Support"),
   },
 
+  EventName.cameraMalfunction: {
+    ET.PERMANENT: NormalPermanentAlert("Camera Malfunction", "Contact Support"),
+  },
+
   # ********** events that affect controls state transitions **********
 
   EventName.pcmEnable: {

selfdrive/controls/lib/lane_planner.py

@@ -5,9 +5,9 @@ from cereal import log
 CAMERA_OFFSET = 0.06  # m from center car to camera
 
 
-def compute_path_pinv(l=50):
+def compute_path_pinv(length=50):
   deg = 3
-  x = np.arange(l*1.0)
+  x = np.arange(length*1.0)
   X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
   pinv = np.linalg.pinv(X)
   return pinv
@@ -21,31 +21,7 @@ def eval_poly(poly, x):
   return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3
 
 
-def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego):
+class LanePlanner:
-  # This will improve behaviour when lanes suddenly widen
-  # these numbers were tested on 2000segments and found to work well
-  lane_width = min(4.0, lane_width)
-  width_poly = l_poly - r_poly
-  prob_mods = []
-  for t_check in [0.0, 1.5, 3.0]:
-    width_at_t = eval_poly(width_poly, t_check * (v_ego + 7))
-    prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
-  mod = min(prob_mods)
-  l_prob = mod * l_prob
-  r_prob = mod * r_prob
-
-  path_from_left_lane = l_poly.copy()
-  path_from_left_lane[3] -= lane_width / 2.0
-  path_from_right_lane = r_poly.copy()
-  path_from_right_lane[3] += lane_width / 2.0
-
-  lr_prob = l_prob + r_prob - l_prob * r_prob
-
-  d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
-  return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
-
-
-class LanePlanner():
   def __init__(self):
     self.l_poly = [0., 0., 0., 0.]
     self.r_poly = [0., 0., 0., 0.]
@@ -59,6 +35,9 @@ class LanePlanner():
     self.l_prob = 0.
     self.r_prob = 0.
 
+    self.l_std = 0.
+    self.r_std = 0.
+
     self.l_lane_change_prob = 0.
     self.r_lane_change_prob = 0.
 
@@ -68,7 +47,9 @@ class LanePlanner():
   def parse_model(self, md):
     if len(md.leftLane.poly):
       self.l_poly = np.array(md.leftLane.poly)
+      self.l_std = float(md.leftLane.std)
       self.r_poly = np.array(md.rightLane.poly)
+      self.r_std = float(md.rightLane.std)
       self.p_poly = np.array(md.path.poly)
     else:
       self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv)  # left line
@@ -78,24 +59,47 @@ class LanePlanner():
     self.r_prob = md.rightLane.prob  # right line prob
 
     if len(md.meta.desireState):
-      self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1]
+      self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft]
-      self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1]
+      self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight]
 
   def update_d_poly(self, v_ego):
     # only offset left and right lane lines; offsetting p_poly does not make sense
     self.l_poly[3] += CAMERA_OFFSET
     self.r_poly[3] += CAMERA_OFFSET
 
+    # Reduce reliance on lanelines that are too far apart or
+    # will be in a few seconds
+    l_prob, r_prob = self.l_prob, self.r_prob
+    width_poly = self.l_poly - self.r_poly
+    prob_mods = []
+    for t_check in [0.0, 1.5, 3.0]:
+      width_at_t = eval_poly(width_poly, t_check * (v_ego + 7))
+      prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
+    mod = min(prob_mods)
+    l_prob *= mod
+    r_prob *= mod
+
+    # Reduce reliance on uncertain lanelines
+    l_std_mod = interp(self.l_std, [.15, .3], [1.0, 0.0])
+    r_std_mod = interp(self.r_std, [.15, .3], [1.0, 0.0])
+    l_prob *= l_std_mod
+    r_prob *= r_std_mod
+
     # Find current lanewidth
-    self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
+    self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty)
     current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
     self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
     speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
     self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
                       (1 - self.lane_width_certainty) * speed_lane_width
 
-    self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego)
+    clipped_lane_width = min(4.0, self.lane_width)
+    path_from_left_lane = self.l_poly.copy()
+    path_from_left_lane[3] -= clipped_lane_width / 2.0
+    path_from_right_lane = self.r_poly.copy()
+    path_from_right_lane[3] += clipped_lane_width / 2.0
 
-  def update(self, v_ego, md):
+    lr_prob = l_prob + r_prob - l_prob * r_prob
-    self.parse_model(md)
+
-    self.update_d_poly(v_ego)
+    d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
+    self.d_poly = lr_prob * d_poly_lane + (1.0 - lr_prob) * self.p_poly.copy()

selfdrive/controls/lib/pathplanner.py

@@ -43,6 +43,7 @@ DESIRES = {
 def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay):
   states[0].x = v_ego * delay
   states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay
+  states[0].y = states[0].x * math.sin(states[0].psi / 2)
   return states
 
 

selfdrive/debug/get_fingerprint.py

@@ -6,7 +6,6 @@
 # - connect to a Panda
 # - run selfdrive/boardd/boardd
 # - launching this script
-# - turn on the car in STOCK MODE (set giraffe switches properly).
 #   Note: it's very important that the car is in stock mode, in order to collect a complete fingerprint
 # - since some messages are published at low frequency, keep this script running for at least 30s,
 #   until all messages are received at least once

selfdrive/debug/test_fw_query_on_routes.py

@@ -37,7 +37,7 @@ if __name__ == "__main__":
   for route in tqdm(routes):
     route = route.rstrip()
     dongle_id, time = route.split('|')
-    qlog_path = f"cd:/{dongle_id}/{time}/1/qlog.bz2"
+    qlog_path = f"cd:/{dongle_id}/{time}/0/qlog.bz2"
 
     if dongle_id in dongles:
       continue
@@ -65,7 +65,7 @@ if __name__ == "__main__":
             live_fingerprint = args.car
 
           if live_fingerprint not in list(TOYOTA_FINGERPRINTS.keys()) + list(HONDA_FINGERPRINTS.keys()) + list(HYUNDAI_FINGERPRINTS.keys()):
-            continue
+            break
 
           candidates = match_fw_to_car(car_fw)
           if (len(candidates) == 1) and (list(candidates)[0] == live_fingerprint):

selfdrive/locationd/locationd.py

@@ -196,14 +196,14 @@ class Localizer():
     orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
     orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)])
     orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
+    initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
     if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:
       cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset")
-      initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
+      self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
-      self.reset_kalman(init_orient=initial_pose_ecef_quat)
       self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat)
     elif gps_est_error > 50:
       cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset")
-      self.reset_kalman()
+      self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
 
     self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R)
     self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R)
@@ -267,12 +267,14 @@ class Localizer():
       self.calib_from_device = self.device_from_calib.T
       self.calibrated = log.calStatus == 1
 
-  def reset_kalman(self, current_time=None, init_orient=None):
+  def reset_kalman(self, current_time=None, init_orient=None, init_pos=None):
     self.filter_time = current_time
     init_x = LiveKalman.initial_x.copy()
     # too nonlinear to init on completely wrong
     if init_orient is not None:
       init_x[3:7] = init_orient
+    if init_pos is not None:
+      init_x[:3] = init_pos
     self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
 
     self.observation_buffer = []

selfdrive/loggerd/loggerd.cc

@@ -67,7 +67,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
     .stream_type = VISION_STREAM_YUV,
     .filename = "fcamera.hevc",
     .frame_packet_name = "frame",
-    .encode_idx_name = "encodeIdx",
     .fps = MAIN_FPS,
     .bitrate = MAIN_BITRATE,
     .is_h265 = true,
@@ -78,7 +77,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
     .stream_type = VISION_STREAM_YUV_FRONT,
     .filename = "dcamera.hevc",
     .frame_packet_name = "frontFrame",
-    .encode_idx_name = "frontEncodeIdx",
     .fps = MAIN_FPS, // on EONs, more compressed this way
     .bitrate = DCAM_BITRATE,
     .is_h265 = true,
@@ -89,7 +87,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
     .stream_type = VISION_STREAM_YUV_WIDE,
     .filename = "ecamera.hevc",
     .frame_packet_name = "wideFrame",
-    .encode_idx_name = "wideEncodeIdx",
     .fps = MAIN_FPS,
     .bitrate = MAIN_BITRATE,
     .is_h265 = true,
@@ -235,9 +232,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) {
   s.num_encoder += 1;
   pthread_mutex_unlock(&s.rotate_lock);
 
-  PubSocket *idx_sock = PubSocket::create(s.ctx, cameras_logged[cam_idx].encode_idx_name);
-  assert(idx_sock != NULL);
-
   LoggerHandle *lh = NULL;
 
   while (!do_exit) {
@@ -377,8 +371,12 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) {
 
         // publish encode index
         MessageBuilder msg;
-        auto eidx = msg.initEvent().initEncodeIdx();
+        // this is really ugly
+        auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initFrontEncodeIdx() :
+                    (cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideEncodeIdx() : msg.initEvent().initEncodeIdx());
         eidx.setFrameId(extra.frame_id);
+        eidx.setTimestampSof(extra.timestamp_sof);
+        eidx.setTimestampEof(extra.timestamp_eof);
   #ifdef QCOM2
         eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C);
   #else
@@ -390,10 +388,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) {
         eidx.setSegmentId(out_id);
 
         auto bytes = msg.toBytes();
-
-        if (idx_sock->send((char*)bytes.begin(), bytes.size()) < 0) {
-          printf("err sending encodeIdx pkt: %s\n", strerror(errno));
-        }
         if (lh) {
           lh_log(lh, bytes.begin(), bytes.size(), false);
         }
@@ -453,8 +447,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) {
     visionstream_destroy(&stream);
   }
 
-  delete idx_sock;
-
   if (encoder_inited) {
     LOG("encoder destroy");
     encoder_close(&encoder);
@@ -606,6 +598,10 @@ static void bootlog() {
 int main(int argc, char** argv) {
   int err;
 
+#ifdef QCOM
+  setpriority(PRIO_PROCESS, 0, -12);
+#endif
+
   if (argc > 1 && strcmp(argv[1], "--bootlog") == 0) {
     bootlog();
     return 0;
@@ -620,8 +616,6 @@ int main(int argc, char** argv) {
   record_front = Params().read_db_bool("RecordFront");
 #endif
 
-  setpriority(PRIO_PROCESS, 0, -12);
-
   clear_locks();
 
   signal(SIGINT, (sighandler_t)set_do_exit);
@@ -751,7 +745,7 @@ int main(int argc, char** argv) {
 
     if (s.logger.part > -1) {
       new_segment = true;
-      if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE) {
+      if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE && s.num_encoder > 0) {
         for (int cid=0;cid<=MAX_CAM_IDX;cid++) {
           // this *should* be redundant on tici since all camera frames are synced
           new_segment &= (((s.rotate_state[cid].stream_frame_id >= s.rotate_state[cid].last_rotate_frame_id + segment_length * MAIN_FPS) &&

selfdrive/manager.py

@@ -78,8 +78,10 @@ import traceback
 from multiprocessing import Process
 
 # Run scons
-spinner = Spinner(noop=(__name__ != "__main__" or not ANDROID))
+spinner = Spinner()
 spinner.update("0")
+if __name__ != "__main__":
+  spinner.close()
 
 if not prebuilt:
   for retry in [True, False]:
@@ -125,7 +127,8 @@ if not prebuilt:
             print("....%d" % i)
             time.sleep(1)
           subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
-          shutil.rmtree("/tmp/scons_cache")
+          shutil.rmtree("/tmp/scons_cache", ignore_errors=True)
+          shutil.rmtree("/data/scons_cache", ignore_errors=True)
         else:
           print("scons build failed after retry")
           sys.exit(1)
@@ -138,9 +141,8 @@ if not prebuilt:
         cloudlog.error("scons build failed\n" + error_s)
 
         # Show TextWindow
-        no_ui = __name__ != "__main__" or not ANDROID
         error_s = "\n \n".join(["\n".join(textwrap.wrap(e, 65)) for e in errors])
-        with TextWindow("openpilot failed to build\n \n" + error_s, noop=no_ui) as t:
+        with TextWindow("openpilot failed to build\n \n" + error_s) as t:
           t.wait_for_exit()
 
         exit(1)
@@ -371,11 +373,8 @@ def kill_managed_process(name):
         join_process(running[name], 15)
         if running[name].exitcode is None:
           cloudlog.critical("unkillable process %s failed to die!" % name)
-          # TODO: Use method from HARDWARE
+          os.system("date >> /sdcard/unkillable_reboot")
-          if ANDROID:
+          HARDWARE.reboot()
-            cloudlog.critical("FORCE REBOOTING PHONE!")
-            os.system("date >> /sdcard/unkillable_reboot")
-            os.system("reboot")
           raise RuntimeError
       else:
         cloudlog.info("killing %s with SIGKILL" % name)
@@ -398,8 +397,10 @@ def cleanup_all_processes(signal, frame):
 
 
 def send_managed_process_signal(name, sig):
-  if name not in running or name not in managed_processes:
+  if name not in running or name not in managed_processes or \
+     running[name].exitcode is not None:
     return
+
   cloudlog.info(f"sending signal {sig} to {name}")
   os.kill(running[name].pid, sig)
 

selfdrive/modeld/SConscript

@@ -6,10 +6,10 @@ libs = [cereal, messaging, common, 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'z
 TEST_THNEED = False
 
 common_src = [
-  "models/commonmodel.c",
+  "models/commonmodel.cc",
   "runners/snpemodel.cc",
   "transforms/loadyuv.c",
-  "transforms/transform.c"
+  "transforms/transform.cc"
 ]
 
 if arch == "aarch64":
@@ -19,16 +19,20 @@ if arch == "aarch64":
     lenv['CFLAGS'].append("-DUSE_THNEED")
     lenv['CXXFLAGS'].append("-DUSE_THNEED")
 elif arch == "larch64":
-  libs += ['gsl', 'CB', 'pthread']
+  libs += ['gsl', 'CB', 'pthread', 'dl']
+  if not TEST_THNEED:
+    common_src += ["thneed/thneed.cc"]
+    lenv['CFLAGS'].append("-DUSE_THNEED")
+    lenv['CXXFLAGS'].append("-DUSE_THNEED")
 else:
   libs += ['pthread']
 
-  # for tensorflow support
+  # for onnx support
-  common_src += ['runners/tfmodel.cc']
+  common_src += ['runners/onnxmodel.cc']
 
-  # tell runners to use tensorflow
+  # tell runners to use onnx
-  lenv['CFLAGS'].append("-DUSE_TF_MODEL")
+  lenv['CFLAGS'].append("-DUSE_ONNX_MODEL")
-  lenv['CXXFLAGS'].append("-DUSE_TF_MODEL")
+  lenv['CXXFLAGS'].append("-DUSE_ONNX_MODEL")
 
   if arch == "Darwin":
     # fix OpenCL

selfdrive/modeld/dmonitoringmodeld.cc

@@ -3,6 +3,7 @@
 #include <unistd.h>
 #include <signal.h>
 #include <cassert>
+#include <sys/resource.h>
 
 #include "common/visionbuf.h"
 #include "common/visionipc.h"
@@ -23,7 +24,7 @@ static void set_do_exit(int sig) {
 
 int main(int argc, char **argv) {
   int err;
-  set_realtime_priority(51);
+  setpriority(PRIO_PROCESS, 0, -15);
 
 #ifdef QCOM2
   set_core_affinity(5);
@@ -65,7 +66,7 @@ int main(int argc, char **argv) {
       double t2 = millis_since_boot();
 
       // send dm packet
-      dmonitoring_publish(pm, extra.frame_id, res);
+      dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0);
 
       LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
       last = t1;

selfdrive/modeld/modeld.cc

@@ -38,16 +38,15 @@ void* live_thread(void *arg) {
     -1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
     -1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
 
+  Eigen::Matrix<float, 3, 3> fcam_intrinsics;
 #ifndef QCOM2
-  Eigen::Matrix<float, 3, 3> eon_intrinsics;
+  fcam_intrinsics <<
-  eon_intrinsics <<
     910.0, 0.0, 582.0,
     0.0, 910.0, 437.0,
     0.0,   0.0,   1.0;
   float db_s = 0.5; // debayering does a 2x downscale
 #else
-  Eigen::Matrix<float, 3, 3> eon_intrinsics;
+  fcam_intrinsics <<
-  eon_intrinsics <<
     2648.0, 0.0, 1928.0/2,
     0.0, 2648.0, 1208.0/2,
     0.0,   0.0,   1.0;
@@ -69,7 +68,7 @@ void* live_thread(void *arg) {
         extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
       }
 
-      auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen;
+      auto camera_frame_from_road_frame = fcam_intrinsics * extrinsic_matrix_eigen;
       Eigen::Matrix<float, 3, 3> camera_frame_from_ground;
       camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0);
       camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
@@ -112,7 +111,7 @@ int main(int argc, char **argv) {
   assert(err == 0);
 
   // messaging
-  PubMaster pm({"model", "cameraOdometry"});
+  PubMaster pm({"modelV2", "model", "cameraOdometry"});
   SubMaster sm({"pathPlan", "frame"});
 
 #if defined(QCOM) || defined(QCOM2)
@@ -148,7 +147,7 @@ int main(int argc, char **argv) {
 
     // setup filter to track dropped frames
     const float dt = 1. / MODEL_FREQ;
-    const float ts = 5.0;  // 5 s filter time constant
+    const float ts = 10.0;  // filter time constant (s)
     const float frame_filter_k = (dt / ts) / (1. + dt / ts);
     float frames_dropped = 0;
 
@@ -158,6 +157,7 @@ int main(int argc, char **argv) {
     uint32_t frame_id = 0, last_vipc_frame_id = 0;
     double last = 0;
     int desire = -1;
+    uint32_t run_count = 0;
     while (!do_exit) {
       VIPCBuf *buf;
       VIPCBufExtra extra;
@@ -174,12 +174,14 @@ int main(int argc, char **argv) {
 
       if (sm.update(0) > 0){
         // TODO: path planner timeout?
-        desire = ((int)sm["pathPlan"].getPathPlan().getDesire()) - 1;
+        desire = ((int)sm["pathPlan"].getPathPlan().getDesire());
         frame_id = sm["frame"].getFrame().getFrameId();
       }
 
       double mt1 = 0, mt2 = 0;
       if (run_model_this_iter) {
+        run_count++;
+
         float vec_desire[DESIRE_LEN] = {0};
         if (desire >= 0 && desire < DESIRE_LEN) {
           vec_desire[desire] = 1.0;
@@ -194,16 +196,19 @@ int main(int argc, char **argv) {
             model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height,
                              model_transform, NULL, vec_desire);
         mt2 = millis_since_boot();
+        float model_execution_time = (mt2 - mt1) / 1000.0;
 
         // tracked dropped frames
         uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1;
         frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U);
-        float frame_drop_perc = frames_dropped / MODEL_FREQ;
+        if (run_count < 10) frames_dropped = 0;  // let frame drops warm up
+        float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
 
-        model_publish(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
+        model_publish(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time);
-        posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
+        model_publish_v2(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time);
+        posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof);
 
-        LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc);
+        LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio);
         last = mt1;
         last_vipc_frame_id = extra.frame_id;
       }

selfdrive/modeld/models/commonmodel.cc

@@ -59,6 +59,29 @@ void frame_free(ModelFrame* frame) {
   clReleaseMemObject(frame->transformed_y_cl);
 }
 
+void softmax(const float* input, float* output, size_t len) {
+  float max_val = -FLT_MAX;
+  for(int i = 0; i < len; i++) {
+    const float v = input[i];
+    if( v > max_val ) {
+      max_val = v;
+    }
+  }
+
+  float denominator = 0;
+  for(int i = 0; i < len; i++) {
+    float const v = input[i];
+    float const v_exp = expf(v - max_val);
+    denominator += v_exp;
+    output[i] = v_exp;
+  }
+
+  const float inv_denominator = 1. / denominator;
+  for(int i = 0; i < len; i++) {
+    output[i] *= inv_denominator;
+  }
+
+}
 
 float sigmoid(float input) {
   return 1 / (1 + expf(-input));

selfdrive/modeld/models/commonmodel.h

@@ -8,6 +8,7 @@
 #include <CL/cl.h>
 #endif
 
+#include <float.h>
 #include "common/mat.h"
 #include "transforms/transform.h"
 #include "transforms/loadyuv.h"
@@ -16,6 +17,7 @@
 extern "C" {
 #endif
 
+void softmax(const float* input, float* output, size_t len);
 float softplus(float input);
 float sigmoid(float input);
 

selfdrive/modeld/models/dmonitoring.cc

@@ -114,24 +114,39 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
                     resized_width, resized_height,
                     mode);
 
+  // prerotate to be cache aware
+  uint8_t *resized_buf_rot = get_buffer(s->resized_buf_rot, resized_width*resized_height*3/2);
+  uint8_t *resized_y_buf_rot = resized_buf_rot;
+  uint8_t *resized_u_buf_rot = resized_y_buf_rot + (resized_width * resized_height);
+  uint8_t *resized_v_buf_rot = resized_u_buf_rot + ((resized_width/2) * (resized_height/2));
+
+  libyuv::I420Rotate(resized_y_buf, resized_width,
+                     resized_u_buf, resized_width/2,
+                     resized_v_buf, resized_width/2,
+                     resized_y_buf_rot, resized_height,
+                     resized_u_buf_rot, resized_height/2,
+                     resized_v_buf_rot, resized_height/2,
+                     // negative height causes a vertical flip to match previous
+                     resized_width, -resized_height, libyuv::kRotate90);
+
   int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
   float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);
   // one shot conversion, O(n) anyway
   // yuvframe2tensor, normalize
-  for (int r = 0; r < MODEL_HEIGHT/2; r++) {
+  for (int c = 0; c < MODEL_WIDTH/2; c++) {
-    for (int c = 0; c < MODEL_WIDTH/2; c++) {
+    for (int r = 0; r < MODEL_HEIGHT/2; r++) {
       // Y_ul
-      net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c)*resized_height]);
-      // Y_ur
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]);
       // Y_dl
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c)*resized_height]);
+      // Y_ur
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c+1)*resized_height]);
       // Y_dr
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c+1)*resized_height]);
       // U
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + r + (c*resized_height/2)]);
       // V
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + r + (c*resized_height/2)]);
     }
   }
 
@@ -140,6 +155,10 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   //fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file);
   //fclose(dump_yuv_file);
 
+  // *** testing ***
+  // idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320)
+  // imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420))
+
   //FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb");
   //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2);
   //fclose(dump_yuv_file2);
@@ -165,11 +184,12 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   return ret;
 }
 
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res){
+void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time){
   // make msg
   MessageBuilder msg;
   auto framed = msg.initEvent().initDriverState();
   framed.setFrameId(frame_id);
+  framed.setModelExecutionTime(execution_time);
 
   kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
   kj::ArrayPtr<const float> face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta));

selfdrive/modeld/models/dmonitoring.h

@@ -29,6 +29,7 @@ typedef struct DMonitoringModelState {
   bool is_rhd;
   float output[OUTPUT_SIZE];
   std::vector<uint8_t> resized_buf;
+  std::vector<uint8_t> resized_buf_rot;
   std::vector<uint8_t> cropped_buf;
   std::vector<uint8_t> premirror_cropped_buf;
   std::vector<float> net_input_buf;
@@ -36,7 +37,7 @@ typedef struct DMonitoringModelState {
 
 void dmonitoring_init(DMonitoringModelState* s);
 DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height);
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res);
+void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time);
 void dmonitoring_free(DMonitoringModelState* s);
 
 #ifdef __cplusplus

selfdrive/modeld/models/driving.cc

@@ -9,14 +9,17 @@
 #include "common/params.h"
 #include "driving.h"
 
-#define PATH_IDX 0
+#define MIN_VALID_LEN 10.0
-#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1
+#define TRAJECTORY_SIZE 33
-#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2
+#define TRAJECTORY_TIME 10.0
-#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2
+#define TRAJECTORY_DISTANCE 192.0
-#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION
+#define PLAN_IDX 0
-#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2
+#define LL_IDX PLAN_IDX + PLAN_MHP_N*(PLAN_MHP_GROUP_SIZE)
-#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2
+#define LL_PROB_IDX LL_IDX + 4*2*2*33
-#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2
+#define RE_IDX LL_PROB_IDX + 4
+#define LEAD_IDX RE_IDX + 2*2*2*33
+#define LEAD_PROB_IDX LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE)
+#define DESIRE_STATE_IDX LEAD_PROB_IDX + 3
 #define META_IDX DESIRE_STATE_IDX + DESIRE_LEN
 #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
 #define OUTPUT_SIZE  POSE_IDX + POSE_SIZE
@@ -29,6 +32,8 @@
 // #define DUMP_YUV
 
 Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
+float X_IDXS[TRAJECTORY_SIZE];
+float T_IDXS[TRAJECTORY_SIZE];
 
 void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
   frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
@@ -63,9 +68,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
 #endif
 
   // Build Vandermonde matrix
-  for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
+  for(int i = 0; i < TRAJECTORY_SIZE; i++) {
     for(int j = 0; j < POLYFIT_DEGREE - 1; j++) {
-      vander(i, j) = pow(i, POLYFIT_DEGREE-j-1);
+      X_IDXS[i] = (TRAJECTORY_DISTANCE/1024.0) * (pow(i,2));
+      T_IDXS[i] = (TRAJECTORY_TIME/1024.0) * (pow(i,2));
+      vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1);
     }
   }
 }
@@ -76,7 +83,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
                            float *desire_in) {
 #ifdef DESIRE
   if (desire_in != NULL) {
-    for (int i = 0; i < DESIRE_LEN; i++) {
+    for (int i = 1; i < DESIRE_LEN; i++) {
       // Model decides when action is completed
       // so desire input is just a pulse triggered on rising edge
       if (desire_in[i] - s->prev_desire[i] > .99) {
@@ -107,13 +114,12 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
 
   // net outputs
   ModelDataRaw net_outputs;
-  net_outputs.path = &s->output[PATH_IDX];
+  net_outputs.plan = &s->output[PLAN_IDX];
-  net_outputs.left_lane = &s->output[LL_IDX];
+  net_outputs.lane_lines = &s->output[LL_IDX];
-  net_outputs.right_lane = &s->output[RL_IDX];
+  net_outputs.lane_lines_prob = &s->output[LL_PROB_IDX];
+  net_outputs.road_edges = &s->output[RE_IDX];
   net_outputs.lead = &s->output[LEAD_IDX];
-  net_outputs.long_x = &s->output[LONG_X_IDX];
+  net_outputs.lead_prob = &s->output[LEAD_PROB_IDX];
-  net_outputs.long_v = &s->output[LONG_V_IDX];
-  net_outputs.long_a = &s->output[LONG_A_IDX];
   net_outputs.meta = &s->output[DESIRE_STATE_IDX];
   net_outputs.pose = &s->output[POSE_IDX];
   return net_outputs;
@@ -151,35 +157,40 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
   out[3] = y0;
 }
 
-void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) {
+void fill_path(cereal::ModelData::PathData::Builder path, const float * data, float valid_len, int valid_len_idx) {
-  float points_arr[MODEL_PATH_DISTANCE];
+  float points_arr[TRAJECTORY_SIZE];
-  float stds_arr[MODEL_PATH_DISTANCE];
+  float stds_arr[TRAJECTORY_SIZE];
   float poly_arr[POLYFIT_DEGREE];
   float std;
-  float prob;
-  float valid_len;
 
-  // clamp to 5 and MODEL_PATH_DISTANCE
+  for (int i=0; i<TRAJECTORY_SIZE; i++) {
-  valid_len = fmin(MODEL_PATH_DISTANCE, fmax(5, data[MODEL_PATH_DISTANCE*2]));
+    // negative sign because mpc has left positive
-  for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
+    points_arr[i] = -data[30*i + 16];
-    points_arr[i] = data[i] + offset;
+    stds_arr[i] = exp(data[30*(33 + i) + 16]);
-    stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]) + 1e-6;
   }
-  if (has_prob) {
+  std = stds_arr[0];
-    prob =  sigmoid(data[MODEL_PATH_DISTANCE*2 + 1]);
+  poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
-  } else {
-    prob = 1.0;
-  }
-  std = softplus(data[MODEL_PATH_DISTANCE]) + 1e-6;
-  poly_fit(points_arr, stds_arr, poly_arr, valid_len);
 
-  if (std::getenv("DEBUG")){
+  kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
-    kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));
+  path.setPoly(poly);
-    path.setStds(stds);
+  path.setProb(1.0);
+  path.setStd(std);
+  path.setValidLen(valid_len);
+}
 
-    kj::ArrayPtr<const float> points(&points_arr[0], ARRAYSIZE(points_arr));
+void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * data, int ll_idx, float valid_len, int valid_len_idx, float prob) {
-    path.setPoints(points);
+  float points_arr[TRAJECTORY_SIZE];
+  float stds_arr[TRAJECTORY_SIZE];
+  float poly_arr[POLYFIT_DEGREE];
+  float std;
+
+  for (int i=0; i<TRAJECTORY_SIZE; i++) {
+    // negative sign because mpc has left positive
+    points_arr[i] = -data[2*33*ll_idx + 2*i];
+    stds_arr[i] = exp(data[2*33*(4 + ll_idx) + 2*i]);
   }
+  std = stds_arr[0];
+  poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
 
   kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
   path.setPoly(poly);
@@ -188,91 +199,275 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo
   path.setValidLen(valid_len);
 }
 
-void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx, int t_offset) {
+void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) {
-  const double x_scale = 10.0;
+  lead.setProb(prob);
-  const double y_scale = 10.0;
+  lead.setT(t);
+  float xyva_arr[LEAD_MHP_VALS];
+  float xyva_stds_arr[LEAD_MHP_VALS];
+  for (int i=0; i<LEAD_MHP_VALS; i++) {
+    xyva_arr[i] = data[LEAD_MHP_VALS + i];
+    xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
+  }
+  kj::ArrayPtr<const float> xyva(xyva_arr, LEAD_MHP_VALS);
+  kj::ArrayPtr<const float> xyva_stds(xyva_stds_arr, LEAD_MHP_VALS);
+  lead.setXyva(xyva);
+  lead.setXyvaStd(xyva_stds);
+}
 
-  lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE + t_offset]));
+void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) {
-  lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]);
+  lead.setProb(prob);
-  lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]));
+  lead.setDist(data[0]);
-  lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]);
+  lead.setStd(exp(data[LEAD_MHP_VALS]));
-  lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]));
+  // TODO make all msgs same format
-  lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]);
+  lead.setRelY(-data[1]);
-  lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]));
+  lead.setRelYStd(exp(data[LEAD_MHP_VALS + 1]));
-  lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]);
+  lead.setRelVel(data[2]);
-  lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]));
+  lead.setRelVelStd(exp(data[LEAD_MHP_VALS + 2]));
+  lead.setRelA(data[3]);
+  lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3]));
 }
 
 void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
-  kj::ArrayPtr<const float> desire_state(&meta_data[0], DESIRE_LEN);
+  float desire_state_softmax[DESIRE_LEN];
+  float desire_pred_softmax[4*DESIRE_LEN];
+  softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
+  for (int i=0; i<4; i++) {
+    softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
+            &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
+  }
+  kj::ArrayPtr<const float> desire_state(desire_state_softmax, DESIRE_LEN);
   meta.setDesireState(desire_state);
-  meta.setEngagedProb(meta_data[DESIRE_LEN]);
+  meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
-  meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]);
+  meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
-  meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]);
+  meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
-  meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]);
+  meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
-  kj::ArrayPtr<const float> desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE);
+  kj::ArrayPtr<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
   meta.setDesirePrediction(desire_pred);
 }
 
-void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_x_data, const float * long_v_data, const float * long_a_data) {
+void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) {
-  // just doing 10 vals, 1 every sec for now
+  float desire_state_softmax[DESIRE_LEN];
-  float dist_arr[TIME_DISTANCE/10];
+  float desire_pred_softmax[4*DESIRE_LEN];
-  float speed_arr[TIME_DISTANCE/10];
+  softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
-  float accel_arr[TIME_DISTANCE/10];
+  for (int i=0; i<4; i++) {
-  for (int i=0; i<TIME_DISTANCE/10; i++) {
+    softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
-    dist_arr[i] = long_x_data[i*10];
+            &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
-    speed_arr[i] = long_v_data[i*10];
-    accel_arr[i] = long_a_data[i*10];
   }
-  kj::ArrayPtr<const float> dist(&dist_arr[0], ARRAYSIZE(dist_arr));
+  kj::ArrayPtr<const float> desire_state(desire_state_softmax, DESIRE_LEN);
-  longi.setDistances(dist);
+  meta.setDesireState(desire_state);
-  kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
+  meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
-  longi.setSpeeds(speed);
+  meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
-  kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
+  meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
-  longi.setAccelerations(accel);
+  meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
+  kj::ArrayPtr<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
+  meta.setDesirePrediction(desire_pred);
+}
+
+void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
+               int columns, int column_offset, float * plan_t_arr) {
+  float x_arr[TRAJECTORY_SIZE];
+  float y_arr[TRAJECTORY_SIZE];
+  float z_arr[TRAJECTORY_SIZE];
+  //float x_std_arr[TRAJECTORY_SIZE];
+  //float y_std_arr[TRAJECTORY_SIZE];
+  //float z_std_arr[TRAJECTORY_SIZE];
+  float t_arr[TRAJECTORY_SIZE];
+  for (int i=0; i<TRAJECTORY_SIZE; i++) {
+    // column_offset == -1 means this data is X indexed not T indexed
+    if (column_offset >= 0) {
+      t_arr[i] = T_IDXS[i];
+      x_arr[i] = data[i*columns + 0 + column_offset];
+      //x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset];
+    } else {
+      t_arr[i] = plan_t_arr[i];
+      x_arr[i] = X_IDXS[i];
+      //x_std_arr[i] = NAN;
+    }
+    y_arr[i] = data[i*columns + 1 + column_offset];
+    //y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset];
+    z_arr[i] = data[i*columns + 2 + column_offset];
+    //z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset];
+  }
+  kj::ArrayPtr<const float> x(x_arr, TRAJECTORY_SIZE);
+  kj::ArrayPtr<const float> y(y_arr, TRAJECTORY_SIZE);
+  kj::ArrayPtr<const float> z(z_arr, TRAJECTORY_SIZE);
+  //kj::ArrayPtr<const float> x_std(x_std_arr, TRAJECTORY_SIZE);
+  //kj::ArrayPtr<const float> y_std(y_std_arr, TRAJECTORY_SIZE);
+  //kj::ArrayPtr<const float> z_std(z_std_arr, TRAJECTORY_SIZE);
+  kj::ArrayPtr<const float> t(t_arr, TRAJECTORY_SIZE);
+  xyzt.setX(x);
+  xyzt.setY(y);
+  xyzt.setZ(z);
+  //xyzt.setXStd(x_std);
+  //xyzt.setYStd(y_std);
+  //xyzt.setZStd(z_std);
+  xyzt.setT(t);
+}
+
+
+void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
+                     uint32_t vipc_dropped_frames, float frame_drop,
+                     const ModelDataRaw &net_outputs, uint64_t timestamp_eof,
+                     float model_execution_time) {
+  // make msg
+  MessageBuilder msg;
+  auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModelV2();
+  uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
+  framed.setFrameId(vipc_frame_id);
+  framed.setFrameAge(frame_age);
+  framed.setFrameDropPerc(frame_drop * 100);
+  framed.setTimestampEof(timestamp_eof);
+  framed.setModelExecutionTime(model_execution_time);
+
+  // plan 
+  int plan_mhp_max_idx = 0;
+  for (int i=1; i<PLAN_MHP_N; i++) {
+    if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
+        net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
+      plan_mhp_max_idx = i;
+    }
+  }
+
+  float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)];
+  float plan_t_arr[TRAJECTORY_SIZE];
+  for (int i=0; i<TRAJECTORY_SIZE; i++) {
+    plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
+  }
+
+  auto position = framed.initPosition();
+  fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
+  auto velocity = framed.initVelocity();
+  fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
+  auto orientation = framed.initOrientation();
+  fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
+  auto orientation_rate = framed.initOrientationRate();
+  fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr);
+
+  // lane lines
+  auto lane_lines = framed.initLaneLines(4);
+  float lane_line_probs_arr[4];
+  float lane_line_stds_arr[4];
+  for (int i = 0; i < 4; i++) {
+    fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
+    lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]);
+    lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
+  }
+  kj::ArrayPtr<const float> lane_line_probs(lane_line_probs_arr, 4);
+  framed.setLaneLineProbs(lane_line_probs);
+  framed.setLaneLineStds(lane_line_stds_arr);
+
+  // road edges
+  auto road_edges = framed.initRoadEdges(2);
+  float road_edge_stds_arr[2];
+  for (int i = 0; i < 2; i++) {
+    fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
+    road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]);
+  }
+  framed.setRoadEdgeStds(road_edge_stds_arr);
+
+  // meta
+  auto meta = framed.initMeta();
+  fill_meta_v2(meta, net_outputs.meta);
+
+  // leads
+  auto leads = framed.initLeads(LEAD_MHP_SELECTION);
+  int mdn_max_idx = 0;
+  float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};
+  for (int t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) {
+    for (int i=1; i<LEAD_MHP_N; i++) {
+      if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION] >
+          net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION]) {
+        mdn_max_idx = i;
+        fill_lead_v2(leads[t_offset], &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)],
+                     sigmoid(net_outputs.lead_prob[t_offset]), t_offsets[t_offset]);
+      }
+    }
+  }
+  pm.send("modelV2", msg);
 }
 
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
+                   uint32_t vipc_dropped_frames, float frame_drop,
-  uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
+                   const ModelDataRaw &net_outputs, uint64_t timestamp_eof,
+                   float model_execution_time) {
 
+  uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
   MessageBuilder msg;
   auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModel();
   framed.setFrameId(vipc_frame_id);
   framed.setFrameAge(frame_age);
   framed.setFrameDropPerc(frame_drop * 100);
   framed.setTimestampEof(timestamp_eof);
+  framed.setModelExecutionTime(model_execution_time);
 
-  fill_path(framed.initPath(), net_outputs.path, false, 0);
+  // Find the distribution that corresponds to the most probable plan
-  fill_path(framed.initLeftLane(), net_outputs.left_lane, true, 1.8);
+  int plan_mhp_max_idx = 0;
-  fill_path(framed.initRightLane(), net_outputs.right_lane, true, -1.8);
+  for (int i=1; i<PLAN_MHP_N; i++) {
-  fill_longi(framed.initLongitudinal(), net_outputs.long_x, net_outputs.long_v, net_outputs.long_a);
+    if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
+        net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
+      plan_mhp_max_idx = i;
+    }
+  }
+
+  // x pos at 10s is a good valid_len
+  float valid_len = 0;
+  float valid_len_candidate;
+  for (int i=1; i<TRAJECTORY_SIZE; i++) {
+    valid_len_candidate = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*i];
+    if (valid_len_candidate >= valid_len){
+      valid_len = valid_len_candidate;
+    }
+  }
+  // clamp to 10 and MODEL_PATH_DISTANCE
+  valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
+  int valid_len_idx = 0;
+  for (int i=1; i<TRAJECTORY_SIZE; i++) {
+    if (valid_len >= X_IDXS[valid_len_idx]){
+      valid_len_idx = i;
+    }
+  }
+
+  auto lpath = framed.initPath();
+  fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx);
+  
+  auto left_lane = framed.initLeftLane();
+  int ll_idx = 1;
+  fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
+            sigmoid(net_outputs.lane_lines_prob[ll_idx]));
+  auto right_lane = framed.initRightLane();
+  ll_idx = 2;
+  fill_lane_line(right_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
+            sigmoid(net_outputs.lane_lines_prob[ll_idx]));
 
   // Find the distribution that corresponds to the current lead
   int mdn_max_idx = 0;
   int t_offset = 0;
-  for (int i=1; i<LEAD_MDN_N; i++) {
+  for (int i=1; i<LEAD_MHP_N; i++) {
-    if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
+    if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
+        net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
       mdn_max_idx = i;
     }
   }
-  fill_lead(framed.initLead(), net_outputs.lead, mdn_max_idx, t_offset);
+  fill_lead(framed.initLead(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
   // Find the distribution that corresponds to the lead in 2s
   mdn_max_idx = 0;
   t_offset = 1;
-  for (int i=1; i<LEAD_MDN_N; i++) {
+  for (int i=1; i<LEAD_MHP_N; i++) {
-    if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
+    if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
+        net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
       mdn_max_idx = i;
     }
   }
-  fill_lead(framed.initLeadFuture(), net_outputs.lead, mdn_max_idx, t_offset);
+  fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
+
   fill_meta(framed.initMeta(), net_outputs.meta);
 
   pm.send("model", msg);
 }
 
 void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                     uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
+                     uint32_t vipc_dropped_frames, float frame_drop,
+                     const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
   float trans_arr[3];
   float trans_std_arr[3];
   float rot_arr[3];
@@ -280,10 +475,10 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
 
   for (int i =0; i < 3; i++) {
     trans_arr[i] = net_outputs.pose[i];
-    trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6;
+    trans_std_arr[i] = exp(net_outputs.pose[6 + i]);
 
-    rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0;
+    rot_arr[i] = net_outputs.pose[3 + i];
-    rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0;
+    rot_std_arr[i] = exp(net_outputs.pose[9 + i]);
   }
 
   MessageBuilder msg;

selfdrive/modeld/models/driving.h

@@ -17,33 +17,40 @@
 #include <memory>
 #include "messaging.hpp"
 
+#define MODEL_NAME "supercombo_dlc"
+
 #define MODEL_WIDTH 512
 #define MODEL_HEIGHT 256
 #define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
-#define MODEL_NAME "supercombo_dlc"
-
 #define DESIRE_LEN 8
 #define TRAFFIC_CONVENTION_LEN 2
-#define LEAD_MDN_N 5 // probs for 5 groups
+
-#define MDN_VALS 4 // output xyva for each lead group
+#define PLAN_MHP_N 5
-#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
+#define PLAN_MHP_COLUMNS 30
-#define MDN_GROUP_SIZE 11
+#define PLAN_MHP_VALS 30*33
-#define TIME_DISTANCE 100
+#define PLAN_MHP_SELECTION 1
+#define PLAN_MHP_GROUP_SIZE (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION)
+
+#define LEAD_MHP_N 5
+#define LEAD_MHP_VALS 4
+#define LEAD_MHP_SELECTION 3
+#define LEAD_MHP_GROUP_SIZE (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION)
+
 #define POSE_SIZE 12
 
 #define MODEL_FREQ 20
 #define MAX_FRAME_DROP 0.05
 
 struct ModelDataRaw {
-    float *path;
+    float *plan;
-    float *left_lane;
+    float *lane_lines;
-    float *right_lane;
+    float *lane_lines_prob;
+    float *road_edges;
     float *lead;
-    float *long_x;
+    float *lead_prob;
-    float *long_v;
-    float *long_a;
     float *desire_state;
     float *meta;
+    float *desire_pred;
     float *pose;
   };
 
@@ -71,7 +78,12 @@ void model_free(ModelState* s);
 void poly_fit(float *in_pts, float *in_stds, float *out);
 
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof);
+                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
+                   uint64_t timestamp_eof, float model_execution_time);
+void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
+                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
+                   uint64_t timestamp_eof, float model_execution_time);
 void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                     uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof);
+                     uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
+                     uint64_t timestamp_eof);
 #endif