mostly disable lane filter for now

kegman · Apr 7, 2019 · 9926cd4 · 9926cd4
1 parent 78fd669
commit 9926cd4
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 2 deletions.
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
@@ -341,7 +341,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
     "vEgo": CS.vEgo,
     "vEgoRaw": CS.vEgoRaw,
     "angleSteers": CS.steeringAngle,
-    "curvature": VM.calc_curvature((CS.steeringAngle - path_plan.pathPlan.angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
+    "curvature": VM.calc_curvature((LaC.dampened_actual_angle - path_plan.pathPlan.angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
     "steerOverride": CS.steeringPressed,
     "state": state,
     "engageable": not bool(get_events(events, [ET.NO_ENTRY])),

diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
@@ -25,6 +25,7 @@ def __init__(self, CP):
     self.dampened_desired_angle = 0.0
     self.rate_mode = 0.0
     self.angle_mode = 0.0
+    self.dampened_actual_angle = 0.0
 
     KpV = [interp(25.0, CP.steerKpBP, CP.steerKpV)]
     KiV = [interp(25.0, CP.steerKiBP, CP.steerKiV)]
@@ -94,6 +95,7 @@ def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan)
                                       override=steer_override, feedforward=feed_forward, speed=v_ego, deadzone=deadzone)
 
     self.sat_flag = self.pid.saturated
+    self.dampened_actual_angle += 0.1 * (angle_steers - self.dampened_actual_angle)
 
     if CP.steerControlType == car.CarParams.SteerControlType.torque:
       return float(output_steer), float(path_plan.angleSteers)

diff --git a/selfdrive/controls/lib/model_parser.py b/selfdrive/controls/lib/model_parser.py
@@ -54,7 +54,7 @@ def update(self, v_ego, md, v_curv=0.0):
       lane_width_diff = abs(self.lane_width - current_lane_width)
       lane_prob = interp(lane_width_diff, [0.3, interp(v_ego, [20.0, 25.0], [1.0, 0.4])], [1.0, 0.0])
 
-      if (l_prob > 0.5 and r_prob > 0.5 and v_ego > 22.0) or self.lane_prob == 0.0:
+      if (abs(v_curv) < 0.0005 and l_prob > 0.5 and r_prob > 0.5 and v_ego > 22.0) or self.lane_prob == 0.0:
         steer_compensation = 1.2 * v_curv * v_ego
         total_left_divergence = (md.model.leftLane.points[5] - md.model.leftLane.points[0]) * r_prob + steer_compensation
         total_right_divergence = -((md.model.rightLane.points[5] - md.model.rightLane.points[0]) * l_prob + steer_compensation)