diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 71913636e9..f92c6c5e9f 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -105,42 +105,44 @@ class LateralPlanner(): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: - torque_applied = sm['carState'].steeringPressed and \ - ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or - (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) - - blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or - (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - - lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob - - # State transitions - # off + # LaneChangeState.off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_ll_prob = 1.0 + + # LaneChangeState.preLaneChange + elif self.lane_change_state == LaneChangeState.preLaneChange: + # Set lane change direction if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right + else: # If there are no blinkers we will go back to LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none - self.lane_change_state = LaneChangeState.preLaneChange - self.lane_change_ll_prob = 1.0 + torque_applied = sm['carState'].steeringPressed and \ + ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or + (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) + + blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or + (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - # pre - elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting - # starting + # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out over .5s self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0) + # 98% certainty + lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing - # finishing + # LaneChangeState.laneChangeFinishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index c976e740bf..c937511747 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -32ee8c7581957064cc24b6c11e1ccf80295a71ae \ No newline at end of file +1b2f2fdaf6b2fa886f62ef62dd91d5e6a8546a9e \ No newline at end of file