removed hard-coded actuator delay

icmma · Dec 16, 2018 · 5b9708c · 5b9708c
1 parent f2b706c
commit 5b9708c
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 10 deletions.
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
@@ -14,25 +14,25 @@ def __init__(self):
     self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
     self._path_pinv = compute_path_pinv()
 
-    self.lane_width_estimate = 3.7
+    self.lane_width_estimate = 3.2
     self.lane_width_certainty = 1.0
-    self.lane_width = 3.7
+    self.lane_width = 3.2
 
-  def update(self, v_ego, md, LaC=None):
+  def update(self, v_ego, md, CS=None, LaC=None):
     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
 
       lateral_error = 0.0
       try:
-        if LaC is not None and LaC.angle_steers_des_mpc != 0.0:
-          angle_error = LaC.angle_steers_des_mpc - (0.05 * LaC.avg_angle_steers + 0.1 * LaC.projected_angle_steers) / 0.15
-        if LaC is None or angle_error == 0:
-          print("2")
+        lateral_error = 0.0
+        if LaC is not None and CS is not None and CS.CP is not None and LaC.angle_steers_des_mpc != 0.0:
+          angle_error = LaC.angle_steers_des_mpc - (0.05 * LaC.avg_angle_steers + CS.CP.steerActuatorDelay * LaC.projected_angle_steers) / (CS.CP.steerActuatorDelay + 0.05)
+        if angle_error == 0.0 or LaC is None or CS is None or CS.CP is None:
           lateral_error = 0.0
         else:
-          LaC.lateral_error = -1.0 * np.clip(v_ego * 0.15 * math.tan(math.radians(angle_error)), -0.2, 0.2)
+          LaC.lateral_error = -1.0 * np.clip(v_ego * (CS.CP.steerActuatorDelay + 0.05) * math.tan(math.radians(angle_error)), -1.2, 1.2)
           lateral_error = LaC.lateral_error
 
         # only offset left and right lane lines; offsetting p_poly does not make sense
@@ -59,7 +59,7 @@ def update(self, v_ego, md, LaC=None):
       lane_width_diff = abs(self.lane_width - current_lane_width)
       lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0])
 
-      r_prob *= lane_r_prob
+      #r_prob *= lane_r_prob
 
       self.lead_dist = md.model.lead.dist
       self.lead_prob = md.model.lead.prob

diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
@@ -351,7 +351,7 @@ def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel):
       self.last_model = cur_time
       self.model_dead = False
 
-      self.PP.update(CS.vEgo, md, LaC)
+      self.PP.update(CS.vEgo, md, CS, LaC)
 
       if self.last_gps_planner_plan is not None:
         plan = self.last_gps_planner_plan.gpsPlannerPlan