brianhansonxyz
cea422b075
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) <noreply@anthropic.com>
51 lines
1.9 KiB
Python
Executable File
51 lines
1.9 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
from cereal import car
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import Priority, config_realtime_process
|
|
from openpilot.common.swaglog import cloudlog
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
|
import cereal.messaging as messaging
|
|
|
|
def publish_ui_plan(sm, pm, longitudinal_planner):
|
|
ui_send = messaging.new_message('uiPlan')
|
|
ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
|
|
uiPlan = ui_send.uiPlan
|
|
uiPlan.frameId = sm['modelV2'].frameId
|
|
uiPlan.position.x = list(sm['modelV2'].position.x)
|
|
uiPlan.position.y = list(sm['modelV2'].position.y)
|
|
uiPlan.position.z = list(sm['modelV2'].position.z)
|
|
uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist()
|
|
pm.send('uiPlan', ui_send)
|
|
|
|
def plannerd_thread():
|
|
config_realtime_process(5, Priority.CTRL_LOW)
|
|
|
|
cloudlog.info("plannerd is waiting for CarParams")
|
|
params = Params()
|
|
with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg:
|
|
CP = msg
|
|
cloudlog.info("plannerd got CarParams: %s", CP.carName)
|
|
|
|
longitudinal_planner = LongitudinalPlanner(CP)
|
|
pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan'])
|
|
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'frogpilotCarControl', 'frogpilotPlan'],
|
|
poll='modelV2', ignore_avg_freq=['radarState'])
|
|
|
|
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)
|
|
|
|
def main():
|
|
plannerd_thread()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|