Merge pull request #62 from arne182/LDA

Lda working without beeping

Author: arne182

selfdrive/controls/lib/pathplanner.py

@@ -23,6 +23,7 @@ def __init__(self):
 
   def update(self, v_ego, md):
     if md is not None:
+
       p_poly = model_polyfit(md.model.path.points, self._path_pinv)  # predicted path
       l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv)  # left line
       r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv)  # right line
@@ -56,12 +57,12 @@ def update(self, v_ego, md):
       # compute target path
       self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
         l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)
-
+      
       self.r_poly = r_poly
       self.r_prob = r_prob
-
+      
       self.l_poly = l_poly
       self.l_prob = l_prob
-
+      
       self.p_poly = p_poly
       self.p_prob = p_prob

selfdrive/controls/lib/planner.py

@@ -536,9 +536,9 @@ def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel):
     plan_send.plan.vTargetFuture = self.v_acc_future
     plan_send.plan.hasLead = self.mpc1.prev_lead_status
     plan_send.plan.hasLeftLane = bool(self.PP.l_prob > 0.5)
-    plan_send.plan.hasrightLaneDepart = bool(self.PP.r_poly[3] > -0.5 and not CS.rightBlinker)
+    plan_send.plan.hasrightLaneDepart = bool(self.PP.r_poly[3] > -1.15 and not CS.rightBlinker)
     plan_send.plan.hasRightLane = bool(self.PP.r_prob > 0.5)
-    plan_send.plan.hasleftLaneDepart = bool(self.PP.l_poly[3] < 0.5 and not CS.leftBlinker)
+    plan_send.plan.hasleftLaneDepart = bool(self.PP.l_poly[3] < 1.15 and not CS.leftBlinker)
     plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
 
     plan_send.plan.gpsPlannerActive = self.gps_planner_active