diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 7198c6f..9e92e38 100755 --- a/selfdrive/car/hyundai/carstate.py +++ b/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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 7648c97..09345f8 100755 --- a/selfdrive/controls/controlsd.py +++ b/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(): diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index dbf7e33..bb61270 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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='' 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='' mode='w' encoding='utf-8'> + longitudinalPlan.accels = self.a_desired_trajectory.tolist() longitudinalPlan.jerks = self.j_desired_trajectory.tolist() diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index cd35333..a171804 100755 --- a/selfdrive/ui/qt/onroad.cc +++ b/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);