wip

selfdrive/car/hyundai/carstate.py

@@ -411,7 +411,9 @@ class CarState(CarStateBase):
     #   nonAdaptive = false,
     #   speedCluster = 0 )
 
-
+    print("Set limit")
+    print(self.calculate_speed_limit(cp, cp_cam))
+    self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
     self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
 
     return ret

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

selfdrive/controls/lib/longitudinal_planner.py

@@ -265,8 +265,11 @@ 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)
+    # print("LONG PLAN SPEEDS", sys.stderr)
+    # print(longitudinalPlan.speeds, sys.stderr)
+    # LONG PLAN SPEEDS <_io.TextIOWrapper name='<stderr>' mode='w' encoding='utf-8'>
+    # [10.240411, 10.242371, 10.248251, 10.257222, 10.267875, 10.281571, 10.285583, 10.283464, 10.281022, 10.281429, 10.281885, 10.282397, 10.282978, 10.283609, 10.282832, 10.281243, 10.279544] <_io.TextIOWrapper name='<stderr>' mode='w' encoding='utf-8'>
+
     longitudinalPlan.accels = self.a_desired_trajectory.tolist()
     longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
 

selfdrive/ui/qt/onroad.cc

@@ -619,9 +619,9 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
   QColor center_lane_color;
   
   if (is_no_lat_lane_change) {
-    center_lane_color = bg_colors[CENTER_LANE_COLOR];
-  } else {
     center_lane_color = bg_colors[CHANGE_LANE_PATH_COLOR];
+  } else {
+    center_lane_color = bg_colors[CENTER_LANE_COLOR];
   }
 
   QLinearGradient path_gradient(0, height(), 0, 0);