brianhansonxyz
426382960a
Original gate was sm.all_checks() which required modelV2 fresh and
liveCalibration.valid. Both fail spuriously in our setup:
- modelV2 stops at standstill/parked (two-state modeld)
- calibrationd propagates its own freq_ok glitches into liveCalibration.valid
Either condition froze DM pose updates — face_detected stuck False →
maybe_distracted True → awareness decayed to 0 within ~6s of engagement
("TAKE OVER" driver-distraction alert the moment controlsd engaged).
Narrow the gate to only the subs update_states actually consumes
(driverStateV2, carState, controlsState, liveCalibration), check only
alive+valid (skip freq_ok), and skip liveCalibration.valid — rpyCalib
presence is sufficient proof that calibration has produced output.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
111 lines
4.9 KiB
Python
Executable File
111 lines
4.9 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import gc
|
|
|
|
import cereal.messaging as messaging
|
|
from cereal import car
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import set_realtime_priority
|
|
from openpilot.selfdrive.controls.lib.events import Events
|
|
from openpilot.selfdrive.monitoring.driver_monitor import DriverStatus
|
|
|
|
|
|
def dmonitoringd_thread():
|
|
gc.disable()
|
|
set_realtime_priority(2)
|
|
|
|
params = Params()
|
|
pm = messaging.PubMaster(['driverMonitoringState'])
|
|
sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll='driverStateV2')
|
|
|
|
driver_status = DriverStatus(rhd_saved=params.get_bool("IsRhdDetected"))
|
|
|
|
v_cruise_last = 0
|
|
driver_engaged = False
|
|
dbg_prev_valid = True # CLEARPILOT: track valid transitions
|
|
|
|
# 10Hz <- dmonitoringmodeld
|
|
while True:
|
|
sm.update()
|
|
if not sm.updated['driverStateV2']:
|
|
continue
|
|
|
|
# Get interaction
|
|
if sm.updated['carState']:
|
|
v_cruise = sm['carState'].cruiseState.speed
|
|
driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
|
|
v_cruise != v_cruise_last or \
|
|
sm['carState'].steeringPressed or \
|
|
sm['carState'].gasPressed
|
|
v_cruise_last = v_cruise
|
|
|
|
if sm.updated['modelV2']:
|
|
driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo)
|
|
|
|
# Get data from dmonitoringmodeld
|
|
events = Events()
|
|
|
|
# CLEARPILOT: narrow update_states gate. The original sm.all_checks() also
|
|
# required modelV2 fresh (stops at standstill in two-state modeld) and
|
|
# liveCalibration.valid (calibrationd cascades its own freq_ok to valid, which
|
|
# flaps). Both made DM freeze pose → face_detected stuck False → awareness
|
|
# decayed to 0 within 6s of engagement. Narrow the gate to the subs
|
|
# update_states actually reads, and only to alive+valid (skip freq_ok and
|
|
# skip liveCalibration.valid). rpyCalib presence is sufficient to know
|
|
# calibration has produced output.
|
|
if (sm.alive['driverStateV2'] and sm.valid['driverStateV2'] and
|
|
sm.alive['carState'] and sm.valid['carState'] and
|
|
sm.alive['controlsState'] and sm.valid['controlsState'] and
|
|
sm.alive['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) > 0):
|
|
driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
|
|
|
|
# Block engaging after max number of distrations
|
|
if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \
|
|
driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION:
|
|
events.add(car.CarEvent.EventName.tooDistracted)
|
|
|
|
# Update events from driver state
|
|
driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
|
|
|
|
# CLEARPILOT: log on transition to invalid so we can see which sub caused the cascade
|
|
dm_valid = sm.all_checks()
|
|
if dm_valid != dbg_prev_valid and not dm_valid:
|
|
import sys
|
|
bad = [s for s in sm.alive if not (sm.alive[s] and sm.valid[s] and sm.freq_ok.get(s, True))]
|
|
details = [f"{s}(a={sm.alive[s]},v={sm.valid[s]},f={sm.freq_ok[s]})" for s in bad]
|
|
print(f"CLP driverMonitoringState valid=False: {' '.join(details)}", file=sys.stderr, flush=True)
|
|
dbg_prev_valid = dm_valid
|
|
# build driverMonitoringState packet
|
|
dat = messaging.new_message('driverMonitoringState', valid=dm_valid)
|
|
dat.driverMonitoringState = {
|
|
"events": events.to_msg(),
|
|
"faceDetected": driver_status.face_detected,
|
|
"isDistracted": driver_status.driver_distracted,
|
|
"distractedType": sum(driver_status.distracted_types),
|
|
"awarenessStatus": driver_status.awareness,
|
|
"posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
|
|
"posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
|
|
"poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(),
|
|
"poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n,
|
|
"stepChange": driver_status.step_change,
|
|
"awarenessActive": driver_status.awareness_active,
|
|
"awarenessPassive": driver_status.awareness_passive,
|
|
"isLowStd": driver_status.pose.low_std,
|
|
"hiStdCount": driver_status.hi_stds,
|
|
"isActiveMode": driver_status.active_monitoring_mode,
|
|
"isRHD": driver_status.wheel_on_right,
|
|
}
|
|
pm.send('driverMonitoringState', dat)
|
|
|
|
# save rhd virtual toggle every 5 mins
|
|
if (sm['driverStateV2'].frameId % 6000 == 0 and
|
|
driver_status.wheelpos_learner.filtered_stat.n > driver_status.settings._WHEELPOS_FILTER_MIN_COUNT and
|
|
driver_status.wheel_on_right == (driver_status.wheelpos_learner.filtered_stat.M > driver_status.settings._WHEELPOS_THRESHOLD)):
|
|
params.put_bool_nonblocking("IsRhdDetected", driver_status.wheel_on_right)
|
|
|
|
def main():
|
|
dmonitoringd_thread()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|