From 12da9acfddf72e003dd44034d3e9c4e4d3682dd7 Mon Sep 17 00:00:00 2001 From: Brian Hanson Date: Sun, 19 Apr 2026 13:26:36 -0500 Subject: [PATCH] revert modeld/controlsd/plannerd/locationd to first-commit baseline MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Starting point for rebuilding self-drive from a known-good baseline. Reverts the following to their state at f46339c: - selfdrive/modeld/modeld.py (constant 20fps, no variable-rate / standby skip) - selfdrive/modeld/dmonitoringmodeld.py (no carState sub, no standstill skip) - selfdrive/controls/controlsd.py (no parked-cycle skip, no FPCC hoisting, no MDPS split) - selfdrive/controls/lib/longitudinal_planner.py - selfdrive/locationd/calibrationd.py (valid = sm.all_checks again) - selfdrive/locationd/paramsd.py - selfdrive/locationd/torqued.py All non-self-drive features (thermald fan control, speed limit controller, cruise warning signs, UI state transitions, GPS fixes, ClearPilot menu, dashcamd, telemetry, etc.) remain as-is on this branch — only the 4 core self-drive processes are reverted. Co-Authored-By: Claude Opus 4.7 (1M context) --- selfdrive/controls/controlsd.py | 266 ++++-------------- .../controls/lib/longitudinal_planner.py | 35 +-- selfdrive/locationd/calibrationd.py | 9 +- selfdrive/locationd/paramsd.py | 11 +- selfdrive/locationd/torqued.py | 12 +- selfdrive/modeld/dmonitoringmodeld.py | 16 +- selfdrive/modeld/modeld.py | 34 ++- 7 files changed, 94 insertions(+), 289 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 44a32a9..b71e90d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -38,7 +38,6 @@ 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 @@ -81,10 +80,6 @@ class Controls: 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.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: @@ -113,9 +108,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 +164,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 +198,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 +236,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""" @@ -499,18 +447,7 @@ class Controls: 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() + model_suppress = (time.monotonic() - standby_ts) < 2.0 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): @@ -524,14 +461,9 @@ 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: + import sys + print(f"CLP controlsdLagging: remaining={self.rk.remaining:.4f} standstill={CS.standstill} vEgo={CS.vEgo:.2f}", file=sys.stderr) 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']))): @@ -546,60 +478,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 and not model_suppress: + 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 and not model_suppress: + 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 and not model_suppress: 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) and not model_suppress: self.events.add(EventName.paramsdTemporaryError) # conservative HW alert. if the data or frequency are off, locationd will throw an error @@ -613,8 +519,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: @@ -762,19 +676,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 +692,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 +932,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 +946,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 +976,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 +1046,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 +1175,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() @@ -1369,19 +1256,7 @@ class Controls: 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 - + def clearpilot_state_control(self, CC, CS): if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents): current = self.params_memory.get_int("ScreenDisplayMode") @@ -1394,23 +1269,6 @@ class Controls: new_mode = 0 if current == 3 else 3 self.params_memory.put_int("ScreenDisplayMode", new_mode) - - def clearpilot_state_control(self, CC, CS): - - # 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) - return CC def main(): 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/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..527e4ad 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -241,20 +241,23 @@ def main(demo=False): 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 + # CLEARPILOT: power saving — three modes based on driving state + # Full 20fps: lat active or lane changing + # Reduced 4fps: not lat active, not standstill (driving without cruise) + # Standby 0fps: standstill + lat_active = sm['carControl'].latActive + lane_changing = params_memory.get_bool("no_lat_lane_change") + standstill = sm['carState'].standstill + full_rate = lat_active or lane_changing + + # Standby transitions (standstill only) + should_standby = standstill and not full_rate 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") + cloudlog.warning("modeld: standby ON (standstill)") 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. @@ -267,6 +270,19 @@ def main(demo=False): last_vipc_frame_id = meta_main.frame_id continue + # Reduced framerate: 4fps when not lat active and not standstill + # Skip 4 out of every 5 frames (20fps -> 4fps) + # Write standby timestamp so controlsd suppresses transient errors + if not full_rate: + now = _time.monotonic() + if now - last_standby_ts_write > 1.0: + params_memory.put("ModelStandbyTs", str(now)) + last_standby_ts_write = now + if run_count % 5 != 0: + last_vipc_frame_id = meta_main.frame_id + run_count += 1 + continue + desire = DH.desire is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId