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,6 +119,10 @@ class DesireHelper:
 
       # LaneChangeState.laneChangeStarting
       elif self.lane_change_state == LaneChangeState.laneChangeStarting:
+        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)
 

selfdrive/manager/process_config.py

@@ -5,9 +5,7 @@ from openpilot.common.params import Params
 from openpilot.system.hardware import PC, TICI
 from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
 
-from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
 
-RADARLESS = Params().get("Model", encoding='utf-8') in RADARLESS_MODELS
 WEBCAM = os.getenv("USE_WEBCAM") is not None
 
 def driverview(started: bool, params: Params, CP: car.CarParams) -> bool:
@@ -80,7 +78,7 @@ procs = [
   PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad),
   PythonProcess("deleter", "system.loggerd.deleter", always_run),
   PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),
-  PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", always_run, enabled=TICI),
+  PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),
   #PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI),
   PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
   PythonProcess("pandad", "selfdrive.boardd.pandad", always_run),
@@ -88,8 +86,7 @@ procs = [
   NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
   PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI),
   PythonProcess("plannerd", "selfdrive.controls.plannerd", only_onroad),
-  PythonProcess("radard", "selfdrive.controls.radard", only_onroad, enabled=not RADARLESS),
-  PythonProcess("radardless", "selfdrive.controls.radardless", only_onroad, enabled=RADARLESS),
+  PythonProcess("radard", "selfdrive.controls.radard", only_onroad),
   PythonProcess("thermald", "selfdrive.thermald.thermald", always_run),
   PythonProcess("tombstoned", "selfdrive.tombstoned", always_run, enabled=not PC),
   PythonProcess("updated", "selfdrive.updated.updated", always_run, enabled=not PC),