openpilot v0.4.3.2 release

RELEASES.md

@@ -1,3 +1,12 @@
+Version 0.4.3.2 (2018-03-29)
+============================
+ * Improve autofocus
+ * Improve driving when only one lane line is detected
+ * Added fingerprint for Toyota Corolla LE
+ * Fixed Toyota Corolla steer error
+ * Full-screen driving UI
+ * Improved path drawing
+
 Version 0.4.3.1 (2018-03-19)
 ============================
  * Improve autofocus

cereal/Makefile

@@ -1,14 +1,14 @@
 PWD := $(shell pwd)
 
-SRCS := log.capnp car.capnp map.capnp
+SRCS := log.capnp car.capnp mapd.capnp
 
-GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
+GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ gen/cpp/mapd.capnp.c++
 
 UNAME_M ?= $(shell uname -m)
 
 # only generate C++ for docker tests
 ifneq ($(OPTEST),1)
-	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
+	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/mapd.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
 
 # Dont build java on the phone...
 ifeq ($(UNAME_M),x86_64)

cereal/__init__.py

@@ -4,17 +4,6 @@
 CEREAL_PATH = os.path.dirname(os.path.abspath(__file__))
 capnp.remove_import_hook()
 
-if os.getenv("NEWCAPNP"):
-  import tempfile
-  import pyximport
-
-  importers = pyximport.install(build_dir=os.path.join(tempfile.gettempdir(), ".pyxbld"))
-  try:
-    import cereal.gen.cython.log_capnp_cython as log
-    import cereal.gen.cython.car_capnp_cython as car
-  finally:
-    pyximport.uninstall(*importers)
-    del importers
-else:
-  log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))
-  car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))
+log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))
+car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))
+mapd = capnp.load(os.path.join(CEREAL_PATH, "mapd.capnp"))

cereal/log.capnp

@@ -1487,6 +1487,11 @@ struct OrbFeatures {
   ys @2 :List(Float32);
   descriptors @3 :Data;
   octaves @4 :List(Int8);
+
+  # match index to last OrbFeatures
+  # -1 if no match
+  timestampLastEof @5 :UInt64;
+  matches @6: List(Int16);
 }
 
 struct OrbKeyFrame {

cereal/mapd.capnp

@@ -0,0 +1,18 @@
+using Cxx = import "c++.capnp";
+$Cxx.namespace("cereal");
+
+using Java = import "java.capnp";
+$Java.package("ai.comma.openpilot.cereal");
+$Java.outerClassname("Log");
+
+using Log = import "log.capnp";
+
+@0xe1a6ab330ea7205f;
+
+struct MapdRequest {
+  pos @0 :Log.ECEFPoint;
+}
+
+struct MapdResponse {
+  kfs @0 :List(Log.OrbKeyFrame);
+}

common/dbc.py

@@ -25,9 +25,9 @@ def __init__(self, fn):
     self._warned_addresses = set()
 
     # regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py
-    bo_regexp = re.compile("^BO\_ (\w+) (\w+) *: (\w+) (\w+)")
-    sg_regexp = re.compile("^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
-    sgm_regexp = re.compile("^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
+    bo_regexp = re.compile(r"^BO\_ (\w+) (\w+) *: (\w+) (\w+)")
+    sg_regexp = re.compile(r"^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
+    sgm_regexp = re.compile(r"^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
 
     # A dictionary which maps message ids to tuples ((name, size), signals).
     #   name is the ASCII name of the message.

common/fingerprints.py

@@ -64,6 +64,10 @@ class TOYOTA:
   }],
   TOYOTA.COROLLA: [{
     36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+  },
+  # Corolla LE 2017
+  {
+    36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
   }],
   TOYOTA.LEXUS_RXH: [{
     36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8

common/kalman/ekf.py

@@ -1,3 +1,4 @@
+# pylint: skip-file
 import abc
 import numpy as np
 # The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use.

common/params.py

@@ -72,6 +72,8 @@ class UnknownKeyName(Exception):
 
   "Passive": TxType.PERSISTANT,
   "DoUninstall": TxType.CLEAR_ON_MANAGER_START,
+  "ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
+  "IsUpdateAvailable": TxType.PERSISTANT,
 }
 
 def fsync_dir(path):

common/transformations/coordinates.py

@@ -78,12 +78,12 @@ def __init__(self, init_geodetic, init_ecef):
     self.ecef2ned_matrix = self.ned2ecef_matrix.T
 
   @classmethod
-  def from_geodetic(self, init_geodetic):
+  def from_geodetic(cls, init_geodetic):
     init_ecef = geodetic2ecef(init_geodetic)
     return LocalCoord(init_geodetic, init_ecef)
 
   @classmethod
-  def from_ecef(self, init_ecef):
+  def from_ecef(cls, init_ecef):
     init_geodetic = ecef2geodetic(init_ecef)
     return LocalCoord(init_geodetic, init_ecef)
 

launch_chffrplus.sh

@@ -5,11 +5,14 @@ if [ -z "$PASSIVE" ]; then
 fi
 
 function launch {
+  DO_UPDATE=$(cat /data/params/d/ShouldDoUpdate)
   # apply update
-  if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
+  if [ "$DO_UPDATE" == "1" ] && [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
      git reset --hard @{u} &&
      git clean -xdf &&
      exec "${BASH_SOURCE[0]}"
+     echo -n 0 > /data/params/d/ShouldDoUpdate
+     echo -n 0 > /data/params/d/IsUpdateAvailable
   fi
 
   # no cpu rationing for now

selfdrive/boardd/boardd.cc

@@ -37,6 +37,8 @@
 #define SAFETY_ELM327 0xE327
 #define SAFETY_GM 3
 #define SAFETY_HONDA_BOSCH 4
+#define SAFETY_TOYOTA_NOLIMITS 0x1336
+#define SAFETY_ALLOUTPUT 0x1337
 
 namespace {
 
@@ -570,8 +572,13 @@ void *pigeon_thread(void *crap) {
       //printf("got %d\n", len);
       alen += len;
     }
-    if (alen > 0) {
-      pigeon_publish_raw(publisher, dat, alen);
+    if (alen > 0) { 
+      if (dat[0] == (char)0x00){
+        LOGW("received invalid ublox message, resetting pigeon");
+        pigeon_init();
+      } else {
+        pigeon_publish_raw(publisher, dat, alen);
+      }
     }
 
     // 10ms

selfdrive/boardd/boardd.py

@@ -12,7 +12,7 @@
 # USB is optional
 try:
   import usb1
-  from usb1 import USBErrorIO, USBErrorOverflow
+  from usb1 import USBErrorIO, USBErrorOverflow  #pylint: disable=no-name-in-module
 except Exception:
   pass
 

selfdrive/can/packer.py

@@ -62,4 +62,4 @@ def make_can_msg(self, addr, bus, values, counter=-1):
     ("PCM_SPEED", 123),
     ("PCM_GAS", 10),
   ])
-  print s.encode("hex")
+  print s[1].encode("hex")

selfdrive/car/honda/carstate.py

@@ -292,7 +292,6 @@ def update(self, cp):
 # carstate standalone tester
 if __name__ == '__main__':
   import zmq
-  import time
   context = zmq.Context()
 
   class CarParams(object):
@@ -303,6 +302,6 @@ def __init__(self):
   CP = CarParams()
   CS = CarState(CP)
 
-  while 1:
-    CS.update()
-    time.sleep(0.01)
+  # while 1:
+  #   CS.update()
+  #   time.sleep(0.01)

selfdrive/car/honda/old_can_parser.py

@@ -7,7 +7,7 @@
 from common.dbc import dbc
 
 class CANParser(object):
-  def __init__(self, dbc_f, signals, checks=[]):
+  def __init__(self, dbc_f, signals, checks=None):
     ### input:
     # dbc_f   : dbc file
     # signals : List of tuples (name, address, ival) where
@@ -19,6 +19,7 @@ def __init__(self, dbc_f, signals, checks=[]):
     #               monitored.
     #             - frequency is the frequency at which health should be monitored.
 
+    checks = [] if checks is None else checks
     self.msgs_ck = set([check[0] for check in checks])
     self.frqs = dict(checks)
     self.can_valid = False  # start with False CAN assumption

selfdrive/car/mock/interface.py

@@ -25,6 +25,7 @@ def __init__(self, CP, sendcan=None):
     self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port)
 
     self.speed = 0.
+    self.prev_speed = 0.
     self.yaw_rate = 0.
     self.yaw_rate_meas = 0.
 
@@ -85,6 +86,7 @@ def update(self, c):
 
     gps = messaging.recv_sock(self.gps)
     if gps is not None:
+      self.prev_speed = self.speed
       self.speed = gps.gpsLocation.speed
 
     # create message
@@ -93,7 +95,11 @@ def update(self, c):
     # speeds
     ret.vEgo = self.speed
     ret.vEgoRaw = self.speed
-    ret.aEgo = 0.
+    a = self.speed - self.prev_speed
+
+    ret.aEgo = a
+    ret.brakePressed = a < -0.5
+
     self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate
     ret.yawRate = self.yaw_rate
     ret.standstill = self.speed < 0.01

selfdrive/car/toyota/carcontroller.py

@@ -1,4 +1,4 @@
-from common.numpy_fast import clip, interp, int_rnd
+from common.numpy_fast import clip, interp
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
                                            create_steer_command, create_ui_command, \
@@ -19,11 +19,12 @@
 STEER_DELTA_DOWN = 25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
 STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor
 
-# Steer angle limits
+# Steer angle limits (tested at the Crows Landing track and considered ok)
 ANGLE_MAX_BP = [0., 5.]
 ANGLE_MAX_V = [510., 300.]
-ANGLE_DELTA_BP = [0., 5.]
-ANGLE_DELTA_V = [3., 1.]
+ANGLE_DELTA_BP = [0., 5., 15.]
+ANGLE_DELTA_V = [5., .8, .15]     # windup limit
+ANGLE_DELTA_VU = [5., 3.5, 0.4]   # unwind limit
 
 TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
               0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
@@ -66,7 +67,7 @@ def process_hud_alert(hud_alert, audible_alert):
 
   return steer, fcw, sound1, sound2
 
-def ipas_state_transition(steer_angle_enabled, enabled, ipas_state, ipas_reset_counter):
+def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_counter):
 
   if enabled and not steer_angle_enabled:
     #ipas_reset_counter = max(0, ipas_reset_counter - 1)
@@ -78,7 +79,7 @@ def ipas_state_transition(steer_angle_enabled, enabled, ipas_state, ipas_reset_c
     return True, 0
 
   elif enabled and steer_angle_enabled:
-    if steer_angle_enabled and ipas_state != 3:
+    if steer_angle_enabled and not ipas_active:
       ipas_reset_counter += 1
     else:
       ipas_reset_counter = 0
@@ -141,20 +142,25 @@ def update(self, sendcan, enabled, CS, frame, actuators,
     # dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
     # cuts steer torque immediately anyway TODO: monitor if this is a real issue
     # only cut torque when steer state is a known fault
-    if not enabled or CS.steer_state in [18, 50]:
+    if not enabled or CS.steer_state in [9, 25]:
       apply_steer = 0
 
     self.steer_angle_enabled, self.ipas_reset_counter = \
-      ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_state, self.ipas_reset_counter)
-    #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_state
+      ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
+    #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
 
     # steer angle
-    if self.steer_angle_enabled:
+    if self.steer_angle_enabled and CS.ipas_active:
       apply_angle = actuators.steerAngle
-      angle_lim = int_rnd(interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V))
+      angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
       apply_angle = clip(apply_angle, -angle_lim, angle_lim)
 
-      angle_rate_lim = int_rnd(interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V))
+      # windup slower
+      if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
+        angle_rate_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_DELTA_V)
+      else:
+        angle_rate_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_DELTA_VU)
+
       apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
     else:
       apply_angle = CS.angle_steers

selfdrive/car/toyota/carstate.py

@@ -160,8 +160,8 @@ def update(self, cp):
     self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
     # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
     self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
-    self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [2, 10]
-    self.ipas_state = cp.vl['EPS_STATUS']['IPAS_STATE']
+    self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
+    self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
     self.brake_error = 0
     self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
     self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.4.3.1-release"
+#define COMMA_VERSION "0.4.3.2-release"

selfdrive/controls/controlsd.py

@@ -224,10 +224,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
   for b in CS.buttonEvents:
     # button presses for rear view
     if b.type == "leftBlinker" or b.type == "rightBlinker":
-      if b.pressed and rear_view_allowed:
-        rear_view_toggle = True
-      else:
-        rear_view_toggle = False
+      rear_view_toggle = b.pressed and rear_view_allowed
 
     if (b.type == "altButton1" and b.pressed) and not passive:
       rear_view_toggle = not rear_view_toggle

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -45,25 +45,29 @@ int main( )
   auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
   auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
 
-  auto c_left_lane = exp(-(poly_l - yy));
-  auto c_right_lane = exp(poly_r - yy);
+  // given the lane width estimate, this is where we estimate the path given lane lines
+  auto l_phantom = poly_l - lane_width/2.0;
+  auto r_phantom = poly_r + lane_width/2.0;
 
-  auto r_phantom = poly_l - lane_width/2.0;
-  auto l_phantom = poly_r + lane_width/2.0;
-
-  auto path = lr_prob       * (l_prob * r_phantom + r_prob * l_phantom) / (l_prob + r_prob + 0.0001)
+  // best path estimate path is a linear combination of poly_p and the path estimate
+  // given the lane lines
+  auto path = lr_prob       * (l_prob * l_phantom + r_prob * r_phantom) / (l_prob + r_prob + 0.0001)
               + (1-lr_prob) * poly_p;
 
   auto angle = lr_prob      * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
                + (1-lr_prob) * angle_p;
 
+  // instead of using actual lane lines, use their estimated distance from path given lane_width
+  auto c_left_lane = exp(-(path + lane_width/2.0 - yy));
+  auto c_right_lane = exp(path - lane_width/2.0 - yy);
+
   // Running cost
   Function h;
 
   // Distance errors
   h << path - yy;
-  h << l_prob * c_left_lane;
-  h << r_prob * c_right_lane;
+  h << lr_prob * c_left_lane;
+  h << lr_prob * c_right_lane;
 
   // Heading error
   h << (v_ref + 1.0 ) * (angle - psi);