diff --git a/RELEASES.md b/RELEASES.md
index f0280d43f..23c0e27f7 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,9 @@
+Version 0.4.3.1 (2018-03-19)
+============================
+ * Improve autofocus
+ * Add check for MPC solution error
+ * Make first distracted warning visual only
+
 Version 0.4.3 (2018-03-13)
 ==========================
  * Add HDR and autofocus
diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk
index dc49d8f30..b7ac55c97 100644
Binary files a/apk/ai.comma.plus.frame.apk and b/apk/ai.comma.plus.frame.apk differ
diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk
index 9bd91ad0d..c3bcf538a 100644
Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ
diff --git a/cereal/Makefile b/cereal/Makefile
index b384b4904..dcf4cc221 100644
--- a/cereal/Makefile
+++ b/cereal/Makefile
@@ -1,6 +1,6 @@
 PWD := $(shell pwd)
 
-SRCS := log.capnp car.capnp
+SRCS := log.capnp car.capnp map.capnp
 
 GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
 
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 76b89c446..beb6e8a78 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -54,6 +54,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     parkBrake @29;
     manualRestart @30;
     lowSpeedLockout @31;
+    plannerError @32;
   }
 }
 
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 5996cf55a..a4bd57c81 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -119,6 +119,8 @@ struct FrameData {
   globalGain @5 :Int32;
   lensPos @11 :Int32;
   lensSag @12 :Float32;
+  lensErr @13 :Float32;
+  lensTruePos @14 :Float32;
   image @6 :Data;
 
   frameType @7 :FrameType;
@@ -1325,6 +1327,7 @@ struct LiveMpcData {
   delta @3 :List(Float32);
   qpIterations @4 :UInt32;
   calculationTime @5 :UInt64;
+  cost @6 :Float64;
 }
 
 struct LiveLongitudinalMpcData {
@@ -1338,18 +1341,27 @@ struct LiveLongitudinalMpcData {
   qpIterations @7 :UInt32;
   mpcId @8 :UInt32;
   calculationTime @9 :UInt64;
+  cost @10 :Float64;
 }
 
 
-struct ECEFPoint {
+struct ECEFPointDEPRECATED @0xe10e21168db0c7f7 {
   x @0 :Float32;
   y @1 :Float32;
   z @2 :Float32;
 }
 
+struct ECEFPoint @0xc25bbbd524983447 {
+  x @0 :Float64;
+  y @1 :Float64;
+  z @2 :Float64;
+}
+
 struct GPSPlannerPoints {
-  curPos @0 :ECEFPoint;
-  points @1 :List(ECEFPoint);
+  curPosDEPRECATED @0 :ECEFPointDEPRECATED;
+  pointsDEPRECATED @1 :List(ECEFPointDEPRECATED);
+  curPos @6 :ECEFPoint;
+  points @7 :List(ECEFPoint);
   valid @2 :Bool;
   trackName @3 :Text;
   speedLimit @4 :Float32;
@@ -1362,7 +1374,8 @@ struct GPSPlannerPlan {
   trackName @2 :Text;
   speed @3 :Float32;
   acceleration @4 :Float32;
-  points @5 :List(ECEFPoint);
+  pointsDEPRECATED @5 :List(ECEFPointDEPRECATED);
+  points @6 :List(ECEFPoint);
 }
 
 struct TrafficEvent @0xacfa74a094e62626 {
@@ -1408,7 +1421,8 @@ struct UiNavigationEvent {
   type @0: Type;
   status @1: Status;
   distanceTo @2: Float32;
-  endRoadPoint @3: ECEFPoint;
+  endRoadPointDEPRECATED @3: ECEFPointDEPRECATED;
+  endRoadPoint @4: ECEFPoint;
 
   enum Type {
     none @0;
diff --git a/common/transformations/coordinates.py b/common/transformations/coordinates.py
index 14f92559d..0b0bd839a 100644
--- a/common/transformations/coordinates.py
+++ b/common/transformations/coordinates.py
@@ -89,9 +89,11 @@ class LocalCoord(object):
 
 
   def ecef2ned(self, ecef):
+    ecef = np.array(ecef)
     return np.dot(self.ecef2ned_matrix, (ecef - self.init_ecef).T).T
 
   def ned2ecef(self, ned):
+    ned = np.array(ned)
     # Transpose so that init_ecef will broadcast correctly for 1d or 2d ned.
     return (np.dot(self.ned2ecef_matrix, ned.T).T + self.init_ecef)
 
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index a19134e59..55d9dd8cd 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -76,7 +76,6 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
   """Creates an iterable of CAN messages for the UIs."""
   commands = []
 
-  # TODO: Why is X4 always 0xc1? Not implemented yet in canpacker
   acc_hud_values = {
     'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
     'PCM_GAS': hud.pcm_accel,
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 800b82c1d..ac8f8e2cd 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -186,7 +186,7 @@ class CarInterface(object):
       ret.steerRatio = 15.3
       # Acura at comma has modified steering FW, so different tuning for the Neo in that car
       is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310']
-      ret.steerKpV, ret.steerKiV = [[0.1], [0.03]] if is_fw_modified else [[0.8], [0.24]]
+      ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
 
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
@@ -449,7 +449,7 @@ class CarInterface(object):
         events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
       else:
         events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
-    if self.CS.CP.carFingerprint != CAR.CIVIC and ret.vEgo < 0.001:
+    if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
       events.append(create_event('manualRestart', [ET.WARNING]))
 
     cur_time = sec_since_boot()
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index 6721f927d..5cf7951e9 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.4.3-release"
+#define COMMA_VERSION "0.4.3.1-release"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index a66d4871b..207e5907f 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -104,9 +104,9 @@ def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overte
   return CS, events, cal_status, overtemp, free_space
 
 
-def calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status):
+def calc_plan(CS, events, PL, LaC, LoC, v_cruise_kph, awareness_status):
    # plan runs always, independently of the state
-   plan_packet = PL.update(CS, LoC, v_cruise_kph, awareness_status < -0.)
+   plan_packet = PL.update(CS, LaC, LoC, v_cruise_kph, awareness_status < -0.)
    plan = plan_packet.plan
    plan_ts = plan_packet.logMonoTime
 
@@ -401,6 +401,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
   cs_send.init('carState')
   # TODO: override CS.events with all the cumulated events
   cs_send.carState = copy(CS)
+  cs_send.carState.events = events
   carstate.send(cs_send.to_bytes())
 
   # broadcast carControl
@@ -408,7 +409,6 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
   cc_send.init('carControl')
   cc_send.carControl = copy(CC)
   carcontrol.send(cc_send.to_bytes())
-  #print [i.name for i in events]
 
   # publish mpc state at 20Hz
   if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
@@ -418,6 +418,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
     dat.liveMpc.y = list(LaC.mpc_solution[0].y)
     dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
     dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
+    dat.liveMpc.cost = LaC.mpc_solution[0].cost
     livempc.send(dat.to_bytes())
 
   return CC
@@ -517,7 +518,7 @@ def controlsd_thread(gctx, rate=100):
     prof.checkpoint("Sample")
 
     # define plan
-    plan, plan_ts = calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status)
+    plan, plan_ts = calc_plan(CS, events, PL, LaC, LoC, v_cruise_kph, awareness_status)
     prof.checkpoint("Plan")
 
     if not passive:
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index 8c22a6166..5e704982d 100644
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@@ -92,7 +92,7 @@ class AlertManager(object):
         "TAKE CONTROL",
         "User appears distracted",
         AlertStatus.userPrompt, AlertSize.mid,
-        Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1),
+        Priority.LOW, "steerRequired", None, 0., .1, .1),
 
     "driverDistracted": Alert(
         "TAKE CONTROL TO REGAIN SPEED",
@@ -283,6 +283,12 @@ class AlertManager(object):
         AlertStatus.critical, AlertSize.full,
         Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
 
+    "plannerError": Alert(
+        "TAKE CONTROL IMMEDIATELY",
+        "Planner Solution Error",
+        AlertStatus.critical, AlertSize.full,
+        Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
+
     # not loud cancellations (user is in control)
     "noTarget": Alert(
         "Comma Canceled",
@@ -405,6 +411,12 @@ class AlertManager(object):
         AlertStatus.normal, AlertSize.mid,
         Priority.LOW, None, "chimeDouble", .4, 2., 3.),
 
+    "plannerErrorNoEntry": Alert(
+        "Comma Unavailable",
+        "Planner Solution Error",
+        AlertStatus.normal, AlertSize.mid,
+        Priority.LOW, None, "chimeDouble", .4, 2., 3.),
+
     # permanent alerts to display on small UI upper box
     "steerUnavailablePermanent": Alert(
         "STEER FAULT: Restart the car to engage",
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
index e7271f7ec..4d5c67681 100644
--- a/selfdrive/controls/lib/latcontrol.py
+++ b/selfdrive/controls/lib/latcontrol.py
@@ -39,6 +39,7 @@ class LatControl(object):
     self.mpc_solution = libmpc_py.ffi.new("log_t *")
     self.cur_state = libmpc_py.ffi.new("state_t *")
     self.mpc_updated = False
+    self.mpc_nans = False
     self.cur_state[0].x = 0.0
     self.cur_state[0].y = 0.0
     self.cur_state[0].psi = 0.0
@@ -56,6 +57,7 @@ class LatControl(object):
   def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
     cur_time = sec_since_boot()
     self.mpc_updated = False
+    # TODO: this creates issues in replay when rewinding time: mpc won't run
     if self.last_mpc_ts < PL.last_md_ts:
       self.last_mpc_ts = PL.last_md_ts
       self.angle_steers_des_prev = self.angle_steers_des_mpc
@@ -87,9 +89,9 @@ class LatControl(object):
       self.mpc_updated = True
 
       #  Check for infeasable MPC solution
-      nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
+      self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
       t = sec_since_boot()
-      if nans:
+      if self.mpc_nans:
         self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost)
         self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
 
diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp
index 9c6dd31e8..5d0165c09 100644
--- a/selfdrive/controls/lib/lateral_mpc/generator.cpp
+++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp
@@ -115,9 +115,10 @@ int main( )
   ocp.minimizeLSQ(Q, h);
   ocp.minimizeLSQEndTerm(QN, hN);
 
+  // car can't go backward to avoid "circles"
   ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
-  ocp.subjectTo( deg2rad(-25) <= delta <= deg2rad(25));
-  ocp.subjectTo( -0.1 <= t <= 0.1);
+  // more than absolute max steer angle
+  ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
   ocp.setNOD(18);
 
   OCPexport mpc(ocp);
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
index ab5be2f63..5970dab90 100644
--- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
@@ -14,10 +14,11 @@ typedef struct {
 } state_t;
 
 typedef struct {
-    double x[20];
-    double y[20];
-    double psi[20];
-    double delta[20];
+    double x[21];
+    double y[21];
+    double psi[21];
+    double delta[21];
+    double cost;
 } log_t;
 
 void init(double pathCost, double laneCost, double headingCost, double steerRateCost);
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc.c b/selfdrive/controls/lib/lateral_mpc/mpc.c
index 0cae4079d..f4cee7f2d 100644
--- a/selfdrive/controls/lib/lateral_mpc/mpc.c
+++ b/selfdrive/controls/lib/lateral_mpc/mpc.c
@@ -22,10 +22,11 @@ typedef struct {
 
 
 typedef struct {
-  double x[N];
-  double y[N];
-  double psi[N];
-	double delta[N];
+  double x[N+1];
+  double y[N+1];
+  double psi[N+1];
+  double delta[N+1];
+  double cost;
 } log_t;
 
 void init(double pathCost, double laneCost, double headingCost, double steerRateCost){
@@ -102,18 +103,22 @@ int run_mpc(state_t * x0, log_t * solution,
 
   acado_preparationStep();
   acado_feedbackStep();
-  /* printf("lat its: %d\n", acado_getNWSR()); */
-
-	for (i = 0; i <= N; i++){
-		solution->x[i] = acadoVariables.x[i*NX];
-		solution->y[i] = acadoVariables.x[i*NX+1];
-		solution->psi[i] = acadoVariables.x[i*NX+2];
-		solution->delta[i] = acadoVariables.x[i*NX+3];
-	}
-
-  acado_shiftStates(2, 0, 0);
-  acado_shiftControls( 0 );
+  
+  /* printf("lat its: %d\n", acado_getNWSR());  // n iterations
+  printf("Objective: %.6f\n", acado_getObjective());  // solution cost */
+
+  for (i = 0; i <= N; i++){
+    solution->x[i] = acadoVariables.x[i*NX];
+    solution->y[i] = acadoVariables.x[i*NX+1];
+    solution->psi[i] = acadoVariables.x[i*NX+2];
+    solution->delta[i] = acadoVariables.x[i*NX+3];
+  }
+  solution->cost = acado_getObjective();
 
+  // Dont shift states here. Current solution is closer to next timestep than if
+  // we use the old solution as a starting point
+  //acado_shiftStates(2, 0, 0);
+  //acado_shiftControls( 0 );
 
   return acado_getNWSR();
 }
diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
index ed897e3c0..e0b91f055 100644
--- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
@@ -3948,46 +3948,46 @@ acado_macETSlu( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.g[ 21 ]) );
 acado_macETSlu( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.g[ 22 ]) );
 acado_macETSlu( &(acadoWorkspace.QE[ 832 ]), &(acadoWorkspace.g[ 22 ]) );
 acado_macETSlu( &(acadoWorkspace.QE[ 836 ]), &(acadoWorkspace.g[ 23 ]) );
-acadoWorkspace.lb[4] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[0];
-acadoWorkspace.lb[5] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[1];
-acadoWorkspace.lb[6] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[2];
-acadoWorkspace.lb[7] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[3];
-acadoWorkspace.lb[8] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[4];
-acadoWorkspace.lb[9] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[5];
-acadoWorkspace.lb[10] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[6];
-acadoWorkspace.lb[11] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[7];
-acadoWorkspace.lb[12] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[8];
-acadoWorkspace.lb[13] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[9];
-acadoWorkspace.lb[14] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[10];
-acadoWorkspace.lb[15] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[11];
-acadoWorkspace.lb[16] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[12];
-acadoWorkspace.lb[17] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[13];
-acadoWorkspace.lb[18] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[14];
-acadoWorkspace.lb[19] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[15];
-acadoWorkspace.lb[20] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[16];
-acadoWorkspace.lb[21] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[17];
-acadoWorkspace.lb[22] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[18];
-acadoWorkspace.lb[23] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[19];
-acadoWorkspace.ub[4] = (real_t)1.0000000000000001e-01 - acadoVariables.u[0];
-acadoWorkspace.ub[5] = (real_t)1.0000000000000001e-01 - acadoVariables.u[1];
-acadoWorkspace.ub[6] = (real_t)1.0000000000000001e-01 - acadoVariables.u[2];
-acadoWorkspace.ub[7] = (real_t)1.0000000000000001e-01 - acadoVariables.u[3];
-acadoWorkspace.ub[8] = (real_t)1.0000000000000001e-01 - acadoVariables.u[4];
-acadoWorkspace.ub[9] = (real_t)1.0000000000000001e-01 - acadoVariables.u[5];
-acadoWorkspace.ub[10] = (real_t)1.0000000000000001e-01 - acadoVariables.u[6];
-acadoWorkspace.ub[11] = (real_t)1.0000000000000001e-01 - acadoVariables.u[7];
-acadoWorkspace.ub[12] = (real_t)1.0000000000000001e-01 - acadoVariables.u[8];
-acadoWorkspace.ub[13] = (real_t)1.0000000000000001e-01 - acadoVariables.u[9];
-acadoWorkspace.ub[14] = (real_t)1.0000000000000001e-01 - acadoVariables.u[10];
-acadoWorkspace.ub[15] = (real_t)1.0000000000000001e-01 - acadoVariables.u[11];
-acadoWorkspace.ub[16] = (real_t)1.0000000000000001e-01 - acadoVariables.u[12];
-acadoWorkspace.ub[17] = (real_t)1.0000000000000001e-01 - acadoVariables.u[13];
-acadoWorkspace.ub[18] = (real_t)1.0000000000000001e-01 - acadoVariables.u[14];
-acadoWorkspace.ub[19] = (real_t)1.0000000000000001e-01 - acadoVariables.u[15];
-acadoWorkspace.ub[20] = (real_t)1.0000000000000001e-01 - acadoVariables.u[16];
-acadoWorkspace.ub[21] = (real_t)1.0000000000000001e-01 - acadoVariables.u[17];
-acadoWorkspace.ub[22] = (real_t)1.0000000000000001e-01 - acadoVariables.u[18];
-acadoWorkspace.ub[23] = (real_t)1.0000000000000001e-01 - acadoVariables.u[19];
+acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0];
+acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1];
+acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2];
+acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3];
+acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4];
+acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5];
+acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6];
+acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7];
+acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8];
+acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9];
+acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10];
+acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11];
+acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12];
+acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13];
+acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14];
+acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15];
+acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16];
+acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17];
+acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18];
+acadoWorkspace.lb[23] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19];
+acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0];
+acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1];
+acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2];
+acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3];
+acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4];
+acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5];
+acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6];
+acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7];
+acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8];
+acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9];
+acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10];
+acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11];
+acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12];
+acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13];
+acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14];
+acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15];
+acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16];
+acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17];
+acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18];
+acadoWorkspace.ub[23] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19];
 
 for (lRun1 = 0; lRun1 < 40; ++lRun1)
 {
@@ -4477,122 +4477,122 @@ tmp = acadoVariables.x[6] + acadoWorkspace.d[2];
 acadoWorkspace.lbA[0] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[0] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[7] + acadoWorkspace.d[3];
-acadoWorkspace.lbA[1] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[1] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[1] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[1] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[10] + acadoWorkspace.d[6];
 acadoWorkspace.lbA[2] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[2] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[11] + acadoWorkspace.d[7];
-acadoWorkspace.lbA[3] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[3] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[3] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[3] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[14] + acadoWorkspace.d[10];
 acadoWorkspace.lbA[4] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[4] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[15] + acadoWorkspace.d[11];
-acadoWorkspace.lbA[5] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[5] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[5] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[5] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[18] + acadoWorkspace.d[14];
 acadoWorkspace.lbA[6] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[6] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[19] + acadoWorkspace.d[15];
-acadoWorkspace.lbA[7] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[7] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[7] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[7] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[22] + acadoWorkspace.d[18];
 acadoWorkspace.lbA[8] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[8] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[23] + acadoWorkspace.d[19];
-acadoWorkspace.lbA[9] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[9] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[9] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[9] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[26] + acadoWorkspace.d[22];
 acadoWorkspace.lbA[10] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[10] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[27] + acadoWorkspace.d[23];
-acadoWorkspace.lbA[11] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[11] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[11] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[11] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[30] + acadoWorkspace.d[26];
 acadoWorkspace.lbA[12] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[12] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[31] + acadoWorkspace.d[27];
-acadoWorkspace.lbA[13] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[13] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[13] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[13] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[34] + acadoWorkspace.d[30];
 acadoWorkspace.lbA[14] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[14] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[35] + acadoWorkspace.d[31];
-acadoWorkspace.lbA[15] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[15] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[15] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[15] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[38] + acadoWorkspace.d[34];
 acadoWorkspace.lbA[16] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[16] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[39] + acadoWorkspace.d[35];
-acadoWorkspace.lbA[17] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[17] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[17] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[17] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[42] + acadoWorkspace.d[38];
 acadoWorkspace.lbA[18] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[18] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[43] + acadoWorkspace.d[39];
-acadoWorkspace.lbA[19] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[19] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[19] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[19] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[46] + acadoWorkspace.d[42];
 acadoWorkspace.lbA[20] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[20] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[47] + acadoWorkspace.d[43];
-acadoWorkspace.lbA[21] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[21] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[21] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[21] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[50] + acadoWorkspace.d[46];
 acadoWorkspace.lbA[22] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[22] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[51] + acadoWorkspace.d[47];
-acadoWorkspace.lbA[23] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[23] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[23] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[23] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[54] + acadoWorkspace.d[50];
 acadoWorkspace.lbA[24] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[24] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[55] + acadoWorkspace.d[51];
-acadoWorkspace.lbA[25] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[25] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[25] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[25] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[58] + acadoWorkspace.d[54];
 acadoWorkspace.lbA[26] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[26] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[59] + acadoWorkspace.d[55];
-acadoWorkspace.lbA[27] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[27] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[27] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[27] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[62] + acadoWorkspace.d[58];
 acadoWorkspace.lbA[28] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[28] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[63] + acadoWorkspace.d[59];
-acadoWorkspace.lbA[29] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[29] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[29] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[29] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[66] + acadoWorkspace.d[62];
 acadoWorkspace.lbA[30] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[30] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[67] + acadoWorkspace.d[63];
-acadoWorkspace.lbA[31] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[31] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[31] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[31] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[70] + acadoWorkspace.d[66];
 acadoWorkspace.lbA[32] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[32] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[71] + acadoWorkspace.d[67];
-acadoWorkspace.lbA[33] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[33] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[33] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[33] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[74] + acadoWorkspace.d[70];
 acadoWorkspace.lbA[34] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[34] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[75] + acadoWorkspace.d[71];
-acadoWorkspace.lbA[35] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[35] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[35] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[35] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[78] + acadoWorkspace.d[74];
 acadoWorkspace.lbA[36] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[36] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[79] + acadoWorkspace.d[75];
-acadoWorkspace.lbA[37] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[37] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[37] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[37] = (real_t)8.7266462600000005e-01 - tmp;
 tmp = acadoVariables.x[82] + acadoWorkspace.d[78];
 acadoWorkspace.lbA[38] = (real_t)-1.5707963268000000e+00 - tmp;
 acadoWorkspace.ubA[38] = (real_t)1.5707963268000000e+00 - tmp;
 tmp = acadoVariables.x[83] + acadoWorkspace.d[79];
-acadoWorkspace.lbA[39] = (real_t)-4.3633231300000003e-01 - tmp;
-acadoWorkspace.ubA[39] = (real_t)4.3633231300000003e-01 - tmp;
+acadoWorkspace.lbA[39] = (real_t)-8.7266462600000005e-01 - tmp;
+acadoWorkspace.ubA[39] = (real_t)8.7266462600000005e-01 - tmp;
 
 }
 
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
index 8919bd426..b45021a97 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
@@ -18,13 +18,14 @@ def _get_libmpc(mpc_id):
 
 
     typedef struct {
-    double x_ego[20];
-    double v_ego[20];
-    double a_ego[20];
-    double j_ego[20];
-    double x_l[20];
-    double v_l[20];
-    double a_l[20];
+    double x_ego[21];
+    double v_ego[21];
+    double a_ego[21];
+    double j_ego[21];
+    double x_l[21];
+    double v_l[21];
+    double a_l[21];
+    double cost;
     } log_t;
 
     void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc.c b/selfdrive/controls/lib/longitudinal_mpc/mpc.c
index e56eca15d..c9226d67e 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/mpc.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/mpc.c
@@ -22,13 +22,14 @@ typedef struct {
 
 
 typedef struct {
-  double x_ego[N];
-  double v_ego[N];
-  double a_ego[N];
-  double j_ego[N];
-	double x_l[N];
-	double v_l[N];
-	double a_l[N];
+  double x_ego[N+1];
+  double v_ego[N+1];
+  double a_ego[N+1];
+  double j_ego[N+1];
+	double x_l[N+1];
+	double v_l[N+1];
+	double a_l[N+1];
+	double cost;
 } log_t;
 
 void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){
@@ -53,7 +54,7 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j
     if (i > 4){
       f = STEP_MULTIPLIER;
     }
-    acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc) 
+    acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc)
     acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance
     acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration
     acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk
@@ -130,6 +131,7 @@ int run_mpc(state_t * x0, log_t * solution, double l){
 
     solution->j_ego[i] = acadoVariables.u[i];
 	}
+  solution->cost = acado_getObjective();
 
   // Dont shift states here. Current solution is closer to next timestep than if
   // we shift by 0.2 seconds.
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 6814edd34..24ae0f29a 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -112,7 +112,7 @@ class FCWChecker(object):
 
   def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
     mpc_solution_a = list(mpc_solution[0].a_ego)
-    self.last_min_a = min(mpc_solution_a[1:])
+    self.last_min_a = min(mpc_solution_a)
     self.v_lead_max = max(self.v_lead_max, v_lead)
 
     if (fcw_lead > 0.99):
@@ -127,7 +127,7 @@ class FCWChecker(object):
       self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
 
       a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
-      a_delta = min(mpc_solution_a[1:15]) - min(0.0, a_ego)
+      a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
 
       fcw_allowed = all(c >= 10 for c in self.counters.values())
       if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
@@ -164,6 +164,7 @@ class LongitudinalMpc(object):
     dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
     dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
     dat.liveLongitudinalMpc.aLead = list(self.mpc_solution[0].a_l)
+    dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
     dat.liveLongitudinalMpc.aLeadTau = self.l
     dat.liveLongitudinalMpc.qpIterations = qp_iterations
     dat.liveLongitudinalMpc.mpcId = self.mpc_id
@@ -235,10 +236,10 @@ class LongitudinalMpc(object):
     self.v_mpc_future = self.mpc_solution[0].v_ego[10]
 
     # Reset if NaN or goes through lead car
-    dls = np.array(list(self.mpc_solution[0].x_l)[1:]) - np.array(list(self.mpc_solution[0].x_ego)[1:])
+    dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego))
     crashing = min(dls) < -50.0
     nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego)))
-    backwards = min(list(self.mpc_solution[0].v_ego)[1:]) < -0.01
+    backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01
 
     if ((backwards or crashing) and self.prev_lead_status) or nans:
       if t > self.last_cloudlog_t + 5.0:
@@ -336,7 +337,7 @@ class Planner(object):
     self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
 
   # this runs whenever we get a packet that can change the plan
-  def update(self, CS, LoC, v_cruise_kph, user_distracted):
+  def update(self, CS, LaC, LoC, v_cruise_kph, user_distracted):
     cur_time = sec_since_boot()
     v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
 
@@ -459,6 +460,8 @@ class Planner(object):
       events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
     if 'fault' in self.radar_errors:
       events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans:   # TODO: find a better way to detect when MPC did not converge
+      events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
     # Interpolation of trajectory
     dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT  # no greater than dt mpc + dt, to prevent too high extraps
diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd
index 97837ea83..3f63ce391 100755
Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 10f5cca4b..477a99fce 100755
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@@ -466,9 +466,10 @@ def manager_thread():
     # have we seen a panda?
     panda_seen = panda_seen or td is not None
 
-    # start on gps if we have no connection to a panda
-    if not panda_seen:
-      should_start = should_start or passive_starter.update(started_ts, location)
+    # start on gps movement if we haven't seen ignition and are in passive mode
+    should_start = should_start or (not (ignition_seen and td) # seen ignition and panda is connected
+                                    and params.get("Passive") == "1"
+                                    and passive_starter.update(started_ts, location))
 
     # with 2% left, we killall, otherwise the phone will take a long time to boot
     should_start = should_start and avail > 0.02
diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd
index 4321c7100..24b0995d8 100755
Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ
diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord
index c470f35c0..61168b023 100755
Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ
diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c
index 4c1930872..c234511eb 100644
--- a/selfdrive/ui/ui.c
+++ b/selfdrive/ui/ui.c
@@ -921,7 +921,7 @@ static void ui_draw_vision_lanes(UIState *s) {
 
     // draw MPC only if engaged
     if (scene->engaged) {
-      draw_x_y(s, &scene->mpc_x[1], &scene->mpc_y[1], 19, nvgRGBA(255, 0, 0, 255));
+      draw_x_y(s, &scene->mpc_x[0], &scene->mpc_y[0], 20, nvgRGBA(255, 0, 0, 255));
     }
   }
 }
@@ -1190,7 +1190,12 @@ static void ui_draw_vision_speed(UIState *s) {
   nvgFontFace(s->vg, "sans-regular");
   nvgFontSize(s->vg, 36*2.5);
   nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200));
-  nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL);
+
+  if (s->is_metric) {
+    nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL);
+  } else {
+    nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL);
+  }
 }
 
 static void ui_draw_vision_wheel(UIState *s) {
diff --git a/selfdrive/version.py b/selfdrive/version.py
index 9c47ecfce..e1f9e27fa 100644
--- a/selfdrive/version.py
+++ b/selfdrive/version.py
@@ -4,10 +4,11 @@ with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "ve
   version = _versionf.read().split('"')[1]
 
 try:
-  if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]):
+  origin = subprocess.check_output(["git", "config", "--get", "remote.origin.url"])
+  if "-private" in origin:
     upstream = "origin/master"
   else:
-    if 'chffrplus' in version:
+    if 'chffrplus' in origin:
       upstream = "origin/release"
     else:
       upstream = "origin/release2"
diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond
index b9e752d16..0854b8a26 100755
Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ