openpilot v0.3.8.2 release

2017-10-31 02:27:39 -07:00 · 2017-10-31 02:27:39 -07:00 · 187a70f760
parent 48303589e9
commit 187a70f760
70 changed files with 1935 additions and 580 deletions
--- a/Dockerfile.openpilot
+++ b/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
--- a/9
+++ b/9
@ -1,6 +1,9 @@
+
+code_dir := $(shell pwd)
+
+# TODO: Add a global build system
+
 .PHONY: all
-
-# TODO: Add a global build system to openpilot
 all:
-	cd /data/openpilot/selfdrive && PYTHONPATH=/data/openpilot PREPAREONLY=1 /data/openpilot/selfdrive/manager.py
+	cd selfdrive && PYTHONPATH=$(code_dir) PREPAREONLY=1 ./manager.py

--- a/README.md
+++ b/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
 ------
--- a/RELEASES.md
+++ b/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
--- a/cereal/Makefile
+++ b/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:
--- a/cereal/car.capnp
+++ b/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
-    default @0;
+    # does NOT match board setting
+    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?
 }
-
--- a/cereal/log.capnp
+++ b/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;
  }
 }
--- a/common/dbc.py
+++ b/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)
--- a/common/fingerprints.py
+++ b/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": {
-    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
-  }
+  "TOYOTA RAV4 2017": {
+    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)
--- a/common/params.py
+++ b/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)
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit b8c0034c5d6ded3d85a4f0414b3509baae35a34b
+Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 5ea4df111b735ce9efb27ab2e0f87837210f6fc3
+Subproject commit 849f68879d1ceacbf1f9d4174e16e1cd14527383
--- a/selfdrive/boardd/Makefile
+++ b/selfdrive/boardd/Makefile
@ -25,18 +25,16 @@ JSON_FLAGS = -I$(PHONELIBS)/json/src

 EXTRA_LIBS = -lusb

-ifeq ($(OS),GNU/Linux)
-# for Drive PX2
-ZMQ_LIBS = -lczmq -lzmq
-CEREAL_LIBS = -lcapnp -lkj -lcapnp_c
-EXTRA_LIBS = -lusb-1.0 -lpthread
-endif
+# ifeq ($(OS),GNU/Linux)
+# # for Drive PX2
+# ZMQ_LIBS = -lczmq -lzmq
+# CEREAL_LIBS = -lcapnp -lkj -lcapnp_c
+# EXTRA_LIBS = -lusb-1.0 -lpthread
+# 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
--- a/selfdrive/boardd/boardd.cc
+++ b/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");
--- a/selfdrive/boardd/boardd.py
+++ b/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))
-
-    #print can_msgs
+    m = can_list_to_can_capnp(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:
--- a/selfdrive/can/Makefile
+++ b/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
--- a/selfdrive/can/common.h
+++ b/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;
 };
--- a/selfdrive/can/dbc_template.cc
+++ b/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}},
  },
--- a/selfdrive/can/parser.cc
+++ b/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) {
-  int target = (d >> l) & 0xF;
-
-  DEBUG("check checksum %16lx %d", d, l);
-
-  // remove checksum from calculation
-  d &= ~(0xFLL << l);
+unsigned int honda_checksum(unsigned int address, uint64_t d, int l) {
+  d >>= ((8-l)*8); // remove padding
+  d >>= 4; // remove checksum

  int s = 0;
-  while (address > 0) { s += (address & 0xF); address >>= 4; }
-  while (d > 0) { s += (d & 0xF); d >>= 4; }
+  while (address) { s += (address & 0xF); address >>= 4; }
+  while (d) { s += (d & 0xF); d >>= 4; }
  s = 8-s;
  s &= 0xF;

-  DEBUG("   %d = %d\n", target, s);
-  return target == s;
+  return 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
-            || sig->type == HONDA_COUNTER) {
+        if (sig->type != SignalType::DEFAULT) {
          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 != HONDA_COUNTER) {
+              && sig->type == SignalType::DEFAULT) {
            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);
      }
--- a/selfdrive/can/parser.py
+++ b/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 
-    ("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)
+    # sig_name, sig_address, default
+    ("GEAR", 956, 0x20),
+    ("BRAKE_PRESSED", 548, 0),
+    ("GAS_PEDAL", 705, 0),
+
+    ("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),
+    ("STEER_TORQUE_DRIVER", 608, 0),
+    ("STEER_TORQUE_EPS", 608, 0),
+    ("TURN_SIGNALS", 1556, 3),   # 3 is no blinkers
+    ("LKA_STATE", 610, 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),
+    (548, 40),
+    (705, 33),
+
+    (170, 80),
+    (37, 80),
+    (466, 33),
+    (608, 50),
  ]

-  cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0)
-  print cp.vl
+  cp = CANParser("toyota_rav4_2017_pt", signals, checks, 0)
+
+  # print cp.vl

  while True:
    cp.update(int(sec_since_boot()*1e9), True)
--- a/selfdrive/can/process_dbc.py
+++ b/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
-        for address, ((msg_name, _), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs]
+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_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)

--- a/selfdrive/car/init.py
+++ b/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):
-  candidate, fingerprints = fingerprint(logcan)
+# **** for use live only ****
+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)

--- a/selfdrive/car/honda/carcontroller.py
+++ b/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 ***
--- a/selfdrive/car/honda/carstate.py
+++ b/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
-    self.a_ego = 0.
+    # 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, 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_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
+    self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
+    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)
-
--- a/selfdrive/car/honda/interface.py
+++ b/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 
-    # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not 
+    ret.steerKf = 0. # TODO: investigate FF steer control for Honda
+
+    # 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
--- a/selfdrive/car/honda/values.py
+++ b/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]
--- a/selfdrive/car/toyota/init.py
+++ b/selfdrive/car/toyota/init.py
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/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())
--- a/selfdrive/car/toyota/carstate.py
+++ b/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
--- a/selfdrive/car/toyota/interface.py
+++ b/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
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/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)
--- a/selfdrive/common/cereal.mk
+++ b/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 = $(CEREAL_CFLAGS)
+CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
 ifeq ($(CEREAL_LIBS),)
-  CEREAL_LIBS = -L$(BASEDIR)/external/capnp/lib \
-	              -l:libcapnp.a -l:libkj.a -l:libcapnp_c.a
+  CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/x64/lib/ \
+                -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/ \
--- a/selfdrive/common/modeldata.h
+++ b/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;
--- a/selfdrive/common/swaglog.c
+++ b/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));

--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@ -1 +1 @@
-#define OPENPILOT_VERSION "0.3.7"
+#define COMMA_VERSION "0.3.8.2-openpilot"
--- a/selfdrive/config.py
+++ b/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
--- a/selfdrive/controls/controlsd.py
+++ b/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
-  ENABLED = 1
-  PRE_ENABLED = 2
-  SOFT_DISABLING = 3
+  DISABLED = 'disabled'
+  ENABLED = 'enabled'
+  PRE_ENABLED = 'preEnabled'
+  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
-  # always cancel if we have an interceptor
-  CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
+    CC.actuators = actuators

-  # brake discount removes a sharp nonlinearity
-  brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
-  CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
+    CC.cruiseControl.override = True
+    # always cancel if we have an interceptor
+    CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)

-  # TODO: parametrize 0.714 in interface?
-  # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
-  # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
-  CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
+    # brake discount removes a sharp nonlinearity
+    brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
+    CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)

-  CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
-  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
+    # TODO: parametrize 0.714 in interface?
+    # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
+    # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
+    CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))

-  # send car controls over can
-  CI.apply(CC)
+    CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
+    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
-    state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
-    prof.checkpoint("State transition")
+    if not passive:
+      # update control state
+      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 ***
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@ -59,9 +59,9 @@ class AlertManager(object):
        PT.MID, None, "beepSingle", .2, 0., 0.),

    "fcw": Alert(
-        "", 
-        "", 
-        PT.LOW, None, None, .1, .1, .1),
+        "Brake", 
+        "Risk of Collision", 
+        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
--- a/selfdrive/controls/lib/fcw.py
+++ b/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
--- a/selfdrive/controls/lib/latcontrol.py
+++ b/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
--- a/selfdrive/controls/lib/latcontrol_helpers.py
+++ b/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
--- a/selfdrive/controls/lib/lateral_mpc/Makefile
+++ b/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 \
-  $(PHONELIBS)/qpoases/SRC/Constraints.o \
-  $(PHONELIBS)/qpoases/SRC/CyclingManager.o \
-  $(PHONELIBS)/qpoases/SRC/Indexlist.o \
-  $(PHONELIBS)/qpoases/SRC/MessageHandling.o \
-  $(PHONELIBS)/qpoases/SRC/QProblem.o \
-  $(PHONELIBS)/qpoases/SRC/QProblemB.o \
-  $(PHONELIBS)/qpoases/SRC/SubjectTo.o \
-  $(PHONELIBS)/qpoases/SRC/Utils.o \
-  $(PHONELIBS)/qpoases/SRC/EXTRAS/SolutionAnalysis.o \
-  mpc_export/acado_qpoases_interface.o \
+  qp/Bounds.o \
+  qp/Constraints.o \
+  qp/CyclingManager.o \
+  qp/Indexlist.o \
+  qp/MessageHandling.o \
+  qp/QProblem.o \
+  qp/QProblemB.o \
+  qp/SubjectTo.o \
+  qp/Utils.o \
+  qp/EXTRAS/SolutionAnalysis.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 \
--- a/selfdrive/controls/lib/lateral_mpc/generator.cpp
+++ b/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 );
--- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+++ b/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);
 """)
--- a/selfdrive/controls/lib/lateral_mpc/mpc.c
+++ b/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_shiftStates(2, 0, 0);
-  acado_shiftControls( 0 );
+  acado_feedbackStep();
+  /* printf("lat its: %d\n", acado_getNWSR()); */

 	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();
 }
--- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h
+++ b/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. */
--- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c
+++ b/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[1] = + (real_t)5.0000000000000001e-03*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[3] = + (real_t)5.0000000000000001e-03*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[5] = + (real_t)5.0000000000000001e-03*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[7] = + (real_t)5.0000000000000001e-03*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[9] = + (real_t)5.0000000000000001e-03*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[11] = + (real_t)5.0000000000000001e-03*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[13] = + (real_t)5.0000000000000001e-03*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[15] = + (real_t)5.0000000000000001e-03*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[17] = + (real_t)5.0000000000000001e-03*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[19] = + (real_t)5.0000000000000001e-03*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[21] = + (real_t)5.0000000000000001e-03*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[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[23] + rk_eta[23];
+acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
+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[1] = + (real_t)5.0000000000000001e-03*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[3] = + (real_t)5.0000000000000001e-03*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[5] = + (real_t)5.0000000000000001e-03*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[7] = + (real_t)5.0000000000000001e-03*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[9] = + (real_t)5.0000000000000001e-03*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[11] = + (real_t)5.0000000000000001e-03*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[13] = + (real_t)5.0000000000000001e-03*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[15] = + (real_t)5.0000000000000001e-03*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[17] = + (real_t)5.0000000000000001e-03*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[19] = + (real_t)5.0000000000000001e-03*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[21] = + (real_t)5.0000000000000001e-03*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[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[47] + rk_eta[23];
+acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
+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[1] = + (real_t)1.0000000000000000e-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[3] = + (real_t)1.0000000000000000e-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[5] = + (real_t)1.0000000000000000e-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[7] = + (real_t)1.0000000000000000e-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[9] = + (real_t)1.0000000000000000e-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[11] = + (real_t)1.0000000000000000e-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[13] = + (real_t)1.0000000000000000e-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[15] = + (real_t)1.0000000000000000e-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[17] = + (real_t)1.0000000000000000e-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[19] = + (real_t)1.0000000000000000e-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[21] = + (real_t)1.0000000000000000e-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[23] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
+acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
+acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
+acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
+acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
+acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
+acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
+acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
+acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
+acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
+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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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[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];
-acadoWorkspace.rk_ttt += 2.0000000000000001e-01;
+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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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)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 += 1.0000000000000000e+00;
 }
 error = 0;
 return error;
--- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp
+++ b/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 */
--- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
+++ b/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];
 }

--- a/selfdrive/controls/lib/longcontrol.py
+++ b/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:
-  off = 0  # Off
-  pid = 1  # 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
-  starting = 3  # starting and releasing brakes in open loop before giving back to PID
+  # long_control_state labels (see capnp enum):
+  off = 'off'  # Off
+  pid = 'pid'  # moving and tracking targets, with PID control running
+  stopping = 'stopping'  # stopping and changing controls to almost open loop as PID does not fit well at such a low speed
+  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)

--- a/selfdrive/controls/lib/pid.py
+++ b/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):
-    self._k_p = k_p
-    self._k_i = k_i
+  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 # proportional gain
+    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)

--- a/selfdrive/controls/lib/radar_helpers.py
+++ b/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:
--- a/selfdrive/controls/radard.py
+++ b/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" % \
-      #  (ids, round(tracks[ids].dRel, 2), round(tracks[ids].vRel, 2), round(tracks[ids].yRel, 2))
+      if DEBUG:
+        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)]
--- a/selfdrive/debug/dump.py
+++ b/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)

--- a/selfdrive/logcatd/Makefile
+++ b/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../../ \
--- a/selfdrive/loggerd/loggerd
+++ b/selfdrive/loggerd/loggerd
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@ -1,36 +1,67 @@
 #!/usr/bin/env python
 import os
-import time
-
-if os.path.isfile("/init.qcom.rc"):
-  # check if NEOS update is required
-  while 1:
-    if ((not os.path.isfile("/VERSION")
-        or int(open("/VERSION").read()) < 3)
-        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")
-    else:
-      break
-    time.sleep(10)
-
 import sys
+import time
+import fcntl
+import errno
+import signal
+
+if __name__ == "__main__":
+  if os.path.isfile("/init.qcom.rc"):
+    # check if NEOS update is required
+    while ((not os.path.isfile("/VERSION")
+            or int(open("/VERSION").read()) < 3)
+            and not os.path.isfile("/data/media/0/noupdate")):
+      os.system("curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater")
+      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 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
-    ensure_st_up_to_date()
-    start_managed_process("boardd")
+    start_managed_process("pandad")

-  started = False
+  passive = bool(os.getenv("PASSIVE"))
+  passive_starter = LocationStarter()
+
+  started_ts = None
  logger_dead = False
  count = 0
  fan_speed = 0
+  ignition_seen = False

-  # set 5 second timeout on health socket
-  # 5x slower than expected
-  health_sock.RCVTIMEO = 5000
+  health_sock.RCVTIMEO = 1500

  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:
-      if not started:
+    should_start = should_start and avail > 0.02
+
+
+    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
-  system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR)
-  system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR)
+  if os.path.exists(os.path.join(BASEDIR, ".gitmodules")):
+    # update submodules
+    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)
--- a/selfdrive/pandad.py
+++ b/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()
--- a/selfdrive/proclogd/Makefile
+++ b/selfdrive/proclogd/Makefile
@ -38,7 +38,7 @@ proclogd: $(OBJS)
 %.o: %.cc
 	@echo "[ CXX ] $@"
 	$(CXX) $(CXXFLAGS) \
-           $(CEREAL_CFLAGS) \
+           $(CEREAL_CXXFLAGS) \
           $(ZMQ_FLAGS) \
           -I../ \
           -I../../ \
--- a/selfdrive/radar/nidec/interface.py
+++ b/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
-
-
--- a/selfdrive/radar/toyota/init.py
+++ b/selfdrive/radar/toyota/init.py
--- a/selfdrive/radar/toyota/interface.py
+++ b/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
--- a/selfdrive/sensord/gpsd
+++ b/selfdrive/sensord/gpsd
--- a/selfdrive/sensord/sensord
+++ b/selfdrive/sensord/sensord
--- a/selfdrive/test/plant/plant.py
+++ b/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'
-    radar_msg = to_3_byte(d_rel*16.0) + \
-                to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
-                to_3s_byte(int(v_rel*32.0)) + \
-                "0f00000"
-    can_msgs.append([0x400, 0, radar_state_msg, 1])
-    can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
+    if self.rk.frame % 5 == 0:
+      radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00'
+      radar_msg = to_3_byte(d_rel*16.0) + \
+                  to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
+                  to_3s_byte(int(v_rel*32.0)) + \
+                  "0f00000"
+      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 ********
--- a/selfdrive/test/tests/plant/test_longitudinal.py
+++ b/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')
--- a/selfdrive/tombstoned.py
+++ b/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):
-      fn = "/data/tombstones/"+ts
+    for fn, ctime in (now_tombstones - initial_tombstones):
      cloudlog.info("reporting new tombstone %s", fn)
      report_tombstone(fn, client)

--- a/selfdrive/ui/ui.c
+++ b/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) {
-      nvgFillColor(s->vg, nvgRGBA(255, 128, 0, 192));
+    if (!s->passive) {

-      // Add label
-      nvgFontSize(s->vg, labelfontsize);
-      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));
+      // background
+      ui_draw_rounded_rect(s->vg, -15, 0, 570, 180, 20, nvgRGBA(10,10,10,170));

-      // Add label
-      nvgFontSize(s->vg, labelfontsize);
-      nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
-      nvgText(s->vg, 20, 175-30, "OpenPilot: Off", NULL);
-    }
+      if (scene->engaged) {
+        nvgFillColor(s->vg, nvgRGBA(255, 128, 0, 192));

-    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));
+        // Add label
+        nvgFontSize(s->vg, labelfontsize);
+        nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
+        nvgText(s->vg, 20, 175-30, "OpenPilot: On", NULL);
      } 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));
+        nvgFillColor(s->vg, nvgRGBA(195, 195, 195, 192));
+
+        // Add label
+        nvgFontSize(s->vg, labelfontsize);
+        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));
--- a/selfdrive/updated.py
+++ b/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()
--- a/selfdrive/visiond/visiond
+++ b/selfdrive/visiond/visiond