wip

selfdrive/controls/controlsd.py

@@ -210,11 +210,19 @@ 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")
@@ -232,6 +240,7 @@ 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)
@@ -246,7 +255,7 @@ class Controls:
     CC, lac_log = self.state_control(CS)
     CC = self.clearpilot_state_control(CC, CS)
 
-    # Publish data
+    # Publish commands to tx on canbus (not sure why this is called logs)
     self.publish_logs(CS, start_time, CC, lac_log)
 
     self.CS_prev = CS
@@ -655,6 +664,8 @@ 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
@@ -797,6 +808,11 @@ 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
 
@@ -1104,6 +1120,8 @@ 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
@@ -1238,7 +1256,23 @@ 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
-      CC.cruiseControl.cancel = 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
+    
+
     return CC
 
 def main():