diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index e8b63a5b9..6e8d9368c 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -196,7 +196,12 @@ def __init__(self, dbc_name): self.prev_ldwStatus = 0 self.radarVin_idx = 0 - + + self.LDW_NUMB_PERIOD = 200 # 4 seconds at 50Hz + self.should_ldw = False + self.ldw_numb_frame_start = 0 + self.prev_changing_lanes = False + self.isMetric = (self.params.get("IsMetric") == "1") def reset_traffic_events(self): @@ -317,6 +322,17 @@ def update(self, enabled, CS, frame, actuators, \ # Basic highway lane change logic changing_lanes = CS.right_blinker_on or CS.left_blinker_on + + if (frame % 25 == 0): + if (self.prev_changing_lanes and not changing_lanes): #we have a transition from blinkers on to blinkers off, save the frame + self.ldw_numb_frame_start = frame + print("LDW Transition detected, frame (%d)", frame) + + # update the previous state of the blinkers (chaning_lanes) + self.prev_changing_lanes = changing_lanes + + #Determine if we should have LDW or not + self.should_ldw = (frame > (self.ldw_numb_frame_start + self.LDW_NUMB_PERIOD)) #upodate custom UI buttons and alerts CS.UE.update_custom_ui() @@ -738,7 +754,8 @@ def handlePathPlanSocketForCurvatureOnIC(self, pathPlanSocket, alcaStateData, CS self.curv0 = self.ALCA.laneChange_direction * self.laneWidth - self.curv0 self.curv0 = clip(self.curv0, -3.5, 3.5) else: - if CS.enableLdw and (not CS.blinker_on) and (CS.v_ego > 15.6) and (turn_signal_needed == 0): + if self.should_ldw and (CS.enableLdw and (not CS.blinker_on) and (CS.v_ego > 15.6) and (turn_signal_needed == 0)): + self.ldw_numb_frame_start = 0 #reset frame_start for the transition if pp.lProb > LDW_LANE_PROBAB: lLaneC0 = -pp.lPoly[3] if abs(lLaneC0) < LDW_WARNING_2: