openpilot v0.3.8.2 release

Dockerfile.openpilot

@@ -16,4 +16,4 @@ COPY ./selfdrive /tmp/openpilot/selfdrive
 COPY ./phonelibs /tmp/openpilot/phonelibs
 COPY ./pyextra /tmp/openpilot/pyextra
 
-RUN mkdir /tmp/openpilot/selfdrive/test/out
+RUN mkdir -p /tmp/openpilot/selfdrive/test/out

Makefile

@@ -1,6 +1,9 @@
-.PHONY: all
 
-# TODO: Add a global build system to openpilot
+code_dir := $(shell pwd)
+
+# TODO: Add a global build system
+
+.PHONY: all
 all:
-	cd /data/openpilot/selfdrive && PYTHONPATH=/data/openpilot PREPAREONLY=1 /data/openpilot/selfdrive/manager.py
+	cd selfdrive && PYTHONPATH=$(code_dir) PREPAREONLY=1 ./manager.py
 

README.md

@@ -12,7 +12,7 @@ Here are [some](https://www.youtube.com/watch?v=9OwTJFuDI7g) [videos](https://ww
 Hardware
 ------
 
-Right now openpilot supports the [neo research platform](http://github.com/commaai/neo) for vehicle control. We'd like to support other platforms as well.
+Right now openpilot supports the [NEO research platform](http://github.com/commaai/neo) and the [EON Dashcam DevKit](https://shop.comma.ai/products/eon-dashcam-devkit). We'd like to support other platforms as well.
 
 Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` during NEOS setup.
 
@@ -26,15 +26,25 @@ Supported Cars
   - Due to limitations in steering firmware, steering is disabled below 12 mph
   - Note that the hatchback model is not supported
 
-- Honda CR-V Touring 2015-2016 (very alpha!)
+- Honda CR-V Touring 2015-2016
   - Can only be enabled above 25 mph
 
+- Toyota RAV-4 2016+ with TSS-P (alpha!)
+  - Can only be enabled above 20 mph
+
 In Progress Cars
 ------
 
+- Toyota Prius 2017
+
+- Probably all TSS-P Toyota
+
+Community Ported Cars
+------
+
 - Chevy Volt 2016-2018 Premier with Driver Confidence II
 
-- All 2017 Toyota Prius, Corolla, and RAV4
+- Classic Tesla Model S (pre-AP)
 
 Directory structure
 ------

RELEASES.md

@@ -1,3 +1,9 @@
+Version 0.3.8.2 (2017-10-30)
+==========================
+ * Add alpha support for 2017 Toyota RAV4
+ * Stay silent if stock system is connected through giraffe
+ * Minor bug fixes
+
 Version 0.3.7 (2017-09-30)
 ==========================
  * Improved lateral control using model predictive control

cereal/Makefile

@@ -1,8 +1,9 @@
+PWD := $(shell pwd)
+
 SRCS := log.capnp car.capnp
 
 GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
 
-
 UNAME_M ?= $(shell uname -m)
 
 # only generate C++ for docker tests
@@ -16,6 +17,12 @@ endif
 
 endif
 
+ifeq ($(UNAME_M),aarch64)
+CAPNPC=PATH=$(PWD)/../phonelibs/capnp-cpp/aarch64/bin/:$$PATH capnpc
+else
+CAPNPC=capnpc
+endif
+
 .PHONY: all
 all: $(GENS)
 
@@ -26,17 +33,17 @@ clean:
 gen/c/%.capnp.c: %.capnp
 	@echo "[ CAPNPC C ] $@"
 	mkdir -p gen/c/
-	capnpc '$<' -o c:gen/c/
+	$(CAPNPC) '$<' -o c:gen/c/
 
 gen/cpp/%.capnp.c++: %.capnp
 	@echo "[ CAPNPC C++ ] $@"
 	mkdir -p gen/cpp/
-	capnpc '$<' -o c++:gen/cpp/
+	$(CAPNPC) '$<' -o c++:gen/cpp/
 
 gen/java/Car.java gen/java/Log.java: $(SRCS)
 	@echo "[ CAPNPC java ] $@"
 	mkdir -p gen/java/
-	capnpc $^ -o java:gen/java
+	$(CAPNPC) $^ -o java:gen/java
 
 # c-capnproto needs some empty headers
 gen/c/c++.capnp.h gen/c/java.capnp.h:

cereal/car.capnp

@@ -51,6 +51,8 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     modelCommIssue @27;
     brakeHold @28;
     parkBrake @29;
+    manualRestart @30;
+    lowSpeedLockout @31;
   }
 }
 
@@ -63,6 +65,9 @@ struct CarState {
 
   # car speed
   vEgo @1 :Float32;       # best estimate of speed
+  aEgo @16 :Float32;       # best estimate of acceleration
+  vEgoRaw @17 :Float32;       # unfiltered speed
+  standstill @18 :Bool;
   wheelSpeeds @2 :WheelSpeeds;
 
   # gas pedal, 0.0-1.0
@@ -75,6 +80,7 @@ struct CarState {
 
   # steering wheel
   steeringAngle @7 :Float32;   # deg
+  steeringRate @15 :Float32;   # deg/s
   steeringTorque @8 :Float32;  # TODO: standardize units
   steeringPressed @9 :Bool;    # if the user is using the steering wheel
 
@@ -242,6 +248,9 @@ struct CarParams {
   enableGas @4 :Bool;
   enableBrake @5 :Bool;
   enableCruise @6 :Bool;
+  enableCamera @27 :Bool;
+  enableDsu @28 :Bool; # driving support unit
+  enableApgs @29 :Bool; # advanced parking guidance system
 
   minEnableSpeed @18 :Float32;
   safetyModel @19 :Int16;
@@ -254,10 +263,11 @@ struct CarParams {
   brakeMaxV @25 :List(Float32);
 
   enum SafetyModels {
-    # from board
+    # does NOT match board setting
-    default @0;
+    noOutput @0;
     honda @1;
     toyota @2;
+    elm327 @3;
   }
 
   # things about the car in the manual
@@ -276,7 +286,9 @@ struct CarParams {
   # Kp and Ki for the lateral control
   steerKp @16 :Float32;
   steerKi @17 :Float32;
+  steerKf @26 :Float32;
+
+  steerLimitAlert @30 :Bool;
 
   # TODO: Kp and Ki for long control, perhaps not needed?
 }
-

cereal/log.capnp

@@ -36,6 +36,7 @@ struct InitData {
   pandaInfo @8 :PandaInfo;
 
   dirty @9 :Bool;
+  passive @12 :Bool;
 
   enum DeviceType {
     unknown @0;
@@ -327,8 +328,11 @@ struct Live100Data {
   mdMonoTimeDEPRECATED @18 :UInt64;
   planMonoTime @28 :UInt64;
 
+  state @31 :ControlState;
   vEgo @0 :Float32;
+  vEgoRaw @32 :Float32;
   aEgoDEPRECATED @1 :Float32;
+  longControlState @30 :LongControlState;
   vPid @2 :Float32;
   vTargetLead @3 :Float32;
   upAccelCmd @4 :Float32;
@@ -356,6 +360,20 @@ struct Live100Data {
   awarenessStatus @26 :Float32;
 
   angleOffset @27 :Float32;
+
+  enum ControlState {
+    disabled @0;
+    preEnabled @1;
+    enabled @2;
+    softDisabling @3;
+  }
+
+  enum LongControlState {
+    off @0;
+    pid @1;
+    stopping @2;
+    starting @3;
+  }
 }
 
 struct LiveEventData {
@@ -370,6 +388,7 @@ struct ModelData {
   leftLane @2 :PathData;
   rightLane @3 :PathData;
   lead @4 :LeadData;
+  leadNew @6 :List(Float32);
 
   settings @5 :ModelSettings;
 
@@ -454,11 +473,13 @@ struct Plan {
   # longitudinal
   longitudinalValid @2 :Bool;
   vTarget @3 :Float32;
+  vTargetFuture @14 :Float32;
   aTargetMin @4 :Float32;
   aTargetMax @5 :Float32;
   jerkFactor @6 :Float32;
   hasLead @7 :Bool;
   fcw @8 :Bool;
+  longitudinalPlanSource @15 :LongitudinalPlanSource;
 
   # gps trajectory in car frame
   gpsTrajectory @12 :GpsTrajectory;
@@ -467,6 +488,12 @@ struct Plan {
     x @0 :List(Float32);
     y @1 :List(Float32);
   }
+
+  enum LongitudinalPlanSource {
+    cruise @0;
+    mpc1 @1;
+    mpc2 @2;
+  }
 }
 
 struct LiveLocationData {
@@ -1073,6 +1100,7 @@ struct UbloxGnss {
   union {
     measurementReport @0 :MeasurementReport;
     ephemeris @1 :Ephemeris;
+    ionoData @2 :IonoData;
   }
 
   struct MeasurementReport {
@@ -1175,9 +1203,24 @@ struct UbloxGnss {
 
     transmissionTime @34 :Float64;
     fitInterval @35 :Float64;
+
+    toc @36 :Float64;
+  }
+
+  struct IonoData {
+    svHealth @0 :UInt32;
+    tow  @1 :Float64;
+    gpsWeek @2 :Float64;
+
+    ionoAlpha @3 :List(Float64);
+    ionoBeta @4 :List(Float64);
+
+    healthValid @5 :Bool;
+    ionoCoeffsValid @6 :Bool;
   }
 }
 
+
 struct Clocks {
   bootTimeNanos @0 :UInt64;
   monotonicNanos @1 :UInt64;
@@ -1191,6 +1234,21 @@ struct LiveMpcData {
   y @1 :List(Float32);
   psi @2 :List(Float32);
   delta @3 :List(Float32);
+  qpIterations @4 :UInt32;
+  calculationTime @5 :UInt64;
+}
+
+struct LiveLongitudinalMpcData {
+  xEgo @0 :List(Float32);
+  vEgo @1 :List(Float32);
+  aEgo @2 :List(Float32);
+  xLead @3 :List(Float32);
+  vLead @4 :List(Float32);
+  aLead @5 :List(Float32);
+  aLeadTau @6 :Float32;    # lead accel time constant
+  qpIterations @7 :UInt32;
+  mpcId @8 :UInt32;
+  calculationTime @9 :UInt64;
 }
 
 struct Event {
@@ -1234,5 +1292,6 @@ struct Event {
     ubloxGnss @34 :UbloxGnss;
     clocks @35 :Clocks;
     liveMpc @36 :LiveMpcData;
+    liveLongitudinalMpc @37 :LiveLongitudinalMpcData;
   }
 }

common/dbc.py

@@ -192,4 +192,15 @@ class dbc(object):
       else:
         out[arr.index(s[0])] = ival
     return name, out
+
+
+  def get_signals(self, msg):
+    return [sgs.name for sgs in self.msgs[msg][1]]
     
+if __name__ == "__main__":
+   import sys
+   import os
+   from opendbc import DBC_PATH
+
+   dbc_test = dbc(os.path.join(DBC_PATH, sys.argv[1]))
+   print dbc_test.get_signals(0xe4)

common/fingerprints.py

@@ -11,19 +11,23 @@ _FINGERPRINTS = {
     # sent messages
     0xe4: 5, 0x1fa: 8, 0x200: 3, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8,
   },
-  "HONDA ACCORD 2016 TOURING": {
-    1024L: 5, 929L: 8, 1027L: 5, 773L: 7, 1601L: 8, 777L: 8, 1036L: 8, 398L: 3, 1039L: 8, 401L: 8, 145L: 8, 1424L: 5, 660L: 8, 661L: 4, 918L: 7, 985L: 3, 923L: 2, 542L: 7, 927L: 8, 800L: 8, 545L: 4, 420L: 8, 422L: 8, 808L: 8, 426L: 8, 1029L: 8, 432L: 7, 57L: 3, 316L: 8, 829L: 5, 1600L: 5, 1089L: 8, 1057L: 5, 780L: 8, 1088L: 8, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 476L: 4, 1296L: 3, 891L: 8, 1125L: 8, 487L: 4, 892L: 8, 490L: 8, 871L: 8, 1064L: 7, 882L: 2, 884L: 8, 506L: 8, 507L: 1, 380L: 8, 1365L: 5
-  },
   "HONDA CR-V 2016 TOURING": {
     57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8,
     # sent messages
     0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5,
   },
-  "TOYOTA PRIUS 2017": {
+  "TOYOTA RAV4 2017": {
-    896L: 8, 832L: 8, 898L: 8, 899L: 8, 577L: 8, 528L: 8, 529L: 8, 530L: 8, 531L: 8, 532L: 8, 533L: 8, 534L: 8, 535L: 8, 536L: 8, 537L: 8, 538L: 8, 539L: 8, 540L: 8, 541L: 8, 542L: 8, 543L: 8, 544L: 8, 545L: 8, 546L: 8, 547L: 8, 548L: 8, 549L: 8, 550L: 8, 551L: 8, 296L: 6, 553L: 6, 554L: 6, 555L: 6, 556L: 6, 557L: 6, 558L: 6, 559L: 6, 560L: 7, 561L: 8, 562L: 8, 883L: 8, 837L: 8, 833L: 8, 576L: 8, 321L: 4, 834L: 8, 835L: 8, 580L: 8, 581L: 8, 897L: 8, 584L: 8, 1136L: 4, 976L: 8, 977L: 8, 978L: 8, 291L: 7, 881L: 8, 352L: 8, 353L: 7, 867L: 8, 868L: 8, 869L: 8, 1126L: 3, 304L: 7, 880L: 8, 552L: 6, 882L: 8, 979L: 8, 884L: 8, 885L: 8, 836L: 8
+    36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
-  }
+  },
 }
 
+# support additional internal only fingerprints
+try:
+  from common.fingerprints_internal import add_additional_fingerprints
+  add_additional_fingerprints(_FINGERPRINTS)
+except ImportError:
+  pass
+
 def eliminate_incompatible_cars(msg, candidate_cars):
   """Removes cars that could not have sent msg.
 
@@ -50,36 +54,3 @@ def all_known_cars():
   """Returns a list of all known car strings."""
   return _FINGERPRINTS.keys()
 
-# **** for use live only ****
-def fingerprint(logcan):
-  import selfdrive.messaging as messaging
-  from cereal import car
-  from common.realtime import sec_since_boot
-
-  if os.getenv("SIMULATOR") is not None or logcan is None:
-    return ("simulator", None)
-  elif os.getenv("SIMULATOR2") is not None:
-    return ("simulator2", None)
-
-  print "waiting for fingerprint..."
-  candidate_cars = all_known_cars()
-  finger = {}
-  st = None
-  while 1:
-    for a in messaging.drain_sock(logcan, wait_for_one=True):
-      if st is None:
-        st = sec_since_boot()
-      for can in a.can:
-        if can.src == 0:
-          finger[can.address] = len(can.dat)
-        candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
-
-    # if we only have one car choice and it's been 100ms since we got our first message, exit
-    if len(candidate_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1:
-      break
-    elif len(candidate_cars) == 0:
-      print map(hex, finger.keys())
-      raise Exception("car doesn't match any fingerprints")
-
-  print "fingerprinted", candidate_cars[0]
-  return (candidate_cars[0], finger)

common/params.py

@@ -64,7 +64,10 @@ keys = {
   "CloudCalibration": TxType.PERSISTANT,
 # written: controlsd
 # read:    radard
-  "CarParams": TxType.CLEAR_ON_CAR_START}
+  "CarParams": TxType.CLEAR_ON_CAR_START,
+
+  "Passive": TxType.PERSISTANT,
+}
 
 def fsync_dir(path):
   fd = os.open(path, os.O_RDONLY)

opendbc

@@ -1 +1 @@
-Subproject commit b8c0034c5d6ded3d85a4f0414b3509baae35a34b
+Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85

panda

@@ -1 +1 @@
-Subproject commit 5ea4df111b735ce9efb27ab2e0f87837210f6fc3
+Subproject commit 849f68879d1ceacbf1f9d4174e16e1cd14527383

selfdrive/boardd/Makefile

@@ -25,18 +25,16 @@ JSON_FLAGS = -I$(PHONELIBS)/json/src
 
 EXTRA_LIBS = -lusb
 
-ifeq ($(OS),GNU/Linux)
+# ifeq ($(OS),GNU/Linux)
-# for Drive PX2
+# # for Drive PX2
-ZMQ_LIBS = -lczmq -lzmq
+# ZMQ_LIBS = -lczmq -lzmq
-CEREAL_LIBS = -lcapnp -lkj -lcapnp_c
+# CEREAL_LIBS = -lcapnp -lkj -lcapnp_c
-EXTRA_LIBS = -lusb-1.0 -lpthread
+# EXTRA_LIBS = -lusb-1.0 -lpthread
-endif
+# endif
 
 ifeq ($(ARCH),x86_64)
-ZMQ_LIBS = -L$(HOME)/one/external/zmq/lib/ \
+ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
            -l:libczmq.a -l:libzmq.a
-CEREAL_LIBS = -L$(HOME)/one/external/capnp/lib/ \
-              -l:libcapnp.a -l:libcapnp_c.a -l:libkj.a
 EXTRA_LIBS = -lusb-1.0 -lpthread
 CXXFLAGS += -I/usr/include/libusb-1.0
 CFLAGS += -I/usr/include/libusb-1.0

selfdrive/boardd/boardd.cc

@@ -25,7 +25,18 @@
 #include "common/swaglog.h"
 #include "common/timing.h"
 
-int do_exit = 0;
+// double the FIFO size
+#define RECV_SIZE (0x1000)
+#define TIMEOUT 0
+
+#define SAFETY_NOOUTPUT  0
+#define SAFETY_HONDA 1
+#define SAFETY_TOYOTA 2
+#define SAFETY_ELM327 0xE327
+
+namespace {
+
+volatile int do_exit = 0;
 
 libusb_context *ctx = NULL;
 libusb_device_handle *dev_handle;
@@ -35,12 +46,6 @@ bool spoofing_started = false;
 bool fake_send = false;
 bool loopback_can = false;
 
-// double the FIFO size
-#define RECV_SIZE (0x1000)
-#define TIMEOUT 0
-
-#define SAFETY_NOOUTPUT  0x0000
-
 pthread_t safety_setter_thread_handle = -1;
 
 void *safety_setter_thread(void *s) {
@@ -49,6 +54,8 @@ void *safety_setter_thread(void *s) {
 
   LOGW("waiting for params to set safety model");
   while (1) {
+    if (do_exit) return NULL;
+
     const int result = read_db_value(NULL, "CarParams", &value, &value_sz);
     if (value_sz > 0) break;
     usleep(100*1000);
@@ -62,15 +69,33 @@ void *safety_setter_thread(void *s) {
   capnp::FlatArrayMessageReader cmsg(amsg);
   cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
 
-  int safety_model = car_params.getSafetyModel();
+  auto safety_model = car_params.getSafetyModel();
   LOGW("setting safety model: %d", safety_model);
 
+  int safety_setting = 0;
+  switch (safety_model) {
+  case (int)cereal::CarParams::SafetyModels::NO_OUTPUT:
+    safety_setting = SAFETY_NOOUTPUT;
+    break;
+  case (int)cereal::CarParams::SafetyModels::HONDA:
+    safety_setting = SAFETY_HONDA;
+    break;
+  case (int)cereal::CarParams::SafetyModels::TOYOTA:
+    safety_setting = SAFETY_TOYOTA;
+    break;
+  case (int)cereal::CarParams::SafetyModels::ELM327:
+    safety_setting = SAFETY_ELM327;
+    break;
+  default:
+    LOGE("unknown safety model: %d", safety_model);
+  }
+
   pthread_mutex_lock(&usb_lock);
 
   // set in the mutex to avoid race
   safety_setter_thread_handle = -1;
 
-  libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, 0, NULL, 0, TIMEOUT);
+  libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_setting, 0, NULL, 0, TIMEOUT);
 
   pthread_mutex_unlock(&usb_lock);
 
@@ -388,6 +413,8 @@ int set_realtime_priority(int level) {
   return sched_setscheduler(getpid(), SCHED_FIFO, &sa);
 }
 
+}
+
 int main() {
   int err;
   LOGW("starting boardd");

selfdrive/boardd/boardd.py

@@ -116,6 +116,7 @@ def boardd_mock_loop():
   handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
 
   logcan = messaging.sub_sock(context, service_list['can'].port)
+  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
 
   while 1:
     tsc = messaging.drain_sock(logcan, wait_for_one=True)
@@ -129,8 +130,8 @@ def boardd_mock_loop():
     # recv @ 100hz
     can_msgs = can_recv()
     print "sent %d got %d" % (len(snd), len(can_msgs))
-
+    m = can_list_to_can_capnp(can_msgs)
-    #print can_msgs
+    sendcan.send(m.to_bytes())
 
 def boardd_test_loop():
   can_init()
@@ -189,9 +190,45 @@ def boardd_loop(rate=200):
 
     rk.keep_time()
 
+# *** main loop ***
+def boardd_proxy_loop(rate=200, address="192.168.2.251"):
+  rk = Ratekeeper(rate)
+  context = zmq.Context()
+
+  can_init()
+
+  # *** subscribes can
+  logcan = messaging.sub_sock(context, service_list['can'].port, addr=address)
+  # *** publishes to can send
+  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
+
+  while 1:
+    # recv @ 100hz
+    can_msgs = can_recv()
+    #for m in can_msgs:
+    #  print "R:",hex(m[0]), str(m[2]).encode("hex")
+
+    # publish to logger
+    # TODO: refactor for speed
+    if len(can_msgs) > 0:
+      dat = can_list_to_can_capnp(can_msgs, "sendcan")
+      sendcan.send(dat.to_bytes())
+
+    # send can if we have a packet
+    tsc = messaging.recv_sock(logcan)
+    if tsc is not None:
+      cl = can_capnp_to_can_list(tsc.can)
+      #for m in cl:
+      #  print "S:",hex(m[0]), str(m[2]).encode("hex")
+      can_send_many(cl)
+
+    rk.keep_time()
+
 def main(gctx=None):
   if os.getenv("MOCK") is not None:
     boardd_mock_loop()
+  elif os.getenv("PROXY") is not None:
+    boardd_proxy_loop()
   elif os.getenv("BOARDTEST") is not None:
     boardd_test_loop()
   else:

selfdrive/can/Makefile

@@ -18,17 +18,13 @@ CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
 CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
 
 ifeq ($(UNAME_S),Darwin)
-	CEREAL_LIBS := -L /usr/local/lib -lkj -lcapnp
 	ZMQ_LIBS = -L/usr/local/lib -lzmq
 else ifeq ($(OPTEST),1)
 	ZMQ_LIBS = -lzmq
-	CEREAL_LIBS = -lcapnp -lkj
 else ifeq ($(UNAME_M),x86_64)
 	EXTERNAL := ../../external
 	ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include
 	ZMQ_LIBS = -L$(EXTERNAL)/zmq/lib -l:libzmq.a
-	CEREAL_CXXFLAGS := -I$(EXTERNAL)/capnp/include
-	CEREAL_LIBS := -L$(EXTERNAL)/capnp/lib -l:libcapnp.a -l:libkj.a
 else ifeq ($(UNAME_M),aarch64)
 	ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
 	ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a
@@ -44,9 +40,7 @@ CWD := $(shell pwd)
 .PHONY: all
 all: libdbc.so
 
-ifeq ($(UNAME_M),aarch64)
 include ../common/cereal.mk
-endif
 
 # make sure cereal is built
 libdbc.so:: ../../cereal/gen/cpp/log.capnp.h

selfdrive/can/common.h

@@ -38,6 +38,7 @@ enum SignalType {
   DEFAULT,
   HONDA_CHECKSUM,
   HONDA_COUNTER,
+  TOYOTA_CHECKSUM,
 };
 
 struct Signal {
@@ -51,6 +52,7 @@ struct Signal {
 struct Msg {
   const char* name;
   uint32_t address;
+  unsigned int size;
   size_t num_sigs;
   const Signal *sigs;
 };

selfdrive/can/dbc_template.cc

@@ -4,7 +4,7 @@
 
 namespace {
 
-{% for address, msg_name, sigs in msgs %}
+{% for address, msg_name, msg_size, sigs in msgs %}
 const Signal sigs_{{address}}[] = {
   {% for sig in sigs %}
     {
@@ -20,6 +20,8 @@ const Signal sigs_{{address}}[] = {
       .type = SignalType::HONDA_CHECKSUM,
       {% elif checksum_type == "honda" and sig.name == "COUNTER" %}
       .type = SignalType::HONDA_COUNTER,
+      {% elif checksum_type == "toyota" and sig.name == "CHECKSUM" %}
+      .type = SignalType::TOYOTA_CHECKSUM,
       {% else %}
       .type = SignalType::DEFAULT,
       {% endif %}
@@ -29,11 +31,12 @@ const Signal sigs_{{address}}[] = {
 {% endfor %}
 
 const Msg msgs[] = {
-{% for address, msg_name, sigs in msgs %}
+{% for address, msg_name, msg_size, sigs in msgs %}
   {% set address_hex = "0x%X" % address %}
   {
     .name = "{{msg_name}}",
     .address = {{address_hex}},
+    .size = {{msg_size}},
     .num_sigs = ARRAYSIZE(sigs_{{address}}),
     .sigs = sigs_{{address}},
   },

selfdrive/can/parser.cc

@@ -40,26 +40,33 @@ uint64_t read_u64_be(const uint8_t* v) {
           | (uint64_t)v[7]);
 }
 
-bool honda_checksum(int address, uint64_t d, int l) {
+unsigned int honda_checksum(unsigned int address, uint64_t d, int l) {
-  int target = (d >> l) & 0xF;
+  d >>= ((8-l)*8); // remove padding
-
+  d >>= 4; // remove checksum
-  DEBUG("check checksum %16lx %d", d, l);
-
-  // remove checksum from calculation
-  d &= ~(0xFLL << l);
 
   int s = 0;
-  while (address > 0) { s += (address & 0xF); address >>= 4; }
+  while (address) { s += (address & 0xF); address >>= 4; }
-  while (d > 0) { s += (d & 0xF); d >>= 4; }
+  while (d) { s += (d & 0xF); d >>= 4; }
   s = 8-s;
   s &= 0xF;
 
-  DEBUG("   %d = %d\n", target, s);
+  return s;
-  return target == s;
+}
+
+unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) {
+  d >>= ((8-l)*8); // remove padding
+  d >>= 8; // remove checksum
+
+  unsigned int s = l;
+  while (address) { s += address & 0xff; address >>= 8; }
+  while (d) { s += d & 0xff; d >>= 8; }
+  
+  return s & 0xFF;
 }
 
 struct MessageState {
   uint32_t address;
+  unsigned int size;
 
   std::vector<Signal> parse_sigs;
   std::vector<double> vals;
@@ -80,10 +87,10 @@ struct MessageState {
         tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed
       }
 
-      DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp);
+      DEBUG("parse %X %s -> %lld\n", address, sig.name, tmp);
 
       if (sig.type == SignalType::HONDA_CHECKSUM) {
-        if (!honda_checksum(address, dat, sig.bo)) {
+        if (honda_checksum(address, dat, size) != tmp) {
           INFO("%X CHECKSUM FAIL\n", address);
           return false;
         }
@@ -91,6 +98,13 @@ struct MessageState {
         if (!honda_update_counter(tmp)) {
           return false;
         }
+      } else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
+        // DEBUG("CHECKSUM %d %d %018llX - %lld vs %d\n", address, size, dat, tmp, toyota_checksum(address, dat, size));
+
+        if (toyota_checksum(address, dat, size) != tmp) {
+          INFO("%X CHECKSUM FAIL\n", address);
+          return false;
+        }
       }
 
       vals[i] = tmp * sig.factor + sig.offset;
@@ -161,11 +175,12 @@ class CANParser {
         assert(false);
       }
 
-      // track checksums and for this message
+      state.size = msg->size;
+
+      // track checksums and counters for this message
       for (int i=0; i<msg->num_sigs; i++) {
         const Signal *sig = &msg->sigs[i];
-        if (sig->type == HONDA_CHECKSUM
+        if (sig->type != SignalType::DEFAULT) {
-            || sig->type == HONDA_COUNTER) {
           state.parse_sigs.push_back(*sig);
           state.vals.push_back(0);
         }
@@ -178,8 +193,7 @@ class CANParser {
         for (int i=0; i<msg->num_sigs; i++) {
           const Signal *sig = &msg->sigs[i];
           if (strcmp(sig->name, sigop.name) == 0
-              && sig->type != HONDA_CHECKSUM
+              && sig->type == SignalType::DEFAULT) {
-              && sig->type != HONDA_COUNTER) {
             state.parse_sigs.push_back(*sig);
             state.vals.push_back(sigop.default_value);
             break;
@@ -216,7 +230,7 @@ class CANParser {
 
         uint64_t p = read_u64_be(dat);
 
-        DEBUG("  proc %X: %lx\n", cmsg.getAddress(), p);
+        DEBUG("  proc %X: %llx\n", cmsg.getAddress(), p);
 
         state_it->second.parse(sec, cmsg.getBusTime(), p);
       }

selfdrive/can/parser.py

@@ -72,58 +72,101 @@ if __name__ == "__main__":
 
   # cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1)
 
+
+  # signals = [
+  #   ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default 
+  #   ("WHEEL_SPEED_FL", 0x1d0, 0),
+  #   ("WHEEL_SPEED_FR", 0x1d0, 0),
+  #   ("WHEEL_SPEED_RL", 0x1d0, 0),
+  #   ("STEER_ANGLE", 0x14a, 0),
+  #   ("STEER_TORQUE_SENSOR", 0x18f, 0),
+  #   ("GEAR", 0x191, 0),
+  #   ("WHEELS_MOVING", 0x1b0, 1),
+  #   ("DOOR_OPEN_FL", 0x405, 1),
+  #   ("DOOR_OPEN_FR", 0x405, 1),
+  #   ("DOOR_OPEN_RL", 0x405, 1),
+  #   ("DOOR_OPEN_RR", 0x405, 1),
+  #   ("CRUISE_SPEED_PCM", 0x324, 0),
+  #   ("SEATBELT_DRIVER_LAMP", 0x305, 1),
+  #   ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
+  #   ("BRAKE_PRESSED", 0x17c, 0),
+  #   ("CAR_GAS", 0x130, 0),
+  #   ("CRUISE_BUTTONS", 0x296, 0),
+  #   ("ESP_DISABLED", 0x1a4, 1),
+  #   ("HUD_LEAD", 0x30c, 0),
+  #   ("USER_BRAKE", 0x1a4, 0),
+  #   ("STEER_STATUS", 0x18f, 5),
+  #   ("WHEEL_SPEED_RR", 0x1d0, 0),
+  #   ("BRAKE_ERROR_1", 0x1b0, 1),
+  #   ("BRAKE_ERROR_2", 0x1b0, 1),
+  #   ("GEAR_SHIFTER", 0x191, 0),
+  #   ("MAIN_ON", 0x326, 0),
+  #   ("ACC_STATUS", 0x17c, 0),
+  #   ("PEDAL_GAS", 0x17c, 0),
+  #   ("CRUISE_SETTING", 0x296, 0),
+  #   ("LEFT_BLINKER", 0x326, 0),
+  #   ("RIGHT_BLINKER", 0x326, 0),
+  #   ("COUNTER", 0x324, 0),
+  #   ("ENGINE_RPM", 0x17C, 0)
+  # ]
+  # checks = [
+  #   (0x14a, 100), # address, frequency
+  #   (0x158, 100),
+  #   (0x17c, 100),
+  #   (0x191, 100),
+  #   (0x1a4, 50),
+  #   (0x326, 10),
+  #   (0x1b0, 50),
+  #   (0x1d0, 50),
+  #   (0x305, 10),
+  #   (0x324, 10),
+  #   (0x405, 3),
+  # ]
+
+  # cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0)
+
+
   signals = [
-    ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default 
+    # sig_name, sig_address, default
-    ("WHEEL_SPEED_FL", 0x1d0, 0),
+    ("GEAR", 956, 0x20),
-    ("WHEEL_SPEED_FR", 0x1d0, 0),
+    ("BRAKE_PRESSED", 548, 0),
-    ("WHEEL_SPEED_RL", 0x1d0, 0),
+    ("GAS_PEDAL", 705, 0),
-    ("STEER_ANGLE", 0x14a, 0),
+
-    ("STEER_TORQUE_SENSOR", 0x18f, 0),
+    ("WHEEL_SPEED_FL", 170, 0),
-    ("GEAR", 0x191, 0),
+    ("WHEEL_SPEED_FR", 170, 0),
-    ("WHEELS_MOVING", 0x1b0, 1),
+    ("WHEEL_SPEED_RL", 170, 0),
-    ("DOOR_OPEN_FL", 0x405, 1),
+    ("WHEEL_SPEED_RR", 170, 0),
-    ("DOOR_OPEN_FR", 0x405, 1),
+    ("DOOR_OPEN_FL", 1568, 1),
-    ("DOOR_OPEN_RL", 0x405, 1),
+    ("DOOR_OPEN_FR", 1568, 1),
-    ("DOOR_OPEN_RR", 0x405, 1),
+    ("DOOR_OPEN_RL", 1568, 1),
-    ("CRUISE_SPEED_PCM", 0x324, 0),
+    ("DOOR_OPEN_RR", 1568, 1),
-    ("SEATBELT_DRIVER_LAMP", 0x305, 1),
+    ("SEATBELT_DRIVER_UNLATCHED", 1568, 1),
-    ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
+    ("TC_DISABLED", 951, 1),
-    ("BRAKE_PRESSED", 0x17c, 0),
+    ("STEER_ANGLE", 37, 0),
-    ("CAR_GAS", 0x130, 0),
+    ("STEER_FRACTION", 37, 0),
-    ("CRUISE_BUTTONS", 0x296, 0),
+    ("STEER_RATE", 37, 0),
-    ("ESP_DISABLED", 0x1a4, 1),
+    ("GAS_RELEASED", 466, 0),
-    ("HUD_LEAD", 0x30c, 0),
+    ("CRUISE_STATE", 466, 0),
-    ("USER_BRAKE", 0x1a4, 0),
+    ("MAIN_ON", 467, 0),
-    ("STEER_STATUS", 0x18f, 5),
+    ("SET_SPEED", 467, 0),
-    ("WHEEL_SPEED_RR", 0x1d0, 0),
+    ("STEER_TORQUE_DRIVER", 608, 0),
-    ("BRAKE_ERROR_1", 0x1b0, 1),
+    ("STEER_TORQUE_EPS", 608, 0),
-    ("BRAKE_ERROR_2", 0x1b0, 1),
+    ("TURN_SIGNALS", 1556, 3),   # 3 is no blinkers
-    ("GEAR_SHIFTER", 0x191, 0),
+    ("LKA_STATE", 610, 0),
-    ("MAIN_ON", 0x326, 0),
-    ("ACC_STATUS", 0x17c, 0),
-    ("PEDAL_GAS", 0x17c, 0),
-    ("CRUISE_SETTING", 0x296, 0),
-    ("LEFT_BLINKER", 0x326, 0),
-    ("RIGHT_BLINKER", 0x326, 0),
-    ("COUNTER", 0x324, 0),
-    ("ENGINE_RPM", 0x17C, 0)
   ]
   checks = [
-    (0x14a, 100), # address, frequency
+    (548, 40),
-    (0x158, 100),
+    (705, 33),
-    (0x17c, 100),
+
-    (0x191, 100),
+    (170, 80),
-    (0x1a4, 50),
+    (37, 80),
-    (0x326, 10),
+    (466, 33),
-    (0x1b0, 50),
+    (608, 50),
-    (0x1d0, 50),
-    (0x305, 10),
-    (0x324, 10),
-    (0x405, 3),
   ]
 
-  cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0)
+  cp = CANParser("toyota_rav4_2017_pt", signals, checks, 0)
-  print cp.vl
+
+  # print cp.vl
 
   while True:
     cp.update(int(sec_since_boot()*1e9), True)

selfdrive/can/process_dbc.py

@@ -21,10 +21,15 @@ can_dbc = dbc(dbc_fn)
 with open(template_fn, "r") as template_f:
   template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True)
 
-msgs = [(address, msg_name, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
+msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
-        for address, ((msg_name, _), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs]
+        for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs]
-
+
-checksum_type = "honda" if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura") else None
+if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"):
+  checksum_type = "honda"
+elif can_dbc.name.startswith("toyota"):
+  checksum_type = "toyota"
+else:
+  checksum_type = None
 
 parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, len=len)
 

selfdrive/car/__init__.py

@@ -1,5 +1,11 @@
-from common.fingerprints import fingerprint
+import os
+from cereal import car
 
+from common.realtime import sec_since_boot
+from common.fingerprints import eliminate_incompatible_cars, all_known_cars
+
+from selfdrive.swaglog import cloudlog
+import selfdrive.messaging as messaging
 from .honda.interface import CarInterface as HondaInterface
 
 try:
@@ -17,19 +23,65 @@ try:
 except ImportError:
   Sim2Interface = None
 
+
 interfaces = {
   "HONDA CIVIC 2016 TOURING": HondaInterface,
   "ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
   "HONDA ACCORD 2016 TOURING": HondaInterface,
   "HONDA CR-V 2016 TOURING": HondaInterface,
   "TOYOTA PRIUS 2017": ToyotaInterface,
+  "TOYOTA RAV4 2017": ToyotaInterface,
 
   "simulator": SimInterface,
   "simulator2": Sim2Interface
 }
 
-def get_car(logcan, sendcan=None):
+# **** for use live only ****
-  candidate, fingerprints = fingerprint(logcan)
+def fingerprint(logcan, timeout):
+  if os.getenv("SIMULATOR") is not None or logcan is None:
+    return ("simulator", None)
+  elif os.getenv("SIMULATOR2") is not None:
+    return ("simulator2", None)
+
+  finger_st = sec_since_boot()
+
+  cloudlog.warning("waiting for fingerprint...")
+  candidate_cars = all_known_cars()
+  finger = {}
+  st = None
+  while 1:
+    for a in messaging.drain_sock(logcan, wait_for_one=True):
+      if st is None:
+        st = sec_since_boot()
+      for can in a.can:
+        if can.src == 0:
+          finger[can.address] = len(can.dat)
+        candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
+
+    ts = sec_since_boot()
+    # if we only have one car choice and the time_fingerprint since we got our first 
+    # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
+    # broadcast immediately
+    if len(candidate_cars) == 1 and st is not None:
+      time_fingerprint = 1.0 if "TOYOTA" in candidate_cars[0] else 0.1
+      if (ts-st) > time_fingerprint:
+        break
+
+    # bail if no cars left or we've been waiting too long
+    elif len(candidate_cars) == 0 or (timeout and ts-finger_st > timeout):
+      return None, finger
+
+  cloudlog.warning("fingerprinted %s", candidate_cars[0])
+  return (candidate_cars[0], finger)
+
+
+def get_car(logcan, sendcan=None, timeout=None):
+  candidate, fingerprints = fingerprint(logcan, timeout)
+
+  if candidate is None:
+    cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
+    return None, None
+
   interface_cls = interfaces[candidate]
   params = interface_cls.get_params(candidate, fingerprints)
 

selfdrive/car/honda/carcontroller.py

@@ -7,7 +7,13 @@ from common.realtime import sec_since_boot
 from selfdrive.config import CruiseButtons
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.controls.lib.drive_helpers import rate_limit
+
 from . import hondacan
+from .values import AH
+
+# msgs sent for steering controller by camera module on can 0.
+# those messages are mutually exclusive on non-rav4 and rav4 cars
+CAMERA_MSGS = [0xe4, 0x194]
 
 
 def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
@@ -41,17 +47,6 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
 
   return brake, braking, brake_steady
 
-class AH:
-  #[alert_idx, value]
-  # See dbc files for info on values"
-  NONE           = [0, 0]
-  FCW            = [1, 0x8]
-  STEER          = [2, 1]
-  BRAKE_PRESSED  = [3, 10]
-  GEAR_NOT_D     = [4, 6]
-  SEATBELT       = [5, 5]
-  SPEED_TOO_HIGH = [6, 8]
-
 
 def process_hud_alert(hud_alert):
   # initialize to no alert
@@ -75,10 +70,11 @@ HUDData = namedtuple("HUDData",
                       "lanes", "beep", "X8", "chime", "acc_alert"])
 
 class CarController(object):
-  def __init__(self):
+  def __init__(self, enable_camera=True):
     self.braking = False
     self.brake_steady = 0.
     self.brake_last = 0.
+    self.enable_camera = enable_camera
 
   def update(self, sendcan, enabled, CS, frame, actuators, \
              pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
@@ -87,7 +83,7 @@ class CarController(object):
     """ Controls thread """
 
     # TODO: Make the accord work.
-    if CS.accord:
+    if CS.accord or not self.enable_camera:
       return
 
     # *** apply brake hysteresis ***

selfdrive/car/honda/carstate.py

@@ -1,12 +1,12 @@
 import os
 import time
 from cereal import car
-import common.numpy_fast as np
+from common.numpy_fast import interp
 from common.realtime import sec_since_boot
 import selfdrive.messaging as messaging
 from selfdrive.can.parser import CANParser
 from selfdrive.config import Conversions as CV
-
+import numpy as np
 
 def parse_gear_shifter(can_gear_shifter, is_acura):
 
@@ -59,6 +59,7 @@ def get_can_signals(CP):
       ("WHEEL_SPEED_RL", 0x1d0, 0),
       ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("STEER_ANGLE", 0x14a, 0),
+      ("STEER_ANGLE_RATE", 0x14a, 0),
       ("STEER_TORQUE_SENSOR", 0x18f, 0),
       ("GEAR", 0x191, 0),
       ("WHEELS_MOVING", 0x1b0, 1),
@@ -115,6 +116,7 @@ def get_can_signals(CP):
       ("WHEEL_SPEED_RL", 0x1d0, 0),
       ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("STEER_ANGLE", 0x156, 0),
+      ("STEER_ANGLE_RATE", 0x156, 0),
       ("STEER_TORQUE_SENSOR", 0x18f, 0),
       ("GEAR", 0x1a3, 0),
       ("WHEELS_MOVING", 0x1b0, 1),
@@ -167,6 +169,7 @@ def get_can_signals(CP):
       ("WHEEL_SPEED_RL", 0x1d0, 0),
       ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("STEER_ANGLE", 0x156, 0),
+      ("STEER_ANGLE_RATE", 0x156, 0),
       #("STEER_TORQUE_SENSOR", 0x18f, 0),
       ("GEAR", 0x191, 0),
       ("WHEELS_MOVING", 0x1b0, 1),
@@ -218,6 +221,7 @@ def get_can_signals(CP):
       ("WHEEL_SPEED_RL", 0x1d0, 0),
       ("WHEEL_SPEED_RR", 0x1d0, 0),
       ("STEER_ANGLE", 0x156, 0),
+      ("STEER_ANGLE_RATE", 0x156, 0),
       ("STEER_TORQUE_SENSOR", 0x18f, 0),
       ("GEAR", 0x191, 0),
       ("WHEELS_MOVING", 0x1b0, 1),
@@ -304,8 +308,17 @@ class CarState(object):
     self.left_blinker_on = 0
     self.right_blinker_on = 0
 
-    # TODO: actually make this work
+    # vEgo kalman filter
-    self.a_ego = 0.
+    dt = 0.01
+    self.v_ego_x = np.matrix([[0.0], [0.0]])
+    self.v_ego_A = np.matrix([[1.0, dt], [0.0, 1.0]])
+    self.v_ego_C = np.matrix([1.0, 0.0])
+    self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
+    self.v_ego_R = 1e3
+    # import control
+    # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
+    # self.v_ego_K = np.transpose(K)
+    self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
 
   def update(self, can_pub_main=None):
     cp = self.cp
@@ -351,9 +364,16 @@ class CarState(object):
     self.v_wheel_rl = cp.vl[0x1D0]['WHEEL_SPEED_RL']
     self.v_wheel_rr = cp.vl[0x1D0]['WHEEL_SPEED_RR']
     self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
+
     # blend in transmission speed at low speed, since it has more low speed accuracy
-    self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
+    self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
-    self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
+    speed = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
+    self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, speed)
+
+    self.v_ego_raw = speed
+    self.v_ego = float(self.v_ego_x[0])
+    self.a_ego = float(self.v_ego_x[1])
+
     if self.CP.enableGas:
       # this is a hack
       self.user_gas = cp.vl[0x201]['INTERCEPTOR_GAS']
@@ -362,6 +382,7 @@ class CarState(object):
     if self.civic:
       can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
+      self.angle_steers_rate = cp.vl[0x14A]['STEER_ANGLE_RATE']
       self.gear = 0  # TODO: civic has CVT... needs rev engineering
       self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
       self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
@@ -375,6 +396,7 @@ class CarState(object):
     elif self.accord:
       can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
+      self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE']
       self.gear = 0  # TODO: accord has CVT... needs rev engineering
       self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
       self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
@@ -388,6 +410,7 @@ class CarState(object):
     elif self.crv:
       can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
+      self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE']
       self.gear = cp.vl[0x191]['GEAR']
       self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
       self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
@@ -401,6 +424,7 @@ class CarState(object):
     elif self.acura:
       can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
       self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
+      self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE']
       self.gear = cp.vl[0x1A3]['GEAR']
       self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
       self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
@@ -448,7 +472,7 @@ if __name__ == '__main__':
   import time
   from selfdrive.services import service_list
   context = zmq.Context()
-  logcan = messaging.sub_sock(context, service_list['can'].port) 
+  logcan = messaging.sub_sock(context, service_list['can'].port)
 
   class CarParams(object):
     def __init__(self):
@@ -461,4 +485,3 @@ if __name__ == '__main__':
   while 1:
     CS.update()
     time.sleep(0.01)
-

selfdrive/car/honda/interface.py

@@ -4,17 +4,19 @@ import time
 import numpy as np
 from common.numpy_fast import clip
 from common.realtime import sec_since_boot
-
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
-from .carstate import CarState
-from .carcontroller import CarController, AH
-from selfdrive.boardd.boardd import can_capnp_to_can_list
 from cereal import car
-
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
+from selfdrive.car.honda.carstate import CarState
+from selfdrive.car.honda.carcontroller import CAMERA_MSGS
+from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH
 
+try:
+  from .carcontroller import CarController
+except ImportError:
+  CarController = None
 
 def get_compute_gb():
   # generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
@@ -61,31 +63,6 @@ def get_compute_gb():
   return _compute_gb
 
 
-# Car button codes
-class CruiseButtons:
-  RES_ACCEL   = 4
-  DECEL_SET   = 3
-  CANCEL      = 2
-  MAIN        = 1
-
-
-#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
-class CM:
-  MUTE = 0
-  SINGLE = 3
-  DOUBLE = 4
-  REPEATED = 1
-  CONTINUOUS = 2
-
-
-#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
-class BP:
-  MUTE = 0
-  SINGLE = 3
-  TRIPLE = 2
-  REPEATED = 1
-
-
 class CarInterface(object):
   def __init__(self, CP, logcan, sendcan=None):
     self.logcan = logcan
@@ -104,7 +81,7 @@ class CarInterface(object):
     # sending if read only is False
     if sendcan is not None:
       self.sendcan = sendcan
-      self.CC = CarController()
+      self.CC = CarController(CP.enableCamera)
 
     if self.CS.accord:
       # self.accord_msg = []
@@ -127,11 +104,14 @@ class CarInterface(object):
     ret.enableSteer = True
     ret.enableBrake = True
 
-    # pedal
+    ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
     ret.enableGas = 0x201 in fingerprint
+    print "ECU Camera Simulated: ", ret.enableCamera
+    print "ECU Gas Interceptor: ", ret.enableGas
+
     ret.enableCruise = not ret.enableGas
 
-    # FIXME: hardcoding honda civic 2016 touring params so they can be used to 
+    # FIXME: hardcoding honda civic 2016 touring params so they can be used to
     # scale unknown params for other cars
     m_civic = 2923./2.205 + std_cargo
     l_civic = 2.70
@@ -176,13 +156,15 @@ class CarInterface(object):
     else:
       raise ValueError("unsupported car %s" % candidate)
 
-    # min speed to enable ACC. if car can do stop and go, then set enabling speed 
+    ret.steerKf = 0. # TODO: investigate FF steer control for Honda
-    # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not 
+
+    # min speed to enable ACC. if car can do stop and go, then set enabling speed
+    # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
     # conflict with PCM acc
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS
 
     ret.aR = ret.l - ret.aF
-    # TODO: get actual value, for now starting with reasonable value for 
+    # TODO: get actual value, for now starting with reasonable value for
     # civic and scaling by mass and wheelbase
     ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
 
@@ -197,11 +179,14 @@ class CarInterface(object):
     # no max steer limit VS speed
     ret.steerMaxBP = [0.]  # m/s
     ret.steerMaxV = [1.]   # max steer allowed
+
     ret.gasMaxBP = [0.]  # m/s
     ret.gasMaxV = [0.6] if ret.enableGas else [0.] # max gas allowed
     ret.brakeMaxBP = [5., 20.]  # m/s
     ret.brakeMaxV = [1., 0.8]   # max brake allowed
 
+    ret.steerLimitAlert = True
+
     return ret
 
   compute_gb = staticmethod(get_compute_gb())
@@ -219,6 +204,9 @@ class CarInterface(object):
 
     # speeds
     ret.vEgo = self.CS.v_ego
+    ret.aEgo = self.CS.a_ego
+    ret.vEgoRaw = self.CS.v_ego_raw
+    ret.standstill = self.CS.standstill
     ret.wheelSpeeds.fl = self.CS.v_wheel_fl
     ret.wheelSpeeds.fr = self.CS.v_wheel_fr
     ret.wheelSpeeds.rl = self.CS.v_wheel_rl
@@ -236,8 +224,8 @@ class CarInterface(object):
     ret.brakePressed = self.CS.brake_pressed != 0
 
     # steering wheel
-    # TODO: units
     ret.steeringAngle = self.CS.angle_steers
+    ret.steeringRate = self.CS.angle_steers_rate
 
     # gear shifter lever
     ret.gearShifter = self.CS.gear_shifter
@@ -249,7 +237,7 @@ class CarInterface(object):
     ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
     ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
     ret.cruiseState.available = bool(self.CS.main_on)
-    ret.cruiseState.speedOffset = self.CS.cruise_speed_offset 
+    ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
 
     # TODO: button presses
     buttonEvents = []
@@ -368,7 +356,7 @@ class CarInterface(object):
         events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
 
     if self.CP.enableCruise:
-      # KEEP THIS EVENT LAST! send enable event if button is pressed and there are 
+      # KEEP THIS EVENT LAST! send enable event if button is pressed and there are
       # NO_ENTRY events, so controlsd will display alerts. Also not send enable events
       # too close in time, so a no_entry will not be followed by another one.
       # TODO: button press should be the only thing that triggers enble

selfdrive/car/honda/values.py

@@ -0,0 +1,34 @@
+# Car button codes
+class CruiseButtons:
+  RES_ACCEL   = 4
+  DECEL_SET   = 3
+  CANCEL      = 2
+  MAIN        = 1
+
+
+#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
+class CM:
+  MUTE = 0
+  SINGLE = 3
+  DOUBLE = 4
+  REPEATED = 1
+  CONTINUOUS = 2
+
+
+#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
+class BP:
+  MUTE = 0
+  SINGLE = 3
+  TRIPLE = 2
+  REPEATED = 1
+
+class AH:
+  #[alert_idx, value]
+  # See dbc files for info on values"
+  NONE           = [0, 0]
+  FCW            = [1, 0x8]
+  STEER          = [2, 1]
+  BRAKE_PRESSED  = [3, 10]
+  GEAR_NOT_D     = [4, 6]
+  SEATBELT       = [5, 5]
+  SPEED_TOO_HIGH = [6, 8]

selfdrive/car/toyota/carcontroller.py

@@ -0,0 +1,236 @@
+from common.numpy_fast import clip, interp
+from common.realtime import sec_since_boot
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.controls.lib.drive_helpers import rate_limit
+from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
+                                           create_steer_command, create_ui_command, \
+                                           create_ipas_steer_command, create_accel_command
+
+
+ACCEL_HYST_GAP = 0.02  # don't change accel command for small oscilalitons within this value
+ACCEL_MAX = 1500  # 1.5 m/s2
+ACCEL_MIN = -3000 # 3   m/s2
+ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
+
+STEER_MAX = 1500
+STEER_DELTA_UP = 10        # 1.5s time to peak torque
+STEER_DELTA_DOWN = 45      # lower than 50 otherwise the Rav4 faults (Prius seems ok though)
+STEER_ERROR_MAX = 500      # max delta between torque cmd and torque motor
+
+STEER_IPAS_MAX = 340
+STEER_IPAS_DELTA_MAX = 3
+
+class CAR:
+  PRIUS = "TOYOTA PRIUS 2017"
+  RAV4 = "TOYOTA RAV4 2017"
+
+class ECU:
+  CAM = 0 # camera
+  DSU = 1 # driving support unit
+  APGS = 2 # advanced parking guidance system
+
+
+TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
+              0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
+              0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
+              0x383]
+
+# addr, [ecu, bus, 1/freq*100, vl]
+STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   2, '\x00\x00\x00\x46'),
+               0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   3, '\xf4\x01\x90\x83\x00\x37'),
+
+               0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0,   3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
+               0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0,   3, '\x00\x00\x00\x00\x00\x00\x8c'),
+               0x2E6: (ECU.DSU, (CAR.PRIUS,), 0,   3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
+               0x2E7: (ECU.DSU, (CAR.PRIUS,), 0,   3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
+
+               0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
+               0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
+               0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
+               0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x10\x01\x00\x10\x01\x00'),
+               0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1,   5, '\x00\x00\x00\x00\x00\x00\x01'),
+               0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0,   5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
+
+               0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
+               0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1,   7, '\x00\x1e\x00\x00\x00\x80\x07'),
+
+               0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0,  20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
+               0x33E: (ECU.DSU, (CAR.PRIUS,), 0,  20, '\x0f\xff\x26\x40\x00\x1f\x00'),
+               0x365: (ECU.DSU, (CAR.PRIUS,), 0,  20, '\x00\x00\x00\x80\x03\x00\x08'),
+               0x365: (ECU.DSU, (CAR.RAV4,), 0,  20, '\x00\x00\x00\x80\xfc\x00\x08'),
+               0x366: (ECU.DSU, (CAR.PRIUS,), 0,  20, '\x00\x00\x4d\x82\x40\x02\x00'),
+               0x366: (ECU.DSU, (CAR.RAV4,), 0,  20, '\x00\x72\x07\xff\x09\xfe\x00'),
+
+               0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0,  40, '\x06\x00'),
+
+               0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
+               0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
+               0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
+               0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
+               0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
+               0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
+               0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x20\x20\xAD'),
+               0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'),
+               0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'),
+               0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
+               0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
+               0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'),
+               0x411: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x20\x00\x00\x10\x00\x80\x00'),
+               0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
+               0x470: (ECU.DSU, (CAR.PRIUS,), 1, 100, '\x00\x00\x02\x7a'),
+              }
+
+
+def check_ecu_msgs(fingerprint, candidate, ecu):
+  # return True if fingerprint contains messages normally sent by a given ecu
+  ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and 
+                                         candidate in STATIC_MSGS[x][1] and 
+                                         STATIC_MSGS[x][2] == 0)]
+
+  return any(msg for msg in fingerprint if msg in ecu_msgs)
+
+
+def accel_hysteresis(accel, accel_steady, enabled):
+
+  # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
+  if not enabled:
+    # send 0 when disabled, otherwise acc faults
+    accel_steady = 0.
+  elif accel > accel_steady + ACCEL_HYST_GAP:
+    accel_steady = accel - ACCEL_HYST_GAP
+  elif accel < accel_steady - ACCEL_HYST_GAP:
+    accel_steady = accel + ACCEL_HYST_GAP
+  accel = accel_steady
+
+  return accel, accel_steady
+
+
+def process_hud_alert(hud_alert, audible_alert):
+  # initialize to no alert
+  hud1 = 0
+  hud2 = 0
+  if hud_alert in ['steerRequired', 'fcw']:
+    if audible_alert == 'chimeRepeated':
+      hud1 = 3
+    else:
+      hud1 = 1
+  if audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']:
+    # TODO: find a way to send single chimes
+    hud2 = 1
+
+  return hud1, hud2
+
+
+class CarController(object):
+  def __init__(self, car_fingerprint, enable_camera, enable_dsu, enable_apg):
+    self.braking = False
+    # redundant safety check with the board
+    self.controls_allowed = True
+    self.last_steer = 0.
+    self.accel_steady = 0.
+    self.car_fingerprint = car_fingerprint
+    self.alert_active = False
+
+    self.fake_ecus = set()
+    if enable_camera: self.fake_ecus.add(ECU.CAM)
+    if enable_dsu: self.fake_ecus.add(ECU.DSU)
+    if enable_apg: self.fake_ecus.add(ECU.APGS)
+
+  def update(self, sendcan, enabled, CS, frame, actuators,
+             pcm_cancel_cmd, hud_alert, audible_alert):
+
+    # *** compute control surfaces ***
+    tt = sec_since_boot()
+
+    # steer torque is converted back to CAN reference (positive when steering right)
+    apply_accel = actuators.gas - actuators.brake
+    apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
+    apply_accel = int(round(clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)))
+
+    # steer torque is converted back to CAN reference (positive when steering right)
+    apply_steer = int(round(actuators.steer * STEER_MAX))
+
+    max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX)
+    min_lim = max(min(CS.steer_torque_motor - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX)
+
+    apply_steer = clip(apply_steer, min_lim, max_lim)
+
+    # slow rate if steer torque increases in magnitude
+    if self.last_steer > 0:
+      apply_steer = clip(apply_steer, max(self.last_steer - STEER_DELTA_DOWN, - STEER_DELTA_UP), self.last_steer + STEER_DELTA_UP)
+    else:
+      apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
+
+    if not enabled and CS.pcm_acc_status:
+      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
+      pcm_cancel_cmd = 1
+
+    # 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
+    if not enabled or CS.steer_error:
+      apply_steer = 0
+
+    self.last_steer = apply_steer
+    self.last_accel = apply_accel
+
+    can_sends = []
+
+    #*** control msgs ***
+    #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
+
+    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
+    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
+    # on consecutive messages
+    if ECU.CAM in self.fake_ecus:
+      can_sends.append(create_steer_command(apply_steer, frame))
+
+    if ECU.APGS in self.fake_ecus:
+      can_sends.append(create_ipas_steer_command(apply_steer))
+
+    # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
+    if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
+      if ECU.DSU in self.fake_ecus:
+        can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd))
+      else:
+        can_sends.append(create_accel_command(0, pcm_cancel_cmd))
+
+    if frame % 10 == 0 and ECU.CAM in self.fake_ecus:
+      for addr in TARGET_IDS:
+        can_sends.append(create_video_target(frame/10, addr))
+
+    # ui mesg is at 100Hz but we send asap if:
+    # - there is something to display
+    # - there is something to stop displaying
+    hud1, hud2 = process_hud_alert(hud_alert, audible_alert)
+    if ((hud1 or hud2) and not self.alert_active) or \
+       (not (hud1 or hud2) and self.alert_active):
+      send_ui = True
+      self.alert_active = not self.alert_active
+    else:
+      send_ui = False
+
+    if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
+      can_sends.append(create_ui_command(hud1, hud2))
+
+    #*** static msgs ***
+
+    for addr, (ecu, cars, bus, fr_step, vl) in STATIC_MSGS.iteritems():
+      if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
+        # send msg!
+        # special cases
+
+        if fr_step == 5 and ecu == ECU.CAM and bus == 1:
+          cnt = (((frame / 5) % 7) + 1) << 5
+          vl = chr(cnt) + vl
+        elif addr in (0x489, 0x48a) and bus == 0:
+          # add counter for those 2 messages (last 4 bits)
+          cnt = ((frame/100)%0xf) + 1
+          if addr == 0x48a:
+            # 0x48a has a 8 preceding the counter
+            cnt += 1 << 7
+          vl += chr(cnt)
+
+        can_sends.append(make_can_msg(addr, vl, bus, False))
+
+
+    sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())

selfdrive/car/toyota/carstate.py

@@ -0,0 +1,187 @@
+import os
+import time
+from common.realtime import sec_since_boot
+import selfdrive.messaging as messaging
+from selfdrive.car.toyota.carcontroller import CAR
+from selfdrive.can.parser import CANParser
+from selfdrive.config import Conversions as CV
+import numpy as np
+
+def parse_gear_shifter(can_gear, car_fingerprint):
+
+  if car_fingerprint == CAR.PRIUS:
+    if can_gear == 0x0:
+      return "park"
+    elif can_gear == 0x1:
+      return "reverse"
+    elif can_gear == 0x2:
+      return "neutral"
+    elif can_gear == 0x3:
+      return "drive"
+    elif can_gear == 0x4:
+      return "brake"
+  elif car_fingerprint == CAR.RAV4:
+    if can_gear == 0x20:
+      return "park"
+    elif can_gear == 0x10:
+      return "reverse"
+    elif can_gear == 0x8:
+      return "neutral"
+    elif can_gear == 0x0:
+      return "drive"
+    elif can_gear == 0x1:
+      return "sport"
+
+  return "unknown"
+
+
+def get_can_parser(CP):
+  # this function generates lists for signal, messages and initial values
+  if CP.carFingerprint == CAR.PRIUS:
+    dbc_f = 'toyota_prius_2017_pt.dbc'
+    signals = [
+      ("GEAR", 295, 0),
+      ("BRAKE_PRESSED", 550, 0),
+      ("GAS_PEDAL", 581, 0),
+    ]
+    checks = [
+      (550, 40),
+      (581, 33)
+    ]
+  elif CP.carFingerprint == CAR.RAV4:
+    dbc_f = 'toyota_rav4_2017_pt.dbc'
+    signals = [
+      ("GEAR", 956, 0x20),
+      ("BRAKE_PRESSED", 548, 0),
+      ("GAS_PEDAL", 705, 0),
+    ]
+    checks = [
+      (548, 40),
+      (705, 33)
+    ]
+
+  # TODO: DOORS, GAS_PEDAL, BRAKE_PRESSED for RAV4
+  signals += [
+    # sig_name, sig_address, default
+    ("WHEEL_SPEED_FL", 170, 0),
+    ("WHEEL_SPEED_FR", 170, 0),
+    ("WHEEL_SPEED_RL", 170, 0),
+    ("WHEEL_SPEED_RR", 170, 0),
+    ("DOOR_OPEN_FL", 1568, 1),
+    ("DOOR_OPEN_FR", 1568, 1),
+    ("DOOR_OPEN_RL", 1568, 1),
+    ("DOOR_OPEN_RR", 1568, 1),
+    ("SEATBELT_DRIVER_UNLATCHED", 1568, 1),
+    ("TC_DISABLED", 951, 1),
+    ("STEER_ANGLE", 37, 0),
+    ("STEER_FRACTION", 37, 0),
+    ("STEER_RATE", 37, 0),
+    ("GAS_RELEASED", 466, 0),
+    ("CRUISE_STATE", 466, 0),
+    ("MAIN_ON", 467, 0),
+    ("SET_SPEED", 467, 0),
+    ("LOW_SPEED_LOCKOUT", 467, 0),
+    ("STEER_TORQUE_DRIVER", 608, 0),
+    ("STEER_TORQUE_EPS", 608, 0),
+    ("TURN_SIGNALS", 1556, 3),   # 3 is no blinkers
+    ("LKA_STATE", 610, 0),
+  ]
+
+  checks += [
+    (170, 80),
+    (37, 80),
+    (466, 33),
+    (467, 33),
+    (608, 50),
+    (610, 25),
+  ]
+
+  return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
+
+
+class CarState(object):
+  def __init__(self, CP, logcan):
+
+    self.CP = CP
+    self.left_blinker_on = 0
+    self.right_blinker_on = 0
+
+    # initialize can parser
+    self.cp = get_can_parser(CP)
+    self.car_fingerprint = CP.carFingerprint
+
+    # vEgo kalman filter
+    dt = 0.01
+    self.v_ego_x = np.matrix([[0.0], [0.0]])
+    self.v_ego_A = np.matrix([[1.0, dt], [0.0, 1.0]])
+    self.v_ego_C = np.matrix([1.0, 0.0])
+    self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
+    self.v_ego_R = 1e3
+    # import control
+    # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
+    # self.v_ego_K = np.transpose(K)
+    self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
+
+  def update(self):
+    cp = self.cp
+    cp.update(int(sec_since_boot() * 1e9), False)
+
+    # copy can_valid
+    self.can_valid = cp.can_valid
+
+    if self.car_fingerprint == CAR.PRIUS:
+      can_gear = cp.vl[295]['GEAR']
+      self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
+      self.pedal_gas = cp.vl[581]['GAS_PEDAL']
+    elif self.car_fingerprint == CAR.RAV4:
+      can_gear = cp.vl[956]['GEAR']
+      self.brake_pressed = cp.vl[548]['BRAKE_PRESSED']
+      self.pedal_gas = cp.vl[705]['GAS_PEDAL']
+
+    # update prevs, update must run once per loop
+    self.prev_left_blinker_on = self.left_blinker_on
+    self.prev_right_blinker_on = self.right_blinker_on
+
+    # ******************* parse out can *******************
+    self.door_all_closed = not any([cp.vl[1568]['DOOR_OPEN_FL'], cp.vl[1568]['DOOR_OPEN_FR'],
+                                    cp.vl[1568]['DOOR_OPEN_RL'], cp.vl[1568]['DOOR_OPEN_RR']])
+    self.seatbelt = not cp.vl[1568]['SEATBELT_DRIVER_UNLATCHED']
+    # whitelist instead of blacklist, safer at the expense of disengages
+    self.steer_error = False
+    self.brake_error = 0
+    self.esp_disabled = cp.vl[951]['TC_DISABLED']
+    # calc best v_ego estimate, by averaging two opposite corners
+    self.v_wheel_fl = cp.vl[170]['WHEEL_SPEED_FL']
+    self.v_wheel_fr = cp.vl[170]['WHEEL_SPEED_FR']
+    self.v_wheel_rl = cp.vl[170]['WHEEL_SPEED_RL']
+    self.v_wheel_rr = cp.vl[170]['WHEEL_SPEED_RR']
+    self.v_wheel = (
+      cp.vl[170]['WHEEL_SPEED_FL'] + cp.vl[170]['WHEEL_SPEED_FR'] +
+      cp.vl[170]['WHEEL_SPEED_RL'] + cp.vl[170]['WHEEL_SPEED_RR']) / 4. * CV.KPH_TO_MS
+
+    # Kalman filter
+    self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, self.v_wheel)
+    self.v_ego_raw = self.v_wheel
+    self.v_ego = float(self.v_ego_x[0])
+    self.a_ego = float(self.v_ego_x[1])
+    self.standstill = not self.v_wheel > 0.001
+
+    self.angle_steers = cp.vl[37]['STEER_ANGLE'] + cp.vl[37]['STEER_FRACTION']
+    self.angle_steers_rate = cp.vl[37]['STEER_RATE']
+    self.gear_shifter = parse_gear_shifter(can_gear, self.car_fingerprint)
+    self.main_on = cp.vl[467]['MAIN_ON']
+    self.left_blinker_on = cp.vl[1556]['TURN_SIGNALS'] == 1
+    self.right_blinker_on = cp.vl[1556]['TURN_SIGNALS'] == 2
+
+    # we could use the override bit from dbc, but it's triggered at too high torque values
+    self.steer_override = abs(cp.vl[608]['STEER_TORQUE_DRIVER']) > 100
+    self.steer_error = cp.vl[610]['LKA_STATE'] == 50
+    self.steer_torque_driver = cp.vl[608]['STEER_TORQUE_DRIVER']
+    self.steer_torque_motor = cp.vl[608]['STEER_TORQUE_EPS']
+
+    self.user_brake = 0
+    self.v_cruise_pcm = cp.vl[467]['SET_SPEED']
+    self.pcm_acc_status = cp.vl[466]['CRUISE_STATE']
+    self.car_gas = self.pedal_gas
+    self.gas_pressed = not cp.vl[466]['GAS_RELEASED']
+    self.low_speed_lockout = cp.vl[467]['LOW_SPEED_LOCKOUT'] == 2

selfdrive/car/toyota/interface.py

@@ -0,0 +1,246 @@
+#!/usr/bin/env python
+import os
+import time
+import common.numpy_fast as np
+from selfdrive.config import Conversions as CV
+from selfdrive.car.toyota.carstate import CarState, CAR
+from selfdrive.car.toyota.carcontroller import CarController, ECU, check_ecu_msgs
+from cereal import car
+from selfdrive.services import service_list
+import selfdrive.messaging as messaging
+from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
+
+
+class CarInterface(object):
+  def __init__(self, CP, logcan, sendcan=None):
+    self.logcan = logcan
+    self.CP = CP
+
+    self.frame = 0
+    self.can_invalid_count = 0
+    self.gas_pressed_prev = False
+    self.brake_pressed_prev = False
+    self.cruise_enabled_prev = False
+
+    # *** init the major players ***
+    self.CS = CarState(CP, self.logcan)
+
+    # sending if read only is False
+    if sendcan is not None:
+      self.sendcan = sendcan
+      self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
+
+  @staticmethod
+  def get_params(candidate, fingerprint):
+
+    # kg of standard extra cargo to count for drive, gas, etc...
+    std_cargo = 136
+
+    ret = car.CarParams.new_message()
+
+    ret.carName = "toyota"
+    ret.radarName = "toyota"
+    ret.carFingerprint = candidate
+
+    ret.safetyModel = car.CarParams.SafetyModels.toyota
+
+    ret.enableSteer = True
+    ret.enableBrake = True
+
+    # pedal
+    ret.enableCruise = True
+
+    # FIXME: hardcoding honda civic 2016 touring params so they can be used to
+    # scale unknown params for other cars
+    m_civic = 2923./2.205 + std_cargo
+    l_civic = 2.70
+    aF_civic = l_civic * 0.4
+    aR_civic = l_civic - aF_civic
+    j_civic = 2500
+    cF_civic = 85400
+    cR_civic = 90000
+
+    stop_and_go = True
+    ret.m = 3045./2.205 + std_cargo
+    ret.l = 2.70
+    ret.aF = ret.l * 0.44
+    ret.sR = 14.5 #Rav4 2017, TODO: find exact value for Prius
+    if candidate == CAR.PRIUS:
+      ret.steerKp, ret.steerKi = 0.2, 0.01
+    elif candidate == CAR.RAV4:  # rav4 control seem to be ok with integrators
+      ret.steerKp, ret.steerKi = 0.2, 0.05
+    ret.steerKf = 0.00007818594    # full torque for 10 deg at 80mph
+
+    # min speed to enable ACC. if car can do stop and go, then set enabling speed
+    # to a negative value, so it won't matter.
+    if candidate == CAR.PRIUS:
+      ret.minEnableSpeed = -1.
+    elif candidate == CAR.RAV4:   # TODO: hack Rav4 to do stop and go
+      ret.minEnableSpeed = 19. * CV.MPH_TO_MS
+
+    ret.aR = ret.l - ret.aF
+    # TODO: get actual value, for now starting with reasonable value for
+    # civic and scaling by mass and wheelbase
+    ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
+
+    # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
+    # mass and CG position, so all cars will have approximately similar dyn behaviors
+    ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic)
+    ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic)
+
+    # no rear steering, at least on the listed cars above
+    ret.chi = 0.
+
+    # 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.gasMaxBP = [0.]
+    ret.gasMaxV = [0.5]
+    ret.brakeMaxBP = [5., 20.]
+    ret.brakeMaxV = [1., 0.8]
+
+    ret.enableCamera = not check_ecu_msgs(fingerprint, candidate, ECU.CAM)
+    ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU)
+    ret.enableApgs = False # not check_ecu_msgs(fingerprint, candidate, ECU.APGS)
+    print "ECU Camera Simulated: ", ret.enableCamera
+    print "ECU DSU Simulated: ", ret.enableDsu
+    print "ECU APGS Simulated: ", ret.enableApgs
+    ret.enableGas = True
+
+    ret.steerLimitAlert = False
+
+    return ret
+
+  @staticmethod
+  def compute_gb(accel, speed):
+    # toyota interface is already in accelration cmd, so conversion to gas-brake it's a pass-through.
+    return accel
+
+  # returns a car.CarState
+  def update(self, c):
+    # ******************* do can recv *******************
+    can_pub_main = []
+    canMonoTimes = []
+
+    self.CS.update()
+
+    # create message
+    ret = car.CarState.new_message()
+
+    # speeds
+    ret.vEgo = self.CS.v_ego
+    ret.vEgoRaw = self.CS.v_ego_raw
+    ret.aEgo = self.CS.a_ego
+    ret.standstill = self.CS.standstill
+    ret.wheelSpeeds.fl = self.CS.v_wheel_fl
+    ret.wheelSpeeds.fr = self.CS.v_wheel_fr
+    ret.wheelSpeeds.rl = self.CS.v_wheel_rl
+    ret.wheelSpeeds.rr = self.CS.v_wheel_rr
+
+    # gear shifter
+    ret.gearShifter = self.CS.gear_shifter
+
+    # gas pedal
+    ret.gas = self.CS.car_gas / 256.0
+    ret.gasPressed = self.CS.pedal_gas > 0
+
+    # brake pedal
+    ret.brake = self.CS.user_brake
+    ret.brakePressed = self.CS.brake_pressed != 0
+
+    # steering wheel
+    ret.steeringAngle = self.CS.angle_steers
+    ret.steeringRate = self.CS.angle_steers_rate
+
+    ret.steeringTorque = 0
+    ret.steeringPressed = self.CS.steer_override
+
+    # cruise state
+    ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
+    ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
+    ret.cruiseState.available = bool(self.CS.main_on)
+    ret.cruiseState.speedOffset = 0.
+
+    # TODO: button presses
+    buttonEvents = []
+
+    if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
+      be = car.CarState.ButtonEvent.new_message()
+      be.type = 'leftBlinker'
+      be.pressed = self.CS.left_blinker_on != 0
+      buttonEvents.append(be)
+
+    if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
+      be = car.CarState.ButtonEvent.new_message()
+      be.type = 'rightBlinker'
+      be.pressed = self.CS.right_blinker_on != 0
+      buttonEvents.append(be)
+
+    ret.buttonEvents = buttonEvents
+
+    # events
+    events = []
+    if not self.CS.can_valid:
+      self.can_invalid_count += 1
+      if self.can_invalid_count >= 5:
+        events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    else:
+      self.can_invalid_count = 0
+    if not ret.gearShifter == 'drive' and self.CP.enableDsu:
+      events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if not self.CS.door_all_closed:
+      events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if not self.CS.seatbelt:
+      events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if self.CS.esp_disabled and self.CP.enableDsu:
+      events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+    if not self.CS.main_on and self.CP.enableDsu:
+      events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
+    if ret.gearShifter == 'reverse' and self.CP.enableDsu:
+      events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if self.CS.steer_error:
+      events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
+    if self.CS.low_speed_lockout:
+      events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY]))
+    if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu:
+      events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
+      if c.actuators.gas > 0.1:
+        # some margin on the actuator to not false trigger cancellation while stopping
+        events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
+      if ret.vEgo < 0.001:
+        # while in standstill, send a user alert
+        events.append(create_event('manualRestart', [ET.WARNING]))
+
+    # enable request in prius is simple, as we activate when Toyota is active (rising edge)
+    if ret.cruiseState.enabled and not self.cruise_enabled_prev:
+      events.append(create_event('pcmEnable', [ET.ENABLE]))
+    elif not ret.cruiseState.enabled:
+      events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
+
+    # disable on pedals rising edge or when brake is pressed and speed isn't zero
+    if (ret.gasPressed and not self.gas_pressed_prev) or \
+       (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
+      events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+
+    if ret.gasPressed:
+      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
+    self.cruise_enabled_prev = ret.cruiseState.enabled
+
+    return ret.as_reader()
+
+  # pass in a car.CarControl
+  # to be called @ 100hz
+  def apply(self, c):
+
+    self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
+                   c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
+                   c.hudControl.audibleAlert)
+
+    self.frame += 1
+    return False

selfdrive/car/toyota/toyotacan.py

@@ -0,0 +1,83 @@
+import struct
+import common.numpy_fast as np
+from selfdrive.config import Conversions as CV
+
+
+# *** Toyota specific ***
+
+def fix(msg, addr):
+  checksum = 0
+  idh = (addr & 0xff00) >> 8
+  idl = (addr & 0xff)
+  
+  checksum = idh + idl + len(msg) + 1
+  for d_byte in msg:
+    checksum += ord(d_byte)
+  
+  #return msg + chr(checksum & 0xFF)
+  return msg + struct.pack("B", checksum & 0xFF)
+  
+    
+def make_can_msg(addr, dat, alt, cks=False):
+  if cks:
+    dat = fix(dat, addr)
+  return [addr, 0, dat, alt]
+  
+  
+def create_video_target(frame, addr):
+  counter = frame & 0xff
+  msg = struct.pack("!BBBBBBB", counter, 0x03, 0xff, 0x00, 0x00, 0x00, 0x00)
+  return make_can_msg(addr, msg, 1, True)
+
+
+def create_ipas_steer_command(steer):
+
+  """Creates a CAN message for the Toyota Steer Command."""
+  if steer < 0:
+    move = 0x60
+    steer = 0xfff + steer + 1
+  elif steer > 0:
+    move = 0x20
+  else:
+    move = 0x40
+
+  mode = 0x30 if steer else 0x10
+  
+  steer_h = (steer & 0xF00) >> 8
+  steer_l = steer & 0xff
+
+  msg = struct.pack("!BBBBBBB", mode | steer_h, steer_l, 0x10, 0x00, move, 0x40, 0x00)
+
+  return make_can_msg(0x266, msg, 0, True)
+
+def create_steer_command(steer, raw_cnt):
+  """Creates a CAN message for the Toyota Steer Command."""
+  # from 0x80 to 0xff
+  counter = ((raw_cnt & 0x3f) << 1) | 0x80
+  if steer != 0:
+    counter |= 1
+    
+  # hud
+  # 00 => Regular
+  # 40 => Actively Steering (with beep)
+  # 80 => Actively Steering (without beep) 
+  hud = 0x00
+
+  msg = struct.pack("!BhB", counter, steer, hud)
+
+  return make_can_msg(0x2e4, msg, 0, True)
+
+
+def create_accel_command(accel, pcm_cancel):
+  # TODO: find the exact canceling bit
+  state = 0xc0 + pcm_cancel # this allows automatic restart from hold without driver cmd
+
+  msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00)
+
+  return make_can_msg(0x343, msg, 0, True)
+
+
+def create_ui_command(hud1, hud2):
+  msg = struct.pack('!BBBBBBBB', 0x54, 0x04 + hud1 + (hud2 << 4), 0x0C, 
+                                 0x00, 0x00, 0x2C, 0x38, 0x02) 
+  return make_can_msg(0x412, msg, 0, False)

selfdrive/common/cereal.mk

@@ -2,27 +2,31 @@ UNAME_M ?= $(shell uname -m)
 UNAME_S ?= $(shell uname -s)
 
 
-ifeq ($(UNAME_S),Darwin)
 
 CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
-CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include
 
+ifeq ($(OPTEST),1)
+
+CEREAL_LIBS = -lcapnp -lkj
+
+else ifeq ($(UNAME_S),Darwin)
+
+CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include
 CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \
               $(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \
               $(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a
 
 else ifeq ($(UNAME_M),x86_64)
 
-CEREAL_CFLAGS = -I$(BASEDIR)/external/capnp/include
+CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
-CEREAL_CXXFLAGS = $(CEREAL_CFLAGS)
 ifeq ($(CEREAL_LIBS),)
-  CEREAL_LIBS = -L$(BASEDIR)/external/capnp/lib \
+  CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/x64/lib/ \
-	              -l:libcapnp.a -l:libkj.a -l:libcapnp_c.a
+                -L$(PHONELIBS)/capnp-c/x64/lib/ \
+                -l:libcapnp.a -l:libkj.a -l:libcapnp_c.a
 endif
 
 else
 
-CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
 CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
 ifeq ($(CEREAL_LIBS),)
   CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/aarch64/lib/ \

selfdrive/common/modeldata.h

@@ -1,8 +1,10 @@
 #ifndef MODELDATA_H
 #define MODELDATA_H
 
+#define MODEL_PATH_DISTANCE 50
+
 typedef struct PathData {
-  float points[50];
+  float points[MODEL_PATH_DISTANCE];
   float prob;
   float std;
 } PathData;

selfdrive/common/swaglog.c

@@ -57,7 +57,7 @@ static void cloudlog_init() {
   if (dongle_id) {
     cloudlog_bind_locked("dongle_id", dongle_id);
   }
-  cloudlog_bind_locked("version", OPENPILOT_VERSION);
+  cloudlog_bind_locked("version", COMMA_VERSION);
   bool dirty = !getenv("CLEAN");
   json_append_member(s.ctx_j, "dirty", json_mkbool(dirty));
 

selfdrive/common/version.h

@@ -1 +1 @@
-#define OPENPILOT_VERSION "0.3.7"
+#define COMMA_VERSION "0.3.8.2-openpilot"

selfdrive/config.py

@@ -51,7 +51,7 @@ class ImageParams:
     self.VPY = self.VPY_R + to_int(shift[1])  # current vanishing point with shift
 
 class UIParams:
-  lidar_x, lidar_y, lidar_zoom = 384, 960, 8
+  lidar_x, lidar_y, lidar_zoom = 384, 960, 6
   lidar_car_x, lidar_car_y = lidar_x/2., lidar_y/1.1
   car_hwidth = 1.7272/2 * lidar_zoom
   car_front = 2.6924 * lidar_zoom

selfdrive/controls/controlsd.py

@@ -5,7 +5,6 @@ from copy import copy
 import zmq
 from cereal import car, log
 from common.numpy_fast import clip
-from common.fingerprints import fingerprint
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.profiler import Profiler
 from common.params import Params
@@ -42,10 +41,10 @@ class Calibration:
 
 
 class State:
-  DISABLED = 0
+  DISABLED = 'disabled'
-  ENABLED = 1
+  ENABLED = 'enabled'
-  PRE_ENABLED = 2
+  PRE_ENABLED = 'preEnabled'
-  SOFT_DISABLING = 3
+  SOFT_DISABLING = 'softDisabling'
 
 
 # True when actuators are controlled
@@ -280,7 +279,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
                                       CS.steeringPressed)
 
   # *** gas/brake PID loop ***
-  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed,
+  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill,
                                               v_cruise_kph, plan.vTarget,
                                               [plan.aTargetMin, plan.aTargetMax],
                                               plan.jerkFactor, CP)
@@ -290,7 +289,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
                                CS.steeringPressed, plan.dPoly, angle_offset, VM, PL)
 
   # send a "steering required alert" if saturation count has reached the limit
-  if LaC.sat_flag:
+  if LaC.sat_flag and CP.steerLimitAlert:
     AM.add("steerSaturated", enabled)
 
   if CP.enableCruise and CS.cruiseState.enabled:
@@ -305,38 +304,40 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
 
 def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate,
               carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
-              LaC, LoC, angle_offset):
+              LaC, LoC, angle_offset, passive):
 
   # ***** control the car *****
 
   CC = car.CarControl.new_message()
 
-  CC.enabled = isEnabled(state)
+  if not passive:
 
-  CC.actuators = actuators
+    CC.enabled = isEnabled(state)
 
-  CC.cruiseControl.override = True
+    CC.actuators = actuators
-  # always cancel if we have an interceptor
-  CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
 
-  # brake discount removes a sharp nonlinearity
+    CC.cruiseControl.override = True
-  brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
+    # always cancel if we have an interceptor
-  CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
+    CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
 
-  # TODO: parametrize 0.714 in interface?
+    # brake discount removes a sharp nonlinearity
-  # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
+    brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
-  # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
+    CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
-  CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
 
-  CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
+    # TODO: parametrize 0.714 in interface?
-  CC.hudControl.speedVisible = isEnabled(state)
+    # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
-  CC.hudControl.lanesVisible = isEnabled(state)
+    # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
-  CC.hudControl.leadVisible = plan.hasLead
+    CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
-  CC.hudControl.visualAlert = AM.visual_alert
-  CC.hudControl.audibleAlert = AM.audible_alert
 
-  # send car controls over can
+    CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
-  CI.apply(CC)
+    CC.hudControl.speedVisible = isEnabled(state)
+    CC.hudControl.lanesVisible = isEnabled(state)
+    CC.hudControl.leadVisible = plan.hasLead
+    CC.hudControl.visualAlert = AM.visual_alert
+    CC.hudControl.audibleAlert = AM.audible_alert
+
+    # send car controls over can
+    CI.apply(CC)
 
   # ***** publish state to logger *****
   # publish controls state at 100Hz
@@ -358,10 +359,15 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
 
   # car state
   dat.live100.vEgo = CS.vEgo
+  dat.live100.vEgoRaw = CS.vEgoRaw
   dat.live100.angleSteers = CS.steeringAngle
   dat.live100.steerOverride = CS.steeringPressed
 
+  # high level control state
+  dat.live100.state = state
+
   # longitudinal control state
+  dat.live100.longControlState = LoC.long_control_state
   dat.live100.vPid = float(LoC.v_pid)
   dat.live100.vCruise = float(v_cruise_kph)
   dat.live100.upAccelCmd = float(LoC.pid.p)
@@ -420,13 +426,20 @@ def controlsd_thread(gctx, rate=100):
 
   context = zmq.Context()
 
+  params = Params()
+
   # pub
   live100 = messaging.pub_sock(context, service_list['live100'].port)
   carstate = messaging.pub_sock(context, service_list['carState'].port)
   carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
-  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
   livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
 
+  passive = params.get("Passive") != "0"
+  if not passive:
+    sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
+  else:
+    sendcan = None
+
   # sub
   thermal = messaging.sub_sock(context, service_list['thermal'].port)
   health = messaging.sub_sock(context, service_list['health'].port)
@@ -434,15 +447,28 @@ def controlsd_thread(gctx, rate=100):
   logcan = messaging.sub_sock(context, service_list['can'].port)
 
   CC = car.CarControl.new_message()
-  CI, CP = get_car(logcan, sendcan)
+
+  CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
+
+  if CI is None:
+    if passive:
+      return
+    else:
+      raise Exception("unsupported car")
+
+  if passive:
+    CP.safetyModel = car.CarParams.SafetyModels.noOutput
+
   PL = Planner(CP)
   LoC = LongControl(CI.compute_gb)
   VM = VehicleModel(CP)
   LaC = LatControl(VM)
   AM = AlertManager()
 
+  if not passive:
+    AM.add("startup", False)
+
   # write CarParams
-  params = Params()
   params.put("CarParams", CP.to_bytes())
 
   state = State.DISABLED
@@ -484,9 +510,10 @@ def controlsd_thread(gctx, rate=100):
     plan, plan_ts = calc_plan(CS, events, PL, LoC)
     prof.checkpoint("Plan")
 
-    # update control state
+    if not passive:
-    state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
+      # update control state
-    prof.checkpoint("State transition")
+      state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
+      prof.checkpoint("State transition")
 
     # compute actuators
     actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
@@ -497,7 +524,7 @@ def controlsd_thread(gctx, rate=100):
     # publish data
     CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
                    rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
-                   rear_view_toggle, awareness_status, LaC, LoC, angle_offset)
+                   rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive)
     prof.checkpoint("Sent")
 
     # *** run loop at fixed rate ***

selfdrive/controls/lib/alertmanager.py

@@ -59,9 +59,9 @@ class AlertManager(object):
         PT.MID, None, "beepSingle", .2, 0., 0.),
 
     "fcw": Alert(
-        "", 
+        "Brake", 
-        "", 
+        "Risk of Collision", 
-        PT.LOW, None, None, .1, .1, .1),
+        PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
 
     "steerSaturated": Alert(
         "Take Control", 
@@ -74,7 +74,7 @@ class AlertManager(object):
         PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
 
     "preDriverDistracted": Alert(
-        "Take Control ", 
+        "Take Control", 
         "User Distracted", 
         PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
 
@@ -98,6 +98,11 @@ class AlertManager(object):
         "Steer Temporary Unavailable", 
         PT.LOW, None, "chimeDouble", .4, 0., 3.),
 
+    "manualRestart": Alert(
+        "Take Control",
+        "Resume Driving Manually",
+        PT.LOW, None, None, .0, 0., 1.),
+
     # Non-entry only alerts
     "wrongCarModeNoEntry": Alert(
         "Comma Unavailable", 
@@ -134,6 +139,11 @@ class AlertManager(object):
         "Park Brake Engaged", 
         PT.LOW, None, "chimeDouble", .4, 2., 3.),
 
+    "lowSpeedLockoutNoEntry": Alert(
+        "Comma Unavailable",
+        "Cruise Fault: Restart the Car",
+        PT.LOW, None, "chimeDouble", .4, 2., 3.),
+
     # Cancellation alerts causing disabling
     "overheat": Alert(
         "Take Control Immediately", 
@@ -231,6 +241,11 @@ class AlertManager(object):
         "No Close Lead", 
         PT.HIGH, None, "chimeDouble", .4, 2., 3.),
 
+    "speedTooLow": Alert(
+        "Comma Canceled",
+        "Speed Too Low", 
+        PT.HIGH, None, "chimeDouble", .4, 2., 3.),
+
     # Cancellation alerts causing non-entry
     "overheatNoEntry": Alert(
         "Comma Unavailable", 
@@ -331,7 +346,6 @@ class AlertManager(object):
   def __init__(self):
     self.activealerts = []
     self.current_alert = None
-    self.add("startup", False)
 
   def alertPresent(self):
     return len(self.activealerts) > 0

selfdrive/controls/lib/fcw.py

@@ -8,13 +8,13 @@ def calc_ttc(l1):
   # if l1 is None, return max ttc immediately
   if not l1:
     return MAX_TTC
-  # this function returns the time to collision (ttc), assuming that 
+  # this function returns the time to collision (ttc), assuming that
   # ARel will stay constant TODO: review this assumptions
   # change sign to rel quantities as it's going to be easier for calculations
   vRel = -l1.vRel
   aRel = -l1.aRel
 
-  # assuming that closing gap ARel comes from lead vehicle decel, 
+  # assuming that closing gap ARel comes from lead vehicle decel,
   # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
   # This helps underweighting ARel when v_lead is close to zero.
   t_decel = 2.
@@ -44,10 +44,10 @@ class ForwardCollisionWarning(object):
     a_acc_on  = -2.0         # with system on, above this limit of desired decel, we should trigger fcw
     a_acc_off = -2.5         # with system off, above this limit of desired decel, we should trigger fcw
     ttc_thrs = 2.5           # ttc threshold for fcw
-    v_fcw_min = 2.           # no fcw below 2m/s
+    v_fcw_min = 5.           # no fcw below 5m/s
     steer_angle_th = 40.     # deg, no fcw above this steer angle
     cur_time = sec_since_boot()
- 
+
     ttc = calc_ttc(AC.l1)
     a_fcw = a_acc_on if CS.cruiseState.enabled else a_acc_off
 
@@ -59,8 +59,8 @@ class ForwardCollisionWarning(object):
       self.violation_time = np.minimum(self.violation_time + self.dt, violation_thrs)
     else:
       self.violation_time = np.maximum(self.violation_time - 2*self.dt, 0)
- 
+
-    # fire FCW  
+    # fire FCW
     self.active = self.violation_time >= violation_thrs and cur_time > (self.last_active + fcw_t_delta)
     if self.active:
       self.last_active = cur_time

selfdrive/controls/lib/latcontrol.py

@@ -20,7 +20,7 @@ def get_steer_max(CP, v_ego):
 
 class LatControl(object):
   def __init__(self, VM):
-    self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, pos_limit=1.0)
+    self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
     self.setup_mpc()
 
     self.y_des = -1  # Legacy
@@ -57,14 +57,15 @@ class LatControl(object):
       # account for actuation delay
       self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR)
 
+      v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
       self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
                           l_poly, r_poly, p_poly,
-                          PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego, PL.PP.lane_width)
+                          PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width)
 
       delta_desired = self.mpc_solution[0].delta[1]
       self.cur_state[0].delta = delta_desired
 
-      self.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset
+      self.angle_steers_des = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset)
       self.mpc_updated = True
 
     if v_ego < 0.3 or not active:
@@ -74,7 +75,8 @@ class LatControl(object):
       steer_max = get_steer_max(VM.CP, v_ego)
       self.pid.pos_limit = steer_max
       self.pid.neg_limit = -steer_max
-      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override)
+      steer_feedforward = self.angle_steers_des * v_ego**2  # proportional to realigning tire momentum (~ lateral accel)
+      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward)
 
     self.sat_flag = self.pid.saturated
     return output_steer

selfdrive/controls/lib/latcontrol_helpers.py

@@ -50,9 +50,9 @@ def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
   return steer_des, curvature
 
 
-def compute_path_pinv():
+def compute_path_pinv(l=50):
   deg = 3
-  x = np.arange(50.0)
+  x = np.arange(l*1.0)
   X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
   pinv = np.linalg.pinv(X)
   return pinv

selfdrive/controls/lib/lateral_mpc/Makefile

@@ -1,3 +1,4 @@
+
 CC = clang
 CXX = clang++
 
@@ -8,7 +9,7 @@ UNAME_M := $(shell uname -m)
 CFLAGS = -O3 -fPIC -I.
 CXXFLAGS = -O3 -fPIC -I.
 
-QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC 
+QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC
 
 ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
 
@@ -19,17 +20,17 @@ ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_ca
 endif
 
 OBJS = \
-  $(PHONELIBS)/qpoases/SRC/Bounds.o \
+  qp/Bounds.o \
-  $(PHONELIBS)/qpoases/SRC/Constraints.o \
+  qp/Constraints.o \
-  $(PHONELIBS)/qpoases/SRC/CyclingManager.o \
+  qp/CyclingManager.o \
-  $(PHONELIBS)/qpoases/SRC/Indexlist.o \
+  qp/Indexlist.o \
-  $(PHONELIBS)/qpoases/SRC/MessageHandling.o \
+  qp/MessageHandling.o \
-  $(PHONELIBS)/qpoases/SRC/QProblem.o \
+  qp/QProblem.o \
-  $(PHONELIBS)/qpoases/SRC/QProblemB.o \
+  qp/QProblemB.o \
-  $(PHONELIBS)/qpoases/SRC/SubjectTo.o \
+  qp/SubjectTo.o \
-  $(PHONELIBS)/qpoases/SRC/Utils.o \
+  qp/Utils.o \
-  $(PHONELIBS)/qpoases/SRC/EXTRAS/SolutionAnalysis.o \
+  qp/EXTRAS/SolutionAnalysis.o \
-  mpc_export/acado_qpoases_interface.o \
+	mpc_export/acado_qpoases_interface.o \
   mpc_export/acado_integrator.o \
   mpc_export/acado_solver.o \
   mpc_export/acado_auxiliary_functions.o \
@@ -43,6 +44,23 @@ all: libcommampc.so
 libcommampc.so: $(OBJS)
 	$(CXX) -shared -o '$@' $^ -lm
 
+
+qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
+	@echo "[ CXX ] $@"
+	mkdir -p qp
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I mpc_export/ \
+           $(QPOASES_FLAGS) \
+           -c -o '$@' '$<'
+
+qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp
+	@echo "[ CXX ] $@"
+	mkdir -p qp/EXTRAS
+	$(CXX) $(CXXFLAGS) -MMD \
+           -I mpc_export/ \
+           $(QPOASES_FLAGS) \
+           -c -o '$@' '$<'
+
 %.o: %.cpp
 	@echo "[ CXX ] $@"
 	$(CXX) $(CXXFLAGS) -MMD \

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -79,7 +79,7 @@ int main( )
 
   Q(3,3) = 1.0;
 
-  Q(4,4) = 1.0;
+  Q(4,4) = 0.5;
 
   // Terminal cost
   Function hN;
@@ -118,7 +118,8 @@ int main( )
   mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
   mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
   mpc.set( INTEGRATOR_TYPE, INT_RK4 );
-  mpc.set( NUM_INTEGRATOR_STEPS, 250 );
+  mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 500);
 
   mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
   mpc.set( QP_SOLVER, QP_QPOASES );

selfdrive/controls/lib/lateral_mpc/libmpc_py.py

@@ -4,7 +4,6 @@ import subprocess
 from cffi import FFI
 
 mpc_dir = os.path.dirname(os.path.abspath(__file__))
-
 libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
 subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
 
@@ -22,7 +21,7 @@ typedef struct {
 } log_t;
 
 void init();
-void run_mpc(state_t * x0, log_t * solution,
+int run_mpc(state_t * x0, log_t * solution,
              double l_poly[4], double r_poly[4], double p_poly[4],
              double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);
 """)

selfdrive/controls/lib/lateral_mpc/mpc.c

@@ -44,7 +44,7 @@ void init(){
   for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
 }
 
-void run_mpc(state_t * x0, log_t * solution,
+int run_mpc(state_t * x0, log_t * solution,
              double l_poly[4], double r_poly[4], double p_poly[4],
              double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){
 
@@ -84,10 +84,8 @@ void run_mpc(state_t * x0, log_t * solution,
 
 
   acado_preparationStep();
-  acado_feedbackStep( );
+  acado_feedbackStep();
-
+  /* printf("lat its: %d\n", acado_getNWSR()); */
-  acado_shiftStates(2, 0, 0);
-  acado_shiftControls( 0 );
 
 	for (i = 0; i <= N; i++){
 		solution->x[i] = acadoVariables.x[i*NX];
@@ -96,5 +94,9 @@ void run_mpc(state_t * x0, log_t * solution,
 		solution->delta[i] = acadoVariables.x[i*NX+3];
 	}
 
-  return;
+  acado_shiftStates(2, 0, 0);
+  acado_shiftControls( 0 );
+
+
+  return acado_getNWSR();
 }

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h

@@ -82,7 +82,7 @@ extern "C"
 /** Total number of QP optimization variables. */
 #define ACADO_QP_NV 54
 /** Number of integration steps per shooting interval. */
-#define ACADO_RK_NIS 5
+#define ACADO_RK_NIS 1
 /** Number of Runge-Kutta stages per integration step. */
 #define ACADO_RK_NSTAGES 4
 /** Providing interface for arrival cost. */

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c

@@ -71,7 +71,7 @@ out[22] = ((od[1]*xd[23])*od[0]);
 out[23] = (real_t)(1.0000000000000000e+00);
 }
 
-/* Fixed step size:0.01 */
+/* Fixed step size:0.05 */
 int acado_integrate( real_t* const rk_eta, int resetIntegrator )
 {
 int error;
@@ -118,7 +118,7 @@ acadoWorkspace.rk_xxx[40] = rk_eta[40];
 acadoWorkspace.rk_xxx[41] = rk_eta[41];
 acadoWorkspace.rk_xxx[42] = rk_eta[42];
 
-for (run1 = 0; run1 < 5; ++run1)
+for (run1 = 0; run1 < 1; ++run1)
 {
 acadoWorkspace.rk_xxx[0] = + rk_eta[0];
 acadoWorkspace.rk_xxx[1] = + rk_eta[1];
@@ -145,106 +145,106 @@ acadoWorkspace.rk_xxx[21] = + rk_eta[21];
 acadoWorkspace.rk_xxx[22] = + rk_eta[22];
 acadoWorkspace.rk_xxx[23] = + rk_eta[23];
 acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
-acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[0] + rk_eta[0];
+acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[1] + rk_eta[1];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[2] + rk_eta[2];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[3] + rk_eta[3];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[4] + rk_eta[4];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[5] + rk_eta[5];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[6] + rk_eta[6];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[7] + rk_eta[7];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[8] + rk_eta[8];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[9] + rk_eta[9];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[10] + rk_eta[10];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[11] + rk_eta[11];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[12] + rk_eta[12];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[13] + rk_eta[13];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[14] + rk_eta[14];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[15] + rk_eta[15];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[16] + rk_eta[16];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[17] + rk_eta[17];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[18] + rk_eta[18];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[19] + rk_eta[19];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[20] + rk_eta[20];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[21] + rk_eta[21];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[22] + rk_eta[22];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[23] + rk_eta[23];
+acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
 acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[24] + rk_eta[0];
+acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[25] + rk_eta[1];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[26] + rk_eta[2];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[27] + rk_eta[3];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[28] + rk_eta[4];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[29] + rk_eta[5];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[30] + rk_eta[6];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[31] + rk_eta[7];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[32] + rk_eta[8];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[33] + rk_eta[9];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[34] + rk_eta[10];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[35] + rk_eta[11];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[36] + rk_eta[12];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[37] + rk_eta[13];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[38] + rk_eta[14];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[39] + rk_eta[15];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[40] + rk_eta[16];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[41] + rk_eta[17];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[42] + rk_eta[18];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[43] + rk_eta[19];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[44] + rk_eta[20];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[45] + rk_eta[21];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[46] + rk_eta[22];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[47] + rk_eta[23];
+acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23];
 acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
+acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
+acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
+acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
+acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
+acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
+acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
+acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
+acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
+acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
+acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
+acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
+acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
+acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
+acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
+acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
+acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
+acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
+acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
+acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
+acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
+acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
+acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
+acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
+acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
 acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) );
-rk_eta[0] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[48] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[72];
+rk_eta[0] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[72];
-rk_eta[1] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[49] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[73];
+rk_eta[1] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[73];
-rk_eta[2] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[50] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[74];
+rk_eta[2] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[74];
-rk_eta[3] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[51] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[75];
+rk_eta[3] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[75];
-rk_eta[4] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[52] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[76];
+rk_eta[4] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[76];
-rk_eta[5] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[53] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[77];
+rk_eta[5] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[77];
-rk_eta[6] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[54] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[78];
+rk_eta[6] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[78];
-rk_eta[7] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[55] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[79];
+rk_eta[7] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[79];
-rk_eta[8] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[80];
+rk_eta[8] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[80];
-rk_eta[9] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[81];
+rk_eta[9] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[81];
-rk_eta[10] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[82];
+rk_eta[10] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[82];
-rk_eta[11] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[83];
+rk_eta[11] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[83];
-rk_eta[12] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[84];
+rk_eta[12] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[84];
-rk_eta[13] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[85];
+rk_eta[13] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[85];
-rk_eta[14] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[86];
+rk_eta[14] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[86];
-rk_eta[15] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[87];
+rk_eta[15] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[87];
-rk_eta[16] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[88];
+rk_eta[16] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[88];
-rk_eta[17] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[89];
+rk_eta[17] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[89];
-rk_eta[18] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[90];
+rk_eta[18] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[90];
-rk_eta[19] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[91];
+rk_eta[19] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[91];
-rk_eta[20] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[92];
+rk_eta[20] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[92];
-rk_eta[21] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[93];
+rk_eta[21] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[93];
-rk_eta[22] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[94];
+rk_eta[22] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[94];
-rk_eta[23] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[95];
+rk_eta[23] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[95];
-acadoWorkspace.rk_ttt += 2.0000000000000001e-01;
+acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
 }
 error = 0;
 return error;

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp

@@ -41,7 +41,7 @@
 /** Maximum number of constraints. */
 #define QPOASES_NCMAX      100
 /** Maximum number of working set recalculations. */
-#define QPOASES_NWSRMAX    462
+#define QPOASES_NWSRMAX    500
 /** Print level for qpOASES. */
 #define QPOASES_PRINTLEVEL PL_NONE
 /** The value of EPS */

selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c

@@ -213,22 +213,22 @@ tmpQ2[0] = + tmpFx[0];
 tmpQ2[1] = + tmpFx[4];
 tmpQ2[2] = + tmpFx[8];
 tmpQ2[3] = + tmpFx[12];
-tmpQ2[4] = + tmpFx[16];
+tmpQ2[4] = + tmpFx[16]*(real_t)5.0000000000000000e-01;
 tmpQ2[5] = + tmpFx[1];
 tmpQ2[6] = + tmpFx[5];
 tmpQ2[7] = + tmpFx[9];
 tmpQ2[8] = + tmpFx[13];
-tmpQ2[9] = + tmpFx[17];
+tmpQ2[9] = + tmpFx[17]*(real_t)5.0000000000000000e-01;
 tmpQ2[10] = + tmpFx[2];
 tmpQ2[11] = + tmpFx[6];
 tmpQ2[12] = + tmpFx[10];
 tmpQ2[13] = + tmpFx[14];
-tmpQ2[14] = + tmpFx[18];
+tmpQ2[14] = + tmpFx[18]*(real_t)5.0000000000000000e-01;
 tmpQ2[15] = + tmpFx[3];
 tmpQ2[16] = + tmpFx[7];
 tmpQ2[17] = + tmpFx[11];
 tmpQ2[18] = + tmpFx[15];
-tmpQ2[19] = + tmpFx[19];
+tmpQ2[19] = + tmpFx[19]*(real_t)5.0000000000000000e-01;
 tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16];
 tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17];
 tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18];
@@ -253,7 +253,7 @@ tmpR2[0] = + tmpFu[0];
 tmpR2[1] = + tmpFu[1];
 tmpR2[2] = + tmpFu[2];
 tmpR2[3] = + tmpFu[3];
-tmpR2[4] = + tmpFu[4];
+tmpR2[4] = + tmpFu[4]*(real_t)5.0000000000000000e-01;
 tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4];
 }
 
@@ -1965,7 +1965,7 @@ tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5];
 tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1];
 tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2];
 tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3];
-tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4];
+tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]*(real_t)5.0000000000000000e-01;
 objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4];
 }
 

selfdrive/controls/lib/longcontrol.py

@@ -11,11 +11,11 @@ BRAKE_THRESHOLD_TO_PID = 0.2
 
 class LongCtrlState:
   #*** this function handles the long control state transitions
-  # long_control_state labels:
+  # long_control_state labels (see capnp enum):
-  off = 0  # Off
+  off = 'off'  # Off
-  pid = 1  # moving and tracking targets, with PID control running
+  pid = 'pid'  # moving and tracking targets, with PID control running
-  stopping = 2  # stopping and changing controls to almost open loop as PID does not fit well at such a low speed
+  stopping = 'stopping'  # stopping and changing controls to almost open loop as PID does not fit well at such a low speed
-  starting = 3  # starting and releasing brakes in open loop before giving back to PID
+  starting = 'starting'  # starting and releasing brakes in open loop before giving back to PID
 
 
 def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
@@ -57,7 +57,7 @@ _KI_BP = [0., 35.]
 _KI_V = [0.18, 0.12]
 
 stopping_brake_rate = 0.2  # brake_travel/s while trying to stop
-starting_brake_rate = 0.6  # brake_travel/s while releasing on restart
+starting_brake_rate = 0.8  # 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
 
@@ -80,7 +80,7 @@ class LongControl(object):
     self.pid.reset()
     self.v_pid = v_pid
 
-  def update(self, active, v_ego, brake_pressed, v_cruise, v_target_lead, a_target,
+  def update(self, active, v_ego, brake_pressed, standstill, v_cruise, v_target_lead, a_target,
              jerk_factor, CP):
 
     # actuation limits
@@ -100,9 +100,11 @@ class LongControl(object):
     self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\
                                                        v_target, self.v_pid, output_gb, brake_pressed)
 
+    v_ego_pid = max(v_ego, 0.3)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
+
     # *** long control behavior based on state
     if self.long_control_state == LongCtrlState.off:
-      self.v_pid = v_ego  # do nothing
+      self.v_pid = v_ego_pid  # do nothing
       output_gb = 0.
       self.pid.reset()
 
@@ -129,13 +131,12 @@ class LongControl(object):
 
       self.pid.pos_limit = gas_max
       self.pid.neg_limit = - brake_max
-      v_ego_pid = max(v_ego, 0.3)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
       output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor)
 
     # intention is to stop, switch to a different brake control until we stop
     elif self.long_control_state == LongCtrlState.stopping:
       # TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego
-      if v_ego > 0. or output_gb > -brake_stopping_target:
+      if not standstill or output_gb > -brake_stopping_target:
         output_gb -= stopping_brake_rate / rate
       output_gb = clip(output_gb, -brake_max, gas_max)
 

selfdrive/controls/lib/pid.py

@@ -3,9 +3,10 @@ from common.numpy_fast import clip, interp
 import numbers
 
 class PIController(object):
-  def __init__(self, k_p, k_i, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
+  def __init__(self, k_p, k_i, k_f=0., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
-    self._k_p = k_p
+    self._k_p = k_p # proportional gain
-    self._k_i = k_i
+    self._k_i = k_i # integrale gain
+    self.k_f = k_f  # feedforward gain
 
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
@@ -56,18 +57,19 @@ class PIController(object):
     self.saturated = False
     self.control = 0
 
-  def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False):
+  def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False, feedforward=0.):
     self.speed = speed
     self.jerk_factor = jerk_factor
 
     error = float(setpoint - measurement)
     self.p = error * self.k_p
+    f = feedforward * self.k_f
 
     if override:
       self.i -= self.i_unwind_rate * float(np.sign(self.i))
     else:
       i = self.i + error * self.k_i * self.i_rate
-      control = self.p + i
+      control = self.p + f + i
 
       if self.convert is not None:
         control = self.convert(control, speed=self.speed)
@@ -78,7 +80,7 @@ class PIController(object):
          (error <= 0 and (control >= self.neg_limit or i > 0.0)):
         self.i = i
 
-    control = self.p + self.i
+    control = self.p + f + self.i
     if self.convert is not None:
       control = self.convert(control, speed=self.speed)
 

selfdrive/controls/lib/radar_helpers.py

@@ -95,13 +95,15 @@ class Track(object):
     # rel speed is very hard to estimate from vision
     if dist_to_vision < 4.0 and rel_speed_diff < 10.:
       # vision point is never stationary
-      self.stationary = False
-      self.vision = True
       self.vision_cnt += 1
+      # don't trust 1 or 2 fusions until model quality is much better
+      if self.vision_cnt >= 3:
+        self.vision = True
+        self.stationary = False
 
   def get_key_for_cluster(self):
     # Weigh y higher since radar is inaccurate in this dimension
-    return [self.dRel, self.dPath*2, self.vRel]
+    return [self.dRel, self.yRel*2, self.vRel]
 
 # ******************* Cluster *******************
 
@@ -208,7 +210,7 @@ class Cluster(object):
     lead.fcw = self.is_potential_fcw()
 
   def __str__(self):
-    ret = "x: %7.2f  y: %7.2f  v: %7.2f  a: %7.2f" % (self.dRel, self.yRel, self.vRel, self.aRel)
+    ret = "x: %4.1f  y: %4.1f  v: %4.1f  a: %4.1f  d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aRel, self.dPath)
     if self.stationary:
       ret += " stationary"
     if self.vision:

selfdrive/controls/radard.py

@@ -18,6 +18,7 @@ from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
 from common.kalman.ekf import EKF, SimpleSensor
 
 VISION_ONLY = False
+DEBUG = False
 
 #vision point
 DIMSV = 2
@@ -76,7 +77,6 @@ def radard_thread(gctx=None):
   # Time-alignment
   rate = 20.   # model and radar are both at 20Hz
   tsv = 1./rate
-  rdr_delay = 0.10   # radar data delay in s
   v_len = 20         # how many speed data points to remember for t alignment with rdr data
 
   enabled = 0
@@ -158,7 +158,7 @@ def radard_thread(gctx=None):
 
       # align v_ego by a fixed time to align it with the radar measurement
       cur_time = float(rk.frame)/rate
-      v_ego_t_aligned = np.interp(cur_time - rdr_delay, v_ego_array[1], v_ego_array[0])
+      v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
       d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
 
       # create the track if it doesn't exist or it's a new track
@@ -175,10 +175,17 @@ def radard_thread(gctx=None):
     # publish tracks (debugging)
     dat = messaging.new_message()
     dat.init('liveTracks', len(tracks))
-    #print "NEW TRACKS"
+
+    if DEBUG:
+      print "NEW CYCLE"
+      if VISION_POINT in ar_pts:
+        print "vision", ar_pts[VISION_POINT]
+
     for cnt, ids in enumerate(tracks.keys()):
-      #print "%5s %5s %5s %5s" % \
+      if DEBUG:
-      #  (ids, round(tracks[ids].dRel, 2), round(tracks[ids].vRel, 2), round(tracks[ids].yRel, 2))
+        print "id: %4.0f x:  %4.1f  y: %4.1f  v: %4.1f  d: %4.1f s: %1.0f" % \
+          (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
+           tracks[ids].dPath, tracks[ids].stationary)
       dat.liveTracks[cnt].trackId = ids
       dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
       dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
@@ -210,6 +217,9 @@ def radard_thread(gctx=None):
     else:
       clusters = []
 
+    if DEBUG:
+      for i in clusters:
+        print i
     # *** extract the lead car ***
     lead_clusters = [c for c in clusters
                      if c.is_potential_lead(v_ego)]

selfdrive/debug/dump.py

@@ -2,6 +2,7 @@
 import sys
 import argparse
 import zmq
+import json
 from hexdump import hexdump
 
 import selfdrive.messaging as messaging
@@ -13,11 +14,19 @@ if __name__ == "__main__":
 
   parser = argparse.ArgumentParser(description='Sniff a communcation socket')
   parser.add_argument('--raw', action='store_true')
+  parser.add_argument('--json', action='store_true')
+  parser.add_argument('--addr', default='127.0.0.1')
   parser.add_argument("socket", type=str, nargs='*', help="socket name")
   args = parser.parse_args()
 
   for m in args.socket if len(args.socket) > 0 else service_list:
-    messaging.sub_sock(context, service_list[m].port, poller)
+    if m in service_list:
+      messaging.sub_sock(context, service_list[m].port, poller, addr=args.addr)
+    elif m.isdigit():
+      messaging.sub_sock(context, int(m), poller, addr=args.addr)
+    else:
+      print("service not found")
+      exit(-1)
 
   while 1:
     polld = poller.poll(timeout=1000)
@@ -26,6 +35,8 @@ if __name__ == "__main__":
         continue
       if args.raw:
         hexdump(sock.recv())
+      elif args.json:
+        print(json.loads(sock.recv()))
       else:
         print messaging.recv_one(sock)
 

selfdrive/logcatd/Makefile

@@ -39,7 +39,7 @@ logcatd: $(OBJS)
 	@echo "[ CXX ] $@"
 	$(CXX) $(CXXFLAGS) \
            -I$(PHONELIBS)/android_system_core/include \
-           $(CEREAL_CFLAGS) \
+           $(CEREAL_CXXFLAGS) \
            $(ZMQ_FLAGS) \
            -I../ \
            -I../../ \

selfdrive/manager.py

@@ -1,36 +1,67 @@
 #!/usr/bin/env python
 import os
+import sys
 import time
+import fcntl
+import errno
+import signal
 
-if os.path.isfile("/init.qcom.rc"):
+if __name__ == "__main__":
-  # check if NEOS update is required
+  if os.path.isfile("/init.qcom.rc"):
-  while 1:
+    # check if NEOS update is required
-    if ((not os.path.isfile("/VERSION")
+    while ((not os.path.isfile("/VERSION")
-        or int(open("/VERSION").read()) < 3)
+            or int(open("/VERSION").read()) < 3)
-        and not os.path.isfile("/data/media/0/noupdate")):
+            and not os.path.isfile("/data/media/0/noupdate")):
-      os.system("curl -o /tmp/updater https://openpilot.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater")
+      os.system("curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater")
-    else:
+      time.sleep(10)
-      break
+
-    time.sleep(10)
+  # get a non-blocking stdout
+  child_pid, child_pty = os.forkpty()
+  if child_pid != 0: # parent
+
+    # child is in its own process group, manually pass kill signals
+    signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT))
+    signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM))
+
+    fcntl.fcntl(sys.stdout, fcntl.F_SETFL,
+       fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK)
+
+    while True:
+      try:
+        dat = os.read(child_pty, 4096)
+      except OSError as e:
+        if e.errno == errno.EIO:
+          break
+        continue
+
+      if not dat:
+        break
+
+      try:
+        sys.stdout.write(dat)
+      except (OSError, IOError):
+        pass
+
+    os._exit(os.wait()[1])
 
-import sys
+import hashlib
 import importlib
 import subprocess
-import signal
 import traceback
 from multiprocessing import Process
-from common.basedir import BASEDIR
 
+from common.basedir import BASEDIR
 sys.path.append(os.path.join(BASEDIR, "pyextra"))
 os.environ['BASEDIR'] = BASEDIR
 
 import usb1
-import hashlib
 import zmq
 from setproctitle import setproctitle
 
-from selfdrive.services import service_list
+from common.params import Params
+from common.realtime import sec_since_boot
 
+from selfdrive.services import service_list
 from selfdrive.swaglog import cloudlog
 import selfdrive.messaging as messaging
 from selfdrive.thermal import read_thermal
@@ -38,8 +69,6 @@ from selfdrive.registration import register
 from selfdrive.version import version
 import selfdrive.crash as crash
 
-from common.params import Params
-
 from selfdrive.loggerd.config import ROOT
 
 # comment out anything you don't want to run
@@ -52,10 +81,14 @@ managed_processes = {
   "tombstoned": "selfdrive.tombstoned",
   "logcatd": ("selfdrive/logcatd", ["./logcatd"]),
   "proclogd": ("selfdrive/proclogd", ["./proclogd"]),
-  "boardd": ("selfdrive/boardd", ["./boardd"]),   # switch to c++ boardd
+  "boardd": ("selfdrive/boardd", ["./boardd"]),   # not used directly
+  "pandad": "selfdrive.pandad",
   "ui": ("selfdrive/ui", ["./ui"]),
   "visiond": ("selfdrive/visiond", ["./visiond"]),
-  "sensord": ("selfdrive/sensord", ["./sensord"]), }
+  "sensord": ("selfdrive/sensord", ["./sensord"]),
+  "gpsd": ("selfdrive/sensord", ["./gpsd"]),
+  "updated": "selfdrive.updated",
+}
 
 running = {}
 def get_running():
@@ -67,6 +100,16 @@ unkillable_processes = ['visiond']
 # processes to end with SIGINT instead of SIGTERM
 interrupt_processes = []
 
+persistent_processes = [
+  'logmessaged',
+  'logcatd',
+  'tombstoned',
+  'uploader',
+  'ui',
+  'gpsd',
+  'updated',
+]
+
 car_started_processes = [
   'controlsd',
   'loggerd',
@@ -77,11 +120,13 @@ car_started_processes = [
 ]
 
 def register_managed_process(name, desc, car_started=False):
-  global managed_processes, car_started_processes
+  global managed_processes, car_started_processes, persistent_processes
   print "registering", name
   managed_processes[name] = desc
   if car_started:
     car_started_processes.append(name)
+  else:
+    persistent_processes.append(name)
 
 # ****************** process management functions ******************
 def launcher(proc, gctx):
@@ -95,7 +140,7 @@ def launcher(proc, gctx):
     # exec the process
     mod.main(gctx)
   except KeyboardInterrupt:
-    cloudlog.info("child %s got ctrl-c" % proc)
+    cloudlog.warning("child %s got SIGINT" % proc)
   except Exception:
     # can't install the crash handler becuase sys.excepthook doesn't play nice
     # with threads, so catch it here.
@@ -138,7 +183,7 @@ def prepare_managed_process(p):
       subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0]))
     except subprocess.CalledProcessError:
       # make clean if the build failed
-      cloudlog.info("building %s failed, make clean" % (proc, ))
+      cloudlog.warning("building %s failed, make clean" % (proc, ))
       subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0]))
       subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0]))
 
@@ -161,7 +206,7 @@ def kill_managed_process(name):
         running[name].join(15.0)
         if running[name].exitcode is None:
           cloudlog.critical("FORCE REBOOTING PHONE!")
-          os.system("date > /sdcard/unkillable_reboot")
+          os.system("date >> /sdcard/unkillable_reboot")
           os.system("reboot")
           raise RuntimeError
       else:
@@ -191,6 +236,7 @@ def manage_baseui(start):
     os.system("am force-stop com.baseui")
     baseui_running = False
 
+
 # ****************** run loop ******************
 
 def manager_init():
@@ -292,6 +338,36 @@ def handle_fan(max_temp, fan_speed):
 
   return fan_speed
 
+class LocationStarter(object):
+  def __init__(self):
+    self.last_good_loc = 0
+  def update(self, started_ts, location):
+    rt = sec_since_boot()
+
+    if location is None or location.accuracy > 50 or location.speed < 2:
+      # bad location, stop if we havent gotten a location in a while
+      # dont stop if we're been going for less than a minute
+      if started_ts:
+        if rt-self.last_good_loc > 60. and rt-started_ts > 60:
+          cloudlog.event("location_stop",
+            ts=rt,
+            started_ts=started_ts,
+            last_good_loc=self.last_good_loc,
+            location=location.to_dict() if location else None)
+          return False
+        else:
+          return True
+      else:
+        return False
+
+    self.last_good_loc = rt
+
+    if started_ts:
+      return True
+    else:
+      cloudlog.event("location_start", location=location.to_dict() if location else None)
+      return location.speed*3.6 > 10
+
 def manager_thread():
   global baseui_running
 
@@ -299,37 +375,40 @@ def manager_thread():
   context = zmq.Context()
   thermal_sock = messaging.pub_sock(context, service_list['thermal'].port)
   health_sock = messaging.sub_sock(context, service_list['health'].port)
+  location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port)
 
   cloudlog.info("manager start")
-  cloudlog.info(dict(os.environ))
+  cloudlog.info({"environ": os.environ})
+
+  for p in persistent_processes:
+    start_managed_process(p)
 
-  start_managed_process("logmessaged")
-  start_managed_process("logcatd")
-  start_managed_process("tombstoned")
-  start_managed_process("uploader")
-  start_managed_process("ui")
   manage_baseui(True)
 
   # do this before panda flashing
   setup_eon_fan()
 
   if os.getenv("NOBOARD") is None:
-    from panda import ensure_st_up_to_date
+    start_managed_process("pandad")
-    ensure_st_up_to_date()
+
-    start_managed_process("boardd")
+  passive = bool(os.getenv("PASSIVE"))
+  passive_starter = LocationStarter()
 
-  started = False
+  started_ts = None
   logger_dead = False
   count = 0
   fan_speed = 0
+  ignition_seen = False
 
-  # set 5 second timeout on health socket
+  health_sock.RCVTIMEO = 1500
-  # 5x slower than expected
-  health_sock.RCVTIMEO = 5000
 
   while 1:
     # get health of board, log this in "thermal"
     td = messaging.recv_sock(health_sock, wait=True)
+    location = messaging.recv_sock(location_sock)
+
+    location = location.gpsLocation if location else None
+
     print td
 
     # replace thermald
@@ -357,7 +436,7 @@ def manager_thread():
 
     # uploader is gated based on the phone temperature
     if max_temp > 85.0:
-      cloudlog.info("over temp: %r", max_temp)
+      cloudlog.warning("over temp: %r", max_temp)
       kill_managed_process("uploader")
     elif max_temp < 70.0:
       start_managed_process("uploader")
@@ -366,11 +445,23 @@ def manager_thread():
       logger_dead = True
 
     # start constellation of processes when the car starts
+    ignition = td is not None and td.health.started
+    ignition_seen = ignition_seen or ignition
+
+    should_start = ignition
+
+    # start on gps in passive mode
+    if passive and not ignition_seen:
+      should_start = should_start or passive_starter.update(started_ts, location)
+
     # with 2% left, we killall, otherwise the phone is bricked
-    if td is not None and td.health.started and avail > 0.02:
+    should_start = should_start and avail > 0.02
-      if not started:
+
+
+    if should_start:
+      if not started_ts:
         Params().car_start()
-        started = True
+        started_ts = sec_since_boot()
       for p in car_started_processes:
         if p == "loggerd" and logger_dead:
           kill_managed_process(p)
@@ -379,7 +470,7 @@ def manager_thread():
       manage_baseui(False)
     else:
       manage_baseui(True)
-      started = False
+      started_ts = None
       logger_dead = False
       for p in car_started_processes:
         kill_managed_process(p)
@@ -416,9 +507,10 @@ def get_installed_apks():
 
 # optional, build the c++ binaries and preimport the python for speed
 def manager_prepare():
-  # update submodules
+  if os.path.exists(os.path.join(BASEDIR, ".gitmodules")):
-  system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR)
+    # update submodules
-  system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR)
+    system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR)
+    system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR)
 
   # build cereal first
   subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
@@ -455,24 +547,6 @@ def manager_prepare():
             break
         assert ret == 0
 
-def wait_for_device():
-  while 1:
-    try:
-      context = usb1.USBContext()
-      for device in context.getDeviceList(skip_on_error=True):
-        if (device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc) or \
-           (device.getVendorID() == 0x0483 and device.getProductID() == 0xdf11):
-          bcd = device.getbcdDevice()
-          handle = device.open()
-          handle.claimInterface(0)
-          cloudlog.info("found board")
-          handle.close()
-          return bcd
-    except Exception as e:
-      print "exception", e,
-    print "waiting..."
-    time.sleep(1)
-
 def main():
   if os.getenv("NOLOG") is not None:
     del managed_processes['loggerd']
@@ -481,8 +555,6 @@ def main():
     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']
@@ -510,11 +582,13 @@ def main():
   if params.get("IsRearViewMirror") is None:
     params.put("IsRearViewMirror", "1")
 
+  params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
+
   # put something on screen while we set things up
   if os.getenv("PREPAREONLY") is not None:
     spinner_proc = None
   else:
-    spinner_proc = subprocess.Popen(["./spinner", "loading openpilot..."],
+    spinner_proc = subprocess.Popen(["./spinner", "loading..."],
       cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
       close_fds=True)
   try:
@@ -525,7 +599,7 @@ def main():
       spinner_proc.terminate()
 
   if os.getenv("PREPAREONLY") is not None:
-    sys.exit(0)
+    return
 
   try:
     manager_thread()
@@ -537,3 +611,5 @@ def main():
 
 if __name__ == "__main__":
   main()
+  # manual exit because we are forked
+  sys.exit(0)

selfdrive/pandad.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+# simple boardd wrapper that updates the panda first
+import os
+from panda import ensure_st_up_to_date
+
+def main(gctx=None):
+  ensure_st_up_to_date()
+
+  os.chdir("boardd")
+  os.execvp("./boardd", ["./boardd"])
+
+if __name__ == "__main__":
+    main()

selfdrive/proclogd/Makefile

@@ -38,7 +38,7 @@ proclogd: $(OBJS)
 %.o: %.cc
 	@echo "[ CXX ] $@"
 	$(CXX) $(CXXFLAGS) \
-           $(CEREAL_CFLAGS) \
+           $(CEREAL_CXXFLAGS) \
            $(ZMQ_FLAGS) \
            -I../ \
            -I../../ \

selfdrive/radar/nidec/interface.py

@@ -4,8 +4,6 @@ import numpy as np
 
 from selfdrive.can.parser import CANParser
 
-from selfdrive.boardd.boardd import can_capnp_to_can_list
-
 from cereal import car
 from common.realtime import sec_since_boot
 
@@ -13,10 +11,11 @@ import zmq
 from selfdrive.services import service_list
 import selfdrive.messaging as messaging
 
+
 def _create_nidec_can_parser():
   dbc_f = 'acura_ilx_2016_nidec.dbc'
   radar_messages = [0x400] + range(0x430, 0x43A) + range(0x440, 0x446)
-  signals = zip(['RADAR_STATE'] + 
+  signals = zip(['RADAR_STATE'] +
                 ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
                 ['REL_SPEED'] * 16,
                 [0x400] + radar_messages[1:] * 4,
@@ -25,6 +24,7 @@ def _create_nidec_can_parser():
 
   return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
 
+
 class RadarInterface(object):
   def __init__(self):
     # radar
@@ -32,6 +32,8 @@ class RadarInterface(object):
     self.track_id = 0
     self.radar_fault = False
 
+    self.delay = 0.1  # Delay of radar
+
     # Nidec
     self.rcp = _create_nidec_can_parser()
 
@@ -79,11 +81,10 @@ class RadarInterface(object):
     ret.points = self.pts.values()
     return ret
 
+
 if __name__ == "__main__":
   RI = RadarInterface()
   while 1:
     ret = RI.update()
     print(chr(27) + "[2J")
     print ret
-
-

selfdrive/radar/toyota/interface.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+import os
+import numpy as np
+from selfdrive.can.parser import CANParser
+from cereal import car
+from common.realtime import sec_since_boot
+import zmq
+from selfdrive.services import service_list
+import selfdrive.messaging as messaging
+
+def _create_radard_can_parser():
+  dbc_f = 'toyota_prius_2017_adas.dbc'
+  radar_messages = range(0x210, 0x220)
+  msg_n = len(radar_messages)
+  msg_last = radar_messages[-1]
+  signals = zip(['LONG_DIST'] * msg_n + ['NEW_TRACK'] * msg_n + ['LAT_DIST'] * msg_n +
+                ['REL_SPEED'] * msg_n + ['VALID'] * msg_n,
+                radar_messages * 5,
+                [255] * msg_n + [1] * msg_n + [0] * msg_n + [0] * msg_n + [0] * msg_n)
+  checks = zip(radar_messages, [20]*msg_n)
+
+  return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
+
+class RadarInterface(object):
+  def __init__(self):
+    # radar
+    self.pts = {}
+    self.track_id = 0
+
+    self.delay = 0.0  # Delay of radar
+
+    # Nidec
+    self.rcp = _create_radard_can_parser()
+
+    context = zmq.Context()
+    self.logcan = messaging.sub_sock(context, service_list['can'].port)
+
+  def update(self):
+    canMonoTimes = []
+
+    updated_messages = set()
+    while 1:
+      tm = int(sec_since_boot() * 1e9)
+      updated_messages.update(self.rcp.update(tm, True))
+      # TODO: use msg_last
+      if 0x21f in updated_messages:
+        break
+
+    ret = car.RadarState.new_message()
+    errors = []
+    if not self.rcp.can_valid:
+      errors.append("commIssue")
+    ret.errors = errors
+    ret.canMonoTimes = canMonoTimes
+    #print "NEW TRACKS"
+    for ii in updated_messages:
+      cpt = self.rcp.vl[ii]
+      if cpt['LONG_DIST'] < 255 and cpt['VALID']:
+        #print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1))
+        if ii not in self.pts or cpt['NEW_TRACK']:
+          self.pts[ii] = car.RadarState.RadarPoint.new_message()
+          self.pts[ii].trackId = self.track_id
+          self.track_id += 1
+        self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
+        self.pts[ii].yRel = -cpt['LAT_DIST']  # in car frame's y axis, left is positive
+        self.pts[ii].vRel = cpt['REL_SPEED']
+        self.pts[ii].aRel = float('nan')
+        self.pts[ii].yvRel = float('nan')
+      else:
+        if ii in self.pts:
+          del self.pts[ii]
+
+    ret.points = self.pts.values()
+    return ret
+
+if __name__ == "__main__":
+  RI = RadarInterface()
+  while 1:
+    ret = RI.update()
+    print(chr(27) + "[2J")
+    print ret

selfdrive/test/plant/plant.py

@@ -21,10 +21,10 @@ from selfdrive.car.honda.interface import CarInterface
 
 from cereal import car
 from common.dbc import dbc
-acura = dbc(os.path.join(DBC_PATH, "acura_ilx_2016_can.dbc"))
+honda = dbc(os.path.join(DBC_PATH, "honda_civic_touring_2016_can.dbc"))
 
 # Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
-CP = CarInterface.get_params("ACURA ILX 2016 ACURAWATCH PLUS", {0x201})
+CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {0x201})
 
 
 def car_plant(pos, speed, grade, gas, brake):
@@ -41,7 +41,7 @@ def car_plant(pos, speed, grade, gas, brake):
   frontal_area = 2.2
   air_density = 1.225
   gas_to_peak_linear_slope = 3.33
-  brake_to_peak_linear_slope = 0.2
+  brake_to_peak_linear_slope = 0.3
   creep_accel_v = [1., 0.]
   creep_accel_bp = [0., 1.5]
 
@@ -66,7 +66,7 @@ def car_plant(pos, speed, grade, gas, brake):
   return speed, acceleration
 
 def get_car_can_parser():
-  dbc_f = 'acura_ilx_2016_can.dbc'
+  dbc_f = 'honda_civic_touring_2016_can.dbc'
   signals = [
     ("STEER_TORQUE", 0xe4, 0),
     ("STEER_TORQUE_REQUEST", 0xe4, 0),
@@ -92,7 +92,7 @@ class Plant(object):
 
   def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0):
     self.rate = rate
-    self.civic = False
+    self.civic = True
     self.brake_only = False
 
     if not Plant.messaging_initialized:
@@ -113,10 +113,11 @@ class Plant(object):
     self.user_gas = 0
     self.computer_brake,self.user_brake = 0,0
     self.brake_pressed = 0
+    self.angle_steer_rate = 0
     self.distance, self.distance_prev = 0., 0.
     self.speed, self.speed_prev = speed, speed
     self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0
-    self.gear_shifter = 4   # D gear
+    self.gear_shifter = 8   # D gear
     self.pedal_gas = 0
     self.cruise_setting = 0
 
@@ -211,7 +212,7 @@ class Plant(object):
 
     # ******** publish the car ********
     vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
-           self.angle_steer, 0, self.gear_choice, speed!=0,
+           self.angle_steer, self.angle_steer_rate, 0, self.gear_choice, speed!=0,
            0, 0, 0, 0,
            self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed, 0.,
            self.user_gas, cruise_buttons, self.esp_disabled, 0,
@@ -219,7 +220,7 @@ class Plant(object):
            self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
            self.pedal_gas, self.cruise_setting,
            # append one more zero for gas interceptor
-           0,0,0,0]
+           0,0,0,0,0,0]
 
     # TODO: publish each message at proper frequency
     can_msgs = []
@@ -228,25 +229,27 @@ class Plant(object):
       indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
       for i in indxs:
         msg_struct[sgs[i]] = vls[i]
-      if msg in cks_msgs:
+
+      if "COUNTER" in honda.get_signals(msg):
         msg_struct["COUNTER"] = self.rk.frame % 4
 
-      msg_data = acura.encode(msg, msg_struct)
+      msg_data = honda.encode(msg, msg_struct)
 
-      if msg in cks_msgs:
+      if "CHECKSUM" in honda.get_signals(msg):
         msg_data = fix(msg_data, msg)
 
       can_msgs.append([msg, 0, msg_data, 0])
 
     # add the radar message
     # TODO: use the DBC
-    radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00'
+    if self.rk.frame % 5 == 0:
-    radar_msg = to_3_byte(d_rel*16.0) + \
+      radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00'
-                to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
+      radar_msg = to_3_byte(d_rel*16.0) + \
-                to_3s_byte(int(v_rel*32.0)) + \
+                  to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
-                "0f00000"
+                  to_3s_byte(int(v_rel*32.0)) + \
-    can_msgs.append([0x400, 0, radar_state_msg, 1])
+                  "0f00000"
-    can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
+      can_msgs.append([0x400, 0, radar_state_msg, 1])
+      can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
     Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
 
     # ******** publish a fake model going straight and fake calibration ********

selfdrive/test/tests/plant/test_longitudinal.py

@@ -6,13 +6,14 @@ os.environ['NOCRASH'] = '1'
 import time
 import unittest
 import shutil
-
 import matplotlib
 matplotlib.use('svg')
 
 from selfdrive.config import Conversions as CV, CruiseButtons as CB
 from selfdrive.test.plant.maneuver import Maneuver
 import selfdrive.manager as manager
+from common.params import Params
+
 
 def create_dir(path):
   try:
@@ -79,7 +80,7 @@ maneuvers = [
     initial_speed = 20.,
     lead_relevancy=True,
     initial_distance_lead=35.,
-    speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
+    speed_lead_values = [20., 20., 0.],
     speed_lead_breakpoints = [0., 15., 35.0],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
   ),
@@ -89,7 +90,7 @@ maneuvers = [
     initial_speed = 20.,
     lead_relevancy=True,
     initial_distance_lead=35.,
-    speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
+    speed_lead_values = [20., 20., 0.],
     speed_lead_breakpoints = [0., 15., 25.0],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
   ),
@@ -220,6 +221,8 @@ class LongitudinalControl(unittest.TestCase):
     setup_output()
 
     shutil.rmtree('/data/params', ignore_errors=True)
+    params = Params()
+    params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
 
     manager.gctx = {}
     manager.prepare_managed_process('radard')

selfdrive/tombstoned.py

@@ -11,7 +11,8 @@ from selfdrive.version import version
 from selfdrive.swaglog import cloudlog
 
 def get_tombstones():
-  return [fn for fn in os.listdir("/data/tombstones") if fn.startswith("tombstone")]
+  return [("/data/tombstones/"+fn, int(os.stat("/data/tombstones/"+fn).st_ctime) )
+          for fn in os.listdir("/data/tombstones") if fn.startswith("tombstone")]
 
 def report_tombstone(fn, client):
   mtime = os.path.getmtime(fn)
@@ -73,8 +74,7 @@ def main(gctx):
   while True:
     now_tombstones = set(get_tombstones())
 
-    for ts in (now_tombstones - initial_tombstones):
+    for fn, ctime in (now_tombstones - initial_tombstones):
-      fn = "/data/tombstones/"+ts
       cloudlog.info("reporting new tombstone %s", fn)
       report_tombstone(fn, client)
 

selfdrive/ui/ui.c

@@ -136,6 +136,7 @@ typedef struct UIState {
   int awake_timeout;
 
   bool is_metric;
+  bool passive;
 } UIState;
 
 static void set_awake(UIState *s, bool awake) {
@@ -281,6 +282,15 @@ static void ui_init(UIState *s) {
   glDisable(GL_DEPTH_TEST);
 
   assert(glGetError() == GL_NO_ERROR);
+
+  {
+    char *value;
+    const int result = read_db_value(NULL, "Passive", &value, NULL);
+    if (result == 0) {
+      s->passive = value[0] == '1';
+      free(value);
+    }
+  }
 }
 
 
@@ -701,25 +711,9 @@ static void ui_draw_world(UIState *s) {
     return;
   }
 
-  /******************************************
-   * Add background rect so it's easier to see in 
-   * light background scenes 
-   ******************************************/
-  // Draw background around speed text
-
-  // Left side
-  ui_draw_rounded_rect(s->vg, -15, 0, 570, 180, 20, nvgRGBA(10,10,10,170));
-
-  // Right side
-  ui_draw_rounded_rect(s->vg, 1920-530, 0, 150, 180, 20, nvgRGBA(10,10,10,170));
-  /******************************************/
-
   draw_steering(s, scene->v_ego, scene->angle_steers);
 
-  // draw paths
   if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) {
-    draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(128, 0, 255, 255));
-
     draw_model_path(
         s, scene->model.left_lane,
         nvgRGBA(0, (int)(255 * scene->model.left_lane.prob), 0, 128));
@@ -727,19 +721,19 @@ static void ui_draw_world(UIState *s) {
         s, scene->model.right_lane,
         nvgRGBA(0, (int)(255 * scene->model.right_lane.prob), 0, 128));
 
-    draw_x_y(s, scene->mpc_x, scene->mpc_y, 50, nvgRGBA(255, 0, 0, 255));
+    // draw paths
+    if (!s->passive) {
+      draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(128, 0, 255, 255));
+
+      draw_x_y(s, scene->mpc_x, scene->mpc_y, 50, nvgRGBA(255, 0, 0, 255));
+    }
   }
 
   if (scene->lead_status) {
     char radar_str[16];
 
-    /******************************************
-     * Add background rect so it's easier to see in 
-     * light background scenes 
-     ******************************************/
     // Draw background for radar text
     ui_draw_rounded_rect(s->vg, 578, 0, 195, 180, 20, nvgRGBA(10,10,10,170));
-    /******************************************/
 
     if (s->is_metric) {
       int lead_v_rel = (int)(3.6 * scene->lead_v_rel);
@@ -790,38 +784,48 @@ static void ui_draw_vision(UIState *s) {
     float defaultfontsize = 128.0f;
     float labelfontsize = 65.0f;
 
-    if (scene->engaged) {
+    if (!s->passive) {
-      nvgFillColor(s->vg, nvgRGBA(255, 128, 0, 192));
 
-      // Add label
+      // background
-      nvgFontSize(s->vg, labelfontsize);
+      ui_draw_rounded_rect(s->vg, -15, 0, 570, 180, 20, nvgRGBA(10,10,10,170));
-      nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
-      nvgText(s->vg, 20, 175-30, "OpenPilot: On", NULL);
-    } else {
-      nvgFillColor(s->vg, nvgRGBA(195, 195, 195, 192));
 
-      // Add label
+      if (scene->engaged) {
-      nvgFontSize(s->vg, labelfontsize);
+        nvgFillColor(s->vg, nvgRGBA(255, 128, 0, 192));
-      nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
-      nvgText(s->vg, 20, 175-30, "OpenPilot: Off", NULL);
-    }
 
-    nvgFontSize(s->vg, defaultfontsize);
+        // Add label
-    if (scene->v_cruise != 255 && scene->v_cruise != 0) {
+        nvgFontSize(s->vg, labelfontsize);
-      if (s->is_metric) {
+        nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
-        snprintf(speed_str, sizeof(speed_str), "%3d KPH",
+        nvgText(s->vg, 20, 175-30, "OpenPilot: On", NULL);
-                 (int)(scene->v_cruise + 0.5));
       } else {
-        /* Convert KPH to MPH. Using an approximated mph to kph 
+        nvgFillColor(s->vg, nvgRGBA(195, 195, 195, 192));
-        conversion factor of 1.609 because this is what the Honda
+
-        hud seems to be using */
+        // Add label
-        snprintf(speed_str, sizeof(speed_str), "%3d MPH",
+        nvgFontSize(s->vg, labelfontsize);
-                 (int)(scene->v_cruise * 0.621504 + 0.5));
+        nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
+        nvgText(s->vg, 20, 175-30, "OpenPilot: Off", NULL);
+      }
+
+      nvgFontSize(s->vg, defaultfontsize);
+      if (scene->v_cruise != 255 && scene->v_cruise != 0) {
+        if (s->is_metric) {
+          snprintf(speed_str, sizeof(speed_str), "%3d KPH",
+                   (int)(scene->v_cruise + 0.5));
+        } else {
+          /* Convert KPH to MPH. Using an approximated mph to kph 
+          conversion factor of 1.609 because this is what the Honda
+          hud seems to be using */
+          snprintf(speed_str, sizeof(speed_str), "%3d MPH",
+                   (int)(scene->v_cruise * 0.621504 + 0.5));
+        }
+        nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE);
+        nvgText(s->vg, 480, 95, speed_str, NULL);
       }
-      nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE);
-      nvgText(s->vg, 480, 95, speed_str, NULL);
     }
 
+
+    // speed background
+    ui_draw_rounded_rect(s->vg, 1920-530, 0, 150, 180, 20, nvgRGBA(10,10,10,170));
+
     // Add label
     nvgFontSize(s->vg, labelfontsize);
     nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 192));

selfdrive/updated.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+
+# simple service that waits for network access and tries to update every 3 hours
+
+import os
+import time
+import subprocess
+
+def main(gctx=None):
+  if not os.getenv("CLEAN"):
+    return
+
+  while True:
+    # try network
+    r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
+    if r:
+      time.sleep(60)
+      continue
+
+    # try fetch
+    r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--depth=1"])
+    if r:
+      time.sleep(60)
+      continue
+
+    time.sleep(60*60*3)
+
+if __name__ == "__main__":
+    main()