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))