wip

selfdrive/controls/controlsd.py

@@ -118,13 +118,15 @@ class Controls:
     # detect sound card presence and ensure successful init
     sounds_available = HARDWARE.get_sound_card_online()
 
+
     car_recognized = self.CP.carName != 'mock'
 
     # cleanup old params
+    # clearpilot: evaluate
     if not self.CP.experimentalLongitudinalAvailable:
       self.params.remove("ExperimentalLongitudinalEnabled")
-    if not self.CP.openpilotLongitudinalControl:
-      self.params.remove("ExperimentalMode")
+#    if not self.CP.openpilotLongitudinalControl:
+#      self.params.remove("ExperimentalMode")
 
     self.CC = car.CarControl.new_message()
     self.CS_prev = car.CarState.new_message()
@@ -389,6 +391,7 @@ class Controls:
     # Handle lane change
     # CLEARPILOT - Disabled lane change helper
     # CLEARPILOT TODO: Make this a toggle
+    # Clearpilot abstract this into a behavior
     NoLaneChange = False
     if not NoLaneChange:    
       if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
@@ -663,6 +666,7 @@ class Controls:
     clearpilot_disable_lat_on_lane_change = True
 
     # Enable blinkers while lane changing
+    # Abstract this into behavior
     if model_v2.meta.laneChangeState != LaneChangeState.off:
       CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
       CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
@@ -935,6 +939,7 @@ class Controls:
   def params_thread(self, evt):
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
+      # Clearpilot experimental mode - maybe change things here
       if self.CP.openpilotLongitudinalControl and not self.frogpilot_variables.conditional_experimental_mode:
         self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.speed_limit_controller and SpeedLimitController.experimental_mode
       self.personality = self.read_personality_param()
@@ -1101,7 +1106,9 @@ class Controls:
     self.FPCC.alwaysOnLateral &= self.speed_check
     self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.always_on_lateral_pause_speed) or CS.standstill
 
-    if self.CP.openpilotLongitudinalControl and self.frogpilot_variables.conditional_experimental_mode:
+    # clearpilot allow experimental in stock long
+    # removed "self.CP.openpilotLongitudinalControl and"
+    if self.frogpilot_variables.conditional_experimental_mode:
       self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimental
 
     self.drive_distance += CS.vEgo * DT_CTRL
@@ -1123,6 +1130,7 @@ class Controls:
         self.params_storage.put_int_nonblocking("FrogPilotDrives", current_total_drives + 1)
         self.drive_added = True
 
+    # Clearpilot - todo: override conditional on cruise button tap
     # if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.experimental_mode_via_lkas:
       # if self.frogpilot_variables.conditional_experimental_mode:
       #   conditional_status = self.params_memory.get_int("CEStatus")
@@ -1157,7 +1165,9 @@ class Controls:
   def update_frogpilot_params(self):
     self.always_on_lateral_pause_speed = self.always_on_lateral and self.params.get_int("PauseAOLOnBrake")
 
-    self.frogpilot_variables.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental")
+    # clearpilot allow experimental in stock long
+    # removed "self.CP.openpilotLongitudinalControl and"
+    self.frogpilot_variables.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
 
     custom_alerts = self.params.get_bool("CustomAlerts")
     self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert")
@@ -1174,7 +1184,9 @@ class Controls:
     device_management = self.params.get_bool("DeviceManagement")
     self.increase_thermal_limits = device_management and self.params.get_bool("IncreaseThermalLimits")
 
-    experimental_mode_activation = self.CP.openpilotLongitudinalControl and self.params.get_bool("ExperimentalModeActivation")
+    # clearpilot allow experimental in stock long
+    # removed "self.CP.openpilotLongitudinalControl and"
+    experimental_mode_activation = self.params.get_bool("ExperimentalModeActivation")
     self.frogpilot_variables.experimental_mode_via_distance = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaDistance")
     self.experimental_mode_via_lkas = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaLKAS")
 
@@ -1219,6 +1231,7 @@ class Controls:
 
   def update_clearpilot_events(self, CS):
     if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
+      exit() # Just cause a crash for testing
       self.events.add(EventName.clpDebug)
       # clearpilot_notice(CP_NOTICE_DEBUG, "LKAS Debug Action Invoked")
       foo = "bar"