55 lines
1.9 KiB
Python
55 lines
1.9 KiB
Python
from cereal import custom, car
|
|
from common.conversions import Conversions as CV
|
|
|
|
EventName = car.CarEvent.EventName
|
|
HandsOnWheelState = custom.DriverMonitoringStateSP.HandsOnWheelState
|
|
|
|
_PRE_ALERT_THRESHOLD = 150 # 15s
|
|
_PROMPT_ALERT_THRESHOLD = 300 # 30s
|
|
_TERMINAL_ALERT_THRESHOLD = 600 # 60s
|
|
|
|
_MIN_MONITORING_SPEED = 10 * CV.KPH_TO_MS # No monitoring under 10kph
|
|
|
|
|
|
class HandsOnWheelStatus():
|
|
def __init__(self):
|
|
self.hands_on_wheel_state = HandsOnWheelState.none
|
|
self.hands_off_wheel_cnt = 0
|
|
|
|
def update(self, events, steering_wheel_engaged, ctrl_active, v_ego, always_on, hands_on_wheel_monitoring):
|
|
if v_ego < _MIN_MONITORING_SPEED or \
|
|
(not always_on and not ctrl_active) or \
|
|
(always_on and not ctrl_active and self.hands_on_wheel_state == HandsOnWheelState.terminal) or \
|
|
not hands_on_wheel_monitoring:
|
|
self.hands_on_wheel_state = HandsOnWheelState.none
|
|
self.hands_off_wheel_cnt = 0
|
|
return
|
|
|
|
if steering_wheel_engaged:
|
|
# Driver has hands on steering wheel
|
|
self.hands_on_wheel_state = HandsOnWheelState.ok
|
|
self.hands_off_wheel_cnt = 0
|
|
return
|
|
|
|
self.hands_off_wheel_cnt += 1
|
|
alert = None
|
|
|
|
if self.hands_off_wheel_cnt >= _TERMINAL_ALERT_THRESHOLD:
|
|
# terminal red alert: disengagement required
|
|
self.hands_on_wheel_state = HandsOnWheelState.terminal
|
|
alert = EventName.keepHandsOnWheel
|
|
elif self.hands_off_wheel_cnt >= _PROMPT_ALERT_THRESHOLD:
|
|
# prompt orange alert
|
|
self.hands_on_wheel_state = HandsOnWheelState.critical
|
|
alert = EventName.promptKeepHandsOnWheel
|
|
elif self.hands_off_wheel_cnt >= _PRE_ALERT_THRESHOLD:
|
|
# pre green alert
|
|
self.hands_on_wheel_state = HandsOnWheelState.warning
|
|
alert = EventName.preKeepHandsOnWheel
|
|
else:
|
|
# hands off wheel for acceptable period of time.
|
|
self.hands_on_wheel_state = HandsOnWheelState.minor
|
|
|
|
if alert is not None:
|
|
events.add(alert)
|