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():

selfdrive/controls/lib/desire_helper.py

@@ -156,12 +156,13 @@ class DesireHelper:
     self.prev_one_blinker = one_blinker
     self.turn_completed &= one_blinker
 
-    if self.turn_direction != TurnDirection.none:
-      self.desire = TURN_DESIRES[self.turn_direction]
-    elif not self.turn_completed:
-      self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
-    else:
-      self.desire = log.Desire.none
+    # Clearpilot test: disabling turn desires
+    # if self.turn_direction != TurnDirection.none:
+    #   self.desire = TURN_DESIRES[self.turn_direction]
+    # elif not self.turn_completed:
+    #   self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+    # else:
+    #   self.desire = log.Desire.none
 
     # Send keep pulse once per second during LaneChangeStart.preLaneChange
     if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):

selfdrive/controls/lib/longitudinal_planner.py

@@ -20,6 +20,8 @@ from openpilot.system.version import get_short_branch
 
 from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
 
+import sys
+
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
@@ -263,6 +265,8 @@ class LongitudinalPlanner:
     longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
 
     longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
+    print("LONG PLAN SPEEDS", sys.stderr)
+    print(longitudinalPlan.speeds, sys.stderr)
     longitudinalPlan.accels = self.a_desired_trajectory.tolist()
     longitudinalPlan.jerks = self.j_desired_trajectory.tolist()