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) <noreply@anthropic.com>
This commit is contained in:
@@ -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))
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user