wip

selfdrive/controls/controlsd.py

@@ -210,19 +210,11 @@ class Controls:
     self.previous_speed_limit = 0
     self.random_event_timer = 0
     self.speed_limit_timer = 0
+
     self.green_light_mac = MovingAverageCalculator()
+
     self.update_frogpilot_params()
 
-    # Clearpilot variables go here
-    # These persist between frames.
-
-  # These variables reset every frame
-  def clearpilot_reset_frame_state(self):
-    # Clearpilot variables
-    self.force_set_speed = 0
-    self.force_cancel = False
-    self.force_resume = False
-
   def set_initial_state(self):
     if REPLAY:
       controls_state = Params().get("ReplayControlsState")
@@ -240,7 +232,6 @@ class Controls:
     CS = self.data_sample()
     cloudlog.timestamp("Data sampled")
 
-    self.clearpilot_reset_frame_state()
     self.update_events(CS)
     self.update_frogpilot_events(CS)
     self.update_clearpilot_events(CS)
@@ -255,7 +246,7 @@ class Controls:
     CC, lac_log = self.state_control(CS)
     CC = self.clearpilot_state_control(CC, CS)
 
-    # Publish commands to tx on canbus (not sure why this is called logs)
+    # Publish data
     self.publish_logs(CS, start_time, CC, lac_log)
 
     self.CS_prev = CS
@@ -664,8 +655,6 @@ class Controls:
 
     # Check which actuators can be enabled
     standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
-    
-    # Clearpilot note: this is the check for always on lateral
     CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                    (not standstill or self.joystick_mode)
     CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
@@ -808,11 +797,6 @@ class Controls:
 
     CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
     CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) and self.CP.openpilotLongitudinalControl # IMPORTANT POSSIBLY PATCH needs and oplong
-    
-    # CLearpilot
-    if self.force_cancel:
-      CC.cruiseControl.cancel = True
-
     if self.joystick_mode and self.sm.recv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
       CC.cruiseControl.cancel = True
 
@@ -1120,8 +1104,6 @@ class Controls:
     self.FPCC.alwaysOnLateral &= self.always_on_lateral
     self.FPCC.alwaysOnLateral &= self.driving_gear
     self.FPCC.alwaysOnLateral &= self.speed_check
-    
-    # Clearpilot: test: block always on lower than 30.
     self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.always_on_lateral_pause_speed) or CS.standstill
 
     # clearpilot allow experimental in stock long
@@ -1256,23 +1238,8 @@ class Controls:
   def clearpilot_state_control(self, CC, CS):
     if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
       # CC.cruiseControl.resume = True
-      self.force_cancel = True
-
-    # Master control for if lateral is active:
-    # CC.latActive
-      
-    # Current speed in "MS":
-    # CS.vEgo 
-      
-    # Behavior:
-    # Test
-    # (make this a toggle)
-    # Pause lateral below 20 if override
-    # self.params.get_int("PauseLateralSpeed") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) if quality_of_life else 0
-    if CS.vEgo < (20 * CV.MPH_TO_MS) and self.events.contains(ET.OVERRIDE_LATERAL):
-      CC.latActive = False
-    
-
+      CC.cruiseControl.cancel = True
+    # CC.actuators.speed
     return CC
 
 def main():