wip

2024-05-03 01:12:47 -05:00
parent b94be3e23e
commit 02513c94c6
5 changed files with 49 additions and 37 deletions
--- a/selfdrive/controls/controlsd.py
+++ b/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
--- a/selfdrive/controls/lib/desire_helper.py
+++ b/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)

--- a/selfdrive/manager/process_config.py
+++ b/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,16 +78,15 @@ 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("ugpsd", "system.ugpsd", only_onroad, 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),
  PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
  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),