openpilot v0.5.4 release

Dockerfile.openpilot

@@ -17,3 +17,5 @@ COPY ./phonelibs /tmp/openpilot/phonelibs
 COPY ./pyextra /tmp/openpilot/pyextra
 
 RUN mkdir -p /tmp/openpilot/selfdrive/test/out
+RUN make -C /tmp/openpilot/selfdrive/controls/lib/longitudinal_mpc clean
+RUN make -C /tmp/openpilot/selfdrive/controls/lib/lateral_mpc clean

RELEASES.md

@@ -1,3 +1,14 @@
+Version 0.5.4 (2018-09-25)
+========================
+ * New Driving Model
+ * New Driver Monitoring Model
+ * Improve longitudinal mpc in mid-low speed braking
+ * Honda Accord hybrid support thanks to energee!
+ * Ship mpc binaries and sensibly reduce build time
+ * Calibration more stable
+ * More Hyundai and Kia cars supported thanks to emmertex!
+ * Various GM Volt improvements thanks to vntarasov!
+
 Version 0.5.3 (2018-09-03)
 ========================
  * Hyundai Santa Fe support!

cereal/car.capnp

@@ -42,7 +42,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     speedTooLow @17;
     outOfSpace @18;
     overheat @19;
-    calibrationInProgress @20;
+    calibrationIncomplete @20;
     calibrationInvalid @21;
     controlsMismatch @22;
     pcmEnable @23;
@@ -69,6 +69,8 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     promptDriverUnresponsive @44;
     driverUnresponsive @45;
     belowSteerSpeed @46;
+    calibrationProgress @47;
+    lowBattery @48;
   }
 }
 

cereal/log.capnp

@@ -276,6 +276,7 @@ struct ThermalData {
   startedTs @13 :UInt64;
 
   thermalStatus @14 :ThermalStatus;
+  chargerDisabled @17 :Bool;
 
   enum ThermalStatus {
     green @0;   # all processes run

common/dbc.py

@@ -1,7 +1,6 @@
 import re
 import os
 import struct
-import bitstring
 import sys
 import numbers
 from collections import namedtuple, defaultdict
@@ -17,6 +16,7 @@ def int_or_float(s):
   "DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed",
                 "factor", "offset", "tmin", "tmax", "units"])
 
+
 class dbc(object):
   def __init__(self, fn):
     self.name, _ = os.path.splitext(os.path.basename(fn))
@@ -122,6 +122,16 @@ def lookup_msg_id(self, msg_id):
       msg_id = self.msg_name_to_address[msg_id]
     return msg_id
 
+  def reverse_bytes(self, x):
+    return ((x & 0xff00000000000000) >> 56) | \
+           ((x & 0x00ff000000000000) >> 40) | \
+           ((x & 0x0000ff0000000000) >> 24) | \
+           ((x & 0x000000ff00000000) >> 8) | \
+           ((x & 0x00000000ff000000) << 8) | \
+           ((x & 0x0000000000ff0000) << 24) | \
+           ((x & 0x000000000000ff00) << 40) | \
+           ((x & 0x00000000000000ff) << 56)
+
   def encode(self, msg_id, dd):
     """Encode a CAN message using the dbc.
 
@@ -131,35 +141,40 @@ def encode(self, msg_id, dd):
     """
     msg_id = self.lookup_msg_id(msg_id)
 
-    # TODO: Stop using bitstring, which is super slow.
     msg_def = self.msgs[msg_id]
     size = msg_def[0][1]
 
-    bsf = bitstring.Bits(hex="00"*size)
+    result = 0
     for s in msg_def[1]:
       ival = dd.get(s.name)
       if ival is not None:
-        ival = (ival / s.factor) - s.offset
-        ival = int(round(ival))
 
-        # should pack this
+        b2 = s.size
         if s.is_little_endian:
-          ss = s.start_bit
+          b1 = s.start_bit
         else:
-          ss = self.bits_index[s.start_bit]
+          b1 = (s.start_bit // 8) * 8 + (-s.start_bit - 1) % 8
+        bo = 64 - (b1 + s.size)
+
+        ival = (ival / s.factor) - s.offset
+        ival = int(round(ival))
 
+        if s.is_signed and ival < 0:
+          ival = (1 << b2) + ival
 
-        if s.is_signed:
-          tbs = bitstring.Bits(int=ival, length=s.size)
-        else:
-          tbs = bitstring.Bits(uint=ival, length=s.size)
+        shift = b1 if s.is_little_endian else bo
+        mask = ((1 << b2) - 1) << shift
+        dat = (ival & ((1 << b2) - 1)) << shift
+
+        if s.is_little_endian:
+          mask = self.reverse_bytes(mask)
+          dat = self.reverse_bytes(dat)
 
-        lpad = bitstring.Bits(bin="0b"+"0"*ss)
-        rpad = bitstring.Bits(bin="0b"+"0"*(8*size-(ss+s.size)))
-        tbs = lpad+tbs+rpad
+        result &= ~mask
+        result |= dat
 
-        bsf |= tbs
-    return bsf.tobytes()
+    result = struct.pack('>Q', result)
+    return result[:size]
 
   def decode(self, x, arr=None, debug=False):
     """Decode a CAN message using the dbc.
@@ -195,55 +210,77 @@ def decode(self, x, arr=None, debug=False):
     if debug:
       print name
 
-    blen = 8*len(x[2])
-
-    st = x[2].rjust(8, '\x00')
+    st = x[2].ljust(8, '\x00')
     le, be = None, None
-    size = msg[0][1]
 
     for s in msg[1]:
       if arr is not None and s[0] not in arr:
         continue
 
-      # big or little endian?
-      #   see http://vi-firmware.openxcplatform.com/en/master/config/bit-numbering.html
-      if s[3] is False:
-        ss = self.bits_index[s[1]]
-        if be is None:
-          be = struct.unpack(">Q", st)[0]
-        x2_int = be
-        data_bit_pos = (blen - (ss + s[2]))
+      start_bit = s[1]
+      signal_size = s[2]
+      little_endian = s[3]
+      signed = s[4]
+      factor = s[5]
+      offset = s[6]
+
+      b2 = signal_size
+      if little_endian:
+        b1 = start_bit
       else:
+        b1 = (start_bit // 8) * 8 + (-start_bit - 1) % 8
+      bo = 64 - (b1 + signal_size)
+
+      if little_endian:
         if le is None:
           le = struct.unpack("<Q", st)[0]
-        x2_int = le >> (64 - 8 * size)
-        ss = s[1]
-        data_bit_pos = ss
+        shift_amount = b1
+        tmp = le
+      else:
+        if be is None:
+          be = struct.unpack(">Q", st)[0]
+        shift_amount = bo
+        tmp = be
 
-      if data_bit_pos < 0:
+      if shift_amount < 0:
         continue
-      ival = (x2_int >> data_bit_pos) & ((1 << (s[2])) - 1)
 
-      if s[4] and (ival & (1<<(s[2]-1))): # signed
-        ival -= (1<<s[2])
+      tmp = (tmp >> shift_amount) & ((1 << b2) - 1)
+      if signed and (tmp >> (b2 - 1)):
+        tmp -= (1 << b2)
 
-      # control the offset
-      ival = (ival * s[5]) + s[6]
-      #if debug:
-      #  print "%40s  %2d %2d  %7.2f %s" % (s[0], s[1], s[2], ival, s[-1])
+      tmp = tmp * factor + offset
+
+      # if debug:
+      #   print "%40s  %2d %2d  %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1])
 
       if arr is None:
-        out[s[0]] = ival
+        out[s[0]] = tmp
       else:
-        out[arr.index(s[0])] = ival
+        out[arr.index(s[0])] = tmp
     return name, out
 
   def get_signals(self, msg):
     msg = self.lookup_msg_id(msg)
     return [sgs.name for sgs in self.msgs[msg][1]]
 
+
 if __name__ == "__main__":
    from opendbc import DBC_PATH
+   import numpy as np
+
+   dbc_test = dbc(os.path.join(DBC_PATH, 'toyota_prius_2017_pt_generated.dbc'))
+   msg = ('STEER_ANGLE_SENSOR', {'STEER_ANGLE': -6.0, 'STEER_RATE': 4, 'STEER_FRACTION': -0.2})
+   encoded = dbc_test.encode(*msg)
+   decoded = dbc_test.decode((0x25, 0, encoded))
+   assert decoded == msg
+
+   dbc_test = dbc(os.path.join(DBC_PATH, 'hyundai_santa_fe_2019_ccan.dbc'))
+   decoded = dbc_test.decode((0x2b0, 0, "\xfa\xfe\x00\x07\x12"))
+   assert np.isclose(decoded[1]['SAS_Angle'], -26.2)
+
+   msg = ('SAS11', {'SAS_Stat': 7.0, 'MsgCount': 0.0, 'SAS_Angle': -26.200000000000003, 'SAS_Speed': 0.0, 'CheckSum': 0.0})
+   encoded = dbc_test.encode(*msg)
+   decoded = dbc_test.decode((0x2b0, 0, encoded))
 
-   dbc_test = dbc(os.path.join(DBC_PATH, sys.argv[1]))
-   print dbc_test.get_signals(0xe4)
+   assert decoded == msg

common/filter_simple.py

@@ -0,0 +1,10 @@
+class FirstOrderFilter():
+  # first order filter
+  def __init__(self, x0, ts, dt):
+    self.k = (dt / ts) / (1. + dt / ts)
+    self.x = x0
+
+  def update(self, x):
+    self.x = (1. - self.k) * self.x + self.k * x
+
+

common/transformations/model.py

@@ -57,6 +57,8 @@
 bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
   get_view_frame_from_road_frame(0, 0, 0, model_height))
 
+model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics))
+
 # 'camera from model camera'
 def get_model_height_transform(camera_frame_from_road_frame, height):
   camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([

selfdrive/boardd/Makefile

@@ -13,8 +13,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
              -Werror=return-type \
              -Werror=format-extra-args
 
-CFLAGS = -std=gnu11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
-CXXFLAGS = -std=c++11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
+CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
 
 ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
 ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \

selfdrive/car/gm/carcontroller.py

@@ -4,7 +4,7 @@
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.gm import gmcan
-from selfdrive.car.gm.values import CAR, DBC
+from selfdrive.car.gm.values import CAR, DBC, AccState
 from selfdrive.can.packer import CANPacker
 
 
@@ -29,11 +29,11 @@ def __init__(self, car_fingerprint):
     self.ADAS_KEEPALIVE_STEP = 10
     # pedal lookups, only for Volt
     MAX_GAS = 3072              # Only a safety limit
-    ZERO_GAS = 2048
+    self.ZERO_GAS = 2048
     MAX_BRAKE = 350             # Should be around 3.5m/s^2, including regen
     self.MAX_ACC_REGEN = 1404  # ACC Regen braking is slightly less powerful than max regen paddle
     self.GAS_LOOKUP_BP = [-0.25, 0., 0.5]
-    self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
+    self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, MAX_GAS]
     self.BRAKE_LOOKUP_BP = [-1., -0.25]
     self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
 
@@ -83,7 +83,6 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
       return
 
     P = self.params
-
     # Send CAN commands.
     can_sends = []
     canbus = self.canbus
@@ -131,12 +130,18 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
       if (frame % 4) == 0:
         idx = (frame / 4) % 4
 
-        at_full_stop = enabled and CS.standstill
-        near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
+        car_stopping = apply_gas < P.ZERO_GAS
+        standstill = CS.pcm_acc_status == AccState.STANDSTILL
+        at_full_stop = enabled and standstill and car_stopping
+        near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
         can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop))
 
-        at_full_stop = enabled and CS.standstill
-        can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
+        # Auto-resume from full stop by resetting ACC control
+        acc_enabled = enabled
+        if standstill and not car_stopping:
+          acc_enabled = False
+
+        can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, acc_enabled, at_full_stop))
 
       # Send dashboard UI commands (ACC status), 25hz
       if (frame % 4) == 0:

selfdrive/car/gm/gmcan.py

@@ -60,17 +60,15 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st
 
 def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop):
 
-  if apply_brake == 0:
-    mode = 0x1
-  else:
+  mode = 0x1
+  if apply_brake > 0:
     mode = 0xa
 
-    if at_full_stop:
-      mode = 0xd
-    # TODO: this is to have GM bringing the car to complete stop,
-    # but currently it conflicts with OP controls, so turned off.
-    #elif near_stop:
-    #  mode = 0xb
+  if near_stop:
+    mode = 0xb
+
+  if at_full_stop:
+    mode = 0xd
 
   brake = (0x1000 - apply_brake) & 0xfff
   checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff