calibrationd: publish valid based on calStatus, not sm.all_checks
Previous behavior gated liveCalibration.valid on calibrationd's own sm.all_checks(). Upstream freq glitches (e.g. carState polling-pattern artifacts) flapped liveCalibration.valid to False, which cascaded into locationd: its filterInitialized check requires sm.allAliveAndValid(), so flapped valid kept locationd uninitialized. While uninitialized, locationd still published liveLocationKalman but with empty/garbage angularVelocityCalibrated fields. paramsd's Kalman drank the garbage and converged to steerRatio ≈ 0, stiffnessFactor ≈ 0 — which controlsd clamped to 0.1 each and fed into VM.calc_curvature, producing nonsense curvature commands and visibly jerky steering. "valid" semantically asks whether the calibration data is trustworthy — that's a question about convergence (calStatus == calibrated), not about input freshness. Switching the gate removes the cascade: once calibration completes, liveCalibration.valid stays True stably, locationd initializes, paramsd gets clean observations, steerRatio converges to the real value. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -263,7 +263,6 @@ def main() -> NoReturn:
|
|||||||
sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll='cameraOdometry')
|
sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll='cameraOdometry')
|
||||||
|
|
||||||
calibrator = Calibrator(param_put=True)
|
calibrator = Calibrator(param_put=True)
|
||||||
dbg_prev_valid = True # CLEARPILOT: track valid transitions
|
|
||||||
|
|
||||||
while 1:
|
while 1:
|
||||||
timeout = 0 if sm.frame == -1 else 100
|
timeout = 0 if sm.frame == -1 else 100
|
||||||
@@ -285,14 +284,13 @@ def main() -> NoReturn:
|
|||||||
|
|
||||||
# 4Hz driven by cameraOdometry
|
# 4Hz driven by cameraOdometry
|
||||||
if sm.frame % 5 == 0:
|
if sm.frame % 5 == 0:
|
||||||
cal_valid = sm.all_checks()
|
# CLEARPILOT: publish valid based on calibration status, not upstream sm.all_checks().
|
||||||
# CLEARPILOT: log per-sub detail on transition to invalid — goes to calibrationd.log
|
# Original openpilot gated valid on fresh inputs, but that caused a cascade:
|
||||||
if cal_valid != dbg_prev_valid and not cal_valid:
|
# upstream freq glitches → liveCalibration.valid=False → locationd stays
|
||||||
import sys
|
# uninitialized → paramsd fed garbage → bogus steerRatio/stiffnessFactor → erratic
|
||||||
bad = [s for s in sm.alive if not (sm.alive[s] and sm.valid[s] and sm.freq_ok.get(s, True))]
|
# steering. "valid" semantically means "calibration data is trustworthy"; that's a
|
||||||
details = [f"{s}(a={sm.alive[s]},v={sm.valid[s]},f={sm.freq_ok[s]})" for s in bad]
|
# question about calibration convergence, not input freshness.
|
||||||
print(f"CLP liveCalibration valid=False: {' '.join(details)}", file=sys.stderr, flush=True)
|
cal_valid = calibrator.cal_status == log.LiveCalibrationData.Status.calibrated
|
||||||
dbg_prev_valid = cal_valid
|
|
||||||
calibrator.send_data(pm, cal_valid)
|
calibrator.send_data(pm, cal_valid)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user