diff --git a/RELEASES.md b/RELEASES.md
index f0280d43f4..23c0e27f7e 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 8848c152d8..32662af456 100644
--- a/apk/ai.comma.plus.frame.apk
+++ b/apk/ai.comma.plus.frame.apk
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:384aa635025948050a42eb8994e21452adecaa0ba4909e1aab0b2dd3c64f5b67
-size 2105683
+oid sha256:02ad55a9fb7472bf4ec72e3392aa3be8369417283d85e40145d7c691e620f9ef
+size 2105763
diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk
index b9f3e40c75..f5357ad9e0 100644
--- a/apk/ai.comma.plus.offroad.apk
+++ b/apk/ai.comma.plus.offroad.apk
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:d994ff54de0a618538a9ada328dc6196604f22a3a25051fe8e477390be29d28a
-size 15407210
+oid sha256:cd3d8969b69bea8db3ada2d87acea3def3dab6a3cec9c8f24578425aee902343
+size 15469848
diff --git a/cereal/Makefile b/cereal/Makefile
index b384b4904e..dcf4cc2214 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 76b89c4468..beb6e8a783 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 5996cf55a4..a4bd57c810 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 14f92559d1..0b0bd839ad 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 a19134e59a..55d9dd8cd0 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 800b82c1d1..ac8f8e2cd8 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 6721f927da..5cf7951e93 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 a66d4871b0..207e5907f8 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 8c22a6166a..5e704982d6 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 e7271f7ec6..4d5c67681e 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 9c6dd31e87..5d0165c091 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 ab5be2f63f..5970dab903 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 0cae4079de..f4cee7f2d0 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 8d1814e583..4bc7398d5d 100644
--- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2fe4faa6b70f8cb9fe000412093579a5bf96890831d62c83d01deff94cdb8d19
+oid sha256:b5b42642b33d01c204d8093bee36923e59f284fae90629c584e1fb6e4369cc2c
 size 393169
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
index 8919bd4263..b45021a97f 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 e56eca15da..c9226d67e8 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 6814edd34f..24ae0f29a0 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 20480d7c04..324094d34f 100755
--- a/selfdrive/loggerd/loggerd
+++ b/selfdrive/loggerd/loggerd
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:a5ff2875db45291ee03af11b77ad4cf42b8dd4241939261c4990c8b255b894b7
+oid sha256:b22914500af41c3602576ed9b953b6eb974bf17e5febea6f5dfbf637d266f0fc
 size 1412368
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 10f5cca4b7..477a99fced 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 51e88b6f3c..12dcac3ea8 100755
--- a/selfdrive/sensord/gpsd
+++ b/selfdrive/sensord/gpsd
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:01f3447e6bd823579ee85de485fce87d8ebc815d8f51d0679d02b19e3d86eedb
+oid sha256:4538d9fc96c9ef164dbf8f39e28c13e07a6f7653caf8eb27821c244d1ce1698e
 size 981400
diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord
index c3500bd22e..8f05733784 100755
--- a/selfdrive/sensord/sensord
+++ b/selfdrive/sensord/sensord
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:0895148012045433f3319106d01273a788457146b9b44d36b34dea46a6bbee0c
+oid sha256:526aaeeba44256c1ed75c12adede4ddd2a868f9c919e979ee515443e5f8d85e3
 size 972296
diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c
index 4c19308723..c234511ebb 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 9c47ecfce6..e1f9e27fa8 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 66b5378eed..7a8d9efd3a 100755
--- a/selfdrive/visiond/visiond
+++ b/selfdrive/visiond/visiond
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:c2353fba048e2c84c75e25cd6801e49cfeab3a95c0b75d583c668a2f377d0fb4
-size 13381984
+oid sha256:9644aeee10abb334360de52a3e49df288c789132ac4a1c64ac073daf4b950396
+size 13382576