From cea422b0750f037854babce66d1fea0c1fd01a05 Mon Sep 17 00:00:00 2001 From: Brian Hanson Date: Sun, 26 Apr 2026 12:12:38 -0500 Subject: [PATCH] locationd: ignore GPS as Kalman input; expand park-cached-output to onroad consumers MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit locationd: ignore gpsLocation observations entirely (clearpilot_disable_gps const at the top of handle_gps; falls through to determine_gps_mode's no-GPS path). gpsd.py keeps publishing real GPS for UI / dashcam / clock / night-mode — only locationd ignores it. The previous gpsd.py path was hard-coding vNED=[0,0,0] while the car was moving 28 m/s, feeding the Kalman contradictory GPS-vs-IMU velocity observations that propagated into latcontrol_torque through liveLocationKalman.angularVelocityCalibrated and caused a real "drift right on straight roads" symptom. controlsd: short-circuit state_control() while parked. Skips LaC/LoC PID, MPC, model_v2 reads, lane-change logic — all wasted work when the car isn't moving. Returns the same enabled=False/latActive=False/longActive=False CC the original code would have produced, so publish_logs runs unchanged and card.controls_update keeps the sendcan / tester-present heartbeat flowing at 100Hz. controlsd CPU dropped from ~59% to ~3% in park. controlsd: also wire self.FPCC.noLatLaneChange = True/False in the existing lane-change suppression branch. The Hyundai carcontroller already reads off frogpilot_variables.no_lat_lane_change for the no-steer signal, but the UI's distinctive yellow CHANGE_LANE_PATH_COLOR (onroad.cc:901) reads from frogpilotCarControl.NoLatLaneChange — which nobody was setting, so the yellow line never appeared during lane-change suppression. plannerd: skip longitudinal_planner.update + publish + publish_ui_plan while parked. Downstream controlsd already short-circuits in park, so longitudinalPlan / uiPlan staleness is fine. frogpilot_process: same idea — skip frogpilot_planner.update + publish while parked. dmonitoringmodeld: mirror the cached-output trick from modeld. Add carState to the SubMaster, cache the last (model_output, dsp_execution_time) tuple, and serve it on every frame while gear is in park instead of running DSP inference on the driver camera. Together with the existing modeld park-cached-output, the heavy onroad processing pipeline (modeld → plannerd → controlsd → carcontroller) now all idles in park and only fires when the car leaves park. Co-Authored-By: Claude Opus 4.7 (1M context) --- selfdrive/controls/controlsd.py | 21 +++++++++++++++++++++ selfdrive/controls/plannerd.py | 4 ++++ selfdrive/frogpilot/frogpilot_process.py | 4 +++- selfdrive/locationd/locationd.cc | 8 +++++++- selfdrive/modeld/dmonitoringmodeld.py | 16 +++++++++++++--- 5 files changed, 48 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 66ef835..c0f9623 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -665,6 +665,25 @@ class Controls: def state_control(self, CS): """Given the state, this function returns a CarControl packet""" + # CLEARPILOT: short-circuit while parked. Skip LaC/LoC PID, MPC, model_v2 + # reads, lane-change logic — none of it matters when the car isn't moving. + # publish_logs still runs and still triggers carcontroller.apply via + # card.controls_update, so the sendcan heartbeats / tester-present messages + # keep flowing at 100Hz and the car doesn't fault. Saves ~30% controlsd CPU + # in park. + if CS.gearShifter == car.CarState.GearShifter.park: + CC = car.CarControl.new_message() + CC.enabled = False + CC.latActive = False + CC.longActive = False + CC.actuators.longControlState = self.LoC.long_control_state + self.LaC.reset() + self.LoC.reset(v_pid=CS.vEgo) + self.frogpilot_variables.no_lat_lane_change = False + self.FPCC.noLatLaneChange = False + lac_log = log.ControlsState.LateralDebugState.new_message() + return CC, lac_log + # Update VehicleModel lp = self.sm['liveParameters'] x = max(lp.stiffnessFactor, 0.1) @@ -705,8 +724,10 @@ class Controls: if model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change: CC.latActive = False self.frogpilot_variables.no_lat_lane_change = True + self.FPCC.noLatLaneChange = True else: self.frogpilot_variables.no_lat_lane_change = False + self.FPCC.noLatLaneChange = False if CS.leftBlinker or CS.rightBlinker: self.last_blinker_frame = self.sm.frame diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 71dc5fe..6906fdf 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -34,6 +34,10 @@ def plannerd_thread(): while True: sm.update() if sm.updated['modelV2']: + # CLEARPILOT: skip planning while parked. The downstream consumer (controlsd) + # already short-circuits in park, so longitudinalPlan/uiPlan staleness is fine. + if sm['carState'].gearShifter == car.CarState.GearShifter.park: + continue longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) publish_ui_plan(sm, pm, longitudinal_planner) diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py index c6b4dfb..65b4667 100755 --- a/selfdrive/frogpilot/frogpilot_process.py +++ b/selfdrive/frogpilot/frogpilot_process.py @@ -85,7 +85,9 @@ def frogpilot_thread(): frogpilot_planner = FrogPilotPlanner(CP) frogpilot_planner.update_frogpilot_params() - if sm.updated['modelV2']: + # CLEARPILOT: skip planner work while parked. + parked = sm['carState'].gearShifter == car.CarState.GearShifter.park + if sm.updated['modelV2'] and not parked: frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotNavigation'], sm['liveLocationKalman'], sm['modelV2'], sm['radarState']) frogpilot_planner.publish(sm, pm) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 0274b6e..2535c2c 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -308,12 +308,18 @@ void Localizer::input_fake_gps_observations(double current_time) { } void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { + // CLEARPILOT: GPS disabled as a Kalman input. gpsd still publishes real GPS for + // UI / dashcam / clock / night-mode; only locationd ignores it. Falls through to + // determine_gps_mode() which is openpilot's existing no-GPS path (fake observations + // to bound position uncertainty). To re-enable, set this to false. + const bool clearpilot_disable_gps = true; + bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); - if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { + if (clearpilot_disable_gps || !log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { //this->gps_valid = false; this->determine_gps_mode(current_time); return; diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index ef403b4..5b9c9dd 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -7,7 +7,7 @@ import ctypes import numpy as np from pathlib import Path -from cereal import messaging +from cereal import car, messaging from cereal.messaging import PubMaster, SubMaster from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.swaglog import cloudlog @@ -128,10 +128,15 @@ def main(): assert vipc_client.is_connected() cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") - sm = SubMaster(["liveCalibration"]) + sm = SubMaster(["liveCalibration", "carState"]) pm = PubMaster(["driverStateV2"]) calib = np.zeros(CALIB_LEN, dtype=np.float32) + # CLEARPILOT: cache last model output to serve while gear is in park — + # mirrors the same trick modeld uses. Skips DSP inference on the driver + # camera when the car is stationary; downstream dmonitoringd still gets + # a fresh publish each frame. + last_model_output = None # last = 0 while True: @@ -143,8 +148,13 @@ def main(): if sm.updated["liveCalibration"]: calib[:] = np.array(sm["liveCalibration"].rpyCalib) + parked = sm["carState"].gearShifter == car.CarState.GearShifter.park t1 = time.perf_counter() - model_output, dsp_execution_time = model.run(buf, calib) + if parked and last_model_output is not None: + model_output, dsp_execution_time = last_model_output + else: + model_output, dsp_execution_time = model.run(buf, calib) + last_model_output = (model_output, dsp_execution_time) 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))