wip

selfdrive/controls/controlsd.py

@@ -311,28 +311,30 @@ class Controls:
     # Handle lane change
     # CLEARPILOT - Disabled lane change helper
     # CLEARPILOT TODO: Make this a toggle
-    # if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
-    #   direction = self.sm['modelV2'].meta.laneChangeDirection
-    #   if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
-    #      (CS.rightBlindspot and direction == LaneChangeDirection.right):
-    #     if self.loud_blindspot_alert:
-    #       self.events.add(EventName.laneChangeBlockedLoud)
-    #     else:
-    #       self.events.add(EventName.laneChangeBlocked)
-    #   else:
-    #     if direction == LaneChangeDirection.left:
-    #       if self.sm['frogpilotPlan'].laneWidthLeft >= self.lane_detection_width:
-    #         self.events.add(EventName.preLaneChangeLeft)
-    #       else:
-    #         self.events.add(EventName.noLaneAvailable)
-    #     else:
-    #       if self.sm['frogpilotPlan'].laneWidthRight >= self.lane_detection_width:
-    #         self.events.add(EventName.preLaneChangeRight)
-    #       else:
-    #         self.events.add(EventName.noLaneAvailable)
-    # elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
-    #                                                 LaneChangeState.laneChangeFinishing):
-    #   self.events.add(EventName.laneChange)
+    NoLaneChange = False
+    if not NoLaneChange:    
+      if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
+        direction = self.sm['modelV2'].meta.laneChangeDirection
+        if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
+          (CS.rightBlindspot and direction == LaneChangeDirection.right):
+          if self.loud_blindspot_alert:
+            self.events.add(EventName.laneChangeBlockedLoud)
+          else:
+            self.events.add(EventName.laneChangeBlocked)
+        else:
+          if direction == LaneChangeDirection.left:
+            if self.sm['frogpilotPlan'].laneWidthLeft >= self.lane_detection_width:
+              self.events.add(EventName.preLaneChangeLeft)
+            else:
+              self.events.add(EventName.noLaneAvailable)
+          else:
+            if self.sm['frogpilotPlan'].laneWidthRight >= self.lane_detection_width:
+              self.events.add(EventName.preLaneChangeRight)
+            else:
+              self.events.add(EventName.noLaneAvailable)
+      elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
+                                                      LaneChangeState.laneChangeFinishing):
+        self.events.add(EventName.laneChange)
 
     for i, pandaState in enumerate(self.sm['pandaStates']):
       # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
@@ -628,10 +630,15 @@ class Controls:
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
 
+    # CLEARPILOT test lane change
+    clearpilot_disable_lat_on_lane_change = True
+
     # Enable blinkers while lane changing
     if model_v2.meta.laneChangeState != LaneChangeState.off:
       CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
       CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
+      if clearpilot_disable_lat_on_lane_change:
+        CC.latActive = False
 
     if CS.leftBlinker or CS.rightBlinker:
       self.last_blinker_frame = self.sm.frame

selfdrive/controls/lib/desire_helper.py

@@ -10,6 +10,7 @@ TurnDirection = log.Desire
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
 
+
 DESIRES = {
   LaneChangeDirection.none: {
     LaneChangeState.off: log.Desire.none,
@@ -61,6 +62,9 @@ class DesireHelper:
     self.update_frogpilot_params()
 
   def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan):
+    # Clearpilot test lane change no lat
+    clearpilot_disable_lat_on_lane_change = True
+
     v_ego = carstate.vEgo
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
     below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
@@ -71,7 +75,7 @@ class DesireHelper:
       desired_lane = frogpilotPlan.laneWidthLeft if carstate.leftBlinker else frogpilotPlan.laneWidthRight
       lane_available = desired_lane >= self.lane_detection_width
 
-    if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
+    if (not clearpilot_disable_lat_on_lane_change and not lateral_active) or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
       self.lane_change_state = LaneChangeState.off
       self.lane_change_direction = LaneChangeDirection.none
       self.turn_direction = TurnDirection.none
@@ -115,12 +119,16 @@ class DesireHelper:
 
       # 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)
+        if clearpilot_disable_lat_on_lane_change:
+          if not one_blinker:
+            self.lane_change_state = LaneChangeState.laneChangeFinishing
+        else:
+          # fade out over .5s
+          self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
 
-        # 98% certainty
-        if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
-          self.lane_change_state = LaneChangeState.laneChangeFinishing
+          # 98% certainty
+          if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
+            self.lane_change_state = LaneChangeState.laneChangeFinishing
 
       # LaneChangeState.laneChangeFinishing
       elif self.lane_change_state == LaneChangeState.laneChangeFinishing: