From 47321e38677a2a107ea1e0c02141767154422273 Mon Sep 17 00:00:00 2001 From: brian Date: Sun, 26 Apr 2026 08:40:02 -0500 Subject: [PATCH] restore driving logic to pre-variable-fps baseline Wholesale revert of driving-relevant files to the snapshot in /projects/openpilot/archive/clearpilot (HEAD 980f0aa). Goal: get known-good driving behavior back, then re-introduce optimizations slowly to track down a "feels like the wheel pulls right" regression. Files restored from baseline: - selfdrive/controls/controlsd.py - selfdrive/controls/lib/events.py - selfdrive/controls/lib/longitudinal_planner.py - selfdrive/modeld/modeld.py - selfdrive/modeld/dmonitoringmodeld.py - selfdrive/locationd/calibrationd.py - selfdrive/locationd/paramsd.py - selfdrive/locationd/torqued.py - selfdrive/car/interfaces.py - selfdrive/car/hyundai/carstate.py (CAN-FD telemetry preserved as a commented block for future re-enable) - selfdrive/monitoring/dmonitoringd.py - selfdrive/frogpilot/controls/frogpilot_planner.py - common/realtime.py Intentionally NOT restored (kept as current): - selfdrive/thermald/* (fan/power tuning kept) - selfdrive/car/hyundai/carcontroller.py + hyundaicanfd.py (perf-only hoist of no_lat_lane_change Params read; behavior-equivalent) - cereal/services.py, cereal/custom.capnp (additive only) - selfdrive/manager/*, common/params.cc (heavy ClearPilot infrastructure: bench mode, log dir, dashcamd, gpsd, params) - All selfdrive/ui/, selfdrive/clearpilot/, system/clearpilot/ UI features will be re-wired in a follow-up commit. Co-Authored-By: Claude Opus 4.7 (1M context) --- common/realtime.py | 2 +- selfdrive/car/hyundai/carstate.py | 84 +++-- selfdrive/car/interfaces.py | 9 - selfdrive/controls/controlsd.py | 318 +++++------------- selfdrive/controls/lib/events.py | 2 +- .../controls/lib/longitudinal_planner.py | 35 +- .../frogpilot/controls/frogpilot_planner.py | 16 +- selfdrive/locationd/calibrationd.py | 9 +- selfdrive/locationd/paramsd.py | 11 +- selfdrive/locationd/torqued.py | 12 +- selfdrive/modeld/dmonitoringmodeld.py | 16 +- selfdrive/modeld/modeld.py | 38 +-- selfdrive/monitoring/dmonitoringd.py | 24 +- 13 files changed, 154 insertions(+), 422 deletions(-) diff --git a/common/realtime.py b/common/realtime.py index b676cd4..aad4baa 100755 --- a/common/realtime.py +++ b/common/realtime.py @@ -12,7 +12,7 @@ from openpilot.system.hardware import PC # time step for each process DT_CTRL = 0.01 # controlsd DT_MDL = 0.05 # model -DT_TRML = 0.25 # thermald and manager — 4 Hz +DT_TRML = 0.5 # thermald and manager DT_DMON = 0.05 # driver monitoring diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 3adca8e..3d4d727 100755 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -13,7 +13,6 @@ from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \ CANFD_CAR, Buttons, CarControllerParams from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.clearpilot.telemetry import tlog PREV_BUTTON_SAMPLES = 8 CLUSTER_SAMPLE_RATE = 20 # frames @@ -48,10 +47,6 @@ class CarState(CarStateBase): self.is_metric = False self.buttons_counter = 0 - # CLEARPILOT: cache to avoid per-cycle atomic writes to /dev/shm (eats CPU via fsync/flock) - self._prev_car_speed_limit = None - self._prev_car_is_metric = None - self.cruise_info = {} # On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz @@ -214,15 +209,10 @@ class CarState(CarStateBase): self.lkas_previously_enabled = self.lkas_enabled self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"] - # CLEARPILOT: gate on change — see same fix in update_canfd - car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_conv - if car_speed_limit != self._prev_car_speed_limit: - self.params_memory.put_float("CarSpeedLimit", car_speed_limit) - self._prev_car_speed_limit = car_speed_limit - if self.is_metric != self._prev_car_is_metric: - self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0") - self._prev_car_is_metric = self.is_metric - + # self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam)) + self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv) + self.params_memory.put_float("CarCruiseDisplayActual", cp_cruise.vl["SCC11"]["VSetDis"]) + return ret @@ -425,23 +415,63 @@ class CarState(CarStateBase): # nonAdaptive = false, # speedCluster = 0 ) - # CLEARPILOT: gate on change — these writes run 100Hz, each is an atomic fsync/flock transaction - car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor - if car_speed_limit != self._prev_car_speed_limit: - self.params_memory.put_float("CarSpeedLimit", car_speed_limit) - self._prev_car_speed_limit = car_speed_limit - if self.is_metric != self._prev_car_is_metric: - self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0") - self._prev_car_is_metric = self.is_metric + # 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) - # CLEARPILOT: telemetry logging — disabled, re-enable when needed + # CLEARPILOT: CAN-FD telemetry — preserved but disabled. Re-enable by uncommenting (also restore the import). + # from openpilot.selfdrive.clearpilot.telemetry import tlog + # # speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam # scc = cp_cam.vl["SCC_CONTROL"] if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp.vl["SCC_CONTROL"] # cluster = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"] - # tlog("car", { ... }) - # tlog("cruise", { ... }) - # tlog("speed_limit", { ... }) - # tlog("buttons", { ... }) + # + # tlog("car", { + # "vEgo": round(ret.vEgo, 3), + # "vEgoRaw": round(ret.vEgoRaw, 3), + # "aEgo": round(ret.aEgo, 3), + # "steeringAngleDeg": round(ret.steeringAngleDeg, 1), + # "gear": str(ret.gearShifter), + # "brakePressed": ret.brakePressed, + # "gasPressed": ret.gasPressed, + # "standstill": ret.standstill, + # "leftBlinker": ret.leftBlinker, + # "rightBlinker": ret.rightBlinker, + # }) + # + # tlog("cruise", { + # "enabled": ret.cruiseState.enabled, + # "available": ret.cruiseState.available, + # "speed": round(ret.cruiseState.speed, 3), + # "standstill": ret.cruiseState.standstill, + # "accFaulted": ret.accFaulted, + # "ACCMode": scc.get("ACCMode", 0), + # "VSetDis": scc.get("VSetDis", 0), + # "aReqRaw": round(scc.get("aReqRaw", 0), 3), + # "aReqValue": round(scc.get("aReqValue", 0), 3), + # "DISTANCE_SETTING": scc.get("DISTANCE_SETTING", 0), + # "ACC_ObjDist": round(scc.get("ACC_ObjDist", 0), 1), + # }) + # + # tlog("speed_limit", { + # "SPEED_LIMIT_1": cluster.get("SPEED_LIMIT_1", 0), + # "SPEED_LIMIT_2": cluster.get("SPEED_LIMIT_2", 0), + # "SPEED_LIMIT_3": cluster.get("SPEED_LIMIT_3", 0), + # "SCHOOL_ZONE": cluster.get("SCHOOL_ZONE", 0), + # "CHIME_1": cluster.get("CHIME_1", 0), + # "CHIME_2": cluster.get("CHIME_2", 0), + # "SPEED_CHANGE_BLINKING": cluster.get("SPEED_CHANGE_BLINKING", 0), + # "calculated": self.calculate_speed_limit(cp, cp_cam), + # "is_metric": self.is_metric, + # }) + # + # tlog("buttons", { + # "cruise_button": self.cruise_buttons[-1], + # "main_button": self.main_buttons[-1], + # "lkas_enabled": self.lkas_enabled, + # "main_enabled": self.main_enabled, + # }) return ret diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 0c39d12..a46bf44 100755 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -484,19 +484,10 @@ class CarInterfaceBase(ABC): self.silent_steer_warning = True events.add(EventName.steerTempUnavailableSilent) else: - # CLEARPILOT: log once per instance of this warning - if not getattr(self, '_steer_fault_logged', False): - import sys - print(f"CLP steerTempUnavailable: steerFaultTemporary={cs_out.steerFaultTemporary} " - f"steeringPressed={cs_out.steeringPressed} standstill={cs_out.standstill} " - f"steering_unpressed={self.steering_unpressed} steeringAngleDeg={cs_out.steeringAngleDeg:.1f} " - f"steeringTorque={cs_out.steeringTorque:.1f} vEgo={cs_out.vEgo:.2f}", file=sys.stderr) - self._steer_fault_logged = True events.add(EventName.steerTempUnavailable) else: self.no_steer_warning = False self.silent_steer_warning = False - self._steer_fault_logged = False if cs_out.steerFaultPermanent: events.add(EventName.steerUnavailable) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 44a32a9..e050505 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -37,8 +37,6 @@ from openpilot.system.version import get_short_branch from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED, PROBABILITY, MovingAverageCalculator 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 @@ -49,7 +47,7 @@ CAMERA_OFFSET = 0.04 REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ TESTING_CLOSET = "TESTING_CLOSET" in os.environ -IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd", "telemetryd", "dashcamd"} +IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"} ThermalStatus = log.DeviceState.ThermalStatus State = log.ControlsState.OpenpilotState @@ -79,11 +77,7 @@ class Controls: self.params_storage = Params("/persist/params") self.params_memory.put_bool("CPTLkasButtonAction", False) - self.params_memory.put_int("ScreenDisplayMode", 0) - - # ClearPilot speed processing - self.speed_state = SpeedState() - self.speed_state_frame = 0 + self.params_memory.put_bool("ScreenDisplayMode", 0) self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS @@ -113,9 +107,8 @@ class Controls: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'testJoystick', 'frogpilotPlan', 'gpsLocation'] + self.camera_packets + self.sensor_packets, - ignore_alive=ignore+['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'], - ignore_valid=['testJoystick', 'gpsLocation'], + 'testJoystick', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets, + ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], frequency=int(1/DT_CTRL)) self.joystick_mode = self.params.get_bool("JoystickDebugMode") @@ -170,26 +163,6 @@ class Controls: self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = None self.not_running_prev = None - self.lat_requested_ts = 0 # CLEARPILOT: timestamp when lat was first requested (ramp-up delay + comm issue suppression) - self.prev_lat_requested = False - # CLEARPILOT: gate frogpilotCarControl send on change (3 static bools, was publishing 100Hz) - self._prev_fpcc = (None, None, None, None, None) - self._last_fpcc_send_frame = 0 - # CLEARPILOT: hysteresis counters for transient-filter on cascade alerts. A blip - # under 50ms (5 cycles at 100Hz) is typical of MSGQ conflate + slow-polling-consumer - # freq-measurement artifacts — not a real comm issue. Persistent failure still - # alerts after 50ms, well inside a driver's reaction window. - self._hyst_commissue = 0 - self._hyst_locationd_tmp = 0 - self._hyst_paramsd_tmp = 0 - self._hyst_posenet = 0 - self.HYST_CYCLES = 5 - # CLEARPILOT: parked-cycle skip — in park+ignition-on, run the full step() only - # every 10th cycle. CAN parse + CAN TX still happen every cycle; outer logic - # (events, state machine, PID/MPC, publishing) runs at 10Hz. Cached message - # bytes are re-published on skipped cycles so downstream freq_ok stays OK. - self._cached_controlsState_bytes = None - self._cached_carControl_bytes = None self.steer_limited = False self.desired_curvature = 0.0 self.experimental_mode = False @@ -224,8 +197,6 @@ class Controls: self.always_on_lateral_main = self.always_on_lateral and self.params.get_bool("AlwaysOnLateralMain") self.drive_added = False - self.driving_gear = False - self.was_driving_gear = False self.fcw_random_event_triggered = False self.holiday_theme_alerted = False self.onroad_distance_pressed = False @@ -264,51 +235,27 @@ class Controls: CS = self.data_sample() cloudlog.timestamp("Data sampled") - # CLEARPILOT: handle debug-button press + display-mode transitions on every - # cycle — button edge events only live in the cycle's CS.buttonEvents and - # would otherwise be dropped on skipped cycles. - self.handle_screen_mode(CS) + self.update_events(CS) + self.update_frogpilot_events(CS) + self.update_clearpilot_events(CS) - # CLEARPILOT: in park, only run the full step() every 10th cycle. data_sample - # above still runs (CAN parse, button detection). Below, card.controls_update - # still runs (CAN TX heartbeat, counters increment). The skipped outer logic - # (events, state machine, PID/MPC, publishing) causes at most ~100ms lag on - # button→state transitions, which is fine in park. Cached message bytes are - # re-sent so downstream consumers see steady 100Hz. - parked = CS.gearShifter == car.CarState.GearShifter.park - full_cycle = (not parked) or (self.sm.frame % 10 == 0) or (self._cached_controlsState_bytes is None) + cloudlog.timestamp("Events updated") - if full_cycle: - self.update_events(CS) - self.update_frogpilot_events(CS) - self.update_clearpilot_events(CS) + if not self.CP.passive and self.initialized: + # Update control state + self.state_transition(CS) - cloudlog.timestamp("Events updated") + # Compute actuators (runs PID loops and lateral MPC) + CC, lac_log = self.state_control(CS) + CC = self.clearpilot_state_control(CC, CS) - if not self.CP.passive and self.initialized: - # Update control state - self.state_transition(CS) + # Publish data + self.publish_logs(CS, start_time, CC, lac_log) - # Compute actuators (runs PID loops and lateral MPC) - CC, lac_log = self.state_control(CS) - CC = self.clearpilot_state_control(CC, CS) - - # Publish data (also sends CAN TX via card.controls_update inside) - self.publish_logs(CS, start_time, CC, lac_log) - - self.CS_prev = CS - - # Update FrogPilot variables - self.update_frogpilot_variables(CS) - else: - # CAN TX heartbeat: keep counters incrementing and CAN frames flowing to the car - if not self.CP.passive and self.initialized: - self.card.controls_update(self.CC, self.frogpilot_variables) - # Re-publish cached messages so downstream freq_ok checks don't trip - self.pm.send('controlsState', self._cached_controlsState_bytes) - self.pm.send('carControl', self._cached_carControl_bytes) - self.CS_prev = CS + self.CS_prev = CS + # Update FrogPilot variables + self.update_frogpilot_variables(CS) def data_sample(self): """Receive data from sockets and update carState""" @@ -494,24 +441,6 @@ class Controls: # All events here should at least have NO_ENTRY and SOFT_DISABLE. num_events = len(self.events) - # CLEARPILOT: compute model standby suppression early — used by multiple checks below - try: - standby_ts = float(self.params_memory.get("ModelStandbyTs") or "0") - except (ValueError, TypeError): - standby_ts = 0 - now = time.monotonic() - model_suppress = (now - standby_ts) < 2.0 - - # CLEARPILOT: suppress commIssue/location/params errors for 2s after lat was requested - # Model FPS jumps from 4 to 20 on engage, causing transient freq check failures - # lat_requested_ts is set in state_control on the False->True transition of LatRequested - # CLEARPILOT: 2s window — long enough to cover the modeld rate transition burst - # without hiding real comms issues. Extending this further risks masking a genuine - # fault that demands immediate driver takeover. - lat_engage_suppress = (now - self.lat_requested_ts) < 2.0 - - # CLEARPILOT: expose suppress flags + standby_ts for the debug dumper in step() - not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES): self.events.add(EventName.processNotRunning) @@ -524,17 +453,10 @@ class Controls: self.events.add(EventName.cameraMalfunction) elif not self.sm.all_freq_ok(self.camera_packets): self.events.add(EventName.cameraFrameRate) - # CLEARPILOT: alert only when avg cycle time exceeds 20ms (≈50Hz effective). - # Stock rk.lagging fires at 11.1ms (90% of 10ms) which the Hyundai CAN load - # routinely crosses while driving — that's normal, not a fault. 50Hz control is - # still plenty responsive. `rk.lagging` is still used defensively elsewhere - # (lines ~479, 492) to skip secondary checks when slightly overloaded. - avg_dt = sum(self.rk._dts) / len(self.rk._dts) - alert_lagging = avg_dt > 0.020 - if not REPLAY and alert_lagging and not model_suppress and not lat_engage_suppress: + if not REPLAY and self.rk.lagging: self.events.add(EventName.controlsdLagging) if not self.radarless_model: - if not model_suppress and (len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState']))): + if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])): self.events.add(EventName.radarFault) if not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) @@ -546,60 +468,34 @@ class Controls: # generic catch-all. ideally, a more specific event should be added above instead has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE)) no_system_errors = (not has_disable_events) or (len(self.events) == num_events) - # CLEARPILOT: commIssue fires ONLY when messages actually aren't flowing. If all - # subscribers are alive and CAN isn't timing out, comms are fine — any `valid=False` - # is an upstream self-declaration that cascades from the MSGQ conflate + - # slow-polling-consumer freq_ok measurement artifact. The car drives correctly - # during these cascades (content is still usable), so we stop raising the banner. - # Genuine comms failures — missing messages or CAN RX timeout — still fire. - comms_really_broken = (not self.sm.all_alive()) or self.card.can_rcv_timeout - commissue_cond = comms_really_broken and no_system_errors and not model_suppress and not lat_engage_suppress - if commissue_cond: - self._hyst_commissue += 1 - else: - self._hyst_commissue = 0 - if self._hyst_commissue >= self.HYST_CYCLES: - self.events.add(EventName.commIssue) - if commissue_cond: - # Log ONCE per entry into comms-really-broken state — no hyst counter so it doesn't - # spam every cycle (hyst changes every cycle and would make logs dict always differ). + if (not self.sm.all_checks() or self.card.can_rcv_timeout) and no_system_errors: + if not self.sm.all_alive(): + self.events.add(EventName.commIssue) + elif not self.sm.all_freq_ok(): + self.events.add(EventName.commIssueAvgFreq) + else: # invalid or can_rcv_timeout. + self.events.add(EventName.commIssue) + logs = { 'invalid': [s for s, valid in self.sm.valid.items() if not valid], 'not_alive': [s for s, alive in self.sm.alive.items() if not alive], + 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], 'can_rcv_timeout': self.card.can_rcv_timeout, } if logs != self.logged_comm_issue: - import sys, json - print("CLP commIssue " + json.dumps(logs), file=sys.stderr, flush=True) + cloudlog.event("commIssue", error=True, **logs) self.logged_comm_issue = logs else: self.logged_comm_issue = None if not (self.CP.notCar and self.joystick_mode): - # CLEARPILOT: suppress locationd/paramsd "temporary error" when we're in the - # freq-only cascade (same root as the commIssue suppression above). These events - # cascade from upstream freq_ok artifacts — locationd's filterInitialized latches - # False if calibrationd tips invalid due to freq, which cascades here. - # Real failures still fire commIssue above via alive/valid/can_rcv_timeout gate. - freq_only_cascade = (not self.sm.all_checks()) and not comms_really_broken - - # deviceFalling is real-physics (shock sensor), no hysteresis, no cascade suppression. + if not self.sm['liveLocationKalman'].posenetOK: + self.events.add(EventName.posenetInvalid) if not self.sm['liveLocationKalman'].deviceStable: self.events.add(EventName.deviceFalling) - - posenet_bad = not self.sm['liveLocationKalman'].posenetOK and not model_suppress and not lat_engage_suppress and not freq_only_cascade - self._hyst_posenet = self._hyst_posenet + 1 if posenet_bad else 0 - if self._hyst_posenet >= self.HYST_CYCLES: - self.events.add(EventName.posenetInvalid) - - locd_bad = not self.sm['liveLocationKalman'].inputsOK and not model_suppress and not lat_engage_suppress and not freq_only_cascade - self._hyst_locationd_tmp = self._hyst_locationd_tmp + 1 if locd_bad else 0 - if self._hyst_locationd_tmp >= self.HYST_CYCLES: + if not self.sm['liveLocationKalman'].inputsOK: self.events.add(EventName.locationdTemporaryError) - - paramsd_bad = not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY) and not model_suppress and not lat_engage_suppress and not freq_only_cascade - self._hyst_paramsd_tmp = self._hyst_paramsd_tmp + 1 if paramsd_bad else 0 - if self._hyst_paramsd_tmp >= self.HYST_CYCLES: + if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): self.events.add(EventName.paramsdTemporaryError) # conservative HW alert. if the data or frequency are off, locationd will throw an error @@ -613,8 +509,16 @@ class Controls: if self.cruise_mismatch_counter > int(6. / DT_CTRL): self.events.add(EventName.cruiseMismatch) - # CLEARPILOT: FCW disabled — car's own radar AEB works better and triggers reliably. - # The comma FCW was producing false positives and adds nothing over the stock system. + # Check for FCW + stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25 + model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking + planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled + if planner_fcw or model_fcw: + self.events.add(EventName.fcw) + # self.fcw_random_event_triggered = True + # elif self.fcw_random_event_triggered and self.random_events: + # self.events.add(EventName.yourFrogTriedToKillMe) + # self.fcw_random_event_triggered = False for m in messaging.drain_sock(self.log_sock, wait_for_one=False): try: @@ -637,7 +541,7 @@ class Controls: self.distance_traveled = 0 self.distance_traveled += CS.vEgo * DT_CTRL - if self.sm['modelV2'].frameDropPerc > 20 and not model_suppress: + if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) @@ -724,14 +628,6 @@ class Controls: # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES self.active = self.state in ACTIVE_STATES - - # CLEARPILOT: engagement telemetry disabled — was running at 100Hz, causing CPU load - # tlog("engage", { - # "state": self.state.name if hasattr(self.state, 'name') else str(self.state), - # "enabled": self.enabled, "active": self.active, - # "cruise_enabled": CS.cruiseState.enabled, "cruise_available": CS.cruiseState.available, - # "brakePressed": CS.brakePressed, - # }) if self.active: self.current_alert_types.append(ET.WARNING) @@ -762,19 +658,8 @@ class Controls: # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill - # CLEARPILOT: lat_requested ignores momentary MDPS faults so modeld doesn't drop rate during - # brief steerFaultTemporary blips (stale predictions on recovery caused a fault feedback loop). - # lat_engaged gates the actual steering command and still respects the fault. - lat_requested = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and \ - (not standstill or self.joystick_mode) - lat_engaged = lat_requested and not CS.steerFaultTemporary and not CS.steerFaultPermanent - - now_ts = time.monotonic() - if lat_requested and not self.prev_lat_requested: - self.lat_requested_ts = now_ts - self.prev_lat_requested = lat_requested - self.FPCC.latRequested = lat_requested - CC.latActive = lat_engaged and (now_ts - self.lat_requested_ts) >= 0.25 + 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 actuators = CC.actuators @@ -789,13 +674,11 @@ class Controls: CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right - no_lat_lane_change = model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change - if no_lat_lane_change: + if model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change: CC.latActive = False - # CLEARPILOT: noLatLaneChange now lives on frogpilotCarControl (see update_frogpilot_variables). - # Also mirrored to frogpilot_variables so in-process carcontroller reads it without IPC. - self.FPCC.noLatLaneChange = no_lat_lane_change - self.frogpilot_variables.no_lat_lane_change = no_lat_lane_change + self.params_memory.put_bool("no_lat_lane_change", True) + else: + self.params_memory.put_bool("no_lat_lane_change", False) if CS.leftBlinker or CS.rightBlinker: self.last_blinker_frame = self.sm.frame @@ -1031,8 +914,6 @@ class Controls: controlsState.lateralControlState.torqueState = lac_log self.pm.send('controlsState', dat) - # CLEARPILOT: cache for re-publication on parked-skip cycles - self._cached_controlsState_bytes = dat.to_bytes() # onroadEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): @@ -1047,7 +928,6 @@ class Controls: cc_send.valid = CS.canValid cc_send.carControl = CC self.pm.send('carControl', cc_send) - self._cached_carControl_bytes = cc_send.to_bytes() # copy CarControl to pass to CarInterface on the next iteration self.CC = CC @@ -1078,11 +958,6 @@ class Controls: while True: self.step() self.rk.monitor_time() - # CLEARPILOT: accumulated debt from startup/blocking calls never recovers. - # Reset the rate keeper when catastrophically behind — prevents a one-time - # ~8s fingerprinting stall from poisoning the lag metric for the whole session. - if self.rk.remaining < -1.0: - self.rk._next_frame_time = time.monotonic() + self.rk._interval except SystemExit: e.set() t.join() @@ -1153,11 +1028,13 @@ class Controls: elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight: self.events.add(EventName.turningRight) - # CLEARPILOT: check crash file once per second, not every cycle (stat syscall at 100Hz hurts) - if self.sm.frame % 100 == 0: - self._crashed_file_exists = os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) - if getattr(self, '_crashed_file_exists', False) and self.crashed_timer < 10: + if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) and self.crashed_timer < 10: self.events.add(EventName.openpilotCrashed) + + # if self.random_events and not self.openpilot_crashed_triggered: + # self.events.add(EventName.openpilotCrashedRandomEvents) + # self.openpilot_crashed_triggered = True + self.crashed_timer += DT_CTRL # if self.speed_limit_alert or self.speed_limit_confirmation: @@ -1280,18 +1157,10 @@ class Controls: self.FPCC.trafficModeActive = self.frogpilot_variables.traffic_mode and self.params_memory.get_bool("TrafficModeActive") - # CLEARPILOT: send only when any field changes, with 1Hz keepalive - # Saves ~7ms/sec of capnp+zmq work (was sending 100Hz despite static contents) - # latRequested and noLatLaneChange are edge-triggered too (rare flips) - fpcc_tuple = (self.FPCC.alwaysOnLateral, self.FPCC.speedLimitChanged, self.FPCC.trafficModeActive, - self.FPCC.latRequested, self.FPCC.noLatLaneChange) - if fpcc_tuple != self._prev_fpcc or (self.sm.frame - self._last_fpcc_send_frame) >= 100: - fpcc_send = messaging.new_message('frogpilotCarControl') - fpcc_send.valid = CS.canValid - fpcc_send.frogpilotCarControl = self.FPCC - self.pm.send('frogpilotCarControl', fpcc_send) - self._prev_fpcc = fpcc_tuple - self._last_fpcc_send_frame = self.sm.frame + fpcc_send = messaging.new_message('frogpilotCarControl') + fpcc_send.valid = CS.canValid + fpcc_send.frogpilotCarControl = self.FPCC + self.pm.send('frogpilotCarControl', fpcc_send) if self.params_memory.get_bool("FrogPilotTogglesUpdated"): self.update_frogpilot_params() @@ -1364,53 +1233,32 @@ class Controls: self.frogpilot_variables.use_ev_tables = self.params.get_bool("EVTable") def update_clearpilot_events(self, CS): - if (len(CS.buttonEvents) > 0): + if (len(CS.buttonEvents) > 0): print (CS.buttonEvents) - if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents): - self.events.add(EventName.clpDebug) - def handle_screen_mode(self, CS): - """CLEARPILOT: tracks driving_gear, auto-resets display, and cycles - ScreenDisplayMode on debug-button presses. Must run every cycle so button - edge events aren't lost during parked-skip mode (edges happen in carstate - edge detection and only appear in that one cycle's CS.buttonEvents).""" - self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) - - # 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") - - if self.driving_gear: - # Onroad: 0→4, 1→2, 2→3, 3→4, 4→2 (never back to auto via button) - transitions = {0: 4, 1: 2, 2: 3, 3: 4, 4: 2} - new_mode = transitions.get(current, 0) - else: - # Not in drive: any except 3 → 3, state 3 → 0 - new_mode = 0 if current == 3 else 3 - - self.params_memory.put_int("ScreenDisplayMode", new_mode) + # Uncomment to alert when lkas button pressed + # if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents): + # self.events.add(EventName.clpDebug) def clearpilot_state_control(self, CC, CS): + if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents): + + # Uncomment for debug testing. + # if self.params_memory.get_bool("CPTLkasButtonAction"): + # self.params_memory.put_bool("CPTLkasButtonAction", False) + # else: + # self.params_memory.put_bool("CPTLkasButtonAction", True) + + # Rotate display mode. These are mostly used in the frontend ui app. + max_display_mode = 2 + current_display_mode = self.params_memory.get_int("ScreenDisplayMode") + current_display_mode = current_display_mode + 1 + if current_display_mode > max_display_mode: + current_display_mode = 0 + self.params_memory.put_int("ScreenDisplayMode", current_display_mode) - # ClearPilot speed processing (~2 Hz at 100 Hz loop) - 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) - + # Leftovers + # self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH ) return CC def main(): diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index d0805fc..6d23406 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -778,8 +778,8 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"), }, - # CLEARPILOT: alert suppressed — event still fires for screen toggle and future actions EventName.clpDebug: { + ET.PERMANENT: clp_debug_notice, }, EventName.noGps: { diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index fbcad91..bb61270 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -143,10 +143,6 @@ class LongitudinalPlanner: self.params = Params() self.params_memory = Params("/dev/shm/params") - # CLEARPILOT: track model FPS for dynamic dt adjustment - self.model_fps = 20 - self._dbg_prev_valid = True # CLEARPILOT: diagnostic logging on valid transitions - self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS self.release = get_short_branch() == "clearpilot" @@ -177,19 +173,6 @@ class LongitudinalPlanner: return x, v, a, j def update(self, sm): - # CLEARPILOT: read actual model FPS and adjust dt accordingly - model_fps_raw = self.params_memory.get("ModelFps") - if model_fps_raw is not None: - try: - fps = int(model_fps_raw) - if fps > 0 and fps != self.model_fps: - self.model_fps = fps - self.dt = 1.0 / fps - self.v_desired_filter.dt = self.dt - self.v_desired_filter.update_alpha(2.0) # rc=2.0, same as __init__ - except (ValueError, ZeroDivisionError): - pass - self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo @@ -256,10 +239,7 @@ class LongitudinalPlanner: self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone - # CLEARPILOT: scale crash_cnt threshold by FPS — at 20fps need 3 frames (0.15s), - # at 4fps need 1 frame (0.25s already exceeds 0.15s window) - fcw_threshold = max(0, int(0.15 * self.model_fps) - 1) # 20fps->2, 4fps->0 - self.fcw = self.mpc.crash_cnt > fcw_threshold and not sm['carState'].standstill + self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill if self.fcw: cloudlog.info("FCW triggered") @@ -278,18 +258,7 @@ class LongitudinalPlanner: def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') - valid = sm.all_checks(service_list=['carState', 'controlsState']) - # CLEARPILOT: log on transition into invalid — stderr goes to our plannerd.log - if valid != self._dbg_prev_valid and not valid: - import sys - print( - "CLP longitudinalPlan valid=False: " - f"carState(a={sm.alive['carState']},v={sm.valid['carState']},f={sm.freq_ok['carState']}) " - f"controlsState(a={sm.alive['controlsState']},v={sm.valid['controlsState']},f={sm.freq_ok['controlsState']})", - file=sys.stderr, flush=True - ) - self._dbg_prev_valid = valid - plan_send.valid = valid + plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index a04c4bf..78bf726 100755 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -58,9 +58,6 @@ class FrogPilotPlanner: self.params = Params() self.params_memory = Params("/dev/shm/params") - # CLEARPILOT: track valid transitions so we only log when it flips, not every cycle - self._dbg_prev_valid = True - self.cem = ConditionalExperimentalMode() self.lead_one = Lead() self.mtsc = MapTurnSpeedController() @@ -245,18 +242,7 @@ class FrogPilotPlanner: def publish(self, sm, pm): frogpilot_plan_send = messaging.new_message('frogpilotPlan') - valid = sm.all_checks(service_list=['carState', 'controlsState']) - # CLEARPILOT: log on transition into invalid — stderr goes to our plannerd.log - if valid != self._dbg_prev_valid and not valid: - import sys - print( - "CLP frogpilotPlan valid=False: " - f"carState(a={sm.alive['carState']},v={sm.valid['carState']},f={sm.freq_ok['carState']}) " - f"controlsState(a={sm.alive['controlsState']},v={sm.valid['controlsState']},f={sm.freq_ok['controlsState']})", - file=sys.stderr, flush=True - ) - self._dbg_prev_valid = valid - frogpilot_plan_send.valid = valid + frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan frogpilotPlan.accelerationJerk = A_CHANGE_COST * (float(self.jerk) if self.lead_one.status else 1) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index a92d622..6e154bf 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -284,14 +284,7 @@ def main() -> NoReturn: # 4Hz driven by cameraOdometry if sm.frame % 5 == 0: - # CLEARPILOT: publish valid based on calibration status, not upstream sm.all_checks(). - # Original openpilot gated valid on fresh inputs, but that caused a cascade: - # upstream freq glitches → liveCalibration.valid=False → locationd stays - # uninitialized → paramsd fed garbage → bogus steerRatio/stiffnessFactor → erratic - # steering. "valid" semantically means "calibration data is trustworthy"; that's a - # question about calibration convergence, not input freshness. - cal_valid = calibrator.cal_status == log.LiveCalibrationData.Status.calibrated - calibrator.send_data(pm, cal_valid) + calibrator.send_data(pm, sm.all_checks()) if __name__ == "__main__": diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 7b57e71..0cfae89 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -187,7 +187,6 @@ def main(): avg_offset_valid = True total_offset_valid = True roll_valid = True - dbg_prev_valid = True # CLEARPILOT: track valid transitions while True: sm.update() @@ -257,15 +256,7 @@ def main(): liveParameters.filterState.std = P.tolist() liveParameters.filterState.valid = True - lp_valid = sm.all_checks() - # CLEARPILOT: on transition to invalid, log per-subscriber state to paramsd.log - if lp_valid != dbg_prev_valid and not lp_valid: - import sys - bad = [s for s in sm.alive if not (sm.alive[s] and sm.valid[s] and sm.freq_ok.get(s, True))] - details = [f"{s}(a={sm.alive[s]},v={sm.valid[s]},f={sm.freq_ok[s]})" for s in bad] - print(f"CLP liveParameters valid=False: {' '.join(details)}", file=sys.stderr, flush=True) - dbg_prev_valid = lp_valid - msg.valid = lp_valid + msg.valid = sm.all_checks() if sm.frame % 1200 == 0: # once a minute params = { diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 98d0b29..06f9044 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -226,8 +226,6 @@ def main(demo=False): with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: estimator = TorqueEstimator(CP) - dbg_prev_valid = True # CLEARPILOT: track valid transitions - while True: sm.update() if sm.all_checks(): @@ -238,15 +236,7 @@ def main(demo=False): # 4Hz driven by liveLocationKalman if sm.frame % 5 == 0: - tq_valid = sm.all_checks() - # CLEARPILOT: log per-sub detail on transition to invalid — goes to torqued.log - if tq_valid != dbg_prev_valid and not tq_valid: - import sys - bad = [s for s in sm.alive if not (sm.alive[s] and sm.valid[s] and sm.freq_ok.get(s, True))] - details = [f"{s}(a={sm.alive[s]},v={sm.valid[s]},f={sm.freq_ok[s]})" for s in bad] - print(f"CLP liveTorqueParameters valid=False: {' '.join(details)}", file=sys.stderr, flush=True) - dbg_prev_valid = tq_valid - pm.send('liveTorqueParameters', estimator.get_msg(valid=tq_valid)) + pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) # Cache points every 60 seconds while onroad if sm.frame % 240 == 0: diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 9476fba..ef403b4 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -128,14 +128,10 @@ def main(): assert vipc_client.is_connected() cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") - sm = SubMaster(["liveCalibration", "carState"]) + sm = SubMaster(["liveCalibration"]) pm = PubMaster(["driverStateV2"]) calib = np.zeros(CALIB_LEN, dtype=np.float32) - # CLEARPILOT: cache last model output so we can republish (not re-infer) at standstill. - # Saves ~7% CPU; downstream dmonitoringd sees a steady 10Hz stream with known-good - # last readings (driver can't become distracted relative to a stopped car). - last_model_output = None # last = 0 while True: @@ -147,16 +143,8 @@ def main(): if sm.updated["liveCalibration"]: calib[:] = np.array(sm["liveCalibration"].rpyCalib) - standstill = sm["carState"].standstill - t1 = time.perf_counter() - if standstill and last_model_output is not None: - # CLEARPILOT: reuse last inference at standstill - model_output = last_model_output - dsp_execution_time = 0.0 - else: - model_output, dsp_execution_time = model.run(buf, calib) - last_model_output = model_output + model_output, dsp_execution_time = model.run(buf, calib) t2 = time.perf_counter() pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time)) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index a9ea1b7..abbc39a 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -134,15 +134,11 @@ def main(demo=False): setproctitle(PROCESS_NAME) config_realtime_process(7, 54) - import time as _time cloudlog.warning("setting up CL context") - _t0 = _time.monotonic() cl_context = CLContext() - _t1 = _time.monotonic() - cloudlog.warning("CL context ready in %.3fs; loading model", _t1 - _t0) + cloudlog.warning("CL context ready; loading model") model = ModelState(cl_context) - _t2 = _time.monotonic() - cloudlog.warning("model loaded in %.3fs (total init %.3fs), modeld starting", _t2 - _t1, _t2 - _t0) + cloudlog.warning("models loaded, modeld starting") # visionipc clients while True: @@ -183,9 +179,6 @@ def main(demo=False): model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False - model_standby = False - last_standby_ts_write = 0 - params_memory = Params("/dev/shm/params") nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32) nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32) buf_main, buf_extra = None, None @@ -240,33 +233,6 @@ def main(demo=False): meta_extra = meta_main sm.update(0) - - # CLEARPILOT: two-state modeld — 0fps only when parked (ignition-on means - # engine running in park; ignition-off stops modeld at the manager level). - # Standstill in drive (red light) keeps running so lateral stays responsive - # and liveCalibration/paramsd observations continue. - parked = sm['carState'].gearShifter == car.CarState.GearShifter.park - should_standby = parked - if should_standby and not model_standby: - params_memory.put_bool("ModelStandby", True) - params_memory.put("ModelFps", "0") - model_standby = True - cloudlog.warning("modeld: standby ON") - elif not should_standby and model_standby: - params_memory.put_bool("ModelStandby", False) - params_memory.put("ModelFps", "20") - model_standby = False - run_count = 0 - frame_dropped_filter.x = 0. - cloudlog.warning("modeld: standby OFF") - if model_standby: - now = _time.monotonic() - if now - last_standby_ts_write > 1.0: - params_memory.put("ModelStandbyTs", str(now)) - last_standby_ts_write = now - last_vipc_frame_id = meta_main.frame_id - continue - desire = DH.desire is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 020a201..579e790 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -21,7 +21,6 @@ def dmonitoringd_thread(): v_cruise_last = 0 driver_engaged = False - dbg_prev_valid = True # CLEARPILOT: track valid transitions # 10Hz <- dmonitoringmodeld while True: @@ -44,18 +43,7 @@ def dmonitoringd_thread(): # Get data from dmonitoringmodeld events = Events() - # CLEARPILOT: narrow update_states gate. The original sm.all_checks() also - # required modelV2 fresh (stops at standstill in two-state modeld) and - # liveCalibration.valid (calibrationd cascades its own freq_ok to valid, which - # flaps). Both made DM freeze pose → face_detected stuck False → awareness - # decayed to 0 within 6s of engagement. Narrow the gate to the subs - # update_states actually reads, and only to alive+valid (skip freq_ok and - # skip liveCalibration.valid). rpyCalib presence is sufficient to know - # calibration has produced output. - if (sm.alive['driverStateV2'] and sm.valid['driverStateV2'] and - sm.alive['carState'] and sm.valid['carState'] and - sm.alive['controlsState'] and sm.valid['controlsState'] and - sm.alive['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) > 0): + if sm.all_checks() and len(sm['liveCalibration'].rpyCalib): driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations @@ -66,16 +54,8 @@ def dmonitoringd_thread(): # Update events from driver state driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) - # CLEARPILOT: log on transition to invalid so we can see which sub caused the cascade - dm_valid = sm.all_checks() - if dm_valid != dbg_prev_valid and not dm_valid: - import sys - bad = [s for s in sm.alive if not (sm.alive[s] and sm.valid[s] and sm.freq_ok.get(s, True))] - details = [f"{s}(a={sm.alive[s]},v={sm.valid[s]},f={sm.freq_ok[s]})" for s in bad] - print(f"CLP driverMonitoringState valid=False: {' '.join(details)}", file=sys.stderr, flush=True) - dbg_prev_valid = dm_valid # build driverMonitoringState packet - dat = messaging.new_message('driverMonitoringState', valid=dm_valid) + dat = messaging.new_message('driverMonitoringState', valid=sm.all_checks()) dat.driverMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected,