Skip to content

Commit

Permalink
controlsd: save lat_plan.dPathPoints to local variable (#23462)
Browse files Browse the repository at this point in the history
  • Loading branch information
deanlee authored Jan 10, 2022
1 parent b2beb5e commit 2d08f8b
Showing 1 changed file with 4 additions and 3 deletions.
7 changes: 4 additions & 3 deletions selfdrive/controls/controlsd.py
Original file line number Diff line number Diff line change
Expand Up @@ -537,11 +537,12 @@ def state_control(self, CS):
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):

if len(lat_plan.dPathPoints):
dpath_points = lat_plan.dPathPoints
if len(dpath_points):
# Check if we deviated from the path
# TODO use desired vs actual curvature
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.20
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.20
left_deviation = actuators.steer > 0 and dpath_points[0] < -0.20
right_deviation = actuators.steer < 0 and dpath_points[0] > 0.20

if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
Expand Down

0 comments on commit 2d08f8b

Please sign in to comment.