mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-21 13:33:47 +08:00
Jason Wen
c8debe77db
* Cereal: Use custom reserved structs * fix dmonitoring * sync with upstream * bump cereal * bump cereal * fix * bump cereal * fix * unused * check sm valid
52 lines
1.7 KiB
Python
52 lines
1.7 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 underd 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):
|
|
if v_ego < _MIN_MONITORING_SPEED or not ctrl_active:
|
|
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)
|