openpilot v0.6.1 release

old-commit-hash: 94053536b4

Pipfile

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:c8eab0ec610fccd29115e8451f1088ed71db5ce7575956d08fd76baf62172e28
+oid sha256:6f453d0dd767bc785836bf53d1ba3f4175188b4ca7b9fdcf8d146bb28c0cafb6
-size 2565
+size 2620

Pipfile.lock

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:14e21ae221670a6b9178371274d1b30428f6d136195e8320943e8988764d9bd4
+oid sha256:4e0fb1640f01f83f792d10e8957dc191edd1ff195a1188229730e08ee2198fdc
-size 152571
+size 155198

README.md

@@ -60,13 +60,13 @@ Supported Cars
 
 | Make                 | Model                    | Supported Package    | Lateral | Longitudinal   | No Accel Below   | No Steer Below | Giraffe           |
 | ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
-| Acura                | ILX 2016-17              | AcuraWatch Plus      | Yes     | Yes            | 25mph<sup>1</sup>| 25mph          | Nidec             |
+| Acura                | ILX 2016-18              | AcuraWatch Plus      | Yes     | Yes            | 25mph<sup>1</sup>| 25mph          | Nidec             |
-| Acura                | RDX 2018                 | AcuraWatch Plus      | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Nidec             |
+| Acura                | RDX 2016-18              | AcuraWatch Plus      | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Nidec             |
 | Buick<sup>3</sup>    | Regal 2018               | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Chevrolet<sup>3</sup>| Malibu 2017              | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Chevrolet<sup>3</sup>| Volt 2017-18             | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Cadillac<sup>3</sup> | ATS 2018                 | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
-| Chrysler             | Pacifica 2018            | Adaptive Cruise      | Yes     | Stock          | 0mph             | 9mph           | FCA               |
+| Chrysler             | Pacifica 2017-18         | Adaptive Cruise      | Yes     | Stock          | 0mph             | 9mph           | FCA               |
 | Chrysler             | Pacifica Hybrid 2017-18  | Adaptive Cruise      | Yes     | Stock          | 0mph             | 9mph           | FCA               |
 | Chrysler             | Pacifica Hybrid 2019     | Adaptive Cruise      | Yes     | Stock          | 0mph             | 39mph          | FCA               |
 | GMC<sup>3</sup>      | Acadia Denali 2018       | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
@@ -84,9 +84,9 @@ Supported Cars
 | Honda                | Pilot 2019               | All                  | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Inverted Nidec    |
 | Honda                | Ridgeline 2017-19        | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Nidec             |
 | Hyundai              | Santa Fe 2019            | All                  | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>6</sup>|
-| Hyundai              | Elantra 2017             | SCC + LKAS           | Yes     | Stock          | 19mph            | 34mph          | Custom<sup>6</sup>|
+| Hyundai              | Elantra 2017-19          | SCC + LKAS           | Yes     | Stock          | 19mph            | 34mph          | Custom<sup>6</sup>|
 | Hyundai              | Genesis 2018             | All                  | Yes     | Stock          | 19mph            | 34mph          | Custom<sup>6</sup>|
-| Jeep                 | Grand Cherokee 2017-18   | Adaptive Cruise      | Yes     | Stock          | 0mph             | 9mph           | FCA               |
+| Jeep                 | Grand Cherokee 2016-18   | Adaptive Cruise      | Yes     | Stock          | 0mph             | 9mph           | FCA               |
 | Jeep                 | Grand Cherokee 2019      | Adaptive Cruise      | Yes     | Stock          | 0mph             | 39mph          | FCA               |
 | Kia                  | Optima 2019              | SCC + LKAS           | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>6</sup>|
 | Kia                  | Sorento 2018             | All                  | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>6</sup>|
@@ -96,8 +96,9 @@ Supported Cars
 | Subaru               | Crosstrek 2018           | EyeSight             | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>4</sup>|
 | Subaru               | Impreza 2019             | EyeSight             | Yes     | Stock          | 0mph             | 0mph           | Custom<sup>4</sup>|
 | Toyota               | Avalon 2016              | TSS-P                | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
-| Toyota               | Camry 2018               | All                  | Yes     | Stock          | 0mph<sup>5</sup> | 0mph           | Toyota            |
+| Toyota               | Avalon 2017-18           | All                  | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
-| Toyota               | C-HR 2017-18             | All                  | Yes     | Stock          | 0mph             | 0mph           | Toyota            |
+| Toyota               | Camry 2018-19            | All                  | Yes     | Stock          | 0mph<sup>5</sup> | 0mph           | Toyota            |
+| Toyota               | C-HR 2017-19             | All                  | Yes     | Stock          | 0mph             | 0mph           | Toyota            |
 | Toyota               | Corolla 2017-19          | All                  | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Corolla 2020             | All                  | Yes     | Yes            | 0mph             | 0mph           | Toyota            |
 | Toyota               | Corolla Hatchback 2019   | All                  | Yes     | Yes            | 0mph             | 0mph           | Toyota            |
@@ -110,6 +111,7 @@ Supported Cars
 | Toyota               | Rav4 2017-18             | All                  | Yes     | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph           | Toyota            |
 | Toyota               | Rav4 2019                | All                  | Yes     | Yes            | 0mph             | 0mph           | Toyota            |
 | Toyota               | Rav4 Hybrid 2017-18      | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
+| Toyota               | Sienna 2018              | All                  | Yes     | Yes<sup>2</sup>| 0mph             | 0mph           | Toyota            |
 
 <sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/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. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai).*** <br />
 <sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). <br />

RELEASES.md

@@ -1,3 +1,10 @@
+Version 0.6.1 (2019-07-21)
+========================
+ * Remote SSH with comma prime and [ssh.comma.ai](https://ssh.comma.ai)
+ * Panda code Misra-c2012 compliance, tested against cppcheck coverage
+ * Lockout openpilot after 3 terminal alerts for driver distracted or unresponsive
+ * Toyota Sienna support thanks to wocsor!
+
 Version 0.6 (2019-07-01)
 ========================
  * New model, with double the pixels and ten times the temporal context!

apk/ai.comma.plus.offroad.apk

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:3eaf6c7d0666f721967fdf5441ea254568cbe898ee71f31e55e41c0cdd6cc41b
+oid sha256:01a583ce80c2bf9e1a48c55b485cea03d6160358e0f5a528317040d30b9c0978
-size 18310731
+size 17042125

common/file_helpers.py

@@ -28,7 +28,7 @@ def get_tmpdir_on_same_filesystem(path):
   normpath = os.path.normpath(path)
   parts = normpath.split("/")
   if len(parts) > 1:
-    if parts[1].startswith("raid"):
+    if parts[1].startswith("raid") or parts[1].startswith("datasets"):
       if len(parts) > 2 and parts[2] == "runner":
         return "/{}/runner/tmp".format(parts[1])
       elif len(parts) > 2 and parts[2] == "aws":
@@ -101,3 +101,18 @@ def atomic_write_in_dir(path, **kwargs):
   writer = AtomicWriter(path, **kwargs)
   return writer._open(_get_fileobject_func(writer, os.path.dirname(path)))
 
+def atomic_write_in_dir_neos(path, contents, mode=None):
+  """
+  Atomically writes contents to path using a temporary file in the same directory
+  as path. Useful on NEOS, where `os.link` (required by atomic_write_in_dir) is missing.
+  """
+
+  f = tempfile.NamedTemporaryFile(delete=False, prefix=".tmp", dir=os.path.dirname(path))
+  f.write(contents)
+  f.flush()
+  if mode is not None:
+    os.fchmod(f.fileno(), mode)
+  os.fsync(f.fileno())
+  f.close()
+
+  os.rename(f.name, path)

common/fingerprints.py

@@ -28,10 +28,8 @@ _DEBUG_ADDRESS = {1880: 8}   # reserved for debug purposes
 
 def is_valid_for_fingerprint(msg, car_fingerprint):
   adr = msg.address
-  bus = msg.src
   # ignore addresses that are more than 11 bits
-  return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or \
+  return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or adr >= 0x800
-         bus != 0 or adr >= 0x800
 
 
 def eliminate_incompatible_cars(msg, candidate_cars):

common/params.py

@@ -50,12 +50,14 @@ class UnknownKeyName(Exception):
 
 keys = {
   "AccessToken": [TxType.PERSISTENT],
+  "AthenadPid": [TxType.PERSISTENT],
   "CalibrationParams": [TxType.PERSISTENT],
   "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
   "CompletedTrainingVersion": [TxType.PERSISTENT],
   "ControlsParams": [TxType.PERSISTENT],
   "DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
   "DongleId": [TxType.PERSISTENT],
+  "GithubSshKeys": [TxType.PERSISTENT],
   "GitBranch": [TxType.PERSISTENT],
   "GitCommit": [TxType.PERSISTENT],
   "GitRemote": [TxType.PERSISTENT],
@@ -75,6 +77,7 @@ keys = {
   "ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START],
   "SpeedLimitOffset": [TxType.PERSISTENT],
   "SubscriberInfo": [TxType.PERSISTENT],
+  "TermsVersion": [TxType.PERSISTENT],
   "TrainingVersion": [TxType.PERSISTENT],
   "Version": [TxType.PERSISTENT],
 }

selfdrive/athena/athenad.py

@@ -1,15 +1,22 @@
 #!/usr/bin/env python2.7
 import json
+import jwt
 import os
 import random
+import re
+import select
+import subprocess
+import socket
 import time
 import threading
 import traceback
 import zmq
 import requests
 import six.moves.queue
+from datetime import datetime, timedelta
+from functools import partial
 from jsonrpc import JSONRPCResponseManager, dispatcher
-from websocket import create_connection, WebSocketTimeoutException
+from websocket import create_connection, WebSocketTimeoutException, ABNF
 from selfdrive.loggerd.config import ROOT
 
 import selfdrive.crash as crash
@@ -21,6 +28,7 @@ from selfdrive.version import version, dirty
 
 ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
 HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4)
+LOCAL_PORT_WHITELIST = set([8022])
 
 dispatcher["echo"] = lambda s: s
 payload_queue = six.moves.queue.Queue()
@@ -49,6 +57,7 @@ def handle_long_poll(ws):
       thread.join()
 
 def jsonrpc_handler(end_event):
+  dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event)
   while not end_event.is_set():
     try:
       data = payload_queue.get(timeout=1)
@@ -85,6 +94,109 @@ def uploadFileToUrl(fn, url, headers):
     ret = requests.put(url, data=f, headers=headers, timeout=10)
   return ret.status_code
 
+def startLocalProxy(global_end_event, remote_ws_uri, local_port):
+  try:
+    cloudlog.event("athena startLocalProxy", remote_ws_uri=remote_ws_uri, local_port=local_port)
+
+    if local_port not in LOCAL_PORT_WHITELIST:
+      raise Exception("Requested local port not whitelisted")
+
+    params = Params()
+    dongle_id = params.get("DongleId")
+    private_key = open("/persist/comma/id_rsa").read()
+    identity_token = jwt.encode({'identity':dongle_id, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256')
+
+    ws = create_connection(remote_ws_uri,
+                           cookie="jwt=" + identity_token,
+                           enable_multithread=True)
+
+    ssock, csock = socket.socketpair()
+    local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    local_sock.connect(('127.0.0.1', local_port))
+    local_sock.setblocking(0)
+
+    proxy_end_event = threading.Event()
+    threads = [
+      threading.Thread(target=ws_proxy_recv, args=(ws, local_sock, ssock, proxy_end_event, global_end_event)),
+      threading.Thread(target=ws_proxy_send, args=(ws, local_sock, csock, proxy_end_event))
+    ]
+
+    map(lambda thread: thread.start(), threads)
+
+    return {"success": 1}
+  except Exception as e:
+    traceback.print_exc()
+    raise e
+
+@dispatcher.add_method
+def getPublicKey():
+  if not os.path.isfile('/persist/comma/id_rsa.pub'):
+    return None
+
+  with open('/persist/comma/id_rsa.pub', 'r') as f:
+    return f.read()
+
+@dispatcher.add_method
+def getSshAuthorizedKeys():
+  with open('/system/comma/home/.ssh/authorized_keys', 'r') as f:
+    return f.read()
+
+@dispatcher.add_method
+def getSimInfo():
+  sim_state = subprocess.check_output(['getprop', 'gsm.sim.state']).strip().split(',')
+  network_type = subprocess.check_output(['getprop', 'gsm.network.type']).strip().split(',')
+  mcc_mnc = subprocess.check_output(['getprop', 'gsm.sim.operator.numeric']).strip() or None
+
+  sim_id_aidl_out = subprocess.check_output(['service', 'call', 'iphonesubinfo', '11'])
+  sim_id_aidl_lines = sim_id_aidl_out.split('\n')
+  if len(sim_id_aidl_lines) > 3:
+    sim_id_lines = sim_id_aidl_lines[1:4]
+    sim_id_fragments = [re.search(r"'([0-9\.]+)'", line).group(1) for line in sim_id_lines]
+    sim_id = reduce(lambda frag1, frag2: frag1.replace('.', '') + frag2.replace('.', ''), sim_id_fragments)
+  else:
+    sim_id = None
+
+  return {
+    'sim_id': sim_id,
+    'mcc_mnc': mcc_mnc,
+    'network_type': network_type,
+    'sim_state': sim_state
+  }
+
+def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event):
+  while not (end_event.is_set() or global_end_event.is_set()):
+    try:
+      data = ws.recv()
+      local_sock.sendall(data)
+    except WebSocketTimeoutException:
+      pass
+    except Exception:
+      traceback.print_exc()
+      break
+
+  ssock.close()
+  end_event.set()
+
+def ws_proxy_send(ws, local_sock, signal_sock, end_event):
+  while not end_event.is_set():
+    try:
+      r, _, _ = select.select((local_sock, signal_sock), (), ())
+      if r:
+        if r[0].fileno() == signal_sock.fileno():
+          # got end signal from ws_proxy_recv
+          end_event.set()
+          break
+        data = local_sock.recv(4096)
+        if not data:
+          # local_sock is dead
+          end_event.set()
+          break
+
+        ws.send(data, ABNF.OPCODE_BINARY)
+    except Exception:
+        traceback.print_exc()
+        end_event.set()
+
 def ws_recv(ws, end_event):
   while not end_event.is_set():
     try:
@@ -138,5 +250,7 @@ def main(gctx=None):
 
     time.sleep(backoff(conn_retries))
 
+  params.delete("AthenadPid")
+
 if __name__ == "__main__":
   main()

selfdrive/boardd/boardd.cc

@@ -283,8 +283,9 @@ void can_health(void *s) {
     uint8_t started;
     uint8_t controls_allowed;
     uint8_t gas_interceptor_detected;
-    uint8_t started_signal_detected;
+    uint32_t can_send_errs;
-    uint8_t started_alt;
+    uint32_t can_fwd_errs;
+    uint32_t gmlan_send_errs;
   } health;
 
   // recv from board
@@ -313,8 +314,10 @@ void can_health(void *s) {
   }
   healthData.setControlsAllowed(health.controls_allowed);
   healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
-  healthData.setStartedSignalDetected(health.started_signal_detected);
   healthData.setIsGreyPanda(is_grey_panda);
+  healthData.setCanSendErrs(health.can_send_errs);
+  healthData.setCanFwdErrs(health.can_fwd_errs);
+  healthData.setGmlanSendErrs(health.gmlan_send_errs);
 
   // send to health
   auto words = capnp::messageToFlatArray(msg);

selfdrive/can/parser.cc

@@ -194,32 +194,36 @@ class CANParser {
     : bus(abus) {
     // connect to can on 8006
     context = zmq_ctx_new();
-    subscriber = zmq_socket(context, ZMQ_SUB);
-    zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
-    zmq_setsockopt(subscriber, ZMQ_RCVTIMEO, &timeout, sizeof(int));
 
-    std::string tcp_addr_str;
+    if (tcp_addr.length() > 0) {
+      subscriber = zmq_socket(context, ZMQ_SUB);
+      zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
+      zmq_setsockopt(subscriber, ZMQ_RCVTIMEO, &timeout, sizeof(int));
 
-    if (sendcan) {
+      std::string tcp_addr_str;
-      tcp_addr_str = "tcp://" + tcp_addr + ":8017";
+
-    } else {
+      if (sendcan) {
-      tcp_addr_str = "tcp://" + tcp_addr + ":8006";
+        tcp_addr_str = "tcp://" + tcp_addr + ":8017";
-    }
+      } else {
-    const char *tcp_addr_char = tcp_addr_str.c_str();
+        tcp_addr_str = "tcp://" + tcp_addr + ":8006";
+      }
+      const char *tcp_addr_char = tcp_addr_str.c_str();
 
-    zmq_connect(subscriber, tcp_addr_char);
+      zmq_connect(subscriber, tcp_addr_char);
 
-    // drain sendcan to delete any stale messages from previous runs
+      // drain sendcan to delete any stale messages from previous runs
-    zmq_msg_t msgDrain;
+      zmq_msg_t msgDrain;
-    zmq_msg_init(&msgDrain);
+      zmq_msg_init(&msgDrain);
-    int err = 0;
+      int err = 0;
-    while(err >= 0) {
+      while(err >= 0) {
-      err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT);
+        err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT);
+      }
+    } else {
+      subscriber = NULL;
     }
 
     dbc = dbc_lookup(dbc_name);
-    assert(dbc);
+    assert(dbc); 
-
     for (const auto& op : options) {
       MessageState state = {
         .address = op.address,
@@ -326,6 +330,21 @@ class CANParser {
     }
   }
 
+  void update_string(uint64_t sec, std::string data) {
+    // format for board, make copy due to alignment issues, will be freed on out of scope
+    auto amsg = kj::heapArray<capnp::word>((data.length() / sizeof(capnp::word)) + 1);
+    memcpy(amsg.begin(), data.data(), data.length());
+
+    // extract the messages
+    capnp::FlatArrayMessageReader cmsg(amsg);
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+    auto cans = event.getCan();
+    UpdateCans(sec, cans);
+
+    UpdateValid(sec);
+  }
+
   int update(uint64_t sec, bool wait) {
     int err;
     int result = 0;
@@ -336,7 +355,7 @@ class CANParser {
 
     // multiple recv is fine
     bool first = wait;
-    while (1) {
+    while (subscriber != NULL) {
       if (first) {
         err = zmq_msg_recv(&msg, subscriber, 0);
         first = false;
@@ -432,6 +451,11 @@ int can_update(void* can, uint64_t sec, bool wait) {
   return cp->update(sec, wait);
 }
 
+void can_update_string(void *can, uint64_t sec, const char* dat, int len) {
+  CANParser* cp = (CANParser*)can;
+  cp->update_string(sec, std::string(dat, len));
+}
+
 size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
   CANParser* cp = (CANParser*)can;
 

selfdrive/can/parser_pyx.pxd

@@ -67,6 +67,7 @@ ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name,
                 const char* tcp_addr,
                 int timeout)
 ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait);
+ctypedef void (*can_update_string_func)(void* can, uint64_t sec, const char* dat, int len);
 ctypedef size_t (*can_query_func)(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
 ctypedef void (*can_query_vector_func)(void* can, uint64_t sec, bool *out_can_valid,  vector[SignalValue] &values)
 
@@ -77,6 +78,7 @@ cdef class CANParser:
     dbc_lookup_func dbc_lookup
     can_init_with_vectors_func can_init_with_vectors
     can_update_func can_update
+    can_update_string_func can_update_string
     can_query_vector_func can_query_vector
     map[string, uint32_t] msg_name_to_address
     map[uint32_t, string] address_to_msg_name

selfdrive/can/parser_pyx.pyx

@@ -8,7 +8,7 @@ import numbers
 cdef int CAN_INVALID_CNT = 5
 
 cdef class CANParser:
-  def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1):
+  def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="", timeout=-1):
     self.test_mode_enabled = False
     can_dir = os.path.dirname(os.path.abspath(__file__))
     libdbc_fn = os.path.join(can_dir, "libdbc.so")
@@ -17,6 +17,7 @@ cdef class CANParser:
     self.can_init_with_vectors = <can_init_with_vectors_func>dlsym(libdbc, 'can_init_with_vectors')
     self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
     self.can_update = <can_update_func>dlsym(libdbc, 'can_update')
+    self.can_update_string = <can_update_string_func>dlsym(libdbc, 'can_update_string')
     self.can_query_vector = <can_query_vector_func>dlsym(libdbc, 'can_query_vector')
     if checks is None:
       checks = []
@@ -99,6 +100,19 @@ cdef class CANParser:
 
     return updated_val
 
+  def update_string(self, uint64_t sec, dat):
+    self.can_update_string(self.can, sec, dat, len(dat))
+    return self.update_vl(sec)
+
+  def update_strings(self, uint64_t sec, strings):
+    updated_vals = set()
+
+    for s in strings:
+      updated_val = self.update_string(sec, s)
+      updated_vals.update(updated_val)
+
+    return updated_vals
+
   def update(self, uint64_t sec, bool wait):
     r = (self.can_update(self.can, sec, wait) >= 0)
     updated_val = self.update_vl(sec)

selfdrive/can/tests/test_parser.py

@@ -47,8 +47,9 @@ def run_route(route):
 
   CP = CarInterface.get_params(CAR.CIVIC, {})
   signals, checks = get_can_signals(CP)
-  parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
+  parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1")
-  parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
+  parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1")
+  parser_string = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
 
   if dict_keys_differ(parser_old.vl, parser_new.vl):
     return False
@@ -61,19 +62,29 @@ def run_route(route):
   for msg in lr:
     if msg.which() == 'can':
       t += DT
-      can.send(msg.as_builder().to_bytes())
+      msg_bytes = msg.as_builder().to_bytes()
+      can.send(msg_bytes)
 
       _, updated_old = parser_old.update(t, True)
       _, updated_new = parser_new.update(t, True)
+      updated_string = parser_string.update_string(t, msg_bytes)
 
       if updated_old != updated_new:
         route_ok = False
         print(t, "Diff in seen")
 
+      if updated_new != updated_string:
+        route_ok = False
+        print(t, "Diff in seen string")
+
       if dicts_vals_differ(parser_old.vl, parser_new.vl):
         print(t, "Diff in dict")
         route_ok = False
 
+      if dicts_vals_differ(parser_new.vl, parser_string.vl):
+        print(t, "Diff in dict string")
+        route_ok = False
+
   return route_ok
 
 class TestCanParser(unittest.TestCase):

selfdrive/car/car_helpers.py

@@ -50,6 +50,8 @@ def _get_interface_names():
 # imports from directory selfdrive/car/<name>/
 interfaces = load_interfaces(_get_interface_names())
 
+def only_toyota_left(candidate_cars):
+  return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars)
 
 # BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
 # **** for use live only ****
@@ -59,7 +61,7 @@ def fingerprint(logcan, sendcan):
   elif os.getenv("SIMULATOR") is not None:
     return ("simulator", None, "")
 
-  finger = {}
+  finger = {0: {}, 2:{}}  # collect on bus 0 or 2
   cloudlog.warning("waiting for fingerprint...")
   candidate_cars = all_known_cars()
   can_seen_frame = None
@@ -79,6 +81,7 @@ def fingerprint(logcan, sendcan):
   vin = ""
 
   frame = 0
+
   while True:
     a = messaging.recv_one(logcan)
 
@@ -98,9 +101,12 @@ def fingerprint(logcan, sendcan):
 
       # ignore everything not on bus 0 and with more than 11 bits,
       # which are ussually sporadic and hard to include in fingerprints.
-      # also exclude VIN query response on 0x7e8
+      # also exclude VIN query response on 0x7e8.
-      if can.src == 0 and can.address < 0x800 and can.address != 0x7e8:
+      # Include bus 2 for toyotas to disambiguate cars using camera messages
-        finger[can.address] = len(can.dat)
+      # (ideally should be done for all cars but we can't for Honda Bosch)
+      if (can.src == 0 or (only_toyota_left(candidate_cars) and can.src == 2)) and \
+         can.address < 0x800 and can.address != 0x7e8:
+        finger[can.src][can.address] = len(can.dat)
         candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
 
     if can_seen_frame is None and can_seen:
@@ -110,7 +116,7 @@ def fingerprint(logcan, sendcan):
     # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
     # broadcast immediately
     if len(candidate_cars) == 1 and can_seen_frame is not None:
-      time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1
+      time_fingerprint = 1.0 if only_toyota_left(candidate_cars) else 0.1
       if (frame - can_seen_frame) > (time_fingerprint * 100):
         break
 
@@ -146,6 +152,6 @@ def get_car(logcan, sendcan):
     candidate = "mock"
 
   CarInterface, CarController = interfaces[candidate]
-  params = CarInterface.get_params(candidate, fingerprints, vin)
+  params = CarInterface.get_params(candidate, fingerprints[0], vin)
 
   return CarInterface(params, CarController), params

selfdrive/car/chrysler/carstate.py

@@ -60,7 +60,7 @@ def get_can_parser(CP):
     ("ACC_2", 50),
   ]
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
 def get_camera_parser(CP):
   signals = [
@@ -72,7 +72,7 @@ def get_camera_parser(CP):
   ]
   checks = []
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
 
 
 class CarState(object):

selfdrive/car/chrysler/interface.py

@@ -112,18 +112,17 @@ class CarInterface(object):
     return ret
 
   # returns a car.CarState
-  def update(self, c):
+  def update(self, c, can_strings):
     # ******************* do can recv *******************
-    canMonoTimes = []
+    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
+    self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
 
     self.CS.update(self.cp, self.cp_cam)
 
     # create message
     ret = car.CarState.new_message()
 
-    ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid and self.cp_cam.can_valid
+    ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
 
     # speeds
     ret.vEgo = self.CS.v_ego
@@ -222,7 +221,6 @@ class CarInterface(object):
       events.append(create_event('belowSteerSpeed', [ET.WARNING]))
 
     ret.events = events
-    ret.canMonoTimes = canMonoTimes
 
     self.gas_pressed_prev = ret.gasPressed
     self.brake_pressed_prev = ret.brakePressed

selfdrive/car/chrysler/radar_interface.py

@@ -51,27 +51,24 @@ class RadarInterface(object):
     self.pts = {}
     self.delay = 0.0  # Delay of radar  #TUNE
     self.rcp = _create_radar_can_parser()
+    self.updated_messages = set()
+    self.trigger_msg = LAST_MSG
 
-  def update(self):
+  def update(self, can_strings):
-    canMonoTimes = []
+    tm = int(sec_since_boot() * 1e9)
+    vls = self.rcp.update_strings(tm, can_strings)
+    self.updated_messages.update(vls)
 
-    updated_messages = set()  # set of message IDs (sig_addresses) we've seen
+    if self.trigger_msg not in self.updated_messages:
-
+      return None
-    while 1:
-      tm = int(sec_since_boot() * 1e9)
-      _, vls = self.rcp.update(tm, True)
-      updated_messages.update(vls)
-      if LAST_MSG in updated_messages:
-        break
 
     ret = car.RadarData.new_message()
     errors = []
     if not self.rcp.can_valid:
       errors.append("canError")
     ret.errors = errors
-    ret.canMonoTimes = canMonoTimes
 
-    for ii in updated_messages:  # ii should be the message ID as a number
+    for ii in self.updated_messages:  # ii should be the message ID as a number
       cpt = self.rcp.vl[ii]
       trackId = _address_to_track(ii)
 
@@ -92,11 +89,6 @@ class RadarInterface(object):
 
     # We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid.
     ret.points = [x for x in self.pts.values() if x.dRel != 0]
-    return ret
 
-if __name__ == "__main__":
+    self.updated_messages.clear()
-  RI = RadarInterface(None)
+    return ret
-  while 1:
-    ret = RI.update()
-    print(chr(27) + "[2J")  # clear screen
-    print(ret)

selfdrive/car/ford/carstate.py

@@ -29,7 +29,7 @@ def get_can_parser(CP):
   checks = [
   ]
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
 
 class CarState(object):

selfdrive/car/ford/interface.py

@@ -105,18 +105,16 @@ class CarInterface(object):
     return ret
 
   # returns a car.CarState
-  def update(self, c):
+  def update(self, c, can_strings):
     # ******************* do can recv *******************
-    canMonoTimes = []
+    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-
-    can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
 
     self.CS.update(self.cp)
 
     # create message
     ret = car.CarState.new_message()
 
-    ret.canValid = can_rcv_valid and self.cp.can_valid
+    ret.canValid = self.cp.can_valid
 
     # speeds
     ret.vEgo = self.CS.v_ego
@@ -167,7 +165,6 @@ class CarInterface(object):
       events.append(create_event('steerTempUnavailableMute', [ET.WARNING]))
 
     ret.events = events
-    ret.canMonoTimes = canMonoTimes
 
     self.gas_pressed_prev = ret.gasPressed
     self.brake_pressed_prev = ret.brakePressed

selfdrive/car/ford/radar_interface.py

@@ -28,28 +28,25 @@ class RadarInterface(object):
 
     # Nidec
     self.rcp = _create_radar_can_parser()
+    self.trigger_msg = 0x53f
+    self.updated_messages = set()
 
-  def update(self):
+  def update(self, can_strings):
-    canMonoTimes = []
+    tm = int(sec_since_boot() * 1e9)
+    vls = self.rcp.update_strings(tm, can_strings)
+    self.updated_messages.update(vls)
 
-    updated_messages = set()
+    if self.trigger_msg not in self.updated_messages:
-    while 1:
+      return None
-      tm = int(sec_since_boot() * 1e9)
-      _, vls = self.rcp.update(tm, True)
-      updated_messages.update(vls)
 
-      # TODO: do not hardcode last msg
-      if 0x53f in updated_messages:
-        break
 
     ret = car.RadarData.new_message()
     errors = []
     if not self.rcp.can_valid:
       errors.append("canError")
     ret.errors = errors
-    ret.canMonoTimes = canMonoTimes
 
-    for ii in updated_messages:
+    for ii in self.updated_messages:
       cpt = self.rcp.vl[ii]
 
       if cpt['X_Rel'] > 0.00001:
@@ -78,11 +75,5 @@ class RadarInterface(object):
           del self.pts[ii]
 
     ret.points = self.pts.values()
+    self.updated_messages.clear()
     return ret
-
-if __name__ == "__main__":
-  RI = RadarInterface(None)
-  while 1:
-    ret = RI.update()
-    print(chr(27) + "[2J")
-    print(ret)

selfdrive/car/gm/carstate.py

@@ -47,7 +47,7 @@ def get_powertrain_can_parser(CP, canbus):
       ("CruiseState", "AcceleratorPedal2", 0),
     ]
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
 
 
 class CarState(object):

selfdrive/car/gm/interface.py

@@ -170,15 +170,15 @@ class CarInterface(object):
     return ret
 
   # returns a car.CarState
-  def update(self, c):
+  def update(self, c, can_strings):
-    can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
+    self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
 
     self.CS.update(self.pt_cp)
 
     # create message
     ret = car.CarState.new_message()
 
-    ret.canValid = can_rcv_valid and self.pt_cp.can_valid
+    ret.canValid = self.pt_cp.can_valid
 
     # speeds
     ret.vEgo = self.CS.v_ego

selfdrive/car/gm/radar_interface.py

@@ -52,21 +52,23 @@ class RadarInterface(object):
     print "Using %d as obstacle CAN bus ID" % canbus.obstacle
     self.rcp = create_radar_can_parser(canbus, CP.carFingerprint)
 
-  def update(self):
+    self.trigger_msg = LAST_RADAR_MSG
-    updated_messages = set()
+    self.updated_messages = set()
-    ret = car.RadarData.new_message()
-    while 1:
 
-      if self.rcp is None:
+  def update(self, can_strings):
-        time.sleep(0.05)   # nothing to do
+    if self.rcp is None:
-        return ret
+      time.sleep(0.05)   # nothing to do
+      return car.RadarData.new_message()
 
-      tm = int(sec_since_boot() * 1e9)
+    tm = int(sec_since_boot() * 1e9)
-      _, vls = self.rcp.update(tm, True)
+    vls = self.rcp.update_strings(tm, can_strings)
-      updated_messages.update(vls)
+    self.updated_messages.update(vls)
-      if LAST_RADAR_MSG in updated_messages:
+
-        break
+    if self.trigger_msg not in self.updated_messages:
+      return None
 
+
+    ret = car.RadarData.new_message()
     header = self.rcp.vl[RADAR_HEADER_MSG]
     fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \
       header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \
@@ -83,7 +85,7 @@ class RadarInterface(object):
 
     # Not all radar messages describe targets,
     # no need to monitor all of the self.rcp.msgs_upd
-    for ii in updated_messages:
+    for ii in self.updated_messages:
       if ii == RADAR_HEADER_MSG:
         continue
 
@@ -112,11 +114,5 @@ class RadarInterface(object):
         del self.pts[oldTarget]
 
     ret.points = self.pts.values()
+    self.updated_messages.clear()
     return ret
-
-if __name__ == "__main__":
-  RI = RadarInterface(None)
-  while 1:
-    ret = RI.update()
-    print(chr(27) + "[2J")
-    print(ret)

selfdrive/car/honda/carstate.py

@@ -146,6 +146,7 @@ def get_can_signals(CP):
   # add gas interceptor reading if we are using it
   if CP.enableGasInterceptor:
     signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
+    signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
     checks.append(("GAS_SENSOR", 50))
 
   return signals, checks
@@ -153,7 +154,7 @@ def get_can_signals(CP):
 
 def get_can_parser(CP):
   signals, checks = get_can_signals(CP)
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
 
 def get_cam_can_parser(CP):
@@ -166,7 +167,7 @@ def get_cam_can_parser(CP):
 
   cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus)
 
 class CarState(object):
   def __init__(self, CP):
@@ -261,7 +262,7 @@ class CarState(object):
     # this is a hack for the interceptor. This is now only used in the simulation
     # TODO: Replace tests by toyota so this can go away
     if self.CP.enableGasInterceptor:
-      self.user_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
+      self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
       self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
 
     self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR']

selfdrive/car/honda/interface.py

@@ -358,18 +358,17 @@ class CarInterface(object):
     return ret
 
   # returns a car.CarState
-  def update(self, c):
+  def update(self, c, can_strings):
     # ******************* do can recv *******************
-    canMonoTimes = []
+    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
+    self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
 
     self.CS.update(self.cp, self.cp_cam)
 
     # create message
     ret = car.CarState.new_message()
 
-    ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid
+    ret.canValid = self.cp.can_valid
 
     # speeds
     ret.vEgo = self.CS.v_ego
@@ -547,7 +546,6 @@ class CarInterface(object):
       events.append(create_event('buttonEnable', [ET.ENABLE]))
 
     ret.events = events
-    ret.canMonoTimes = canMonoTimes
 
     # update previous brake/gas pressed
     self.gas_pressed_prev = ret.gasPressed

selfdrive/car/honda/radar_interface.py

@@ -31,25 +31,30 @@ class RadarInterface(object):
 
     # Nidec
     self.rcp = _create_nidec_can_parser()
+    self.trigger_msg = 0x445
+    self.updated_messages = set()
 
-  def update(self):
+  def update(self, can_strings):
-    canMonoTimes = []
-
-    updated_messages = set()
-    ret = car.RadarData.new_message()
-
     # in Bosch radar and we are only steering for now, so sleep 0.05s to keep
     # radard at 20Hz and return no points
     if self.radar_off_can:
       time.sleep(0.05)
-      return ret
+      return car.RadarData.new_message()
+
+    tm = int(sec_since_boot() * 1e9)
+    vls = self.rcp.update_strings(tm, can_strings)
+    self.updated_messages.update(vls)
+
+    if self.trigger_msg not in self.updated_messages:
+      return None
+
+    rr = self._update(self.updated_messages)
+    self.updated_messages.clear()
+    return rr
 
-    while 1:
+
-      tm = int(sec_since_boot() * 1e9)
+  def _update(self, updated_messages):
-      _, vls = self.rcp.update(tm, True)
+    ret = car.RadarData.new_message()
-      updated_messages.update(vls)
-      if 0x445 in updated_messages:
-        break
 
     for ii in updated_messages:
       cpt = self.rcp.vl[ii]
@@ -80,19 +85,7 @@ class RadarInterface(object):
     if self.radar_wrong_config:
       errors.append("wrongConfig")
     ret.errors = errors
-    ret.canMonoTimes = canMonoTimes
 
     ret.points = self.pts.values()
 
     return ret
-
-
-if __name__ == "__main__":
-  class CarParams:
-    radarOffCan = False
-
-  RI = RadarInterface(CarParams)
-  while 1:
-    ret = RI.update()
-    print(chr(27) + "[2J")
-    print(ret)

selfdrive/car/hyundai/carstate.py

@@ -93,7 +93,7 @@ def get_can_parser(CP):
     ("SAS11", 100)
   ]
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
 
 def get_camera_parser(CP):
@@ -119,7 +119,7 @@ def get_camera_parser(CP):
 
   checks = []
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
 
 
 class CarState(object):

selfdrive/car/hyundai/interface.py

@@ -151,17 +151,16 @@ class CarInterface(object):
     return ret
 
   # returns a car.CarState
-  def update(self, c):
+  def update(self, c, can_strings):
     # ******************* do can recv *******************
-    canMonoTimes = []
+    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
+    self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
 
     self.CS.update(self.cp, self.cp_cam)
     # create message
     ret = car.CarState.new_message()
 
-    ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid  # TODO: check cp_cam validity
+    ret.canValid = self.cp.can_valid  # TODO: check cp_cam validity
 
     # speeds
     ret.vEgo = self.CS.v_ego
@@ -269,7 +268,6 @@ class CarInterface(object):
       events.append(create_event('belowSteerSpeed', [ET.WARNING]))
 
     ret.events = events
-    ret.canMonoTimes = canMonoTimes
 
     self.gas_pressed_prev = ret.gasPressed
     self.brake_pressed_prev = ret.brakePressed

selfdrive/car/hyundai/radar_interface.py

@@ -9,16 +9,8 @@ class RadarInterface(object):
     self.pts = {}
     self.delay = 0.1
 
-  def update(self):
+  def update(self, can_strings):
-
     ret = car.RadarData.new_message()
     time.sleep(0.05)  # radard runs on RI updates
 
     return ret
-
-if __name__ == "__main__":
-  RI = RadarInterface(None)
-  while 1:
-    ret = RI.update()
-    print(chr(27) + "[2J")
-    print(ret)

selfdrive/car/mock/interface.py

@@ -80,7 +80,7 @@ class CarInterface(object):
     return ret
 
   # returns a car.CarState
-  def update(self, c):
+  def update(self, c, can_strings):
     self.rk.keep_time()
 
     # get basic data from phone and gps since CAN isn't connected

selfdrive/car/mock/radar_interface.py

@@ -9,15 +9,7 @@ class RadarInterface(object):
     self.pts = {}
     self.delay = 0.1
 
-  def update(self):
+  def update(self, can_strings):
-
     ret = car.RadarData.new_message()
     time.sleep(0.05)  # radard runs on RI updates
     return ret
-
-if __name__ == "__main__":
-  RI = RadarInterface(None)
-  while 1:
-    ret = RI.update()
-    print(chr(27) + "[2J")
-    print(ret)

selfdrive/car/subaru/carstate.py

@@ -37,7 +37,7 @@ def get_powertrain_can_parser(CP):
     ("BodyInfo", 10),
   ]
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
 
 def get_camera_can_parser(CP):
@@ -79,7 +79,7 @@ def get_camera_can_parser(CP):
     ("ES_DashStatus", 10),
   ]
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
 
 
 class CarState(object):

selfdrive/car/subaru/interface.py

@@ -94,16 +94,16 @@ class CarInterface(object):
     return ret
 
   # returns a car.CarState
-  def update(self, c):
+  def update(self, c, can_strings):
-    can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
+    self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-    cam_rcv_valid, _ = self.cam_cp.update(int(sec_since_boot() * 1e9), False)
+    self.cam_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
 
     self.CS.update(self.pt_cp, self.cam_cp)
 
     # create message
     ret = car.CarState.new_message()
 
-    ret.canValid = can_rcv_valid and cam_rcv_valid and self.pt_cp.can_valid and self.cam_cp.can_valid
+    ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
 
     # speeds
     ret.vEgo = self.CS.v_ego

selfdrive/car/subaru/radar_interface.py

@@ -9,16 +9,9 @@ class RadarInterface(object):
     self.pts = {}
     self.delay = 0.1
 
-  def update(self):
+  def update(self, can_strings):
 
     ret = car.RadarData.new_message()
     time.sleep(0.05)  # radard runs on RI updates
 
     return ret
-
-if __name__ == "__main__":
-  RI = RadarInterface(None)
-  while 1:
-    ret = RI.update()
-    print(chr(27) + "[2J")
-    print(ret)

selfdrive/car/toyota/carstate.py

@@ -1,7 +1,7 @@
 import numpy as np
 from common.kalman.simple_kalman import KF1D
-from selfdrive.can.parser import CANParser
 from selfdrive.can.can_define import CANDefine
+from selfdrive.can.parser import CANParser
 from selfdrive.config import Conversions as CV
 from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR
 
@@ -70,9 +70,10 @@ def get_can_parser(CP):
   # add gas interceptor reading if we are using it
   if CP.enableGasInterceptor:
     signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
+    signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
     checks.append(("GAS_SENSOR", 50))
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
 
 
 def get_cam_can_parser(CP):
@@ -82,7 +83,7 @@ def get_cam_can_parser(CP):
   # use steering message to check if panda is connected to frc
   checks = [("STEERING_LKA", 42)]
 
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2, timeout=100)
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
 
 
 class CarState(object):
@@ -118,7 +119,7 @@ class CarState(object):
 
     self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
     if self.CP.enableGasInterceptor:
-      self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
+      self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
     else:
       self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
     self.car_gas = self.pedal_gas

selfdrive/car/toyota/interface.py

@@ -9,7 +9,6 @@ from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
 from selfdrive.swaglog import cloudlog
 
-
 class CarInterface(object):
   def __init__(self, CP, CarController):
     self.CP = CP
@@ -175,6 +174,16 @@ class CarInterface(object):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
       ret.lateralTuning.pid.kf = 0.00007818594
 
+    elif candidate == CAR.SIENNA:
+      stop_and_go = True
+      ret.safetyParam = 73
+      ret.wheelbase = 3.03
+      ret.steerRatio = 16.0
+      tire_stiffness_factor = 0.444
+      ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
+      ret.lateralTuning.pid.kf = 0.00007818594
+
     ret.steerRateCost = 1.
     ret.centerToFront = ret.wheelbase * 0.44
 
@@ -201,8 +210,8 @@ class CarInterface(object):
     # steer, gas, brake limitations VS speed
     ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS]  # breakpoints at 1 and 40 kph
     ret.steerMaxV = [1., 1.]  # 2/3rd torque allowed above 45 kph
-    ret.brakeMaxBP = [5., 20.]
+    ret.brakeMaxBP = [0.]
-    ret.brakeMaxV = [1., 0.8]
+    ret.brakeMaxV = [1.]
 
     ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
     ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU)
@@ -236,22 +245,20 @@ class CarInterface(object):
     return ret
 
   # returns a car.CarState
-  def update(self, c):
+  def update(self, c, can_strings):
     # ******************* do can recv *******************
-    canMonoTimes = []
+    self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
-
-    can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
 
     # run the cam can update for 10s as we just need to know if the camera is alive
     if self.frame < 1000:
-      self.cp_cam.update(int(sec_since_boot() * 1e9), False)
+      self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
 
     self.CS.update(self.cp)
 
     # create message
     ret = car.CarState.new_message()
 
-    ret.canValid = can_rcv_valid and self.cp.can_valid
+    ret.canValid = self.cp.can_valid
 
     # speeds
     ret.vEgo = self.CS.v_ego
@@ -368,7 +375,6 @@ class CarInterface(object):
       events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
 
     ret.events = events
-    ret.canMonoTimes = canMonoTimes
 
     self.gas_pressed_prev = ret.gasPressed
     self.brake_pressed_prev = ret.brakePressed

selfdrive/car/toyota/radar_interface.py

@@ -46,32 +46,36 @@ class RadarInterface(object):
     self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
 
     self.rcp = _create_radar_can_parser(CP.carFingerprint)
+    self.trigger_msg = self.RADAR_B_MSGS[-1]
+    self.updated_messages = set()
+
     # No radar dbc for cars without DSU which are not TSS 2.0
     # TODO: make a adas dbc file for dsu-less models
     self.no_radar = CP.carFingerprint in NO_DSU_CAR and CP.carFingerprint not in TSS2_CAR
 
-  def update(self):
+  def update(self, can_strings):
-
-    ret = car.RadarData.new_message()
-
     if self.no_radar:
       time.sleep(0.05)
-      return ret
+      return car.RadarData.new_message()
+
+    tm = int(sec_since_boot() * 1e9)
+    vls = self.rcp.update_strings(tm, can_strings)
+    self.updated_messages.update(vls)
 
-    canMonoTimes = []
+    if self.trigger_msg not in self.updated_messages:
-    updated_messages = set()
+      return None
-    while 1:
-      tm = int(sec_since_boot() * 1e9)
-      _, vls = self.rcp.update(tm, True)
-      updated_messages.update(vls)
-      if self.RADAR_B_MSGS[-1] in updated_messages:
-        break
 
+    rr =  self._update(self.updated_messages)
+    self.updated_messages.clear()
+
+    return rr
+
+  def _update(self, updated_messages):
+    ret = car.RadarData.new_message()
     errors = []
     if not self.rcp.can_valid:
       errors.append("canError")
     ret.errors = errors
-    ret.canMonoTimes = canMonoTimes
 
     for ii in updated_messages:
       if ii in self.RADAR_A_MSGS:
@@ -105,10 +109,3 @@ class RadarInterface(object):
 
     ret.points = self.pts.values()
     return ret
-
-if __name__ == "__main__":
-  RI = RadarInterface(None)
-  while 1:
-    ret = RI.update()
-    print(chr(27) + "[2J")
-    print(ret)

selfdrive/car/toyota/values.py

@@ -16,6 +16,7 @@ class CAR:
   RAV4_TSS2 = "TOYOTA RAV4 2019"
   COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
   LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
+  SIENNA = "TOYOTA SIENNA XLE 2018"
 
 
 class ECU:
@@ -42,23 +43,23 @@ STATIC_MSGS = [
   (0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
 
   (0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1,   3, '\xf4\x01\x90\x83\x00\x37'),
-  (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH), 1,   3, '\x03\x00\x20\x00\x00\x52'),
+  (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA), 1,   3, '\x03\x00\x20\x00\x00\x52'),
-  (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1,   2, '\x00\x00\x00\x46'),
+  (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1,   2, '\x00\x00\x00\x46'),
-  (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1,   7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
+  (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1,   7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
   (0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1,   7, '\x00\x1e\x00\x00\x00\x80\x07'),
-  (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER), 1,  7, '\x00\x1e\x00\xd4\x00\x00\x5b'),
+  (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA), 1,  7, '\x00\x1e\x00\xd4\x00\x00\x5b'),
-  (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0,   3, '\x00\x00\x00\x00\x00\x00\x8c'),
+  (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0,   3, '\x00\x00\x00\x00\x00\x00\x8c'),
   (0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,   3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
   (0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,   3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
   (0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0,  20, '\x0f\xff\x26\x40\x00\x1f\x00'),
-  (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0,   5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
+  (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0,   5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
   (0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0,  20, '\x00\x00\x00\x80\x03\x00\x08'),
-  (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0,  20, '\x00\x00\x00\x80\xfc\x00\x08'),
+  (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0,  20, '\x00\x00\x00\x80\xfc\x00\x08'),
   (0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0,  20, '\x00\x00\x4d\x82\x40\x02\x00'),
-  (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0,  20, '\x00\x72\x07\xff\x09\xfe\x00'),
+  (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0,  20, '\x00\x72\x07\xff\x09\xfe\x00'),
   (0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'),
-  (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H), 1,  100, '\x00\x00\x01\x79'),
+  (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA), 1,  100, '\x00\x00\x01\x79'),
-  (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
+  (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
 
   (0x292, ECU.APGS, (CAR.PRIUS), 0,   3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
   (0x32E, ECU.APGS, (CAR.PRIUS), 0,  20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
@@ -178,6 +179,9 @@ FINGERPRINTS = {
   {
     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, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 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, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 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, 1228: 8, 1235: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   }],
+  CAR.SIENNA: [{
+    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+  }],
 }
 
 STEER_THRESHOLD = 100
@@ -198,8 +202,9 @@ DBC = {
   CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
   CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
+  CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
 }
 
 NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
 TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
-NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]  # no resume button press required
+NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA]  # no resume button press required

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.6-release"
+#define COMMA_VERSION "0.6.1-release"

selfdrive/controls/controlsd.py

@@ -22,7 +22,7 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.controls.lib.driver_monitor import DriverStatus
+from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS
 from selfdrive.controls.lib.planner import LON_MPC_STEP
 from selfdrive.locationd.calibration_helpers import Calibration, Filter
 
@@ -49,16 +49,22 @@ def events_to_bytes(events):
   return ret
 
 
-def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery,
+def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
                 driver_status, state, mismatch_counter, params):
   """Receive data from sockets and create events for battery, temperature and disk space"""
 
   # Update carstate from CAN and create events
-  CS = CI.update(CC)
+  can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+  CS = CI.update(CC, can_strs)
+
+  sm.update(0)
+
   events = list(CS.events)
   enabled = isEnabled(state)
 
-  sm.update(0)
+  # Check for CAN timeout
+  if not can_strs:
+    events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
   if sm.updated['thermal']:
     overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
@@ -73,6 +79,7 @@ def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_batt
   if free_space:
     events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
 
+
   # Handle calibration
   if sm.updated['liveCalibration']:
     cal_status = sm['liveCalibration'].calStatus
@@ -102,6 +109,9 @@ def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_batt
   if sm.updated['driverMonitoring']:
     driver_status.get_pose(sm['driverMonitoring'], params)
 
+  if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
+    events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
+
   return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
 
 
@@ -402,6 +412,7 @@ def controlsd_thread(gctx=None):
 
   params = Params()
 
+
   # Pub Sockets
   sendcan = messaging.pub_sock(service_list['sendcan'].port)
   controlsstate = messaging.pub_sock(service_list['controlsState'].port)
@@ -414,10 +425,15 @@ def controlsd_thread(gctx=None):
   passive = params.get("Passive") != "0"
 
   sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
+
   logcan = messaging.sub_sock(service_list['can'].port)
+  CI, CP = get_car(logcan, sendcan)
+  logcan.close()
+
+  # TODO: Use the logcan socket from above, but that will currenly break the tests
+  can_sock = messaging.sub_sock(service_list['can'].port, timeout=100)
 
   CC = car.CarControl.new_message()
-  CI, CP = get_car(logcan, sendcan)
   AM = AlertManager()
 
   car_recognized = CP.carName != 'mock'
@@ -469,7 +485,7 @@ def controlsd_thread(gctx=None):
 
     # Sample data and compute car events
     CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
-      data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery,
+      data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
                   driver_status, state, mismatch_counter, params)
     prof.checkpoint("Sample")
 

selfdrive/controls/lib/alerts.py

@@ -298,6 +298,13 @@ ALERTS = [
       AlertStatus.normal, AlertSize.mid,
       Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
 
+  Alert(
+      "tooDistractedNoEntry",
+      "openpilot Unavailable",
+      "Distraction Level Too High",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
   # Cancellation alerts causing soft disabling
   Alert(
       "overheat",

selfdrive/controls/lib/driver_monitor.py

@@ -14,11 +14,12 @@ _PITCH_WEIGHT = 1.5  # pitch matters a lot more
 _METRIC_THRESHOLD = 0.4
 _PITCH_POS_ALLOWANCE = 0.08   # rad, to not be too sensitive on positive pitch
 _PITCH_NATURAL_OFFSET = 0.1   # people don't seem to look straight when they drive relaxed, rather a bit up
-_YAW_NATURAL_OFFSET = 0.08   # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
+_YAW_NATURAL_OFFSET = 0.08    # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
 _STD_THRESHOLD = 0.1          # above this standard deviation consider the measurement invalid
 _DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
 _VARIANCE_FILTER_TS = 20.     # 0.008Hz
 
+MAX_TERMINAL_ALERTS = 3      # not allowed to engage after 3 terminal alerts
 RESIZED_FOCAL = 320.0
 H, W, FULL_W = 320, 160, 426
 
@@ -68,6 +69,7 @@ class DriverStatus():
     self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, DT_DMON)
     self.ts_last_check = 0.
     self.face_detected = False
+    self.terminal_alert_cnt = 0
     self._set_timers()
 
   def _reset_filters(self):
@@ -133,6 +135,7 @@ class DriverStatus():
   def update(self, events, driver_engaged, ctrl_active, standstill):
 
     driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
+    awareness_prev = self.awareness
 
     if (driver_engaged and self.awareness > 0.) or not ctrl_active:
       # always reset if driver is in control (unless we are in red alert state) or op isn't active
@@ -144,15 +147,18 @@ class DriverStatus():
       self.awareness = max(self.awareness - self.step_change, -0.1)
 
     alert = None
-    if self.awareness <= 0.:
+    if self.awareness < 0.:
       # terminal red alert: disengagement required
       alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive'
+      if awareness_prev >= 0.:
+        self.terminal_alert_cnt += 1
     elif self.awareness <= self.threshold_prompt:
       # prompt orange alert
       alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive'
     elif self.awareness <= self.threshold_pre:
       # pre green alert
       alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
+
     if alert is not None:
       events.append(create_event(alert, [ET.WARNING]))
 

selfdrive/controls/lib/fcw.py

@@ -43,8 +43,10 @@ class FCWChecker(object):
       ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
     return ttc
 
-  def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
+  def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
     mpc_solution_a = list(mpc_solution[0].a_ego)
+    a_target = mpc_solution_a[1]
+
     self.last_min_a = min(mpc_solution_a)
     self.v_lead_max = max(self.v_lead_max, v_lead)
 
@@ -62,8 +64,11 @@ class FCWChecker(object):
       a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
       a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
 
-      fcw_allowed = all(c >= 10 for c in self.counters.values())
+      future_fcw_allowed = all(c >= 10 for c in self.counters.values())
-      if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
+      future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
+      current_fcw = a_target < -3.0 and active
+
+      if (future_fcw or current_fcw) and (self.last_fcw_time + 5.0 < cur_time):
         self.last_fcw_time = cur_time
         self.last_fcw_a = self.last_min_a
         return True

selfdrive/controls/lib/planner.py

@@ -201,7 +201,9 @@ class Planner(object):
       self.fcw_checker.reset_lead(cur_time)
 
     blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
-    fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, sm['carState'].aEgo,
+    fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
+                                  sm['controlsState'].active,
+                                  v_ego, sm['carState'].aEgo,
                                   lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
                                   lead_1.yRel, lead_1.vLat,
                                   lead_1.fcw, blinkers) and not sm['carState'].brakePressed

selfdrive/controls/radard.py

@@ -26,6 +26,11 @@ DIMSV = 2
 XV, SPEEDV = 0, 1
 VISION_POINT = -1
 
+path_x = np.arange(0.0, 140.0, 0.1)    # 140 meters is max
+
+# Time-alignment
+rate = 1. / DT_MDL  # model and radar are both at 20Hz
+v_len = 20   # how many speed data points to remember for t alignment with rdr data
 
 class EKFV1D(EKF):
   def __init__(self):
@@ -43,171 +48,133 @@ class EKFV1D(EKF):
     tfj = tf
     return tf, tfj
 
+class RadarD(object):
+  def __init__(self, VM, mocked):
+    self.VM = VM
+    self.mocked = mocked
 
-## fuses camera and radar data for best lead detection
+    self.MP = ModelParser()
-def radard_thread(gctx=None):
+    self.tracks = defaultdict(dict)
-  set_realtime_priority(2)
 
-  # wait for stats about the car to come in from controls
+    self.last_md_ts = 0
-  cloudlog.info("radard is waiting for CarParams")
+    self.last_controls_state_ts = 0
-  CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
-  mocked = CP.carName == "mock"
-  VM = VehicleModel(CP)
-  cloudlog.info("radard got CarParams")
-
-  # import the radar from the fingerprint
-  cloudlog.info("radard is importing %s", CP.carName)
-  RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
-
-  sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
 
-  # Default parameters
+    self.active = 0
-  live_parameters = messaging.new_message()
+    self.steer_angle = 0.
-  live_parameters.init('liveParameters')
+    self.steer_override = False
-  live_parameters.liveParameters.valid = True
-  live_parameters.liveParameters.steerRatio = CP.steerRatio
-  live_parameters.liveParameters.stiffnessFactor = 1.0
 
-  MP = ModelParser()
+    # Kalman filter stuff:
-  RI = RadarInterface(CP)
+    self.ekfv = EKFV1D()
+    self.speedSensorV = SimpleSensor(XV, 1, 2)
 
-  last_md_ts = 0
+    # v_ego
-  last_controls_state_ts = 0
+    self.v_ego = 0.
-
+    self.v_ego_hist_t = deque([0], maxlen=v_len)
-  # *** publish radarState and liveTracks
+    self.v_ego_hist_v = deque([0], maxlen=v_len)
-  radarState = messaging.pub_sock(service_list['radarState'].port)
+    self.v_ego_t_aligned = 0.
-  liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
-
-  path_x = np.arange(0.0, 140.0, 0.1)    # 140 meters is max
-
-  # Time-alignment
-  rate = 1. / DT_MDL  # model and radar are both at 20Hz
-  v_len = 20   # how many speed data points to remember for t alignment with rdr data
-
-  active = 0
-  steer_angle = 0.
-  steer_override = False
-
-  tracks = defaultdict(dict)
-
-  # Kalman filter stuff:
-  ekfv = EKFV1D()
-  speedSensorV = SimpleSensor(XV, 1, 2)
-
-  # v_ego
-  v_ego = 0.
-  v_ego_hist_t = deque([0], maxlen=v_len)
-  v_ego_hist_v = deque([0], maxlen=v_len)
-  v_ego_t_aligned = 0.
-
-  rk = Ratekeeper(rate, print_delay_threshold=None)
-  while 1:
-    rr = RI.update()
 
+  def update(self, frame, delay, sm, rr):
     ar_pts = {}
     for pt in rr.points:
       ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
 
-    sm.update(0)
-
     if sm.updated['liveParameters']:
-        VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
+      self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
 
     if sm.updated['controlsState']:
-      active = sm['controlsState'].active
+      self.active = sm['controlsState'].active
-      v_ego = sm['controlsState'].vEgo
+      self.v_ego = sm['controlsState'].vEgo
-      steer_angle = sm['controlsState'].angleSteers
+      self.steer_angle = sm['controlsState'].angleSteers
-      steer_override = sm['controlsState'].steerOverride
+      self.steer_override = sm['controlsState'].steerOverride
 
-      v_ego_hist_v.append(v_ego)
+      self.v_ego_hist_v.append(self.v_ego)
-      v_ego_hist_t.append(float(rk.frame)/rate)
+      self.v_ego_hist_t.append(float(frame)/rate)
 
-      last_controls_state_ts = sm.logMonoTime['controlsState']
+      self.last_controls_state_ts = sm.logMonoTime['controlsState']
 
     if sm.updated['model']:
-      last_md_ts = sm.logMonoTime['model']
+      self.last_md_ts = sm.logMonoTime['model']
-      MP.update(v_ego, sm['model'])
+      self.MP.update(self.v_ego, sm['model'])
-
 
     # run kalman filter only if prob is high enough
-    if MP.lead_prob > 0.7:
+    if self.MP.lead_prob > 0.7:
-      reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var))
+      reading = self.speedSensorV.read(self.MP.lead_dist, covar=np.matrix(self.MP.lead_var))
-      ekfv.update_scalar(reading)
+      self.ekfv.update_scalar(reading)
-      ekfv.predict(DT_MDL)
+      self.ekfv.predict(DT_MDL)
 
       # When changing lanes the distance to the lead car can suddenly change,
       # which makes the Kalman filter output large relative acceleration
-      if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0:
+      if self.mocked and abs(self.MP.lead_dist - self.ekfv.state[XV]) > 2.0:
-        ekfv.state[XV] = MP.lead_dist
+        self.ekfv.state[XV] = self.MP.lead_dist
-        ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
+        self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init]))
-        ekfv.state[SPEEDV] = 0.
+        self.ekfv.state[SPEEDV] = 0.
 
-      ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])),
+      ar_pts[VISION_POINT] = (float(self.ekfv.state[XV]), np.polyval(self.MP.d_poly, float(self.ekfv.state[XV])),
-                              float(ekfv.state[SPEEDV]), False)
+                              float(self.ekfv.state[SPEEDV]), False)
     else:
-      ekfv.state[XV] = MP.lead_dist
+      self.ekfv.state[XV] = self.MP.lead_dist
-      ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
+      self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init]))
-      ekfv.state[SPEEDV] = 0.
+      self.ekfv.state[SPEEDV] = 0.
 
       if VISION_POINT in ar_pts:
         del ar_pts[VISION_POINT]
 
     # *** compute the likely path_y ***
-    if (active and not steer_override) or mocked:
+    if (self.active and not self.steer_override) or self.mocked:
       # use path from model (always when mocking as steering is too noisy)
-      path_y = np.polyval(MP.d_poly, path_x)
+      path_y = np.polyval(self.MP.d_poly, path_x)
     else:
       # use path from steer, set angle_offset to 0 it does not only report the physical offset
-      path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=live_parameters.liveParameters.angleOffsetAverage)[0]
+      path_y = calc_lookahead_offset(self.v_ego, self.steer_angle, path_x, self.VM, angle_offset=sm['liveParameters'].angleOffsetAverage)[0]
 
     # *** remove missing points from meta data ***
-    for ids in tracks.keys():
+    for ids in self.tracks.keys():
       if ids not in ar_pts:
-        tracks.pop(ids, None)
+        self.tracks.pop(ids, None)
 
     # *** compute the tracks ***
     for ids in ar_pts:
       # ignore standalone vision point, unless we are mocking the radar
-      if ids == VISION_POINT and not mocked:
+      if ids == VISION_POINT and not self.mocked:
         continue
       rpt = ar_pts[ids]
 
       # align v_ego by a fixed time to align it with the radar measurement
-      cur_time = float(rk.frame)/rate
+      cur_time = float(frame)/rate
-      v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_hist_t, v_ego_hist_v)
+      self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v)
 
       d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
       # add sign
       d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
 
       # create the track if it doesn't exist or it's a new track
-      if ids not in tracks:
+      if ids not in self.tracks:
-        tracks[ids] = Track()
+        self.tracks[ids] = Track()
-      tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, self.v_ego_t_aligned, rpt[3], self.steer_override)
 
     # allow the vision model to remove the stationary flag if distance and rel speed roughly match
     if VISION_POINT in ar_pts:
       fused_id = None
       best_score = NO_FUSION_SCORE
-      for ids in tracks:
+      for ids in self.tracks:
-        dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
+        dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - self.tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - self.tracks[ids].yRel)) ** 2)
-        rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
+        rel_speed_diff = abs(ar_pts[VISION_POINT][2] - self.tracks[ids].vRel)
-        tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
+        self.tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
-        if best_score > tracks[ids].vision_score:
+        if best_score > self.tracks[ids].vision_score:
           fused_id = ids
-          best_score = tracks[ids].vision_score
+          best_score = self.tracks[ids].vision_score
 
       if fused_id is not None:
-        tracks[fused_id].vision_cnt += 1
+        self.tracks[fused_id].vision_cnt += 1
-        tracks[fused_id].update_vision_fusion()
+        self.tracks[fused_id].update_vision_fusion()
 
     if DEBUG:
       print("NEW CYCLE")
       if VISION_POINT in ar_pts:
         print("vision", ar_pts[VISION_POINT])
 
-    idens = list(tracks.keys())
+    idens = list(self.tracks.keys())
-    track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
+    track_pts = np.array([self.tracks[iden].get_key_for_cluster() for iden in idens])
 
     # If we have multiple points, cluster them
     if len(track_pts) > 1:
@@ -218,12 +185,12 @@ def radard_thread(gctx=None):
         cluster_i = cluster_idxs[idx]
         if clusters[cluster_i] is None:
           clusters[cluster_i] = Cluster()
-        clusters[cluster_i].add(tracks[idens[idx]])
+        clusters[cluster_i].add(self.tracks[idens[idx]])
 
     elif len(track_pts) == 1:
       # TODO: why do we need this?
       clusters = [Cluster()]
-      clusters[0].add(tracks[idens[0]])
+      clusters[0].add(self.tracks[idens[0]])
     else:
       clusters = []
 
@@ -232,7 +199,7 @@ def radard_thread(gctx=None):
         print(i)
     # *** extract the lead car ***
     lead_clusters = [c for c in clusters
-                     if c.is_potential_lead(v_ego)]
+                     if c.is_potential_lead(self.v_ego)]
     lead_clusters.sort(key=lambda x: x.dRel)
     lead_len = len(lead_clusters)
 
@@ -246,10 +213,10 @@ def radard_thread(gctx=None):
     dat = messaging.new_message()
     dat.init('radarState')
     dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
-    dat.radarState.mdMonoTime = last_md_ts
+    dat.radarState.mdMonoTime = self.last_md_ts
     dat.radarState.canMonoTimes = list(rr.canMonoTimes)
     dat.radarState.radarErrors = list(rr.errors)
-    dat.radarState.controlsStateMonoTime = last_controls_state_ts
+    dat.radarState.controlsStateMonoTime = self.last_controls_state_ts
     if lead_len > 0:
       dat.radarState.leadOne = lead_clusters[0].toRadarState()
       if lead2_len > 0:
@@ -259,10 +226,51 @@ def radard_thread(gctx=None):
     else:
       dat.radarState.leadOne.status = False
 
+    return dat
+
+## fuses camera and radar data for best lead detection
+def radard_thread(gctx=None):
+  set_realtime_priority(2)
+
+  # wait for stats about the car to come in from controls
+  cloudlog.info("radard is waiting for CarParams")
+  CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
+  mocked = CP.carName == "mock"
+  VM = VehicleModel(CP)
+  cloudlog.info("radard got CarParams")
+
+  # import the radar from the fingerprint
+  cloudlog.info("radard is importing %s", CP.carName)
+  RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
+
+  can_sock = messaging.sub_sock(service_list['can'].port)
+  sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
+
+  RI = RadarInterface(CP)
+
+  # *** publish radarState and liveTracks
+  radarState = messaging.pub_sock(service_list['radarState'].port)
+  liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
+
+  rk = Ratekeeper(rate, print_delay_threshold=None)
+  RD = RadarD(VM, mocked)
+
+  while 1:
+    can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+    rr = RI.update(can_strings)
+
+    if rr is None:
+      continue
+
+    sm.update(0)
+
+    dat = RD.update(rk.frame, RI.delay, sm, rr)
     dat.radarState.cumLagMs = -rk.remaining*1000.
+
     radarState.send(dat.to_bytes())
 
     # *** publish tracks for UI debugging (keep last) ***
+    tracks = RD.tracks
     dat = messaging.new_message()
     dat.init('liveTracks', len(tracks))
 

selfdrive/locationd/.gitignore

@@ -1,3 +1,4 @@
 ubloxd
 ubloxd_test
-params_learner
+params_learner
+paramsd

selfdrive/locationd/Makefile

@@ -40,11 +40,11 @@ EXTRA_LIBS += -llog -luuid
 endif
 
 .PHONY: all
-all: ubloxd params_learner
+all: ubloxd paramsd
 
 include ../common/cereal.mk
 
-LOC_OBJS = locationd_yawrate.o params_learner.o \
+LOC_OBJS = locationd_yawrate.o params_learner.o paramsd.o \
            ../common/swaglog.o \
            ../common/params.o \
            ../common/util.o \
@@ -71,7 +71,7 @@ liblocationd.so: $(LOC_OBJS)
             $(ZMQ_SHARED_LIBS) \
             $(EXTRA_LIBS)
 
-params_learner: $(LOC_OBJS)
+paramsd: $(LOC_OBJS)
 	@echo "[ LINK ] $@"
 	$(CXX) -fPIC -o '$@' $^ \
             $(CEREAL_LIBS) \
@@ -115,7 +115,7 @@ ubloxd_test: ubloxd_test.o $(OBJS)
 
 .PHONY: clean
 clean:
-	rm -f ubloxd params_learner liblocationd.so ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(LOC_OBJS) $(DEPS)
+	rm -f ubloxd paramsd liblocationd.so ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(LOC_OBJS) $(DEPS)
 
 -include $(DEPS)
 -include $(LOC_DEPS)

selfdrive/locationd/locationd_yawrate.cc

@@ -1,287 +1,96 @@
 #include <iostream>
-#include <csignal>
 #include <cmath>
 
-#include <czmq.h>
 #include <capnp/message.h>
 #include <capnp/serialize-packed.h>
 #include <eigen3/Eigen/Dense>
-#include "json11.hpp"
 
-#include "cereal/gen/cpp/log.capnp.h"
+#include "locationd_yawrate.h"
-#include "common/swaglog.h"
-#include "common/messaging.h"
-#include "common/params.h"
-#include "common/timing.h"
-#include "params_learner.h"
 
-const int num_polls = 3;
-
-class Localizer
-{
-  Eigen::Matrix2d A;
-  Eigen::Matrix2d I;
-  Eigen::Matrix2d Q;
-  Eigen::Matrix2d P;
-  Eigen::Matrix<double, 1, 2> C_posenet;
-  Eigen::Matrix<double, 1, 2> C_gyro;
-
-  double R_gyro;
-
-  void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
-    double dt = current_time - prev_update_time;
-    prev_update_time = current_time;
-    if (dt < 1.0e-9) {
-      return;
-    }
-
-    // x = A * x;
-    // P = A * P * A.transpose() + dt * Q;
-    // Simplify because A is unity
-    P = P + dt * Q;
-
-    double y = meas - C * x;
-    double S = R + C * P * C.transpose();
-    Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
-    x = x + K * y;
-    P = (I - K * C) * P;
-  }
-
-  void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
-    for (cereal::SensorEventData::Reader sensor_event : sensor_events){
-      if (sensor_event.getType() == 4) {
-        sensor_data_time = current_time;
-
-        double meas = -sensor_event.getGyro().getV()[0];
-        update_state(C_gyro, R_gyro, current_time, meas);
-      }
-    }
 
+void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
+  double dt = current_time - prev_update_time;
+  prev_update_time = current_time;
+  if (dt < 1.0e-9) {
+    return;
   }
 
-  void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
+  // x = A * x;
-    double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
+  // P = A * P * A.transpose() + dt * Q;
-    double meas = camera_odometry.getRot()[2];
+  // Simplify because A is unity
-    update_state(C_posenet, R, current_time, meas);
+  P = P + dt * Q;
-  }
-
-  void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
-    steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
-    car_speed = controls_state.getVEgo();
-    controls_state_time = current_time;
-  }
 
+  double y = meas - C * x;
+  double S = R + C * P * C.transpose();
+  Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
+  x = x + K * y;
+  P = (I - K * C) * P;
+}
 
-public:
+void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
-  Eigen::Vector2d x;
+  for (cereal::SensorEventData::Reader sensor_event : sensor_events){
-  double steering_angle = 0;
+    if (sensor_event.getType() == 4) {
-  double car_speed = 0;
+      sensor_data_time = current_time;
-  double prev_update_time = -1;
-  double controls_state_time = -1;
-  double sensor_data_time = -1;
-
-  Localizer() {
-    A << 1, 0, 0, 1;
-    I << 1, 0, 0, 1;
-
-    Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0);
-    P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0);
-
-    C_posenet << 1, 0;
-    C_gyro << 1, 1;
-    x << 0, 0;
-
-    R_gyro = pow(0.05, 2.0);
-  }
-
-  cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size) {
-    const kj::ArrayPtr<const capnp::word> view((const capnp::word*)msg_dat, msg_size);
-    capnp::FlatArrayMessageReader msg(view);
-    cereal::Event::Reader event = msg.getRoot<cereal::Event>();
-    double current_time = event.getLogMonoTime() / 1.0e9;
-
-    if (prev_update_time < 0) {
-      prev_update_time = current_time;
-    }
 
-    auto type = event.which();
+      double meas = -sensor_event.getGyro().getV()[0];
-    switch(type) {
+      update_state(C_gyro, R_gyro, current_time, meas);
-    case cereal::Event::CONTROLS_STATE:
-      handle_controls_state(event.getControlsState(), current_time);
-      break;
-    case cereal::Event::CAMERA_ODOMETRY:
-      handle_camera_odometry(event.getCameraOdometry(), current_time);
-      break;
-    case cereal::Event::SENSOR_EVENTS:
-      handle_sensor_events(event.getSensorEvents(), current_time);
-      break;
-    default:
-      break;
     }
-
-    return type;
   }
-};
+}
-
-
-
-int main(int argc, char *argv[]) {
-  auto ctx = zmq_ctx_new();
-  auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007");
-  auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003");
-  auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066");
-
-  auto live_parameters_sock = zsock_new_pub("@tcp://*:8064");
-  assert(live_parameters_sock);
-  auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock);
-
-  int err;
-  Localizer localizer;
-
-  zmq_pollitem_t polls[num_polls] = {{0}};
-  polls[0].socket = controls_state_sock;
-  polls[0].events = ZMQ_POLLIN;
-  polls[1].socket = sensor_events_sock;
-  polls[1].events = ZMQ_POLLIN;
-  polls[2].socket = camera_odometry_sock;
-  polls[2].events = ZMQ_POLLIN;
-
-  // Read car params
-  char *value;
-  size_t value_sz = 0;
 
-  LOGW("waiting for params to set vehicle model");
+void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
-  while (true) {
+  double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
-    read_db_value(NULL, "CarParams", &value, &value_sz);
+  double meas = camera_odometry.getRot()[2];
-    if (value_sz > 0) break;
+  update_state(C_posenet, R, current_time, meas);
-    usleep(100*1000);
+}
-  }
-  LOGW("got %d bytes CarParams", value_sz);
 
-  // make copy due to alignment issues
+void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
-  auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
+  steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
-  memcpy(amsg.begin(), value, value_sz);
+  car_speed = controls_state.getVEgo();
-  free(value);
+  controls_state_time = current_time;
+}
 
-  capnp::FlatArrayMessageReader cmsg(amsg);
-  cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
 
-  // Read params from previous run
+Localizer::Localizer() {
-  const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz);
+  A << 1, 0, 0, 1;
+  I << 1, 0, 0, 1;
 
-  std::string fingerprint = car_params.getCarFingerprint();
+  Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0);
-  std::string vin = car_params.getCarVin();
+  P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0);
-  double sR = car_params.getSteerRatio();
-  double x = 1.0;
-  double ao = 0.0;
 
-  if (result == 0){
+  C_posenet << 1, 0;
-    auto str = std::string(value, value_sz);
+  C_gyro << 1, 1;
-    free(value);
+  x << 0, 0;
 
-    std::string err;
+  R_gyro = pow(0.05, 2.0);
-    auto json = json11::Json::parse(str, err);
+}
-    if (json.is_null() || !err.empty()) {
-      std::string log = "Error parsing json: " + err;
-      LOGW(log.c_str());
-    } else {
-      std::string new_fingerprint = json["carFingerprint"].string_value();
-      std::string new_vin = json["carVin"].string_value();
 
-      if (fingerprint == new_fingerprint && vin == new_vin) {
+cereal::Event::Which Localizer::handle_log(const unsigned char* msg_dat, size_t msg_size) {
-        std::string log = "Parameter starting with: " + str;
+  const kj::ArrayPtr<const capnp::word> view((const capnp::word*)msg_dat, msg_size);
-        LOGW(log.c_str());
+  capnp::FlatArrayMessageReader msg(view);
+  cereal::Event::Reader event = msg.getRoot<cereal::Event>();
+  double current_time = event.getLogMonoTime() / 1.0e9;
 
-        sR = json["steerRatio"].number_value();
+  if (prev_update_time < 0) {
-        x = json["stiffnessFactor"].number_value();
+    prev_update_time = current_time;
-        ao = json["angleOffsetAverage"].number_value();
-      }
-    }
   }
 
-  ParamsLearner learner(car_params, ao, x, sR, 1.0);
+  auto type = event.which();
-
+  switch(type) {
-  // Main loop
+  case cereal::Event::CONTROLS_STATE:
-  int save_counter = 0;
+    handle_controls_state(event.getControlsState(), current_time);
-  while (true){
+    break;
-    int ret = zmq_poll(polls, num_polls, 100);
+  case cereal::Event::CAMERA_ODOMETRY:
-
+    handle_camera_odometry(event.getCameraOdometry(), current_time);
-    if (ret == 0){
+    break;
-      continue;
+  case cereal::Event::SENSOR_EVENTS:
-    } else if (ret < 0){
+    handle_sensor_events(event.getSensorEvents(), current_time);
-      break;
+    break;
-    }
+  default:
-
+    break;
-    for (int i=0; i < num_polls; i++) {
-      if (polls[i].revents) {
-        zmq_msg_t msg;
-        err = zmq_msg_init(&msg);
-        assert(err == 0);
-        err = zmq_msg_recv(&msg, polls[i].socket, 0);
-        assert(err >= 0);
-        // make copy due to alignment issues, will be freed on out of scope
-        auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
-        memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
-
-        auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size());
-        zmq_msg_close(&msg);
-
-        if (which == cereal::Event::CONTROLS_STATE){
-          save_counter++;
-
-          double yaw_rate = -localizer.x[0];
-          bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
-
-          // TODO: Fix in replay
-          double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
-
-          double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
-          double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
-
-          // Send parameters at 10 Hz
-          if (save_counter % 10 == 0){
-            capnp::MallocMessageBuilder msg;
-            cereal::Event::Builder event = msg.initRoot<cereal::Event>();
-            event.setLogMonoTime(nanos_since_boot());
-            auto live_params = event.initLiveParameters();
-            live_params.setValid(valid);
-            live_params.setYawRate(localizer.x[0]);
-            live_params.setGyroBias(localizer.x[1]);
-            live_params.setSensorValid(sensor_data_age < 5.0);
-            live_params.setAngleOffset(angle_offset_degrees);
-            live_params.setAngleOffsetAverage(angle_offset_average_degrees);
-            live_params.setStiffnessFactor(learner.x);
-            live_params.setSteerRatio(learner.sR);
-
-            auto words = capnp::messageToFlatArray(msg);
-            auto bytes = words.asBytes();
-            zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
-          }
-
-
-          // Save parameters every minute
-          if (save_counter % 6000 == 0) {
-            json11::Json json = json11::Json::object {
-              {"carVin", vin},
-              {"carFingerprint", fingerprint},
-              {"steerRatio", learner.sR},
-              {"stiffnessFactor", learner.x},
-              {"angleOffsetAverage", angle_offset_average_degrees},
-            };
-
-            std::string out = json.dump();
-            write_db_value(NULL, "LiveParameters", out.c_str(), out.length());
-          }
-        }
-      }
-    }
   }
 
-  zmq_close(controls_state_sock);
+  return type;
-  zmq_close(sensor_events_sock);
-  zmq_close(camera_odometry_sock);
-  zmq_close(live_parameters_sock_raw);
-  return 0;
 }
 
 

selfdrive/locationd/locationd_yawrate.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#define DEGREES_TO_RADIANS 0.017453292519943295
+
+class Localizer
+{
+  Eigen::Matrix2d A;
+  Eigen::Matrix2d I;
+  Eigen::Matrix2d Q;
+  Eigen::Matrix2d P;
+  Eigen::Matrix<double, 1, 2> C_posenet;
+  Eigen::Matrix<double, 1, 2> C_gyro;
+
+  double R_gyro;
+
+  void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
+  void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
+  void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
+  void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
+
+public:
+  Eigen::Vector2d x;
+  double steering_angle = 0;
+  double car_speed = 0;
+  double prev_update_time = -1;
+  double controls_state_time = -1;
+  double sensor_data_time = -1;
+
+  Localizer();
+  cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size);
+};

selfdrive/locationd/params_learner.cc

@@ -2,6 +2,8 @@
 #include <cmath>
 #include <iostream>
 
+#include <capnp/message.h>
+#include <capnp/serialize-packed.h>
 #include "cereal/gen/cpp/log.capnp.h"
 #include "cereal/gen/cpp/car.capnp.h"
 #include "params_learner.h"
@@ -14,14 +16,15 @@ T clip(const T& n, const T& lower, const T& upper) {
 }
 
 ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
-                double angle_offset,
+                             double angle_offset,
-                double stiffness_factor,
+                             double stiffness_factor,
-                double steer_ratio,
+                             double steer_ratio,
-                double learning_rate) :
+                             double learning_rate) :
-    ao(angle_offset * DEGREES_TO_RADIANS),
+  ao(angle_offset * DEGREES_TO_RADIANS),
-    slow_ao(angle_offset * DEGREES_TO_RADIANS),
+  slow_ao(angle_offset * DEGREES_TO_RADIANS),
-    x(stiffness_factor),
+  x(stiffness_factor),
-    sR(steer_ratio) {
+  sR(steer_ratio) {
+
   cF0 = car_params.getTireStiffnessFront();
   cR0 = car_params.getTireStiffnessRear();
 
@@ -73,3 +76,43 @@ bool ParamsLearner::update(double psi, double u, double sa) {
   valid = valid && sR < max_sr_th;
   return valid;
 }
+
+
+extern "C" {
+  void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate) {
+
+    auto amsg = kj::heapArray<capnp::word>((len / sizeof(capnp::word)) + 1);
+    memcpy(amsg.begin(), params, len);
+
+    capnp::FlatArrayMessageReader cmsg(amsg);
+    cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+
+    ParamsLearner * p = new ParamsLearner(car_params, angle_offset, stiffness_factor, steer_ratio, learning_rate);
+    return (void*)p;
+  }
+
+  bool params_learner_update(void * params_learner, double psi, double u, double sa) {
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->update(psi, u, sa);
+  }
+
+  double params_learner_get_ao(void * params_learner){
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->ao;
+  }
+
+  double params_learner_get_x(void * params_learner){
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->x;
+  }
+
+  double params_learner_get_slow_ao(void * params_learner){
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->slow_ao;
+  }
+
+  double params_learner_get_sR(void * params_learner){
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->sR;
+  }
+}

selfdrive/locationd/paramsd.cc

@@ -0,0 +1,174 @@
+#include <czmq.h>
+#include <capnp/message.h>
+#include <capnp/serialize-packed.h>
+
+#include "locationd_yawrate.h"
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/swaglog.h"
+#include "common/messaging.h"
+#include "common/params.h"
+#include "common/timing.h"
+#include "params_learner.h"
+#include "json11.hpp"
+
+const int num_polls = 3;
+
+int main(int argc, char *argv[]) {
+  auto ctx = zmq_ctx_new();
+  auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007");
+  auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003");
+  auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066");
+
+  auto live_parameters_sock = zsock_new_pub("@tcp://*:8064");
+  assert(live_parameters_sock);
+  auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock);
+
+  int err;
+  Localizer localizer;
+
+  zmq_pollitem_t polls[num_polls] = {{0}};
+  polls[0].socket = controls_state_sock;
+  polls[0].events = ZMQ_POLLIN;
+  polls[1].socket = sensor_events_sock;
+  polls[1].events = ZMQ_POLLIN;
+  polls[2].socket = camera_odometry_sock;
+  polls[2].events = ZMQ_POLLIN;
+
+  // Read car params
+  char *value;
+  size_t value_sz = 0;
+
+  LOGW("waiting for params to set vehicle model");
+  while (true) {
+    read_db_value(NULL, "CarParams", &value, &value_sz);
+    if (value_sz > 0) break;
+    usleep(100*1000);
+  }
+  LOGW("got %d bytes CarParams", value_sz);
+
+  // make copy due to alignment issues
+  auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
+  memcpy(amsg.begin(), value, value_sz);
+  free(value);
+
+  capnp::FlatArrayMessageReader cmsg(amsg);
+  cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+
+  // Read params from previous run
+  const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz);
+
+  std::string fingerprint = car_params.getCarFingerprint();
+  std::string vin = car_params.getCarVin();
+  double sR = car_params.getSteerRatio();
+  double x = 1.0;
+  double ao = 0.0;
+
+  if (result == 0){
+    auto str = std::string(value, value_sz);
+    free(value);
+
+    std::string err;
+    auto json = json11::Json::parse(str, err);
+    if (json.is_null() || !err.empty()) {
+      std::string log = "Error parsing json: " + err;
+      LOGW(log.c_str());
+    } else {
+      std::string new_fingerprint = json["carFingerprint"].string_value();
+      std::string new_vin = json["carVin"].string_value();
+
+      if (fingerprint == new_fingerprint && vin == new_vin) {
+        std::string log = "Parameter starting with: " + str;
+        LOGW(log.c_str());
+
+        sR = json["steerRatio"].number_value();
+        x = json["stiffnessFactor"].number_value();
+        ao = json["angleOffsetAverage"].number_value();
+      }
+    }
+  }
+
+  ParamsLearner learner(car_params, ao, x, sR, 1.0);
+
+  // Main loop
+  int save_counter = 0;
+  while (true){
+    int ret = zmq_poll(polls, num_polls, 100);
+
+    if (ret == 0){
+      continue;
+    } else if (ret < 0){
+      break;
+    }
+
+    for (int i=0; i < num_polls; i++) {
+      if (polls[i].revents) {
+        zmq_msg_t msg;
+        err = zmq_msg_init(&msg);
+        assert(err == 0);
+        err = zmq_msg_recv(&msg, polls[i].socket, 0);
+        assert(err >= 0);
+        // make copy due to alignment issues, will be freed on out of scope
+        auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
+        memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
+
+        auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size());
+        zmq_msg_close(&msg);
+
+        if (which == cereal::Event::CONTROLS_STATE){
+          save_counter++;
+
+          double yaw_rate = -localizer.x[0];
+          bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
+
+          // TODO: Fix in replay
+          double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
+
+          double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
+          double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
+
+          // Send parameters at 10 Hz
+          if (save_counter % 10 == 0){
+            capnp::MallocMessageBuilder msg;
+            cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+            event.setLogMonoTime(nanos_since_boot());
+            auto live_params = event.initLiveParameters();
+            live_params.setValid(valid);
+            live_params.setYawRate(localizer.x[0]);
+            live_params.setGyroBias(localizer.x[1]);
+            live_params.setSensorValid(sensor_data_age < 5.0);
+            live_params.setAngleOffset(angle_offset_degrees);
+            live_params.setAngleOffsetAverage(angle_offset_average_degrees);
+            live_params.setStiffnessFactor(learner.x);
+            live_params.setSteerRatio(learner.sR);
+
+            auto words = capnp::messageToFlatArray(msg);
+            auto bytes = words.asBytes();
+            zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
+          }
+
+
+          // Save parameters every minute
+          if (save_counter % 6000 == 0) {
+            json11::Json json = json11::Json::object {
+              {"carVin", vin},
+              {"carFingerprint", fingerprint},
+              {"steerRatio", learner.sR},
+              {"stiffnessFactor", learner.x},
+              {"angleOffsetAverage", angle_offset_average_degrees},
+            };
+
+            std::string out = json.dump();
+            write_db_value(NULL, "LiveParameters", out.c_str(), out.length());
+          }
+        }
+      }
+    }
+  }
+
+  zmq_close(controls_state_sock);
+  zmq_close(sensor_events_sock);
+  zmq_close(camera_odometry_sock);
+  zmq_close(live_parameters_sock_raw);
+  return 0;
+}

selfdrive/locationd/test/test_params_learner.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+import numpy as np
+import unittest
+
+from selfdrive.car.honda.interface import CarInterface
+from selfdrive.car.honda.values import CAR
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.locationd.liblocationd_py import liblocationd  # pylint: disable=no-name-in-module, import-error
+
+
+class TestParamsLearner(unittest.TestCase):
+  def setUp(self):
+
+    self.CP = CarInterface.get_params(CAR.CIVIC, {})
+    bts = self.CP.to_bytes()
+
+    self.params_learner = liblocationd.params_learner_init(len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0)
+
+  def test_convergence(self):
+    # Setup vehicle model with wrong parameters
+    VM_sim = VehicleModel(self.CP)
+    x_target = 0.75
+    sr_target = 14
+    ao_target = -1.0
+    VM_sim.update_params(x_target, sr_target)
+
+    # Run simulation
+    times = np.arange(0, 10*3600, 0.01)
+    angle_offset = np.radians(ao_target)
+    steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset
+    speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25
+
+    for i, t in enumerate(times):
+      u = speeds[i]
+      sa = steering_angles[i]
+      psi = VM_sim.yaw_rate(sa - angle_offset, u)
+      liblocationd.params_learner_update(self.params_learner, psi, u, sa)
+
+    # Verify learned parameters
+    sr = liblocationd.params_learner_get_sR(self.params_learner)
+    ao_slow = np.degrees(liblocationd.params_learner_get_slow_ao(self.params_learner))
+    x = liblocationd.params_learner_get_x(self.params_learner)
+    self.assertAlmostEqual(x_target, x, places=1)
+    self.assertAlmostEqual(ao_target, ao_slow, places=1)
+    self.assertAlmostEqual(sr_target, sr, places=1)
+
+
+
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/loggerd/uploader.py

@@ -168,7 +168,7 @@ class Uploader(object):
 
   def do_upload(self, key, fn):
     try:
-      url_resp = api_get("v1.2/"+self.dongle_id+"/upload_url/", timeout=2, path=key, access_token=self.access_token)
+      url_resp = api_get("v1.2/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.access_token)
       url_resp_json = json.loads(url_resp.text)
       url = url_resp_json['url']
       headers = url_resp_json['headers']
@@ -223,7 +223,7 @@ class Uploader(object):
         try:
           os.unlink(fn)
         except OSError:
-          cloudlog.exception("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz)
+          cloudlog.event("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz)
 
         success = True
       else:

selfdrive/manager.py

@@ -72,12 +72,15 @@ import glob
 import shutil
 import hashlib
 import importlib
+import re
+import stat
 import subprocess
 import traceback
 from multiprocessing import Process
 
 from setproctitle import setproctitle  #pylint: disable=no-name-in-module
 
+from common.file_helpers import atomic_write_in_dir_neos
 from common.params import Params
 import cereal
 ThermalStatus = cereal.log.ThermalData.ThermalStatus
@@ -109,12 +112,14 @@ managed_processes = {
   "pandad": "selfdrive.pandad",
   "ui": ("selfdrive/ui", ["./start.py"]),
   "calibrationd": "selfdrive.locationd.calibrationd",
-  "params_learner": ("selfdrive/locationd", ["./params_learner"]),
+  "paramsd": ("selfdrive/locationd", ["./paramsd"]),
   "visiond": ("selfdrive/visiond", ["./visiond"]),
   "sensord": ("selfdrive/sensord", ["./start_sensord.py"]),
   "gpsd": ("selfdrive/sensord", ["./start_gpsd.py"]),
   "updated": "selfdrive.updated",
-  "athena": "selfdrive.athena.athenad",
+}
+daemon_processes = {
+  "athenad": "selfdrive.athena.athenad",
 }
 android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame")
 
@@ -136,7 +141,6 @@ persistent_processes = [
   'uploader',
   'ui',
   'updated',
-  'athena',
 ]
 
 car_started_processes = [
@@ -146,7 +150,7 @@ car_started_processes = [
   'sensord',
   'radard',
   'calibrationd',
-  'params_learner',
+  'paramsd',
   'visiond',
   'proclogd',
   'ubloxd',
@@ -209,6 +213,29 @@ def start_managed_process(name):
     running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd))
   running[name].start()
 
+def start_daemon_process(name, params):
+  proc = daemon_processes[name]
+  pid_param = name.capitalize() + 'Pid'
+  pid = params.get(pid_param)
+
+  if pid is not None:
+    try:
+      os.kill(int(pid), 0)
+      # process is running (kill is a poorly-named system call)
+      return
+    except OSError:
+      # process is dead
+      pass
+
+  cloudlog.info("starting daemon %s" % name)
+  proc = subprocess.Popen(['python', '-m', proc],
+                         cwd='/',
+                         stdout=open('/dev/null', 'w'),
+                         stderr=open('/dev/null', 'w'),
+                         preexec_fn=os.setpgrp)
+
+  params.put(pid_param, str(proc.pid))
+
 def prepare_managed_process(p):
   proc = managed_processes[p]
   if isinstance(proc, str):
@@ -321,6 +348,12 @@ def manager_thread():
   # save boot log
   subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
 
+  params = Params()
+
+  # start daemon processes
+  for p in daemon_processes:
+    start_daemon_process(p, params)
+
   # start persistent processes
   for p in persistent_processes:
     start_managed_process(p)
@@ -332,7 +365,6 @@ def manager_thread():
   if os.getenv("NOBOARD") is None:
     start_managed_process("pandad")
 
-  params = Params()
   logger_dead = False
 
   while 1:
@@ -420,10 +452,46 @@ def update_apks():
 
       assert success
 
+def update_ssh():
+  ssh_home_dirpath = "/system/comma/home/.ssh/"
+  auth_keys_path = os.path.join(ssh_home_dirpath, "authorized_keys")
+  auth_keys_persist_path = os.path.join(ssh_home_dirpath, "authorized_keys.persist")
+  auth_keys_mode = stat.S_IREAD | stat.S_IWRITE
+
+  params = Params()
+  github_keys = params.get("GithubSshKeys") or ''
+
+  old_keys = open(auth_keys_path).read()
+  has_persisted_keys = os.path.exists(auth_keys_persist_path)
+  if has_persisted_keys:
+    persisted_keys = open(auth_keys_persist_path).read()
+  else:
+    # add host filter
+    persisted_keys = re.sub(r'^(?!.+?from.+? )(ssh|ecdsa)', 'from="10.0.0.0/8,172.16.0.0/12,192.168.0.0/16" \\1', old_keys, flags=re.MULTILINE)
+
+  new_keys = persisted_keys + '\n' + github_keys
+
+  if has_persisted_keys and new_keys == old_keys and os.stat(auth_keys_path)[stat.ST_MODE] == auth_keys_mode:
+    # nothing to do - let's avoid remount
+    return
+
+  try:
+    subprocess.check_call(["mount", "-o", "rw,remount", "/system"])
+    if not has_persisted_keys:
+      atomic_write_in_dir_neos(auth_keys_persist_path, persisted_keys, mode=auth_keys_mode)
+
+    atomic_write_in_dir_neos(auth_keys_path, new_keys, mode=auth_keys_mode)
+  finally:
+    try:
+      subprocess.check_call(["mount", "-o", "ro,remount", "/system"])
+    except:
+      cloudlog.exception("Failed to remount as read-only")
+      # this can fail due to "Device busy" - reboot if so
+      os.system("reboot")
+      raise RuntimeError
+
 def manager_update():
-  if os.path.exists(os.path.join(BASEDIR, "vpn")):
+  update_ssh()
-    cloudlog.info("installing vpn")
-    os.system(os.path.join(BASEDIR, "vpn", "install.sh"))
   update_apks()
 
 def manager_prepare():

selfdrive/messaging.py

@@ -16,17 +16,34 @@ def pub_sock(port, addr="*"):
   sock.bind("tcp://%s:%d" % (addr, port))
   return sock
 
-def sub_sock(port, poller=None, addr="127.0.0.1", conflate=False):
+def sub_sock(port, poller=None, addr="127.0.0.1", conflate=False, timeout=None):
   context = zmq.Context.instance()
   sock = context.socket(zmq.SUB)
   if conflate:
     sock.setsockopt(zmq.CONFLATE, 1)
   sock.connect("tcp://%s:%d" % (addr, port))
   sock.setsockopt(zmq.SUBSCRIBE, b"")
+
+  if timeout is not None:
+    sock.RCVTIMEO = timeout
+
   if poller is not None:
     poller.register(sock, zmq.POLLIN)
   return sock
 
+def drain_sock_raw(sock, wait_for_one=False):
+  ret = []
+  while 1:
+    try:
+      if wait_for_one and len(ret) == 0:
+        dat = sock.recv()
+      else:
+        dat = sock.recv(zmq.NOBLOCK)
+      ret.append(dat)
+    except zmq.error.Again:
+      break
+  return ret
+
 def drain_sock(sock, wait_for_one=False):
   ret = []
   while 1:
@@ -82,24 +99,29 @@ class SubMaster():
     self.valid = {}
     for s in services:
       # TODO: get address automatically from service_list
-      self.sock[s] = sub_sock(service_list[s].port, poller=self.poller, addr=addr, conflate=True)
+      if addr is not None:
+        self.sock[s] = sub_sock(service_list[s].port, poller=self.poller, addr=addr, conflate=True)
       self.freq[s] = service_list[s].frequency
       data = new_message()
       data.init(s)
       self.data[s] = getattr(data, s)
-      self.logMonoTime[s] = data.logMonoTime
+      self.logMonoTime[s] = 0
       self.valid[s] = data.valid
 
   def __getitem__(self, s):
     return self.data[s]
 
   def update(self, timeout=-1):
+    msgs = []
+    for sock, _ in self.poller.poll(timeout):
+      msgs.append(recv_one(sock))
+    self.update_msgs(sec_since_boot(), msgs)
+
+  def update_msgs(self, cur_time, msgs):
     # TODO: add optional input that specify the service to wait for
     self.frame += 1
     self.updated = dict.fromkeys(self.updated, False)
-    cur_time = sec_since_boot()
+    for msg in msgs:
-    for sock, _ in self.poller.poll(timeout):
-      msg = recv_one(sock)
       s = msg.which()
       self.updated[s] = True
       self.rcv_time[s] = cur_time

selfdrive/registration.py

@@ -5,7 +5,7 @@ import struct
 
 from datetime import datetime, timedelta
 from selfdrive.swaglog import cloudlog
-from selfdrive.version import version, training_version, get_git_commit, get_git_branch, get_git_remote
+from selfdrive.version import version, terms_version, training_version, get_git_commit, get_git_branch, get_git_remote
 from common.api import api_get
 from common.params import Params
 from common.file_helpers import mkdirs_exists_ok
@@ -53,6 +53,7 @@ def get_subscriber_info():
 def register():
   params = Params()
   params.put("Version", version)
+  params.put("TermsVersion", terms_version)
   params.put("TrainingVersion", training_version)
   params.put("GitCommit", get_git_commit())
   params.put("GitBranch", get_git_branch())

selfdrive/test/plant/plant.py

@@ -244,6 +244,7 @@ class Plant(object):
            'EPB_STATE',
            'BRAKE_HOLD_ACTIVE',
            'INTERCEPTOR_GAS',
+           'INTERCEPTOR_GAS2',
            'IMPERIAL_UNIT',
            ])
     vls = vls_tuple(
@@ -276,6 +277,7 @@ class Plant(object):
            0,  # EPB State
            0,  # Brake hold
            0,  # Interceptor feedback
+           0,  # Interceptor 2 feedback
            False
            )
 

selfdrive/thermald.py

@@ -2,7 +2,7 @@
 import os
 from smbus2 import SMBus
 from cereal import log
-from selfdrive.version import training_version
+from selfdrive.version import terms_version, training_version
 from selfdrive.swaglog import cloudlog
 import selfdrive.messaging as messaging
 from selfdrive.services import service_list
@@ -216,7 +216,7 @@ def thermald_thread():
       ignition = True
 
     do_uninstall = params.get("DoUninstall") == "1"
-    accepted_terms = params.get("HasAcceptedTerms") == "1"
+    accepted_terms = params.get("HasAcceptedTerms") == terms_version
     completed_training = params.get("CompletedTrainingVersion") == training_version
 
     should_start = ignition

selfdrive/version.py

@@ -59,6 +59,7 @@ except subprocess.CalledProcessError:
   dirty = True
 
 training_version = "0.1.0"
+terms_version = "2"
 
 if __name__ == "__main__":
   print("Dirty: %s" % dirty)

selfdrive/visiond/models/driving.cc

@@ -135,7 +135,14 @@ void poly_fit(float *in_pts, float *in_stds, float *out) {
   Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> lhs = vander.array().colwise() / std.array();
   Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> rhs = pts.array() / std.array();
 
+  // Improve numerical stability
+  Eigen::Matrix<float, POLYFIT_DEGREE, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
+  lhs = lhs * scale.asDiagonal();
+
   // Solve inplace
   Eigen::ColPivHouseholderQR<Eigen::Ref<Eigen::MatrixXf> > qr(lhs);
   p = qr.solve(rhs);
+
+  // Apply scale to output
+  p = p.transpose() * scale.asDiagonal();
 }