Merge pull request #125 from BogGyver/tb205gti-ldw-2

should_ldw changes

selfdrive/car/tesla/carcontroller.py

@@ -190,7 +190,7 @@ def __init__(self, dbc_name):
     self.prev_ldwStatus = 0
 
     self.radarVin_idx = 0
-
+    self.LDW_ENABLE_SPEED = 16 
     self.should_ldw = False
     self.ldw_numb_frame_start = 0
     self.prev_changing_lanes = False
@@ -319,16 +319,21 @@ def update(self, enabled, CS, frame, actuators, \
     if (frame % 5 == 0):
       if (changing_lanes and not self.prev_changing_lanes): #we have a transition from blinkers off to blinkers on, save the frame
         self.ldw_numb_frame_start = frame
-        print("LDW Transition detected, frame (%d)", frame)
-
+        if  (CS.v_ego > self.LDW_ENABLE_SPEED):
+          CS.UE.custom_alert_message(3, "LDW Disabled", 150, 4)
+
       # update the previous state of the blinkers (chaning_lanes      if (self.ALCA.laneChange_enabled > 1):
         self.ldw_numb_frame_start = frame 
       self.prev_changing_lanes = changing_lanes
       if self.alca_enabled:
         self.ldw_numb_frame_start = frame + 200 #we don't want LDW for 2 seconds after ALCA finishes
       #Determine if we should have LDW or not
-      self.should_ldw = (frame > (self.ldw_numb_frame_start + int( 100 * CS.ldwNumbPeriod))) 
+      self.should_ldw = (frame > (self.ldw_numb_frame_start + int( 50 * CS.ldwNumbPeriod)) and CS.v_ego > self.LDW_ENABLE_SPEED)
 
+      if self.should_ldw and self.ldw_numb_frame_start != 0:
+        self.ldw_numb_frame_start = 0
+        CS.UE.custom_alert_message(2, "LDW Enabled", 150, 4)
+
     #upodate custom UI buttons and alerts
     CS.UE.update_custom_ui()
 
@@ -701,8 +706,7 @@ def handlePathPlanSocketForCurvatureOnIC(self, pathPlanSocket, alcaStateData, CS
         self.curv0 = -self.ALCA.laneChange_direction * alcaStateData.alcaLaneWidth * alcaStateData.alcaStep / alcaStateData.alcaTotalSteps #animas late change on IC
         self.curv0 = clip(self.curv0, -3.5, 3.5)
       else:
-        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 self.should_ldw and (CS.enableLdw and (not CS.blinker_on) and (turn_signal_needed == 0)):
           if pp.lProb > LDW_LANE_PROBAB:
             lLaneC0 = -pp.lPoly[3]
             if abs(lLaneC0) < LDW_WARNING_2: