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 @@ class dbc(object):
     if debug:
       print name
 
-    blen = (len(x[2])/2)*8
+    blen = 8*len(x[2])
-    x2_int = int(x[2], 16)
+    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:
+  for msg in can:
-    if msg.src in src_filter:
+    if src_filter is None or msg.src in src_filter:
-      ret.append([msg.address, msg.busTime, msg.dat.encode("hex")])
+      ret.append((msg.address, msg.busTime, msg.dat, msg.src))
-  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])
   return ret
 
 # *** can driver ***

selfdrive/car/honda/can_parser.py

@@ -59,21 +59,21 @@ class CANParser(object):
     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 @@ import numpy as np
 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 @@ class CarInterface(object):
     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 @@ class CarInterface(object):
     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 @@ class CarInterface(object):
     # 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 @@ import selfdrive.messaging as messaging
 
 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

selfdrive/controls/lib/adaptivecruise.py

@@ -1,41 +1,41 @@
-import selfdrive.messaging as messaging
+import math
 import numpy as np
+from common.numpy_fast import clip, interp
+import selfdrive.messaging as messaging
 
 # lookup tables VS speed to determine min and max accels in cruise
-_A_CRUISE_MIN_V  = np.asarray([-1.0, -.8, -.67, -.5, -.30])
+_A_CRUISE_MIN_V  = [-1.0, -.8, -.67, -.5, -.30]
-_A_CRUISE_MIN_BP = np.asarray([   0., 5.,  10., 20.,  40.])
+_A_CRUISE_MIN_BP = [   0., 5.,  10., 20.,  40.]
 
 # need fast accel at very low speed for stop and go
-_A_CRUISE_MAX_V  = np.asarray([1., 1., .8, .5, .30])
+_A_CRUISE_MAX_V  = [1., 1., .8, .5, .30]
-_A_CRUISE_MAX_BP = np.asarray([0.,  5., 10., 20., 40.])
+_A_CRUISE_MAX_BP = [0.,  5., 10., 20., 40.]
 
 def calc_cruise_accel_limits(v_ego):
-  a_cruise_min = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
+  a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-  a_cruise_max = np.interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
+  a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
+  return np.vstack([a_cruise_min, a_cruise_max])
 
-  a_pcm = 1.  # always 1 for now
+_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
-  return np.vstack([a_cruise_min, a_cruise_max]), a_pcm
+_A_TOTAL_MAX_BP = [0., 20., 40.]
-
-_A_TOTAL_MAX_V = np.asarray([1.5, 1.9, 3.2])
-_A_TOTAL_MAX_BP = np.asarray([0., 20., 40.])
 
 def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP):
   #*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration
   # this should avoid accelerating when losing the target in turns
   deg_to_rad = np.pi / 180.  # from can reading to rad
 
-  a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+  a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
   a_y = v_ego**2 * angle_steers * deg_to_rad / (VP.steer_ratio * VP.wheelbase)
-  a_x_allowed = np.sqrt(np.maximum(a_total_max**2 - a_y**2, 0.))
+  a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
 
-  a_target[1] = np.minimum(a_target[1], a_x_allowed)
+  a_target[1] = min(a_target[1], a_x_allowed)
-  a_pcm = np.minimum(a_pcm, a_x_allowed)
+  a_pcm = min(a_pcm, a_x_allowed)
   return a_target, a_pcm
 
 def process_a_lead(a_lead):
   # soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead
   a_lead_threshold = 0.5
-  a_lead = np.minimum(a_lead + a_lead_threshold, 0)
+  a_lead = min(a_lead + a_lead_threshold, 0)
   return a_lead
 
 def calc_desired_distance(v_lead):
@@ -46,12 +46,12 @@ def calc_desired_distance(v_lead):
 
 
 #linear slope
-_L_SLOPE_V = np.asarray([0.40, 0.10])
+_L_SLOPE_V = [0.40, 0.10]
-_L_SLOPE_BP = np.asarray([0.,  40])
+_L_SLOPE_BP = [0.,  40]
 
 # parabola slope
-_P_SLOPE_V = np.asarray([1.0, 0.25])
+_P_SLOPE_V = [1.0, 0.25]
-_P_SLOPE_BP = np.asarray([0., 40])
+_P_SLOPE_BP = [0., 40]
 
 def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
   #*** compute desired speed ***
@@ -64,8 +64,8 @@ def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
   max_runaway_speed = -2. # no slower than 2m/s over the lead
 
   # interpolate the lookups to find the slopes for a give lead speed
-  l_slope = np.interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
+  l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
-  p_slope = np.interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)
+  p_slope = interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)
 
   # this is where parabola and linear curves are tangents  
   x_linear_to_parabola = p_slope / l_slope**2
@@ -79,41 +79,41 @@ def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
     # calculate v_rel_des on one third of the linear slope
     v_rel_des_2 = (d_lead - d_des) * l_slope / 3.
     # take the min of the 2 above
-    v_rel_des = np.minimum(v_rel_des_1, v_rel_des_2)
+    v_rel_des = min(v_rel_des_1, v_rel_des_2)
-    v_rel_des = np.maximum(v_rel_des, max_runaway_speed)
+    v_rel_des = max(v_rel_des, max_runaway_speed)
   elif d_lead < d_des + x_linear_to_parabola:
     v_rel_des = (d_lead - d_des) * l_slope
-    v_rel_des = np.maximum(v_rel_des, max_runaway_speed)
+    v_rel_des = max(v_rel_des, max_runaway_speed)
   else:
-    v_rel_des = np.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
+    v_rel_des = math.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
 
   # compute desired speed
   v_target = v_rel_des + v_lead
 
   # compute v_coast: above this speed we want to coast
   t_lookahead = 1.   # how far in time we consider a_lead to anticipate the coast region
-  v_coast_shift = np.maximum(a_lead * t_lookahead, - v_lead)   # don't consider projections that would make v_lead<0
+  v_coast_shift = max(a_lead * t_lookahead, - v_lead)   # don't consider projections that would make v_lead<0
   v_coast = (v_lead + v_target)/2 + v_coast_shift              # no accel allowed above this line
-  v_coast = np.minimum(v_coast, v_target)
+  v_coast = min(v_coast, v_target)
 
   return v_target, v_coast
 
 def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
   # this function computes the required decel to avoid crashing, given safety offsets
-  a_critical =  - np.maximum(0., v_rel + v_offset)**2/np.maximum(2*(d_lead - d_offset), 0.5)
+  a_critical =  - max(0., v_rel + v_offset)**2/max(2*(d_lead - d_offset), 0.5)
   return a_critical
 
 
 # maximum acceleration adjustment
-_A_CORR_BY_SPEED_V = np.asarray([0.4, 0.4, 0])
+_A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
 # speeds
-_A_CORR_BY_SPEED_BP = np.asarray([0., 5., 20.])
+_A_CORR_BY_SPEED_BP = [0., 5., 20.]
 
 def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
   a_coast_min = -1.0   # never coast faster then -1m/s^2
   # coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
   # regardless v_target
-  if v_ref > np.minimum(v_coast, v_target):
+  if v_ref > min(v_coast, v_target):
     # for smooth coast we can be agrressive and target a point where car would actually crash
     v_offset_coast = 0.
     d_offset_coast = d_des/2. - 4.
@@ -123,31 +123,31 @@ def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_c
       a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast)
       # if lead is decelerating, then offset the coast decel
       a_coast += a_lead_contr
-      a_max = np.maximum(a_coast, a_coast_min)
+      a_max = max(a_coast, a_coast_min)
     else:
       a_max = a_coast_min
   else:
     # same as cruise accel, but add a small correction based on lead acceleration at low speeds
     # when lead car accelerates faster, we can do the same, and vice versa
 
-    a_max = a_max + np.interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
+    a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
-                  * np.clip(-v_rel / 4., -.5, 1)
+                  * clip(-v_rel / 4., -.5, 1)
   return a_max
 
 # arbitrary limits to avoid too high accel being computed
-_A_SAT = np.asarray([-10., 5.])
+_A_SAT = [-10., 5.]
 
 # do not consider a_lead at 0m/s, fully consider it at 10m/s
-_A_LEAD_LOW_SPEED_V = np.asarray([0., 1.])
+_A_LEAD_LOW_SPEED_V = [0., 1.]
 
 # speed break points
-_A_LEAD_LOW_SPEED_BP = np.asarray([0., 10.])
+_A_LEAD_LOW_SPEED_BP = [0., 10.]
 
 # add a small offset to the desired decel, just for safety margin
-_DECEL_OFFSET_V = np.asarray([-0.3, -0.5, -0.5, -0.4, -0.3])
+_DECEL_OFFSET_V = [-0.3, -0.5, -0.5, -0.4, -0.3]
 
 # speed bp: different offset based on the likelyhood that lead decels abruptly
-_DECEL_OFFSET_BP = np.asarray([0., 4., 15., 30, 40.])
+_DECEL_OFFSET_BP = [0., 4., 15., 30, 40.]
 
 
 def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
@@ -159,8 +159,8 @@ def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
   v_rel_pid = v_pid - v_lead
 
   # this is how much lead accel we consider in assigning the desired decel
-  a_lead_contr = a_lead * np.interp(v_lead, _A_LEAD_LOW_SPEED_BP,
+  a_lead_contr = a_lead * interp(v_lead, _A_LEAD_LOW_SPEED_BP,
-                                    _A_LEAD_LOW_SPEED_V) * 0.8
+                                 _A_LEAD_LOW_SPEED_V) * 0.8
 
   # first call of calc_positive_accel_limit is used to shape v_pid
   a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid,
@@ -178,15 +178,15 @@ def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
     pass  # acc target speed is above vehicle speed, so we can use the cruise limits
   elif d_lead > d_offset + 0.01:  # add small value to avoid by zero divisions
     # compute needed accel to get to 1m distance with -1m/s rel speed
-    decel_offset = np.interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V)
+    decel_offset = interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V)
 
     critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset)
-    a_target[0] = np.minimum(decel_offset + critical_decel + a_lead_contr,
+    a_target[0] = min(decel_offset + critical_decel + a_lead_contr,
-                             a_target[0])
+                      a_target[0])
   else:
     a_target[0] = _A_SAT[0]
   # a_min can't be higher than a_max
-  a_target[0] = np.minimum(a_target[0], a_target[1])
+  a_target[0] = min(a_target[0], a_target[1])
   # final check on limits
   a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1])
   a_target = a_target.tolist()
@@ -208,8 +208,8 @@ def calc_jerk_factor(d_lead, v_rel):
   else:
     a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset)
     # increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2
-    jerk_factor = np.maximum(a_critical - a_offset, 0.)/5.
+    jerk_factor = max(a_critical - a_offset, 0.)/5.
-    jerk_factor = np.minimum(jerk_factor, jerk_factor_max)
+    jerk_factor = min(jerk_factor, jerk_factor_max)
   return jerk_factor
 
 
@@ -223,27 +223,27 @@ def calc_ttc(d_rel, v_rel, a_rel, v_lead):
   # assuming that closing gap a_rel comes from lead vehicle decel, then limit a_rel so that v_lead will get to zero in no sooner than t_decel
   # this helps overweighting a_rel when v_lead is close to zero.
   t_decel = 2.
-  a_rel = np.minimum(a_rel, v_lead/t_decel)
+  a_rel = min(a_rel, v_lead/t_decel)
 
   delta = v_rel**2 + 2 * d_rel * a_rel
   # assign an arbitrary high ttc value if there is no solution to ttc
   if delta < 0.1:
     ttc = 5.
-  elif np.sqrt(delta) + v_rel < 0.1:
+  elif math.sqrt(delta) + v_rel < 0.1:
     ttc = 5.
   else:
-    ttc = 2 * d_rel / (np.sqrt(delta) + v_rel)
+    ttc = 2 * d_rel / (math.sqrt(delta) + v_rel)
   return ttc
 
 
 def limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status):
   decel_bp = [0.  ,  40.]
   decel_v  = [-0.3, -0.2]
-  decel = np.interp(v_ego, decel_bp, decel_v)
+  decel = interp(v_ego, decel_bp, decel_v)
   # gives 18 seconds before decel begins (w 6 minute timeout)
   if awareness_status < -0.05:
-    a_target[1] = np.minimum(a_target[1], decel)
+    a_target[1] = min(a_target[1], decel)
-    a_target[0] = np.minimum(a_target[1], a_target[0])
+    a_target[0] = min(a_target[1], a_target[0])
     a_pcm = 0.
   return a_target, a_pcm
 
@@ -258,7 +258,10 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, awareness_statu
   v_target_lead = MAX_SPEED_POSSIBLE
 
   #*** set accel limits as cruise accel/decel limits ***
-  a_target, a_pcm = calc_cruise_accel_limits(v_ego)
+  a_target = calc_cruise_accel_limits(v_ego)
+  # Always 1 for now.
+  a_pcm = 1
+
   #*** limit max accel in sharp turns
   a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP)
   jerk_factor = 0.

selfdrive/controls/lib/drive_helpers.py

@@ -1,7 +1,8 @@
 import numpy as np
+from common.numpy_fast import clip, interp
 
 def rate_limit(new_value, last_value, dw_step, up_step):
-  return np.clip(new_value, last_value + dw_step, last_value + up_step)
+  return clip(new_value, last_value + dw_step, last_value + up_step)
 
 def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
   # simple integral controller that learns how much steering offset to put to have the car going straight
@@ -11,12 +12,12 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, stee
   min_learn_speed = 1.
 
   # learn less at low speed or when turning
-  alpha_v = alpha*(np.maximum(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des))
+  alpha_v = alpha*(max(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des))
 
   # only learn if lateral control is active and if driver is not overriding:
   if lateral_control and not steer_override:
     angle_offset += d_poly[3] * alpha_v
-    angle_offset = np.clip(angle_offset, min_offset, max_offset)
+    angle_offset = clip(angle_offset, min_offset, max_offset)
 
   return angle_offset
 
@@ -44,7 +45,7 @@ def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
     brake_on_offset_v  = [.25, .15]   # min brake command on brake activation. below this no decel is perceived
     brake_on_offset_bp = [15., 30.]     # offset changes VS speed to not have too abrupt decels at high speeds
     # offset the brake command for threshold in the brake system. no brake torque perceived below it
-    brake_on_offset = np.interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
+    brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
     brake_offset = brake_on_offset - brake_hyst_on
     if final_brake > 0.0:
       final_brake += brake_offset

selfdrive/controls/lib/latcontrol.py

@@ -1,4 +1,6 @@
+import math
 import numpy as np
+from common.numpy_fast import clip
 
 def calc_curvature(v_ego, angle_steers, VP, angle_offset=0):
   deg_to_rad = np.pi/180.
@@ -14,7 +16,7 @@ def calc_d_lookahead(v_ego):
   # sqrt on speed is needed to keep, for a given curvature, the y_offset 
   # proportional to speed. Indeed, y_offset is prop to d_lookahead^2
   # 26m at 25m/s
-  d_lookahead = offset_lookahead + np.sqrt(np.maximum(v_ego, 0)) * coeff_lookahead
+  d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * coeff_lookahead
   return d_lookahead
 
 def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VP, angle_offset):
@@ -53,7 +55,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
     Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer)
 
   # still, intergral term should not be bigger then limits
-  Ui_steer = np.clip(Ui_steer, -steer_max, steer_max)
+  Ui_steer = clip(Ui_steer, -steer_max, steer_max)
 
   output_steer = Up_steer + Ui_steer
 
@@ -67,7 +69,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
   if abs(output_steer) > steer_max:
     lateral_control_sat = True
 
-  output_steer = np.clip(output_steer, -steer_max, steer_max)
+  output_steer = clip(output_steer, -steer_max, steer_max)
 
   # if lateral control is saturated for a certain period of time, send an alert for taking control of the car
   # wind
@@ -81,7 +83,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
   if sat_count >= sat_count_limit:
     sat_flag = True
 
-  sat_count = np.clip(sat_count, 0, 1)
+  sat_count = clip(sat_count, 0, 1)
 
   return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
 
@@ -116,5 +118,5 @@ class LatControl(object):
       v_ego, self.y_actual, self.y_des, self.Ui_steer, steer_max,
       steer_override, self.sat_count, enabled, VP.torque_mod, rate)
 
-    final_steer = np.clip(output_steer, -steer_max, steer_max)
+    final_steer = clip(output_steer, -steer_max, steer_max)
     return final_steer, sat_flag

selfdrive/controls/lib/longcontrol.py

@@ -1,4 +1,5 @@
 import numpy as np
+from common.numpy_fast import clip, interp
 from selfdrive.config import Conversions as CV
 
 class LongCtrlState:
@@ -82,15 +83,18 @@ def get_compute_gb():
 # takes in [desired_accel, current_speed] -> [-1.0, 1.0] where -1.0 is max brake and 1.0 is max gas
 compute_gb = get_compute_gb()
 
+
+_KP_BP = [0., 5., 35.]
+_KP_V =  [1.2, 0.8, 0.5]
+
+_kI_BP = [0., 35.]
+_kI_V =  [0.18, 0.12]
+
 def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor, gear, rate):
   #*** This function compute the gb pedal positions in order to track the desired speed
   # proportional and integral terms. More precision at low speed
-  Kp_v =  [1.2, 0.8, 0.5]
+  Kp = interp(v_ego, _KP_BP, _KP_V)
-  Kp_bp = [0., 5., 35.]
+  Ki = interp(v_ego, _kI_BP, _kI_V)
-  Kp = np.interp(v_ego, Kp_bp, Kp_v)
-  Ki_v =  [0.18, 0.12]
-  Ki_bp = [0., 35.]
-  Ki = np.interp(v_ego, Ki_bp, Ki_v)
 
   # scle Kp and Ki by jerk factor drom drive_thread
   Kp = (1. + jerk_factor)*Kp
@@ -98,7 +102,7 @@ def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor
 
   # this is ugly but can speed reports 0 when speed<0.3m/s and we can't have that jump  
   v_ego_min = 0.3
-  v_ego = np.maximum(v_ego, v_ego_min)
+  v_ego = max(v_ego, v_ego_min)
 
   v_error = v_pid - v_ego
 
@@ -126,7 +130,7 @@ def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor
   if output_gb > gas_max or output_gb < -brake_max:
     long_control_sat = True
 
-  output_gb = np.clip(output_gb, -brake_max, gas_max)
+  output_gb = clip(output_gb, -brake_max, gas_max)
 
   return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat
 
@@ -136,8 +140,8 @@ starting_brake_rate = 0.6    # brake_travel/s while releasing on restart
 starting_Ui = 0.5            # Since we don't have much info about acceleration at this point, be conservative
 brake_stopping_target = 0.5  # apply at least this amount of brake to maintain the vehicle stationary
 
-max_speed_error_v  = [1.5, .8]  # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
+_MAX_SPEED_ERROR_BP = [0., 30.]  # speed breakpoints
-max_speed_error_bp = [0., 30.]  # speed breakpoints
+_MAX_SPEED_ERROR_V  = [1.5, .8]  # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
 
 class LongControl(object):
   def __init__(self):
@@ -152,18 +156,19 @@ class LongControl(object):
     self.v_pid = v_pid
 
   def update(self, enabled, v_ego, v_cruise, v_target_lead, a_target, jerk_factor, VP):
+    brake_max_bp = [0., 5., 20., 100.]  # speeds     
+    brake_max_v = [1.0, 1.0, 0.8, 0.8]  # values
+
+    # brake and gas limits
+    brake_max = interp(v_ego, brake_max_bp, brake_max_v)
+
     # TODO: not every time
     if VP.brake_only:
-      gas_max_v = [0, 0]                # values
+      gas_max = 0
     else:
+      gas_max_bp = [0., 100.]             # speeds
       gas_max_v = [0.6, 0.6]            # values
-    gas_max_bp = [0., 100.]             # speeds
+      gas_max = interp(v_ego, gas_max_bp, gas_max_v)
-    brake_max_v = [1.0, 1.0, 0.8, 0.8]  # values
-    brake_max_bp = [0., 5., 20., 100.]  # speeds     
-
-    # brake and gas limits
-    brake_max = np.interp(v_ego, brake_max_bp, brake_max_v)
-    gas_max = np.interp(v_ego, gas_max_bp, gas_max_v)
 
     overshoot_allowance = 2.0    # overshoot allowed when changing accel sign
 
@@ -172,7 +177,7 @@ class LongControl(object):
 
     # limit max target speed based on cruise setting:
     v_cruise_mph = round(v_cruise * CV.KPH_TO_MPH)   # what's displayed in mph on the IC
-    v_target = np.minimum(v_target_lead, v_cruise_mph * CV.MPH_TO_MS / VP.ui_speed_fudge)
+    v_target = min(v_target_lead, v_cruise_mph * CV.MPH_TO_MS / VP.ui_speed_fudge)
 
     max_speed_delta_up = a_target[1]*1.0/rate
     max_speed_delta_down = a_target[0]*1.0/rate
@@ -192,10 +197,10 @@ class LongControl(object):
       #reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
       if ((self.v_pid > v_ego + overshoot_allowance) and
           (v_target < self.v_pid)):
-        self.v_pid = np.maximum(v_target, v_ego + overshoot_allowance)
+        self.v_pid = max(v_target, v_ego + overshoot_allowance)
       elif ((self.v_pid < v_ego - overshoot_allowance) and
           (v_target > self.v_pid)):
-        self.v_pid = np.minimum(v_target, v_ego - overshoot_allowance)
+        self.v_pid = min(v_target, v_ego - overshoot_allowance)
 
       # move v_pid no faster than allowed accel limits
       if (v_target > self.v_pid + max_speed_delta_up):
@@ -207,8 +212,8 @@ class LongControl(object):
 
       # to avoid too much wind up on acceleration, limit positive speed error
       if not VP.brake_only:
-        max_speed_error = np.interp(v_ego, max_speed_error_bp, max_speed_error_v)
+        max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V)
-        self.v_pid = np.minimum(self.v_pid, v_ego + max_speed_error)
+        self.v_pid = min(self.v_pid, v_ego + max_speed_error)
 
       # TODO: removed anti windup on gear change, does it matter?
       output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(v_ego, self.v_pid, \
@@ -217,7 +222,7 @@ class LongControl(object):
     elif self.long_control_state == LongCtrlState.stopping:
       if v_ego > 0. or output_gb > -brake_stopping_target:
         output_gb -= stopping_brake_rate/rate
-      output_gb = np.clip(output_gb, -brake_max, gas_max)
+      output_gb = clip(output_gb, -brake_max, gas_max)
       self.v_pid = v_ego
       self.Ui_accel_cmd = 0.
     # intention is to move again, release brake fast before handling control to PID
@@ -228,7 +233,7 @@ class LongControl(object):
       self.Ui_accel_cmd = starting_Ui
 
     self.last_output_gb = output_gb
-    final_gas = np.clip(output_gb, 0., gas_max)
+    final_gas = clip(output_gb, 0., gas_max)
-    final_brake = -np.clip(output_gb, -brake_max, 0.)
+    final_brake = -clip(output_gb, -brake_max, 0.)
     return final_gas, final_brake
 

selfdrive/controls/lib/pathplanner.py

@@ -1,26 +1,29 @@
-import selfdrive.messaging as messaging
+import math
 import numpy as np
+
+from common.numpy_fast import interp
+import selfdrive.messaging as messaging
 X_PATH = np.arange(0.0, 50.0)
 
 def model_polyfit(points):
   return np.polyfit(X_PATH, map(float, points), 3)
 
 # lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
-_LANE_WIDTH_V = np.asarray([3., 3.8])
+_LANE_WIDTH_V = [3., 3.8]
 
 # break points of speed
-_LANE_WIDTH_BP = np.asarray([0., 31.])
+_LANE_WIDTH_BP = [0., 31.]
 
 def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed):
   #*** this function computes the poly for the center of the lane, averaging left and right polys
-  lane_width = np.interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
+  lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
 
   # lanes in US are ~3.6m wide
   half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
   if l_prob + r_prob > 0.01:
     c_poly = ((l_poly - half_lane_poly) * l_prob +
               (r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
-    c_prob = np.sqrt((l_prob**2 + r_prob**2) / 2.)
+    c_prob = math.sqrt((l_prob**2 + r_prob**2) / 2.)
   else:
     c_poly = np.zeros(4)
     c_prob = 0.

selfdrive/controls/lib/radar_helpers.py

@@ -1,8 +1,10 @@
-import numpy as np
-import platform
 import os
 import sys
+import math
+import platform
+import numpy as np
 
+from common.numpy_fast import clip, interp
 from common.kalman.ekf import FastEKF1D, SimpleSensor
 
 # radar tracks
@@ -51,14 +53,14 @@ class Track(object):
     else:
       # estimate acceleration
       a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
-      a_rel_unfilt = np.clip(a_rel_unfilt, -10., 10.)
+      a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
       self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
 
       v_lat_unfilt = (self.dPath - self.dPathPrev) / ts
       self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
 
       a_lead_unfilt = (self.vLead - self.vLeadPrev) / ts
-      a_lead_unfilt = np.clip(a_lead_unfilt, -10., 10.)
+      a_lead_unfilt = clip(a_lead_unfilt, -10., 10.)
       self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead
 
     if self.stationary:
@@ -232,12 +234,12 @@ class Cluster(object):
     d_path = self.dPath
 
     if enabled:
-      t_lookahead = np.interp(self.dRel, t_lookahead_bp, t_lookahead_v)
+      t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v)
       # correct d_path for lookahead time, considering only cut-ins and no more than 1m impact
-      lat_corr = np.clip(t_lookahead * self.vLat, -1, 0)
+      lat_corr = clip(t_lookahead * self.vLat, -1, 0)
     else:
       lat_corr = 0.
-    d_path = np.maximum(d_path + lat_corr, 0)
+    d_path = max(d_path + lat_corr, 0)
 
     if d_path < 1.5 and not self.stationary and not self.oncoming:
       return True

selfdrive/controls/radard.py

@@ -8,7 +8,6 @@ from collections import defaultdict
 from fastcluster import linkage_vector
 
 import selfdrive.messaging as messaging
-from selfdrive.boardd.boardd import can_capnp_to_can_list_old
 from selfdrive.controls.lib.latcontrol import calc_lookahead_offset
 from selfdrive.controls.lib.pathplanner import PathPlanner
 from selfdrive.config import VehicleParams
@@ -37,7 +36,6 @@ class EKFV1D(EKF):
     self.var_init = 1e2   # ~ model variance when probability is 70%, so good starting point
     self.covar = self.identity * self.var_init
 
-    # self.process_noise = np.asmatrix(np.diag([100, 10]))
     self.process_noise = np.matlib.diag([0.5, 1])
 
   def calc_transfer_fun(self, dt):

selfdrive/manager.py

@@ -282,8 +282,15 @@ def main():
     del managed_processes['loggerd']
   if os.getenv("NOUPLOAD") is not None:
     del managed_processes['uploader']
+  if os.getenv("NOVISION") is not None:
+    del managed_processes['visiond']
   if os.getenv("NOBOARD") is not None:
     del managed_processes['boardd']
+  if os.getenv("LEAN") is not None:
+    del managed_processes['uploader']
+    del managed_processes['loggerd']
+    del managed_processes['logmessaged']
+    del managed_processes['logcatd']
 
   manager_init()
 

selfdrive/radar/nidec/interface.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 import numpy as np
 from selfdrive.car.honda.can_parser import CANParser
-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
 
@@ -39,7 +39,7 @@ class RadarInterface(object):
     while 1:
       for a in messaging.drain_sock(self.logcan, wait_for_one=True):
         canMonoTimes.append(a.logMonoTime)
-        can_pub_radar.extend(can_capnp_to_can_list_old(a.can, [1, 3]))
+        can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3]))
 
       # only run on the 0x445 packets, used for timing
       if any(x[0] == 0x445 for x in can_pub_radar):

selfdrive/test/plant/maneuverplots.py

@@ -59,6 +59,7 @@ class ManeuverPlot(object):
     self.a_target_min_array.append(a_target_min)
     self.a_target_max_array.append(a_target_max)
 
+
   def write_plot(self, path, maneuver_name):
     title = self.title or maneuver_name
     # TODO: Missing plots from the old one:

selfdrive/test/plant/plant.py

@@ -14,7 +14,7 @@ import selfdrive.messaging as messaging
 from selfdrive.config import CruiseButtons
 from selfdrive.car.honda.hondacan import fix
 from selfdrive.car.honda.carstate import get_can_parser
-from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list, can_list_to_can_capnp
+from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp
 
 from selfdrive.car.honda.can_parser import CANParser
 
@@ -141,8 +141,7 @@ class Plant(object):
     # ******** get messages sent to the car ********
     can_msgs = []
     for a in messaging.drain_sock(Plant.sendcan):
-      can_msgs += can_capnp_to_can_list_old(a.sendcan, [0,2])
+      can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0,2]))
-    #print can_msgs
     self.cp.update_can(can_msgs)
 
     # ******** get live100 messages for plotting ***