diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index ae343a5a2..301a07839 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -40,6 +40,9 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.Desire.none + # FrogPilot variables + self.lane_change_wait_timer = 0.0 + def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan, frogpilot_toggles): v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker @@ -53,9 +56,12 @@ class DesireHelper: 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 + self.lane_change_wait_timer = 0.0 # LaneChangeState.preLaneChange elif self.lane_change_state == LaneChangeState.preLaneChange: + self.lane_change_wait_timer += DT_MDL + # Set lane change direction self.lane_change_direction = LaneChangeDirection.left if \ carstate.leftBlinker else LaneChangeDirection.right @@ -64,14 +70,18 @@ class DesireHelper: ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) + if torque_applied: + self.lane_change_wait_timer = frogpilot_toggles.lane_change_delay + blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none - elif torque_applied and not blindspot_detected: + elif torque_applied and not blindspot_detected and self.lane_change_wait_timer >= frogpilot_toggles.lane_change_delay: self.lane_change_state = LaneChangeState.laneChangeStarting + self.lane_change_wait_timer = 0.0 # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: