From 2aaa5319e844eab326cdb8296e318e1ec91b1f30 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Wed, 11 Mar 2020 12:48:51 +1000 Subject: [PATCH] move driver monitor patch to dmonitoringd. --- selfdrive/controls/dmonitoringd.py | 44 +++++++++++++++++++++++- selfdrive/controls/lib/driver_monitor.py | 30 ++-------------- 2 files changed, 46 insertions(+), 28 deletions(-) diff --git a/selfdrive/controls/dmonitoringd.py b/selfdrive/controls/dmonitoringd.py index 3c888d7b3..a6c74571e 100755 --- a/selfdrive/controls/dmonitoringd.py +++ b/selfdrive/controls/dmonitoringd.py @@ -1,12 +1,15 @@ #!/usr/bin/env python3 import gc -from common.realtime import set_realtime_priority +from common.realtime import set_realtime_priority, sec_since_boot from common.params import Params, put_nonblocking import cereal.messaging as messaging from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION from selfdrive.locationd.calibration_helpers import Calibration from selfdrive.controls.lib.gps_helpers import is_rhd_region +from common.params import Params +params = Params() +from selfdrive.dragonpilot.dragonconf import dp_get_last_modified def dmonitoringd_thread(sm=None, pm=None): gc.disable() @@ -40,8 +43,47 @@ def dmonitoringd_thread(sm=None, pm=None): v_cruise_last = 0 driver_engaged = False + # dragonpilot + last_ts = 0 + dp_last_modified = None + dp_enable_driver_safety_check = True + dp_enable_driver_monitoring = True + # 10Hz <- dmonitoringmodeld while True: + cur_time = sec_since_boot() + if cur_time - last_ts >= 5.: + modified = dp_get_last_modified() + if dp_last_modified != modified: + dp_enable_driver_safety_check = False if params.get("DragonEnableDriverSafetyCheck", encoding='utf8') == "0" else True + # load driver monitor val only when safety is on + if dp_enable_driver_safety_check: + dp_enable_driver_monitoring = False if params.get("DragonEnableDriverMonitoring", encoding='utf8') == "0" else True + # load steering monitor timer val only when driver monitor is on + if dp_enable_driver_monitoring: + try: + dp_awareness_time = int(params.get("DragonSteeringMonitorTimer", encoding='utf8')) + except TypeError: + dp_awareness_time = 0. + driver_status.awareness_time = 86400 if dp_awareness_time <= 0. else dp_awareness_time * 60. + dp_last_modified = modified + last_ts = cur_time + + # reset all awareness val + if not dp_enable_driver_safety_check: + dp_enable_driver_monitoring = False + driver_status.awareness = 1. + driver_status.awareness_active = 1. + driver_status.awareness_passive = 1. + driver_status.terminal_alert_cnt = 0 + driver_status.terminal_time = 0 + + # dm don't check rhd, set to true + if not dp_enable_driver_monitoring: + driver_status.awareness_time = 86400 + driver_status.is_rhd_region = True + driver_status.is_rhd_region_checked = True + sm.update() # GPS coords RHD parsing, once every restart diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 3c0a2f50b..2b740000f 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -1,12 +1,9 @@ from common.numpy_fast import interp from math import atan2, sqrt -from common.realtime import DT_DMON, sec_since_boot +from common.realtime import DT_DMON from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter -from common.params import Params -params = Params() -from selfdrive.dragonpilot.dragonconf import dp_get_last_modified _AWARENESS_TIME = 70. # one minute limit without user touching steering wheels make the car enter a terminal status _AWARENESS_PRE_TIME_TILL_TERMINAL = 15. # a first alert is issued 25s before expiration @@ -115,11 +112,7 @@ class DriverStatus(): self.is_rhd_region_checked = False # dragonpilot - self.last_ts = 0 - self.dp_last_modified = None - self.awareness_time = 100 - self.dragon_enable_driver_safety_check = True - self.dragon_enable_driver_monitoring = True + self.awareness_time = 70. self._set_timers(active_monitoring=True) @@ -184,9 +177,6 @@ class DriverStatus(): if len(driver_state.faceOrientation) == 0 or len(driver_state.facePosition) == 0 or len(driver_state.faceOrientationStd) == 0 or len(driver_state.facePositionStd) == 0: return - if not self.dragon_enable_driver_monitoring: - self.is_rhd_region = True - self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy) self.pose.pitch_std = driver_state.faceOrientationStd[0] self.pose.yaw_std = driver_state.faceOrientationStd[1] @@ -222,21 +212,7 @@ class DriverStatus(): self.hi_stds = 0 def update(self, events, driver_engaged, ctrl_active, standstill): - cur_time = sec_since_boot() - if cur_time - self.last_ts >= 5.: - modified = dp_get_last_modified() - if self.dp_last_modified != modified: - self.awareness_time = int(params.get("DragonSteeringMonitorTimer", encoding='utf8')) - self.awareness_time = 100 if self.awareness_time <= 0. else self.awareness_time * 60. - - self.dragon_enable_driver_safety_check = False if params.get("DragonEnableDriverSafetyCheck", encoding='utf8') == "0" else True - if self.dragon_enable_driver_safety_check: - self.dragon_enable_driver_monitoring = False if params.get("DragonEnableDriverMonitoring", encoding='utf8') == "0" else True - else: - self.dragon_enable_driver_monitoring = False - self.dp_last_modified = modified - self.last_ts = cur_time - if (driver_engaged and self.awareness > 0) or not ctrl_active or not self.dragon_enable_driver_safety_check: + if (driver_engaged and self.awareness > 0) or not ctrl_active: # reset only when on disengagement if red reached self.awareness = 1. self.awareness_active = 1.