openpilot v0.2.1 release

LICENSE.openpilot

@@ -1,4 +1,4 @@
-Copyright (c) 2016, comma.ai
+Copyright (c) 2016, Comma.ai, Inc.
 
 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
 

RELEASES.md

@@ -1,3 +1,9 @@
+Version 0.2.1  (2016-12-14)
+===========================
+  * Performance improvements, removal of more numpy
+  * Fix boardd process priority
+  * Make counter timer reset on use of steering wheel
+
 Version 0.2  (2016-12-12)
 =========================
   * Car/Radar abstraction layers have shipped, see cereal/car.capnp

common/dbc.py

@@ -1,6 +1,7 @@
 import re
-from collections import namedtuple
 import bitstring
+from binascii import hexlify
+from collections import namedtuple
 
 def int_or_float(s):
   # return number, trying to maintain int format
@@ -148,8 +149,8 @@ def decode(self, x, arr=None, debug=False):
     if debug:
       print name
 
-    blen = (len(x[2])/2)*8
-    x2_int = int(x[2], 16)
+    blen = 8*len(x[2])
+    x2_int = int(hexlify(x[2]), 16)
 
     for s in msg[1]:
       if arr is not None and s[0] not in arr:

common/numpy_fast.py

@@ -1,2 +1,25 @@
 def clip(x, lo, hi):
   return max(lo, min(hi, x))
+
+
+def interp(x, xp, fp):
+  N = len(xp)
+  if not hasattr(x, '__iter__'):
+    hi = 0
+    while hi < N and x > xp[hi]:
+      hi += 1
+    low = hi - 1
+    return fp[-1] if hi == N and x > xp[low] else (
+      fp[0] if hi == 0 else
+      (x - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low])
+
+  result = []
+  for v in x:
+    hi = 0
+    while hi < N and v > xp[hi]:
+      hi += 1
+    low = hi - 1
+    result.append(fp[-1] if hi == N and v > xp[low] else (fp[
+      0] if hi == 0 else (v - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]
+                                                               ) + fp[low]))
+  return result

selfdrive/boardd/boardd.cc

@@ -4,6 +4,7 @@
 #include <string.h>
 #include <signal.h>
 #include <unistd.h>
+#include <sched.h>
 #include <sys/time.h>
 #include <sys/cdefs.h>
 #include <sys/types.h>
@@ -268,12 +269,20 @@ void *can_health_thread(void *crap) {
   return NULL;
 }
 
+int set_realtime_priority(int level) {
+  // should match python using chrt
+  struct sched_param sa;
+  memset(&sa, 0, sizeof(sa));
+  sa.sched_priority = level;
+  return sched_setscheduler(gettid(), SCHED_FIFO, &sa);
+}
+
 int main() {
   int err;
   printf("boardd: starting boardd\n");
 
   // set process priority
-  err = setpriority(PRIO_PROCESS, 0, -4);
+  err = set_realtime_priority(4);
   printf("boardd: setpriority returns %d\n", err);
 
   // check the environment

selfdrive/boardd/boardd.py

@@ -32,17 +32,11 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'):
     cc.src = can_msg[3]
   return dat
 
-def can_capnp_to_can_list_old(dat, src_filter=[]):
+def can_capnp_to_can_list(can, src_filter=None):
   ret = []
-  for msg in dat:
-    if msg.src in src_filter:
-      ret.append([msg.address, msg.busTime, msg.dat.encode("hex")])
-  return ret
-
-def can_capnp_to_can_list(dat):
-  ret = []
-  for msg in dat.can:
-    ret.append([msg.address, msg.busTime, msg.dat, msg.src])
+  for msg in can:
+    if src_filter is None or msg.src in src_filter:
+      ret.append((msg.address, msg.busTime, msg.dat, msg.src))
   return ret
 
 # *** can driver ***

selfdrive/car/honda/can_parser.py

@@ -59,21 +59,21 @@ def update_can(self, can_recv):
     cn_vl_max = 5   # no more than 5 wrong counter checks
 
     # we are subscribing to PID_XXX, else data from USB
-    for msg, ts, cdat in can_recv:
+    for msg, ts, cdat, _ in can_recv:
       idxs = self._message_indices[msg]
       if idxs:
-        self.msgs_upd += [msg]
+        self.msgs_upd.append(msg)
         # read the entire message
-        out = self.can_dbc.decode([msg, 0, cdat])[1]
+        out = self.can_dbc.decode((msg, 0, cdat))[1]
         # checksum check
         self.ck[msg] = True
         if "CHECKSUM" in out.keys() and msg in self.msgs_ck:
           # remove checksum (half byte)
-          ck_portion = (''.join((cdat[:-1], '0'))).decode('hex')
+          ck_portion = cdat[:-1] + chr(ord(cdat[-1]) & 0xF0)
           # recalculate checksum
           msg_vl = fix(ck_portion, msg)
           # compare recalculated vs received checksum
-          if msg_vl != cdat.decode('hex'):
+          if msg_vl != cdat:
             print hex(msg), "CHECKSUM FAIL"
             self.ck[msg] = False
             self.ok[msg] = False

selfdrive/car/honda/carstate.py

@@ -1,7 +1,7 @@
 import numpy as np
 
 import selfdrive.messaging as messaging
-from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list
+from selfdrive.boardd.boardd import can_capnp_to_can_list
 from selfdrive.config import VehicleParams
 from common.realtime import sec_since_boot
 
@@ -132,7 +132,7 @@ def fingerprint(logcan):
     for a in messaging.drain_sock(logcan, wait_for_one=True):
       if st is None:
         st = sec_since_boot()
-      for adr, _, msg, idx in can_capnp_to_can_list(a):
+      for adr, _, msg, idx in can_capnp_to_can_list(a.can):
         # pedal
         if adr == 0x201 and idx == 0:
           brake_only = False

selfdrive/car/honda/interface.py

@@ -5,7 +5,7 @@
 from selfdrive.config import Conversions as CV
 from selfdrive.car.honda.carstate import CarState
 from selfdrive.car.honda.carcontroller import CarController, AH
-from selfdrive.boardd.boardd import can_capnp_to_can_list_old
+from selfdrive.boardd.boardd import can_capnp_to_can_list
 
 from cereal import car
 
@@ -42,6 +42,7 @@ def __init__(self, read_only=False):
     self.logcan = messaging.sub_sock(context, service_list['can'].port)
 
     self.frame = 0
+    self.can_invalid_count = 0
 
     # *** init the major players ***
     self.CS = CarState(self.logcan)
@@ -61,7 +62,7 @@ def update(self):
     canMonoTimes = []
     for a in messaging.drain_sock(self.logcan):
       canMonoTimes.append(a.logMonoTime)
-      can_pub_main.extend(can_capnp_to_can_list_old(a.can, [0,2]))
+      can_pub_main.extend(can_capnp_to_can_list(a.can, [0,2]))
     self.CS.update(can_pub_main)
 
     # create message
@@ -149,7 +150,11 @@ def update(self):
     # These strings aren't checked at compile time
     errors = []
     if not self.CS.can_valid:
-      errors.append('commIssue')
+      self.can_invalid_count += 1
+      if self.can_invalid_count >= 5:
+        errors.append('commIssue')
+    else:
+      self.can_invalid_count = 0
     if self.CS.steer_error:
       errors.append('steerUnavailable')
     elif self.CS.steer_not_allowed:

selfdrive/controls/controlsd.py

@@ -6,6 +6,8 @@
 
 from cereal import car
 
+from common.numpy_fast import clip
+
 from selfdrive.config import Conversions as CV
 from common.services import service_list
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
@@ -73,7 +75,7 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
   # start the loop
   set_realtime_priority(2)
 
-  rk = Ratekeeper(rate)
+  rk = Ratekeeper(rate, print_delay_threshold=2./1000)
   while 1:
     cur_time = sec_since_boot()
 
@@ -89,6 +91,10 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
       if awareness_status <= 0.:
         AM.add("driverDistracted", enabled)
 
+    # reset awareness status on steering
+    if CS.steeringPressed:
+      awareness_status = 1.0
+
     # handle button presses
     for b in CS.buttonEvents:
       print b
@@ -111,7 +117,7 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
           v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
         elif b.type == "decelCruise":
           v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
-        v_cruise_kph = np.clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+        v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
 
       if not enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
         enable_request = True 
@@ -185,7 +191,7 @@ def controlsd_thread(gctx, rate=100):  #rate in Hz
       enabled = True
 
       # on activation, let's always set v_cruise from where we are, even if PCM ACC is active
-      v_cruise_kph = int(round(np.maximum(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge, V_CRUISE_ENABLE_MIN)))
+      v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge, V_CRUISE_ENABLE_MIN)))
 
       # 6 minutes driver you're on
       awareness_status = 1.0