diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b71e90d..2250890 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -38,6 +38,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUIS from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS from openpilot.selfdrive.clearpilot.telemetry import tlog +from openpilot.selfdrive.clearpilot.speed_logic import SpeedState from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController SOFT_DISABLE_TIME = 3 # seconds @@ -80,6 +81,11 @@ class Controls: self.params_memory.put_bool("CPTLkasButtonAction", False) self.params_memory.put_int("ScreenDisplayMode", 0) + # CLEARPILOT: speed-limit/cruise-warning state machine + park→drive auto-reset + self.speed_state = SpeedState() + self.speed_state_frame = 0 + self.was_driving_gear = False + self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg: @@ -108,8 +114,8 @@ class Controls: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'testJoystick', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets, - ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], + 'testJoystick', 'frogpilotPlan', 'gpsLocation'] + self.camera_packets + self.sensor_packets, + ignore_alive=ignore + ['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'], ignore_valid=['testJoystick', 'gpsLocation'], frequency=int(1/DT_CTRL)) self.joystick_mode = self.params.get_bool("JoystickDebugMode") @@ -1257,6 +1263,12 @@ class Controls: self.events.add(EventName.clpDebug) def clearpilot_state_control(self, CC, CS): + # CLEARPILOT: auto-reset display when shifting into drive from screen-off + if self.driving_gear and not self.was_driving_gear: + if self.params_memory.get_int("ScreenDisplayMode") == 3: + self.params_memory.put_int("ScreenDisplayMode", 0) + self.was_driving_gear = self.driving_gear + if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents): current = self.params_memory.get_int("ScreenDisplayMode") @@ -1269,6 +1281,21 @@ class Controls: new_mode = 0 if current == 3 else 3 self.params_memory.put_int("ScreenDisplayMode", new_mode) + + # ClearPilot speed processing (~2 Hz at 100 Hz loop) — drives speed-limit sign + # and cruise over/under warning sign via memory params read by the UI. + self.speed_state_frame += 1 + if self.speed_state_frame % 50 == 0: + gps = self.sm['gpsLocation'] + has_gps = self.sm.valid['gpsLocation'] and gps.hasFix + speed_ms = gps.speed if has_gps else 0.0 + speed_limit_ms = self.params_memory.get_float("CarSpeedLimit") + is_metric = (self.params_memory.get("CarIsMetric", encoding="utf-8") or "0") == "1" + cruise_speed_ms = CS.cruiseState.speed + cruise_active = CS.cruiseState.enabled + cruise_standstill = CS.cruiseState.standstill + self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric, + cruise_speed_ms, cruise_active, cruise_standstill) return CC def main():