mawei/IQ.Pilot
2
0
Fork 0
mirror of https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git synced 2026-08-06 16:55:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ 36625eb

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-26 12:45:03 -05:00
parent 563022daa3
commit e557c3d8ee
38 changed files with 810 additions and 941 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/log.capnp
+8 -85
View File
@@ -70,12 +70,12 @@ struct OnroadEvent @0xc4fa6047f024e718 {
longitudinalManeuver @30;
steerTempUnavailableSilent @31;
resumeRequired @32;
driverDistracted1 @33;
driverDistracted2 @34;
driverDistracted3 @35;
driverUnresponsive1 @36;
driverUnresponsive2 @37;
driverUnresponsive3 @38;
preDriverDistracted @33;
promptDriverDistracted @34;
driverDistracted @35;
preDriverUnresponsive @36;
promptDriverUnresponsive @37;
driverUnresponsive @38;
belowSteerSpeed @39;
lowBattery @40;
accFaulted @41;
@@ -2183,7 +2183,6 @@ struct DriverStateV2 {
rightBlinkProb @8 :Float32;
sunglassesProb @9 :Float32;
phoneProb @13 :Float32;
sleepProb @14 :Float32;
notReadyProbDEPRECATED @12 :List(Float32);
occludedProbDEPRECATED @10 :Float32;
readyProbDEPRECATED @11 :List(Float32);
@@ -2225,7 +2224,7 @@ struct DriverStateDEPRECATED @0xb83c6cc593ed0a00 {
stdDEPRECATED @2 :Float32;
}
struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 {
struct DriverMonitoringState @0xb83cda094a1da284 {
events @18 :List(OnroadEvent);
faceDetected @1 :Bool;
isDistracted @2 :Bool;
@@ -2251,81 +2250,6 @@ struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 {
eventsDEPRECATED @0 :List(Car.OnroadEventDEPRECATED);
}
struct DriverMonitoringState {
lockout @0 :Bool;
lockoutCount @15 :Int8;
lockoutMinutesRemaining @11 :Int8;
alert3Count @12 :Int8;
noResponseCount @13 :Int8;
noResponseForceDecel @14 :Bool;
alwaysOn @3 :Bool;
alwaysOnLockout @4 :Bool;
alertLevel @5 :AlertLevel;
activePolicy @6 :MonitoringPolicy;
isRHD @7 :Bool;
rhdCalibration @8 :CalibrationState;
visionPolicyState @9 :VisionPolicyState;
wheeltouchPolicyState @10 :WheeltouchPolicyState;
enum AlertLevel {
none @0;
one @1;
two @2;
three @3;
}
enum MonitoringPolicy {
wheeltouch @0;
vision @1;
}
struct VisionPolicyState {
awarenessPercent @0 :Int8;
awarenessStep @1 :Float32;
isDistracted @2 :Bool;
distractedTypes @3 :DistractedTypes;
faceDetected @4 :Bool;
pose @5 :Pose;
wheeltouchFallbackPercent @6 :Int8;
uncertainOffroadAlertPercent @7 :Int8;
struct DistractedTypes {
pose @0: Bool;
eye @1: Bool;
phone @2: Bool;
}
struct Pose {
pitch @0 :Float32;
yaw @1 :Float32;
pitchCalib @2 :CalibrationState;
yawCalib @3 :CalibrationState;
calibrated @4 :Bool;
uncertainty @5 :Float32;
}
}
struct WheeltouchPolicyState {
awarenessPercent @0 :Int8;
awarenessStep @1 :Float32;
driverInteracting @2 :Bool;
}
struct CalibrationState {
calibratedPercent @0 :Int8;
offset @1 :Float32;
}
deprecated :group {
alertCountLockoutPercent @1 :Int8;
alertTimeLockoutPercent @2 :Int8;
}
}
struct Boot {
wallTimeNanos @0 :UInt64;
pstore @4 :Map(Text, Data);
@@ -2646,7 +2570,7 @@ struct Event {
thumbnail @66: Thumbnail;
onroadEvents @134: List(OnroadEvent);
carParams @69: Car.CarParams;
driverMonitoringState @165: DriverMonitoringState;
driverMonitoringState @71: DriverMonitoringState;
livePose @129 :LivePose;
modelV2 @75 :ModelDataV2;
drivingModelData @128 :DrivingModelData;
@@ -2769,7 +2693,6 @@ struct Event {
wifiScanDEPRECATED @29 :List(Legacy.WifiScan);
uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent;
liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED;
driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED;
gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints;
gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan;
applanixRawDEPRECATED @42 :Data;
common/params_keys.h
-1
View File
@@ -41,7 +41,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}},
{"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}},
{"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}},
{"DriverLockoutCount", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, INT, "0"}},
{"AlphaLongitudinalEnabled", {PERSISTENT, BOOL}},
{"ExperimentalMode", {PERSISTENT, BOOL}},
{"ExperimentalModeConfirmed", {PERSISTENT, BOOL}},
iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py
+1 -1
View File
@@ -474,7 +474,7 @@ class CarController(CarControllerBase):
if hud_control.leadDistanceBars != self.lead_distance_bars_last:
self.distance_bar_frame = self.frame
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl and not CS.out.radarDisableFailed:
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
show_distance_bars = self.frame - self.distance_bar_frame < 400
iqdbc_repo/iqdbc/car/volkswagen/carstate.py
+15 -5
View File
@@ -340,11 +340,19 @@ class CarState(CarStateBase):
self.ldw_stock_values = cam_cp.vl["LDW_02"]
awv_values = ext_cp.vl.get("AWV_03", ext_cp.vl.get("ACC_10", {}))
ret.stockFcw = bool(awv_values.get("FCW_Active", 0)) or bool(awv_values.get("AWV2_Freigabe", 0))
if not (self.CP.flags & VolkswagenFlags.DISABLE_RADAR):
awv_values = ext_cp.vl.get("AWV_03", ext_cp.vl.get("ACC_10", {}))
ret.stockFcw = bool(awv_values.get("FCW_Active", 0)) or bool(awv_values.get("AWV2_Freigabe", 0))
else:
ret.stockFcw = False
ret.stockAeb = False
self.acc_type = ext_cp.vl["ACC_18"]["ACC_Typ"]
# Camera harness (DISABLE_RADAR): ext_cp == pt_cp, so ACC_18 reads our own sent value.
# Hardcode acc_type=2 (stop-and-go capable) to avoid self-referential read on first frame.
if self.CP.flags & VolkswagenFlags.DISABLE_RADAR:
self.acc_type = 2
else:
self.acc_type = ext_cp.vl["ACC_18"]["ACC_Typ"]
self.travel_assist_available = bool(pt_cp.vl.get("TA_01", {}).get("Travel_Assist_Available", 0))
ret.cruiseState.available = pt_cp.vl["Motor_51"]["TSK_Status"] in (2, 3, 4, 5)
@@ -764,11 +772,13 @@ class CarState(CarStateBase):
# math.nan → ignore_alive=True so it never contributes to can_valid.
("TA_01", math.nan),
]
if CP.networkLocation == NetworkLocation.fwdCamera:
# AWV_03 (stock radar FCW/AEB) — don't subscribe when DISABLE_RADAR, the radar is silenced
# and our carcontroller sends the replacement. Subscribing causes CAN parser timeout errors.
if CP.networkLocation == NetworkLocation.fwdCamera and not (CP.flags & VolkswagenFlags.DISABLE_RADAR):
pt_messages.append(("AWV_03", 1))
cam_messages = []
if CP.networkLocation == NetworkLocation.gateway:
if CP.networkLocation == NetworkLocation.gateway and not (CP.flags & VolkswagenFlags.DISABLE_RADAR):
cam_messages.append(("AWV_03", 1))
return {
iqdbc_repo/iqdbc/car/volkswagen/mebcan.py
+1 -1
View File
@@ -28,7 +28,7 @@ def create_steering_control(packer, bus, apply_curvature, lkas_enabled, power):
values = {
"Curvature": abs(apply_curvature), # in rad/m
"Curvature_VZ": 1 if apply_curvature > 0 and lkas_enabled else 0,
"Power": 100 if lkas_enabled else 0, # TEST: hard 100%, no ramp
"Power": power if lkas_enabled else 0,
"RequestStatus": 4 if lkas_enabled else 2,
"HighSendRate": lkas_enabled,
}
iqdbc_repo/iqdbc/car/volkswagen/values.py
+2
View File
@@ -141,6 +141,8 @@ class CarControllerParams:
}
elif CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
self.AEB_CONTROL_STEP = 100 # AWV_03 radar-replacement at 1Hz (class default 2 = 50Hz is for MQB ACC_10)
self.AEB_HUD_STEP = 20 # MEB_AWV_01 AEB HUD at 5Hz
self.LDW_STEP = 10
self.ACC_HUD_STEP = 6
self.STEER_DRIVER_ALLOWANCE = 60
iqdbc_repo/iqdbc/safety/modes/volkswagen_meb.h
+2 -2
View File
@@ -244,11 +244,11 @@ static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) {
};
const CurvatureSteeringLimits VOLKSWAGEN_MEB_STEERING_LIMITS = {
.max_curvature = 32767, // TEST: 15-bit max, no curvature ceiling
.max_curvature = 29105,
.curvature_to_can = 149253.7313f,
.send_rate = 0.02f,
.inactive_curvature_is_zero = true,
.max_power = 65535, // TEST: no power ceiling
.max_power = 225, // 90% (raw byte, 0.4 %/bit; matches Python STEERING_POWER_MAX = 90)
};
bool tx = true;
iqpilot/common/atlas_alerts.py
+2 -3
View File
@@ -214,10 +214,9 @@ class NoEntryCard(AlertCard):
def __init__(self,
alert_text_2: str,
alert_text_1: str = "openpilot Unavailable",
visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none,
priority: Tier = Tier.LOW):
visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none):
primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2)
super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0)
super().__init__(primary, secondary, AlertStatus.normal, size, Tier.LOW, visual_alert, AudibleAlert.refuse, 3.0)
class GentleDisableCard(AlertCard):
panda/board/drivers/can_common.h
+10 -9
View File
@@ -237,16 +237,17 @@ void ignition_can_hook(CANPacket_t *msg) {
}
}
}
// Volkswagen MEB / MQBevo exception (Klemmen_Status_01)
// On gateway harness cars this message is on bus 1 (powertrain CAN), not bus 0.
if ((msg->bus == 0U) || (msg->bus == 1U)) {
int len = GET_LEN(msg);
// Volkswagen MEB exception
if ((msg->addr == 0x3C0U) && (len == 4)) {
ignition_can = ((msg->data[2] >> 1U) & 1U) != 0U;
ignition_can_cnt = 0U;
int counter = msg->data[1] & 0xFU;
static int prev_counter_vw_meb = -1;
if ((counter == ((prev_counter_vw_meb + 1) % 16)) && (prev_counter_vw_meb != -1)) {
// Klemmen_Status_01->ZAS_Kl_15
ignition_can = ((msg->data[2] >> 1) & 1U) != 0U;
ignition_can_cnt = 0U;
}
prev_counter_vw_meb = counter;
}
}
}
selfdrive/assets/sounds/pre_alert.wav
BIN
View File
Binary file not shown.
selfdrive/assets/sounds/prompt_distracted.wav
BIN
View File
Binary file not shown.
selfdrive/assets/sounds/warning_immediate.wav
BIN
View File
Binary file not shown.
selfdrive/controls/controlsd.py
+2 -2
View File
@@ -260,7 +260,7 @@ class Controls(IQControlsLayer):
hudControl.leadFollowTime = 1.45
hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
hudControl.audibleAlert = self.sm['selfdriveState'].alertSound
hudControl.driverUnresponsive = self.sm['driverMonitoringState'].noResponseForceDecel
hudControl.driverUnresponsive = self.sm['selfdriveState'].alertType.split('/', 1)[0] == 'driverUnresponsive'
hudControl.rightLaneVisible = True
hudControl.leftLaneVisible = True
@@ -292,7 +292,7 @@ class Controls(IQControlsLayer):
cs.upAccelCmd = float(self.LoC.pid.p)
cs.uiAccelCmd = float(self.LoC.pid.i)
cs.ufAccelCmd = float(self.LoC.pid.f)
cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or
cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
(self.sm['selfdriveState'].state == State.softDisabling))
lat_tuning = self.CP.lateralTuning.which()
selfdrive/debug/cycle_alerts.py
+3 -3
View File
@@ -30,9 +30,9 @@ def cycle_alerts(duration=200, is_metric=False):
(EventName.accFaulted, ET.IMMEDIATE_DISABLE),
# DM sequence
(EventName.driverDistracted1, ET.WARNING),
(EventName.driverDistracted2, ET.WARNING),
(EventName.driverDistracted3, ET.WARNING),
(EventName.preDriverDistracted, ET.WARNING),
(EventName.promptDriverDistracted, ET.WARNING),
(EventName.driverDistracted, ET.WARNING),
]
# debug alerts
selfdrive/modeld/dmonitoringmodeld.py
+1 -2
View File
@@ -75,7 +75,7 @@ def parse_model_output(model_output):
face_descs = model_output[f'face_descs_{ds_suffix}']
parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob', 'sleep_prob']:
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
return parsed
@@ -91,7 +91,6 @@ def fill_driver_data(msg, model_output, ds_suffix):
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
msg.sleepProb = model_output[f'sleep_prob_{ds_suffix}'][0, 0].item()
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
msg = messaging.new_message('driverStateV2', valid=True)
selfdrive/modeld/models/dmonitoring_model.onnx
BIN
View File
Binary file not shown.
selfdrive/modeld/models/dmonitoring_model_metadata.pkl
BIN
View File
Binary file not shown.
selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl
BIN
View File
Binary file not shown.
selfdrive/modeld/models/prebuilt_check.json
+3 -3
View File
@@ -1,9 +1,9 @@
{
"dmonitoring_model": {
"outputs": {
"dmonitoring_model_metadata.pkl": "5999c262b1c25c62e485fb4ced5806d20a8ca59e3ae94e0ff499c0fe3497fedc",
"dmonitoring_model_tinygrad.pkl": "5aca89a35b42376d56f67ccd28a1080806706d546dd8c63f6e1b4c681f8e2c01"
"dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3",
"dmonitoring_model_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52"
},
"signature": "2364ebd4bb95c4b4e539c9b1ba68324b713cbb56617396b73262accf9cdcfbfc"
"signature": "e1eeb5ce45774a816c8da2394e6ee35ebf700b71dff345e0141dbab8ff592349"
}
}
selfdrive/modeld/test_dmonitoringmodeld.py
-19
View File
@@ -1,19 +0,0 @@
import pickle
import numpy as np
from openpilot.selfdrive.modeld.dmonitoringmodeld import get_driverstate_packet, parse_model_output, slice_outputs
from openpilot.selfdrive.modeld.dmonitoringmodeld import METADATA_PATH
def test_sleep_probability_output():
with open(METADATA_PATH, 'rb') as f:
metadata = pickle.load(f)
output = np.zeros(metadata['output_shapes']['outputs'][1], dtype=np.float32)
parsed = parse_model_output(slice_outputs(output, metadata['output_slices']))
parsed['raw_pred'] = b''
msg = get_driverstate_packet(parsed, 1, 0, 0., 0.)
assert msg.driverStateV2.leftDriverData.sleepProb == 0.5
assert msg.driverStateV2.rightDriverData.sleepProb == 0.5
selfdrive/monitoring/README.md
+15
View File
@@ -0,0 +1,15 @@
# driver monitoring (DM)
Uploading driver-facing camera footage is opt-in, but it is encouraged to opt-in to improve the DM model. You can always change your preference using the "Record and Upload Driver Camera" toggle.
## Troubleshooting
Before creating a bug report, go through these troubleshooting steps.
* Ensure the driver-facing camera has a good view of the driver in normal driving positions.
* This can be checked in Settings -> Device -> Preview Driver Camera (when car is off).
* If the camera can't see the driver, the device should be re-mounted.
## Bug report
In order for us to look into DM bug reports, we'll need the driver-facing camera footage. If you don't normally have this enabled, simply enable the toggle for a single drive. Also ensure the "Upload Raw Logs" toggle is enabled before going for a drive.
selfdrive/monitoring/dmonitoringd.py
+6 -18
View File
@@ -1,20 +1,8 @@
#!/usr/bin/env python3
from types import SimpleNamespace
import cereal.messaging as messaging
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process
from openpilot.selfdrive.monitoring.policy import DriverMonitoring
def get_dm_inputs(sm):
return {
'driverStateV2': sm['driverStateV2'],
'liveCalibration': sm['liveCalibration'],
'carState': sm['carState'],
'selfdriveState': SimpleNamespace(enabled=sm['selfdriveState'].enabled or sm['carControl'].latActive),
'modelV2': sm['modelV2'],
}
from openpilot.selfdrive.monitoring.helpers import DriverMonitoring
def dmonitoringd_thread():
@@ -37,9 +25,9 @@ def dmonitoringd_thread():
valid = sm.all_checks()
if demo_mode and sm.valid['driverStateV2']:
DM.run_step(get_dm_inputs(sm), demo=True)
DM.run_step(sm, demo=demo_mode)
elif valid:
DM.run_step(get_dm_inputs(sm), demo=demo_mode)
DM.run_step(sm, demo=demo_mode)
# publish
dat = DM.get_state_packet(valid=valid)
@@ -52,9 +40,9 @@ def dmonitoringd_thread():
# save rhd virtual toggle every 5 mins
if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
DM.wheelpos_offsetter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
DM.wheel_on_right == (DM.wheelpos_offsetter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
params.put_bool("IsRhdDetected", DM.wheel_on_right)
DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right)
def main():
dmonitoringd_thread()
selfdrive/monitoring/helpers.py
+474
View File
@@ -0,0 +1,474 @@
from math import atan2
import numpy as np
from cereal import car, log
import cereal.messaging as messaging
from openpilot.selfdrive.selfdrived.events import Events
from openpilot.common.realtime import DT_DMON
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.common.stat_live import RunningStatFilter
from openpilot.common.transformations.camera import DEVICE_CAMERAS
from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
from openpilot.system.hardware import HARDWARE
EventName = log.OnroadEvent.EventName
# ******************************************************************************************
# NOTE: To fork maintainers.
# Disabling or nerfing safety features will get you and your users banned from our servers.
# We recommend that you do not change these numbers from the defaults.
# ******************************************************************************************
class DRIVER_MONITOR_SETTINGS:
def __init__(self, device_type):
self._DT_DMON = DT_DMON
# ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2
self._AWARENESS_TIME = 30. # passive wheeltouch total timeout
self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15.
self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6.
self._DISTRACTED_TIME = 11. # active monitoring total timeout
self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
self._FACE_THRESHOLD = 0.7
self._EYE_THRESHOLD = 0.65
self._SG_THRESHOLD = 0.9
self._BLINK_THRESHOLD = 0.865
self._PHONE_THRESH = 0.75 if device_type == 'mici' else 0.4
self._PHONE_THRESH2 = 15.0
self._PHONE_MAX_OFFSET = 0.06
self._PHONE_MIN_OFFSET = 0.025
self._PHONE_DATA_AVG = 0.05
self._PHONE_DATA_VAR = 3*0.005
self._PHONE_MAX_COUNT = int(360 / self._DT_DMON)
self._POSE_PITCH_THRESHOLD = 0.3133
self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
self._POSE_YAW_THRESHOLD = 0.4020
self._POSE_YAW_THRESHOLD_SLACK = 0.5042
self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
self._PITCH_NATURAL_THRESHOLD = 0.449
self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
self._PITCH_NATURAL_VAR = 3*0.01
self._YAW_NATURAL_VAR = 3*0.05
self._PITCH_MAX_OFFSET = 0.124
self._PITCH_MIN_OFFSET = -0.0881
self._YAW_MAX_OFFSET = 0.289
self._YAW_MIN_OFFSET = -0.0246
self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
self._DCAM_UNCERTAIN_RESET_COUNT = int(20 / self._DT_DMON)
self._POSESTD_THRESHOLD = 0.3
self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s
self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
self._ALWAYS_ON_ALERT_MIN_SPEED = 11
self._POSE_CALIB_MIN_SPEED = 13 # 30 mph
self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative
self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory"
self._WHEELPOS_CALIB_MIN_SPEED = 11
self._WHEELPOS_THRESHOLD = 0.5
self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side
self._WHEELPOS_DATA_AVG = 0.03
self._WHEELPOS_DATA_VAR = 3*5.5e-5
self._WHEELPOS_MAX_COUNT = -1
self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change
self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts
class DistractedType:
NOT_DISTRACTED = 0
DISTRACTED_POSE = 1 << 0
DISTRACTED_BLINK = 1 << 1
DISTRACTED_PHONE = 1 << 2
class DriverPose:
def __init__(self, settings):
pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2)
yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2)
self.yaw = 0.
self.pitch = 0.
self.roll = 0.
self.yaw_std = 0.
self.pitch_std = 0.
self.roll_std = 0.
self.pitch_offseter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
self.yaw_offseter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
self.calibrated = False
self.low_std = True
self.cfactor_pitch = 1.
self.cfactor_yaw = 1.
class DriverProb:
def __init__(self, raw_priors, max_trackable):
self.prob = 0.
self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable)
self.prob_calibrated = False
class DriverBlink:
def __init__(self):
self.left = 0.
self.right = 0.
# model output refers to center of undistorted+leveled image
EFL = 598.0 # focal length in K
cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw
W, H = (cam.dcam.width, cam.dcam.height) # corrected image has same size as raw
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
# the output of these angles are in device frame
# so from driver's perspective, pitch is up and yaw is right
pitch_net, yaw_net, roll_net = angles_desc
face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H)
yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL)
pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL)
pitch = pitch_net + pitch_focal_angle
yaw = -yaw_net + yaw_focal_angle
# no calib for roll
pitch -= rpy_calib[1]
yaw -= rpy_calib[2]
return roll_net, pitch, yaw
class DriverMonitoring:
def __init__(self, rhd_saved=False, settings=None, always_on=False):
# init policy settings
self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
# init driver status
wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
phone_filter_raw_priors = (self.settings._PHONE_DATA_AVG, self.settings._PHONE_DATA_VAR, 2)
self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
self.phone = DriverProb(raw_priors=phone_filter_raw_priors, max_trackable=self.settings._PHONE_MAX_COUNT)
self.pose = DriverPose(settings=self.settings)
self.blink = DriverBlink()
self.always_on = always_on
self.distracted_types = []
self.driver_distracted = False
self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
self.wheel_on_right = False
self.wheel_on_right_last = None
self.wheel_on_right_default = rhd_saved
self.face_detected = False
self.terminal_alert_cnt = 0
self.terminal_time = 0
self.step_change = 0.
self.active_monitoring_mode = True
self.is_model_uncertain = False
self.hi_stds = 0
self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
self.dcam_uncertain_cnt = 0
self.dcam_reset_cnt = 0
self.params = Params()
self.too_distracted = self.params.get_bool("DriverTooDistracted")
self._reset_awareness()
self._set_timers(active_monitoring=True)
self._reset_events()
def _reset_awareness(self):
self.awareness = 1.
self.awareness_active = 1.
self.awareness_passive = 1.
def _reset_events(self):
self.current_events = Events()
def _set_timers(self, active_monitoring):
if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
if active_monitoring:
self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME
else:
self.step_change = 0.
return # no exploit after orange alert
elif self.awareness <= 0.:
return
if active_monitoring:
# when falling back from passive mode to active mode, reset awareness to avoid false alert
if not self.active_monitoring_mode:
self.awareness_passive = self.awareness
self.awareness = self.awareness_active
self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME
self.active_monitoring_mode = True
else:
if self.active_monitoring_mode:
self.awareness_active = self.awareness
self.awareness = self.awareness_passive
self.threshold_pre = self.settings._AWARENESS_PRE_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME
self.threshold_prompt = self.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME
self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME
self.active_monitoring_mode = False
def _set_policy(self, brake_disengage_prob, car_speed):
bp = brake_disengage_prob
k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
bp_normal = max(min(bp / k1, 0.5),0)
self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_PITCH_THRESHOLD_SLACK,
self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_YAW_THRESHOLD_SLACK,
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
def _get_distracted_types(self):
distracted_types = []
if not self.pose.calibrated:
pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
else:
pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(),
self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(),
self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
yaw_error = abs(yaw_error)
pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD
yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw
if pitch_error > pitch_threshold or yaw_error > yaw_threshold:
distracted_types.append(DistractedType.DISTRACTED_POSE)
if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD:
distracted_types.append(DistractedType.DISTRACTED_BLINK)
if self.phone.prob_calibrated:
using_phone = self.phone.prob > max(min(self.phone.prob_offseter.filtered_stat.M, self.settings._PHONE_MAX_OFFSET), self.settings._PHONE_MIN_OFFSET) \
* self.settings._PHONE_THRESH2
else:
using_phone = self.phone.prob > self.settings._PHONE_THRESH
if using_phone:
distracted_types.append(DistractedType.DISTRACTED_PHONE)
return distracted_types
def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False):
rhd_pred = driver_state.wheelOnRightProb
# calibrates only when there's movement and either face detected
if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
self.wheelpos.prob_offseter.push_and_update(rhd_pred)
self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT
if self.wheelpos.prob_calibrated or demo_mode:
self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
else:
self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
# make sure no switching when engaged
if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
self.wheel_on_right = self.wheel_on_right_last
driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
driver_data.faceOrientationStd, driver_data.facePositionStd)):
return
self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
if self.wheel_on_right:
self.pose.yaw *= -1
self.wheel_on_right_last = self.wheel_on_right
self.pose.pitch_std = driver_data.faceOrientationStd[0]
self.pose.yaw_std = driver_data.faceOrientationStd[1]
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.phone.prob = driver_data.phoneProb
self.distracted_types = self._get_distracted_types()
self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types
or DistractedType.DISTRACTED_POSE in self.distracted_types
or DistractedType.DISTRACTED_BLINK in self.distracted_types) \
and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
self.driver_distraction_filter.update(self.driver_distracted)
# update offseter
# only update when driver is actively driving the car above a certain speed
if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
self.pose.pitch_offseter.push_and_update(self.pose.pitch)
self.pose.yaw_offseter.push_and_update(self.pose.yaw)
self.phone.prob_offseter.push_and_update(self.phone.prob)
self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \
self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
self.phone.prob_calibrated = self.phone.prob_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
if self.face_detected and not self.driver_distracted:
if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD:
if not standstill:
self.dcam_uncertain_cnt += 1
self.dcam_reset_cnt = 0
else:
self.dcam_reset_cnt += 1
if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT:
self.dcam_uncertain_cnt = 0
self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME
self._set_timers(self.face_detected and not self.is_model_uncertain)
if self.face_detected and not self.pose.low_std and not self.driver_distracted:
self.hi_stds += 1
elif self.face_detected and self.pose.low_std:
self.hi_stds = 0
def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
self._reset_events()
# Block engaging until ignition cycle after max number or time of distractions
if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \
self.terminal_time >= self.settings._MAX_TERMINAL_DURATION:
if not self.too_distracted:
self.params.put_bool_nonblocking("DriverTooDistracted", True)
self.too_distracted = True
# Always-on distraction lockout is temporary
if self.too_distracted or (self.always_on and self.awareness <= self.threshold_prompt):
self.current_events.add(EventName.tooDistracted)
always_on_valid = self.always_on and not wrong_gear
if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or \
(not always_on_valid and not op_engaged) or \
(always_on_valid and not op_engaged and self.awareness <= 0):
# always reset on disengage with normal mode; disengage resets only on red if always on
self._reset_awareness()
return
driver_attentive = self.driver_distraction_filter.x < 0.37
awareness_prev = self.awareness
if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0):
if driver_engaged:
self._reset_awareness()
return
# only restore awareness when paying attention and alert is not red
self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*
(1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.)
if self.awareness == 1.:
self.awareness_passive = min(self.awareness_passive + self.step_change, 1.)
# don't display alert banner when awareness is recovering and has cleared orange
if self.awareness > self.threshold_prompt:
return
_reaching_audible = self.awareness - self.step_change <= self.threshold_prompt
_reaching_terminal = self.awareness - self.step_change <= 0
standstill_orange_exemption = standstill and _reaching_audible
always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal
always_on_lowspeed_exemption = always_on_valid and not op_engaged and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED
certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected
maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected
if certainly_distracted or maybe_distracted:
# should always be counting if distracted unless at standstill (lowspeed for always-on) and reaching orange
# also will not be reaching 0 if DM is active when not engaged
if not (standstill_orange_exemption or always_on_red_exemption or (always_on_lowspeed_exemption and _reaching_audible)):
self.awareness = max(self.awareness - self.step_change, -0.1)
alert = None
if self.awareness <= 0.:
# terminal red alert: disengagement required
alert = EventName.driverDistracted if self.active_monitoring_mode else EventName.driverUnresponsive
self.terminal_time += 1
if awareness_prev > 0.:
self.terminal_alert_cnt += 1
elif self.awareness <= self.threshold_prompt:
# prompt orange alert
alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive
elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption:
# pre green alert
alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive
if alert is not None:
self.current_events.add(alert)
def get_state_packet(self, valid=True):
# build driverMonitoringState packet
dat = messaging.new_message('driverMonitoringState', valid=valid)
dat.driverMonitoringState = {
"events": self.current_events.to_msg(),
"faceDetected": self.face_detected,
"isDistracted": self.driver_distracted,
"distractedType": sum(self.distracted_types),
"awarenessStatus": self.awareness,
"posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(),
"poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n,
"phoneProbOffset": self.phone.prob_offseter.filtered_stat.mean(),
"phoneProbValidCount": self.phone.prob_offseter.filtered_stat.n,
"stepChange": self.step_change,
"awarenessActive": self.awareness_active,
"awarenessPassive": self.awareness_passive,
"isLowStd": self.pose.low_std,
"hiStdCount": self.hi_stds,
"isActiveMode": self.active_monitoring_mode,
"isRHD": self.wheel_on_right,
"uncertainCount": self.dcam_uncertain_cnt,
}
return dat
def run_step(self, sm, demo=False):
if demo:
highway_speed = 30
enabled = True
wrong_gear = False
standstill = False
driver_engaged = False
brake_disengage_prob = 1.0
rpyCalib = [0., 0., 0.]
else:
highway_speed = sm['carState'].vEgo
enabled = sm['selfdriveState'].enabled or sm['carControl'].latActive
wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
standstill = sm['carState'].standstill
driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
calib_rpy = get_calibrated_rpy(sm['liveCalibration'])
rpyCalib = calib_rpy.tolist() if calib_rpy is not None else [0., 0., 0.]
self._set_policy(
brake_disengage_prob=brake_disengage_prob,
car_speed=highway_speed,
)
# Parse data from dmonitoringmodeld
self._update_states(
driver_state=sm['driverStateV2'],
cal_rpy=rpyCalib,
car_speed=highway_speed,
op_engaged=enabled,
standstill=standstill,
demo_mode=demo,
)
# Update distraction events
self._update_events(
driver_engaged=driver_engaged,
op_engaged=enabled,
standstill=standstill,
wrong_gear=wrong_gear,
car_speed=highway_speed
)
selfdrive/monitoring/policy.py
-467
View File
@@ -1,467 +0,0 @@
from collections import defaultdict
from math import atan2, radians
import numpy as np
from cereal import car, log
import cereal.messaging as messaging
from openpilot.common.realtime import DT_DMON
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.common.stat_live import RunningStatFilter
from openpilot.common.transformations.camera import DEVICE_CAMERAS
AlertLevel = log.DriverMonitoringState.AlertLevel
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
def to_percent(v):
return int(min(max(v * 100., 0.), 100.))
# ******************************************************************************************
# NOTE: To fork maintainers.
# Disabling or nerfing safety features will get you and your users banned from our servers.
# We recommend that you do not change these numbers from the defaults.
# ******************************************************************************************
class DRIVER_MONITOR_SETTINGS:
def __init__(self):
# https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=OJ:L_202501899
self._ALERT_MIN_SPEED = 2.8 # 10 km/h
self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 5.
self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 15.
self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 25.
self._VISION_POLICY_ALERT_1_TIMEOUT = 5.
self._VISION_POLICY_ALERT_2_TIMEOUT = 8.
self._VISION_POLICY_ALERT_3_TIMEOUT = 13.
# no response = alert_3 sustained for certain amount of time
self._NO_RESPONSE_TIMEOUT = 5.
# lockout specs
self._MAX_ALERT_3 = 2
self._MAX_NO_RESPONSE = 1
self._LOCKOUT_TIMES = [int(60 * n_min / DT_DMON) for n_min in [1, 5, 15, 30]]
self._TIMEOUT_RECOVERY_FACTOR_MAX = 5.
self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25
self._FACE_THRESHOLD = 0.7
self._EYE_THRESHOLD = 0.65
self._SG_THRESHOLD = 0.9
self._BLINK_THRESHOLD = 0.865
self._PHONE_THRESH = 0.5
self._POSE_PITCH_THRESHOLD = 0.3133
self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
self._POSE_YAW_THRESHOLD = 0.4020
self._POSE_YAW_THRESHOLD_SLACK = 0.5042
self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
self._POSE_YAW_MIN_STEER_DEG = 30
self._POSE_YAW_STEER_FACTOR = 0.15
self._POSE_YAW_STEER_MAX_OFFSET = 0.3927
self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
self._PITCH_NATURAL_THRESHOLD = 0.449
self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
self._PITCH_NATURAL_VAR = 3*0.01
self._YAW_NATURAL_VAR = 3*0.05
self._PITCH_MAX_OFFSET = 0.124
self._PITCH_MIN_OFFSET = -0.0881
self._YAW_MAX_OFFSET = 0.289
self._YAW_MIN_OFFSET = -0.0246
self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / DT_DMON)
self._DCAM_UNCERTAIN_RESET_COUNT = int(2 / DT_DMON)
self._HI_STD_THRESHOLD = 0.3
self._HI_STD_FALLBACK_TIME = int(10 / DT_DMON) # fall back to wheel touch if model is uncertain for 10s
self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
self._POSE_CALIB_MIN_SPEED = 13 # 30 mph
self._POSE_OFFSET_MIN_COUNT = int(60 / DT_DMON) # valid data counts before calibration completes, 1min cumulative
self._POSE_OFFSET_MAX_COUNT = int(360 / DT_DMON) # stop deweighting new data after 6 min, aka "short term memory"
self._WHEELPOS_CALIB_MIN_SPEED = 11
self._WHEELPOS_THRESHOLD = 0.5
self._WHEELPOS_FILTER_MIN_COUNT = int(15 / DT_DMON) # allow 15 seconds to converge wheel side
self._WHEELPOS_DATA_AVG = 0.03
self._WHEELPOS_DATA_VAR = 3*5.5e-5
self._WHEELPOS_MAX_COUNT = -1
class DriverPose:
def __init__(self, settings):
pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2)
yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2)
self.yaw = 0.
self.pitch = 0.
self.pitch_offsetter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
self.yaw_offsetter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
self.calibrated = False
self.low_std = True
self.cfactor_pitch = 1.
self.cfactor_yaw = 1.
self.steer_yaw_offset = 0.
class DriverBlink:
def __init__(self):
self.left = 0.
self.right = 0.
# model output refers to center of undistorted+leveled image
ref_undistorted_cam = DEVICE_CAMERAS[("tici", "ar0231")].dcam
dcam_undistorted_FL = 598.0
dcam_undistorted_W, dcam_undistorted_H = (ref_undistorted_cam.width, ref_undistorted_cam.height)
def face_orientation_from_model(orient_model, pos_model, rpy_calib):
pitch_model = orient_model[0]
yaw_model = orient_model[1]
face_pixel_position = ((pos_model[0]+0.5)*dcam_undistorted_W, (pos_model[1]+0.5)*dcam_undistorted_H)
yaw_focal_angle = atan2(face_pixel_position[0] - dcam_undistorted_W//2, dcam_undistorted_FL)
pitch_focal_angle = atan2(face_pixel_position[1] - dcam_undistorted_H//2, dcam_undistorted_FL)
pitch = pitch_model + pitch_focal_angle
yaw = -yaw_model + yaw_focal_angle
pitch -= rpy_calib[1]
yaw -= rpy_calib[2]
return pitch, yaw
class DriverMonitoring:
def __init__(self, rhd_saved=False, settings=None, always_on=False):
# init policy settings
self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS()
# init driver status
wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
self.wheelpos_offsetter = RunningStatFilter(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
self.pose = DriverPose(settings=self.settings)
self.blink = DriverBlink()
self.phone_prob = 0.
self.alert_level = AlertLevel.none
self.always_on = always_on
self.distracted_types = defaultdict(bool)
self.driver_distracted = False
self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, DT_DMON)
self.wheel_on_right = False
self.wheel_on_right_last = None
self.wheel_on_right_default = rhd_saved
self.face_detected = False
self.alert_3_cnt = 0
self.cnt_since_alert_3 = 0
self.no_response_timeout = int(self.settings._NO_RESPONSE_TIMEOUT / DT_DMON)
self.no_response_cnt = 0
self.lockout_active = Params().get_bool("DriverTooDistracted")
self.lockout_count = Params().get("DriverLockoutCount") or 0
self.lockout_duration = self.settings._LOCKOUT_TIMES[min(max(self.lockout_count - 1, 0), len(self.settings._LOCKOUT_TIMES) - 1)]
self.lockout_time_elapsed = 0
self.step_change = 0.
self.active_policy = MonitoringPolicy.vision
self.driver_interacting = False
self.is_model_uncertain = False
self.hi_stds = 0
self.model_std_max = 0.
self.threshold_alert_1 = 0.
self.threshold_alert_2 = 0.
self.dcam_uncertain_cnt = 0
self.dcam_reset_cnt = 0
self._reset_awareness()
self._set_policy(MonitoringPolicy.vision)
def _reset_awareness(self):
self.awareness = 1.
self.last_vision_awareness = 1.
self.last_wheeltouch_awareness = 1.
def _set_policy(self, target_policy):
if self.active_policy == MonitoringPolicy.vision and self.awareness <= self.threshold_alert_2:
if target_policy == MonitoringPolicy.vision:
self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
else:
self.step_change = 0.
return # no exploit after orange alert
elif self.awareness <= 0.:
return
if target_policy == MonitoringPolicy.vision:
# when falling back from passive mode to active mode, reset awareness to avoid false alert
if self.active_policy != MonitoringPolicy.vision:
self.last_wheeltouch_awareness = self.awareness
self.awareness = self.last_vision_awareness
self.threshold_alert_1 = 1. - self.settings._VISION_POLICY_ALERT_1_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
self.threshold_alert_2 = 1. - self.settings._VISION_POLICY_ALERT_2_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
self.active_policy = MonitoringPolicy.vision
else:
if self.active_policy == MonitoringPolicy.vision:
self.last_vision_awareness = self.awareness
self.awareness = self.last_wheeltouch_awareness
self.threshold_alert_1 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
self.threshold_alert_2 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
self.step_change = DT_DMON / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
self.active_policy = MonitoringPolicy.wheeltouch
def _set_pose_strictness(self, brake_disengage_prob, car_speed):
bp = brake_disengage_prob
k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
bp_normal = max(min(bp / k1, 0.5),0)
self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_PITCH_THRESHOLD_SLACK,
self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_YAW_THRESHOLD_SLACK,
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
def _get_distracted_types(self):
self.distracted_types = defaultdict(bool)
if not self.pose.calibrated:
pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
else:
pitch_error = self.pose.pitch - min(max(self.pose.pitch_offsetter.filtered_stat.mean(),
self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
yaw_error = self.pose.yaw - min(max(self.pose.yaw_offsetter.filtered_stat.mean(),
self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional
yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.)
else:
yaw_error = abs(yaw_error)
pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD
yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw
self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold))
self.distracted_types['eye'] = bool((self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD)
self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH)
def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, lowspeed, demo_mode=False, steering_angle_deg=0.):
rhd_pred = driver_state.wheelOnRightProb
# calibrates only when there's movement and either face detected
if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
self.wheelpos_offsetter.push_and_update(rhd_pred)
wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT
if wheelpos_calibrated or demo_mode:
self.wheel_on_right = self.wheelpos_offsetter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
else:
self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
# make sure no switching when engaged
if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
self.wheel_on_right = self.wheel_on_right_last
driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
driver_data.faceOrientationStd, driver_data.facePositionStd)):
return
self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
self.pose.pitch, self.pose.yaw = face_orientation_from_model(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.)
self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR
if self.wheel_on_right:
self.pose.yaw *= -1
self.pose.steer_yaw_offset *= -1
self.wheel_on_right_last = self.wheel_on_right
self.model_std_max = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1])
self.pose.low_std = self.model_std_max < self.settings._HI_STD_THRESHOLD
self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.phone_prob = driver_data.phoneProb
self._get_distracted_types()
self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
self.driver_distraction_filter.update(self.driver_distracted)
# only update offsetter when driver is actively driving the car above a certain speed
if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
self.pose.pitch_offsetter.push_and_update(self.pose.pitch)
self.pose.yaw_offsetter.push_and_update(self.pose.yaw)
self.pose.calibrated = self.pose.pitch_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT and \
self.pose.yaw_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT
if self.face_detected and not self.driver_distracted:
dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD
if dcam_uncertain and not lowspeed:
self.dcam_uncertain_cnt += 1
self.dcam_reset_cnt = 0
else:
self.dcam_reset_cnt += 1
if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT:
self.dcam_uncertain_cnt = 0
self.is_model_uncertain = self.hi_stds >= self.settings._HI_STD_FALLBACK_TIME
self._set_policy(MonitoringPolicy.vision if self.face_detected and not self.is_model_uncertain else MonitoringPolicy.wheeltouch)
if self.face_detected and not self.pose.low_std and not self.driver_distracted:
self.hi_stds += 1
elif self.face_detected and self.pose.low_std:
self.hi_stds = 0
def _update_events(self, driver_engaged, op_engaged, lowspeed, wrong_gear):
self.alert_level = AlertLevel.none
self.driver_interacting = driver_engaged
if self.alert_3_cnt >= self.settings._MAX_ALERT_3 or self.no_response_cnt >= self.settings._MAX_NO_RESPONSE:
if not self.lockout_active:
self.lockout_count += 1
self.lockout_duration = self.settings._LOCKOUT_TIMES[min(self.lockout_count - 1, len(self.settings._LOCKOUT_TIMES) - 1)]
Params().put("DriverLockoutCount", self.lockout_count)
self.lockout_active = True
if self.lockout_active:
self.lockout_time_elapsed += 1
if self.lockout_time_elapsed > self.lockout_duration:
self.lockout_active = False
self.alert_3_cnt = 0
self.cnt_since_alert_3 = 0
self.no_response_cnt = 0
self.lockout_time_elapsed = 0
always_on_valid = self.always_on and not wrong_gear
if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \
(not always_on_valid and not op_engaged) or \
(always_on_valid and not op_engaged and self.awareness <= 0):
# always reset on disengage with normal mode; disengage resets only on red if always on
self._reset_awareness()
return
awareness_prev = self.awareness
_reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1
_reaching_alert_3 = self.awareness - self.step_change <= 0
lowspeed_exemption = lowspeed and _reaching_alert_1
always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3
if self.awareness > 0 and \
((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or lowspeed_exemption):
if self.driver_interacting:
self._reset_awareness()
return
# only restore awareness when paying attention and alert is not red
self.awareness = min(self.awareness + ((self.settings._TIMEOUT_RECOVERY_FACTOR_MAX-self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*
(1.-self.awareness)+self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
if self.awareness == 1.:
self.last_wheeltouch_awareness = min(self.last_wheeltouch_awareness + self.step_change, 1.)
# don't display alert banner when awareness is recovering and has cleared orange
if self.awareness > self.threshold_alert_2:
return
certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected
maybe_distracted = self.is_model_uncertain or not self.face_detected
if certainly_distracted or maybe_distracted:
# should always be counting if distracted unless at low speed and reaching green
# also will not be reaching 0 if DM is active when not engaged
if not (lowspeed_exemption or always_on_exemption):
self.awareness = max(self.awareness - self.step_change, -0.1)
if self.awareness <= 0.:
# terminal alert: disengagement required
self.alert_level = AlertLevel.three
if awareness_prev > 0.:
self.alert_3_cnt += 1
self.cnt_since_alert_3 = 0
else:
self.cnt_since_alert_3 += 1
if self.cnt_since_alert_3 == self.no_response_timeout:
self.no_response_cnt += 1
else:
if self.awareness <= self.threshold_alert_2:
self.alert_level = AlertLevel.two
elif self.awareness <= self.threshold_alert_1:
self.alert_level = AlertLevel.one
def get_state_packet(self, valid=True):
# build driverMonitoringState packet
dat = messaging.new_message('driverMonitoringState', valid=valid)
dm = dat.driverMonitoringState
dm.lockout = self.lockout_active
dm.lockoutCount = self.lockout_count
if self.lockout_active:
dm.lockoutMinutesRemaining = max(1, round((self.lockout_duration - self.lockout_time_elapsed) * DT_DMON / 60.))
dm.alert3Count = self.alert_3_cnt
dm.noResponseCount = self.no_response_cnt
dm.noResponseForceDecel = self.alert_level == AlertLevel.three and self.cnt_since_alert_3 >= self.no_response_timeout
dm.alwaysOn = self.always_on
dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2
dm.alertLevel = self.alert_level
dm.activePolicy = self.active_policy
dm.isRHD = self.wheel_on_right
dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos_offsetter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT)
dm.rhdCalibration.offset = self.wheelpos_offsetter.filtered_stat.M
dm.visionPolicyState.awarenessPercent = to_percent(self.last_vision_awareness if self.active_policy != MonitoringPolicy.vision else self.awareness)
dm.visionPolicyState.awarenessStep = self.step_change if self.active_policy == MonitoringPolicy.vision else 0.
dm.visionPolicyState.isDistracted = self.driver_distracted
dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose']
dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye']
dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone']
dm.visionPolicyState.faceDetected = self.face_detected
dm.visionPolicyState.pose.pitch = self.pose.pitch
dm.visionPolicyState.pose.yaw = self.pose.yaw
dm.visionPolicyState.pose.calibrated = self.pose.calibrated
dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offsetter.filtered_stat.M
dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offsetter.filtered_stat.M
dm.visionPolicyState.pose.uncertainty = self.model_std_max
dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME)
dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / self.settings._DCAM_UNCERTAIN_ALERT_COUNT)
dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.last_wheeltouch_awareness if self.active_policy == MonitoringPolicy.vision else self.awareness)
dm.wheeltouchPolicyState.awarenessStep = 0. if self.active_policy == MonitoringPolicy.vision else self.step_change
dm.wheeltouchPolicyState.driverInteracting = self.driver_interacting
return dat
def run_step(self, sm, demo=False):
if demo:
car_speed = 30
enabled = True
wrong_gear = False
lowspeed = False
driver_engaged = False
brake_disengage_prob = 1.0
steering_angle_deg = 0.0
rpyCalib = [0., 0., 0.]
else:
car_speed = sm['carState'].vEgo
enabled = sm['selfdriveState'].enabled
wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
lowspeed = car_speed < self.settings._ALERT_MIN_SPEED
driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
steering_angle_deg = sm['carState'].steeringAngleDeg
rpyCalib = sm['liveCalibration'].rpyCalib
self._set_pose_strictness(
brake_disengage_prob=brake_disengage_prob,
car_speed=car_speed,
)
# Parse data from dmonitoringmodeld
self._update_states(
driver_state=sm['driverStateV2'],
cal_rpy=rpyCalib,
car_speed=car_speed,
op_engaged=enabled,
lowspeed=lowspeed,
demo_mode=demo,
steering_angle_deg=steering_angle_deg,
)
# Update distraction events
self._update_events(
driver_engaged=driver_engaged,
op_engaged=enabled,
lowspeed=lowspeed,
wrong_gear=wrong_gear,
)
selfdrive/monitoring/test_dmonitoringd.py
-24
View File
@@ -1,24 +0,0 @@
from types import SimpleNamespace
import pytest
from openpilot.selfdrive.monitoring.dmonitoringd import get_dm_inputs
@pytest.mark.parametrize("enabled, lat_active, expected", [
(False, False, False),
(True, False, True),
(False, True, True),
(True, True, True),
])
def test_iq_enabled_adapter(enabled, lat_active, expected):
sm = {
'driverStateV2': object(),
'liveCalibration': object(),
'carState': object(),
'selfdriveState': SimpleNamespace(enabled=enabled),
'modelV2': object(),
'carControl': SimpleNamespace(latActive=lat_active),
}
assert get_dm_inputs(sm)['selfdriveState'].enabled == expected
selfdrive/monitoring/test_monitoring.py
+156 -113
View File
@@ -1,15 +1,19 @@
from cereal import log
import numpy as np
import pytest
from cereal import log, car
from openpilot.common.realtime import DT_DMON
from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS
from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS
from openpilot.system.hardware import HARDWARE
EventName = log.OnroadEvent.EventName
dm_settings = DRIVER_MONITOR_SETTINGS()
dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
TEST_TIMESPAN = 120 # seconds
DISTRACTED_SECONDS_TO_ORANGE = dm_settings._VISION_POLICY_ALERT_2_TIMEOUT + 1
DISTRACTED_SECONDS_TO_RED = dm_settings._VISION_POLICY_ALERT_3_TIMEOUT + 1
INVISIBLE_SECONDS_TO_ORANGE = dm_settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + 1
INVISIBLE_SECONDS_TO_RED = dm_settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + 1
DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1
INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1
def make_msg(face_detected, distracted=False, model_uncertain=False):
ds = log.DriverStateV2.new_message()
@@ -33,7 +37,7 @@ msg_ATTENTIVE = make_msg(True)
msg_DISTRACTED = make_msg(True, distracted=True)
msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True)
msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True)
msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._HI_STD_THRESHOLD*1.5)
msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5)
# driver interaction with car
car_interaction_DETECTED = True
@@ -47,66 +51,51 @@ always_true = [True] * int(TEST_TIMESPAN / DT_DMON)
always_false = [False] * int(TEST_TIMESPAN / DT_DMON)
class TestMonitoring:
def _run_seq(self, msgs, interaction, engaged, lowspeed):
def _run_seq(self, msgs, interaction, engaged, standstill):
DM = DriverMonitoring()
alert_lvls = []
events = []
for idx in range(len(msgs)):
DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], lowspeed[idx])
DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx])
# cal_rpy and car_speed don't matter here
# evaluate events at 10Hz for tests
DM._update_events(interaction[idx], engaged[idx], lowspeed[idx], 0)
alert_lvls.append(DM.alert_level)
assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs"
return alert_lvls, DM
DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0)
events.append(DM.current_events)
assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs"
return events, DM
def _assert_no_events(self, events):
assert all(not len(e) for e in events)
# engaged, driver is attentive all the time
def test_fully_aware_driver(self):
alert_lvls, d_status = self._run_seq(always_attentive, always_false, always_true, always_false)
assert all(a == 0 for a in alert_lvls)
assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.vision
events, _ = self._run_seq(always_attentive, always_false, always_true, always_false)
self._assert_no_events(events)
# engaged, driver is distracted and does nothing
def test_fully_distracted_driver(self):
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
s = d_status.settings
assert alert_lvls[int(s._VISION_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0
assert alert_lvls[int((s._VISION_POLICY_ALERT_1_TIMEOUT + \
(s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1
assert alert_lvls[int((s._VISION_POLICY_ALERT_2_TIMEOUT + \
(s._VISION_POLICY_ALERT_3_TIMEOUT - s._VISION_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2
assert alert_lvls[int((s._VISION_POLICY_ALERT_3_TIMEOUT + \
(TEST_TIMESPAN - 10 - s._VISION_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3
events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0
assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \
((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
EventName.preDriverDistracted
assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \
((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert events[int((d_status.settings._DISTRACTED_TIME + \
((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted
assert isinstance(d_status.awareness, float)
# engaged, distracted past red and beyond the no-response window -> unavailability response + lockout
def test_distracted_lockout(self):
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
assert alert_lvls[int(DISTRACTED_SECONDS_TO_RED / DT_DMON)] == 3
assert d_status.lockout_active
assert d_status.lockout_time_elapsed > 0
assert d_status.lockout_count >= 1
# no face -> wheeltouch red, sustained past the no-response timeout -> unavailability response + lockout
def test_invisible_lockout(self):
_, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch
assert d_status.lockout_active
assert d_status.lockout_count >= 1
# engaged, no face detected the whole time, no action
def test_fully_invisible_driver(self):
alert_lvls, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
s = d_status.settings
assert alert_lvls[int(s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT + \
(s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + \
(s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + \
(TEST_TIMESPAN - 10 - s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3
assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch
events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0
assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \
((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
EventName.preDriverUnresponsive
assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \
((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert events[int((d_status.settings._AWARENESS_TIME + \
((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive
# engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
# - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention
@@ -117,13 +106,13 @@ class TestMonitoring:
[msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON))
interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \
[car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON))
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)] == 0
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)] == 2
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)] == 2
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)] == 0
events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0
assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0
# engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \
# driver dodges, and then touches wheel to no avail, disengages and reengages
@@ -141,31 +130,31 @@ class TestMonitoring:
= [True] * int(1/DT_DMON)
op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \
= [False] * int(0.5/DT_DMON)
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)] == 2
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)] == 3
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] == 3
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)] == 0
events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
assert events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted
assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted
assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0
# engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
# - both actions should clear the alert, but momentary appearance should not
def test_sometimes_transparent_commuter(self):
for _visible_time in (0.5, 10):
ds_vector = always_no_face[:]*2
interaction_vector = always_false[:]*2
ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \
[msg_ATTENTIVE] * int(_visible_time/DT_DMON)
interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0
if _visible_time == 0.5:
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 2
elif _visible_time == 10:
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0
_visible_time = np.random.choice([0.5, 10])
ds_vector = always_no_face[:]*2
interaction_vector = always_false[:]*2
ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \
[msg_ATTENTIVE] * int(_visible_time/DT_DMON)
interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0
if _visible_time == 0.5:
assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.preDriverUnresponsive
elif _visible_time == 10:
assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0
# engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
# - only disengage will clear the alert
@@ -177,51 +166,105 @@ class TestMonitoring:
ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON)
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)] == 0
events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive
assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive
assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive
assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0
# disengaged, always distracted driver
# - dm should stay quiet when not engaged
def test_pure_dashcam_user(self):
alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
assert all(a == 0 for a in alert_lvls)
events, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
assert sum(len(event) for event in events) == 0
# engaged, car stops at traffic light, down to orange, no action, then car starts moving
# - should only reach green when stopped, but continues counting down on launch
def test_long_traffic_light_victim(self):
_redlight_time = 60 # seconds
lowspeed_vector = always_true[:]
lowspeed_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON)
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector)
s = d_status.settings
assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0
_alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT
assert alert_lvls[int((_redlight_time+0.5)/DT_DMON)] == 1
assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2
# engaged, distracted while moving, then car stops after reaching orange
# - should reset timer to pre green at low speed
def test_distracted_then_stops(self):
_stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange
lowspeed_vector = always_false[:]
lowspeed_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON)
alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector)
# just before and briefly after stopping: orange alert; goes away quickly after stopped
assert alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2
assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0
standstill_vector = always_true[:]
standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON)
events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \
EventName.preDriverDistracted
assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted
assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted
# engaged, model is somehow uncertain and driver is distracted
# - should fall back to wheel touch after uncertain alert
def test_somehow_indecisive_model(self):
ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON)
interaction_vector = always_false[:]
alert_lvls, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
s = d_status.settings
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)] == 1
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 2
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 3
events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
assert EventName.preDriverUnresponsive in \
events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names
assert EventName.promptDriverUnresponsive in \
events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
assert EventName.driverUnresponsive in \
events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
@pytest.mark.parametrize("enabled_state, lat_active_state, expected", [
(False, False, False), # Both Disabled
(True, False, True), # OP Enabled, Lat Inactive
(False, True, True), # OP Disabled, Lat Active (e.g. AOL)
(True, True, True) # Both Active
])
def test_enabled_states(enabled_state, lat_active_state, expected):
"""
Test DriverMonitoring.run_step with all 4 combinations of:
- selfdriveState.enabled (True/False)
- carControl.latActive (True/False)
"""
cs = car.CarState.new_message()
cs.vEgo = 30.0
cs.gearShifter = car.CarState.GearShifter.drive
cs.standstill = False
cs.steeringPressed = False
cs.gasPressed = False
ss = log.SelfdriveState.new_message()
ss.enabled = enabled_state
cc = car.CarControl.new_message()
cc.latActive = lat_active_state
mv2 = log.ModelDataV2.new_message()
mv2.meta.disengagePredictions.brakeDisengageProbs = [0.0]
lc = log.LiveCalibrationData.new_message()
lc.rpyCalib = [0.0, 0.0, 0.0]
ds = make_msg(False)
sm = {
'carState': cs,
'selfdriveState': ss,
'carControl': cc,
'modelV2': mv2,
'liveCalibration': lc,
'driverStateV2': ds
}
driver_monitoring = DriverMonitoring()
# run_test doesn't assign enabled to a variable, so we need to spy on _update_events to see its value
captured_args = []
original_update_events = driver_monitoring._update_events
def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
captured_args.append(op_engaged)
return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed)
driver_monitoring._update_events = spy_update_events
driver_monitoring.run_step(sm, demo=False)
# Assertion
assert len(captured_args) == 1, "Expected _update_events to be called exactly once"
actual_enabled = captured_args[0]
assert actual_enabled == expected, f"Expected op_engaged={expected}, but got {actual_enabled}"
selfdrive/selfdrived/events.py
+11 -18
View File
@@ -79,13 +79,6 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4)
def too_distracted_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
if sm['driverMonitoringState'].lockout:
mins_left = sm['driverMonitoringState'].lockoutMinutesRemaining
return NoEntryAlert("Too Distracted", f"{mins_left} minute{'s' if mins_left != 1 else ''} Left", priority=Priority.HIGH)
return NoEntryAlert("Pay Attention to Engage", priority=Priority.HIGH)
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating'
return Alert(
@@ -367,15 +360,15 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8),
},
EventName.driverDistracted1: {
EventName.preDriverDistracted: {
ET.PERMANENT: Alert(
"Pay Attention",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, .1),
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.driverDistracted2: {
EventName.promptDriverDistracted: {
ET.PERMANENT: Alert(
"Pay Attention",
"Driver Distracted",
@@ -383,7 +376,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverDistracted3: {
EventName.driverDistracted: {
ET.PERMANENT: Alert(
"DISENGAGE IMMEDIATELY",
"Driver Distracted",
@@ -391,7 +384,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
},
EventName.driverUnresponsive1: {
EventName.preDriverUnresponsive: {
ET.PERMANENT: Alert(
"Touch Steering Wheel: No Face Detected",
"",
@@ -399,7 +392,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1),
},
EventName.driverUnresponsive2: {
EventName.promptDriverUnresponsive: {
ET.PERMANENT: Alert(
"Touch Steering Wheel",
"Driver Unresponsive",
@@ -407,7 +400,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverUnresponsive3: {
EventName.driverUnresponsive: {
ET.PERMANENT: Alert(
"DISENGAGE IMMEDIATELY",
"Driver Unresponsive",
@@ -639,7 +632,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
},
EventName.tooDistracted: {
ET.NO_ENTRY: too_distracted_alert,
ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"),
},
EventName.excessiveActuation: {
@@ -885,14 +878,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
if HARDWARE.get_device_type() == 'mici':
EVENTS.update({
EventName.driverDistracted1: {
EventName.preDriverDistracted: {
ET.PERMANENT: Alert(
"Pay Attention",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, 2),
Priority.LOW, VisualAlert.none, AudibleAlert.none, 2),
},
EventName.driverDistracted2: {
EventName.promptDriverDistracted: {
ET.PERMANENT: Alert(
"Pay Attention",
"Driver Distracted",
selfdrive/selfdrived/selfdrived.py
+7 -21
View File
@@ -54,8 +54,6 @@ EventName = log.OnroadEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type
SafetyModel = car.CarParams.SafetyModel
TurnDirection = custom.IQTurnSignalDirection
AlertLevel = log.DriverMonitoringState.AlertLevel
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
@@ -149,7 +147,6 @@ class SelfdriveD(GapButtonActions):
self.events_prev = []
self.logged_comm_issue = None
self.not_running_prev = None
self.dm_lockout_set = False
self.experimental_mode = False
self.personality = get_sanitize_int_param(
"LongitudinalPersonality",
@@ -186,6 +183,7 @@ class SelfdriveD(GapButtonActions):
self.events_iq = IQEvents()
self.events_iq_prev = []
self._cached_dm_event_names: tuple[int, ...] = ()
self._cached_plan_event_names: tuple[int, ...] = ()
self._cached_model_event_names: tuple[int, ...] = ()
self._cached_nav_event_names: tuple[int, ...] = ()
@@ -207,6 +205,10 @@ class SelfdriveD(GapButtonActions):
if self.sm.updated['iqPlan']:
self._cached_plan_event_names = tuple(event.name.raw for event in self._get_longitudinal_plan_ext().events)
def _refresh_cached_dm_events(self) -> None:
if self.sm.updated['driverMonitoringState']:
self._cached_dm_event_names = tuple(event.name.raw for event in self.sm['driverMonitoringState'].events)
def _refresh_cached_model_events(self) -> None:
if not self.sm.updated['iqDriveModelData']:
return
@@ -295,24 +297,8 @@ class SelfdriveD(GapButtonActions):
self.events.add(EventName.resumeBlocked)
if not self.CP.notCar:
if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set:
self.params.put_bool("DriverTooDistracted", True)
self.dm_lockout_set = True
elif not self.sm['driverMonitoringState'].lockout and self.dm_lockout_set:
self.params.remove("DriverTooDistracted")
self.dm_lockout_set = False
if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout:
self.events.add(EventName.tooDistracted)
vision_dm = self.sm['driverMonitoringState'].activePolicy == MonitoringPolicy.vision
if self.sm['driverMonitoringState'].alertLevel == AlertLevel.one:
self.events.add(EventName.driverDistracted1 if vision_dm else EventName.driverUnresponsive1)
elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.two:
self.events.add(EventName.driverDistracted2 if vision_dm else EventName.driverUnresponsive2)
elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.three:
self.events.add(EventName.driverDistracted3 if vision_dm else EventName.driverUnresponsive3)
self._refresh_cached_dm_events()
self._add_event_names(self._cached_dm_event_names)
self._refresh_cached_plan_events()
self._add_iq_event_names(self._cached_plan_event_names)
selfdrive/selfdrived/tests/test_driver_monitoring.py
-32
View File
@@ -1,32 +0,0 @@
from types import SimpleNamespace
from cereal import car, log
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
EventName = log.OnroadEvent.EventName
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
def test_driver_monitoring_alert_stages():
expected_sounds = {
EventName.driverDistracted1: AudibleAlert.preAlert,
EventName.driverDistracted2: AudibleAlert.promptDistracted,
EventName.driverDistracted3: AudibleAlert.warningImmediate,
EventName.driverUnresponsive1: AudibleAlert.none,
EventName.driverUnresponsive2: AudibleAlert.promptDistracted,
EventName.driverUnresponsive3: AudibleAlert.warningImmediate,
}
for event_name, audible_alert in expected_sounds.items():
assert EVENTS[event_name][ET.PERMANENT].audible_alert == audible_alert
def test_driver_monitoring_lockout_alert():
callback = EVENTS[EventName.tooDistracted][ET.NO_ENTRY]
sm = {'driverMonitoringState': SimpleNamespace(lockout=True, lockoutMinutesRemaining=5)}
alert = callback(None, None, sm, False, 0, None)
assert alert.alert_text_1 == "5 minutes Left"
assert alert.alert_text_2 == "Too Distracted"
selfdrive/test/process_replay/migration.py
+12 -37
View File
@@ -455,46 +455,21 @@ def migrate_onroadEvents(msgs):
return ops, [], []
@migration(inputs=["driverMonitoringStateDEPRECATED"])
@migration(inputs=["driverMonitoringState"])
def migrate_driverMonitoringState(msgs):
ops = []
for index, msg in msgs:
old = msg.driverMonitoringStateDEPRECATED
new_msg = messaging.new_message('driverMonitoringState', valid=msg.valid, logMonoTime=msg.logMonoTime)
dm = new_msg.driverMonitoringState
dm.isRHD = old.isRHD
dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision if old.isActiveMode else \
log.DriverMonitoringState.MonitoringPolicy.wheeltouch
msg = msg.as_builder()
events = []
for event in msg.driverMonitoringState.eventsDEPRECATED:
try:
if not str(event.name).endswith('DEPRECATED'):
# dict converts name enum into string representation
events.append(log.OnroadEvent(**event.to_dict()))
except RuntimeError: # Member was null
traceback.print_exc()
AlertLevel = log.DriverMonitoringState.AlertLevel
event_to_alert_level = {
'driverDistracted1': AlertLevel.one, 'driverUnresponsive1': AlertLevel.one,
'driverDistracted2': AlertLevel.two, 'driverUnresponsive2': AlertLevel.two,
'driverDistracted3': AlertLevel.three, 'driverUnresponsive3': AlertLevel.three,
}
for event in old.events:
level = event_to_alert_level.get(str(event.name))
if level is not None:
dm.alertLevel = level
break
dm.lockout = any(str(event.name) == 'tooDistracted' for event in old.events)
dm.visionPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessStatus if old.isActiveMode else old.awarenessActive) * 100)))
dm.visionPolicyState.awarenessStep = old.stepChange if old.isActiveMode else 0.
dm.visionPolicyState.isDistracted = old.isDistracted
dm.visionPolicyState.distractedTypes.pose = bool(old.distractedType & 1)
dm.visionPolicyState.distractedTypes.eye = bool(old.distractedType & 2)
dm.visionPolicyState.distractedTypes.phone = bool(old.distractedType & 4)
dm.visionPolicyState.faceDetected = old.faceDetected
dm.visionPolicyState.pose.pitchCalib.offset = old.posePitchOffset
dm.visionPolicyState.pose.pitchCalib.calibratedPercent = int(min(100, old.posePitchValidCount / 1200 * 100))
dm.visionPolicyState.pose.yawCalib.offset = old.poseYawOffset
dm.visionPolicyState.pose.yawCalib.calibratedPercent = int(min(100, old.poseYawValidCount / 1200 * 100))
dm.visionPolicyState.pose.calibrated = old.posePitchValidCount >= 1200 and old.poseYawValidCount >= 1200
dm.visionPolicyState.wheeltouchFallbackPercent = int(min(100, old.hiStdCount / 200 * 100))
dm.visionPolicyState.uncertainOffroadAlertPercent = int(min(100, old.uncertainCount / 1200 * 100))
dm.wheeltouchPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessPassive if old.isActiveMode else old.awarenessStatus) * 100)))
dm.wheeltouchPolicyState.awarenessStep = 0. if old.isActiveMode else old.stepChange
ops.append((index, new_msg.as_reader()))
msg.driverMonitoringState.events = events
ops.append((index, msg.as_reader()))
return ops, [], []
selfdrive/ui/mici/layouts/onboarding.py
+1 -1
View File
@@ -200,7 +200,7 @@ class TrainingGuideDMTutorial(Widget):
looking_center = False
# stay at 100% once reached
if (dm_state.visionPolicyState.faceDetected and looking_center) or self._progress.x > 0.99:
if (dm_state.faceDetected and looking_center) or self._progress.x > 0.99:
slow = self._progress.x < 0.25
duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION
self._progress.x += 1.0 / (duration * gui_app.target_fps)
selfdrive/ui/mici/onroad/driver_camera_dialog.py
+19 -18
View File
@@ -1,14 +1,20 @@
import pyray as rl
from cereal import car, log, messaging
from cereal import log, messaging
from msgq.visionipc import VisionStreamType
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
from openpilot.selfdrive.ui.ui_state import ui_state, device
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.widgets.nav_widget import NavWidget
from openpilot.system.ui.widgets.label import gui_label
EventName = log.OnroadEvent.EventName
EVENT_TO_INT = EventName.schema.enumerants
class DriverCameraView(CameraView):
def _calc_frame_matrix(self, rect: rl.Rectangle):
base = super()._calc_frame_matrix(rect)
@@ -110,14 +116,10 @@ class DriverCameraDialog(NavWidget):
return
msg = messaging.new_message('selfdriveState')
if dm_state is not None:
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
alert_sounds = {
'one': AudibleAlert.preAlert,
'two': AudibleAlert.promptDistracted,
'three': AudibleAlert.warningImmediate,
}
msg.selfdriveState.alertSound = alert_sounds.get(str(dm_state.alertLevel), AudibleAlert.none)
if dm_state is not None and len(dm_state.events):
event_name = EVENT_TO_INT[dm_state.events[0].name]
if event_name is not None and event_name in EVENTS and ET.PERMANENT in EVENTS[event_name]:
msg.selfdriveState.alertSound = EVENTS[event_name][ET.PERMANENT].audible_alert
self._pm.send('selfdriveState', msg)
def _render_dm_alerts(self, rect: rl.Rectangle):
@@ -125,30 +127,29 @@ class DriverCameraDialog(NavWidget):
dm_state = ui_state.sm["driverMonitoringState"]
self._publish_alert_sound(dm_state)
is_vision = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
awareness_pct = dm_state.visionPolicyState.awarenessPercent if is_vision else dm_state.wheeltouchPolicyState.awarenessPercent
gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height),
f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
color=rl.Color(0, 0, 0, 180))
gui_label(rect, f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
gui_label(rect, f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
color=rl.Color(255, 255, 255, int(255 * 0.9)))
if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none:
if not dm_state.events:
return
alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}"
# Show first event (only one should be active at a time)
event_name_str = str(dm_state.events[0].name).split('.')[-1]
alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT
shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height)
gui_label(shadow_rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
gui_label(shadow_rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
alignment=alignment,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
color=rl.Color(0, 0, 0, 180))
gui_label(rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
gui_label(rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
alignment=alignment,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
color=rl.Color(255, 255, 255, int(255 * 0.9)))
@@ -165,7 +166,7 @@ class DriverCameraDialog(NavWidget):
def _draw_face_detection(self, rect: rl.Rectangle):
dm_state = ui_state.sm["driverMonitoringState"]
driver_data = self.driver_state_renderer.get_driver_data()
if not dm_state.visionPolicyState.faceDetected:
if not dm_state.faceDetected:
return
# Get face position and orientation
selfdrive/ui/mici/onroad/driver_state.py
+25 -21
View File
@@ -6,12 +6,12 @@ from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.widgets import Widget
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net
AlertSize = log.SelfdriveState.AlertSize
DEBUG = False
ACTIVE_ACCENT = rl.Color(0x0C, 0x94, 0x96, 0xFF)
CONE_COLOR_ORANGE = (255, 115, 0)
LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6)
LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3)
@@ -21,7 +21,6 @@ class DriverStateRenderer(Widget):
BASE_SIZE = 60
LINES_ANGLE_INCREMENT = 5
LINES_STALE_ANGLES = 3.0 # seconds
AWARENESS_UNFULL_PERCENT = 95
def __init__(self, lines: bool = False, inset: bool = False):
super().__init__()
@@ -36,15 +35,11 @@ class DriverStateRenderer(Widget):
self._is_active = False
self._is_rhd = False
self._face_detected = False
self._face_pitch = 0.
self._face_yaw = 0.
self._should_draw = False
self._force_active = False
self._looking_center = False
self._awareness_unfull = False
self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps)
self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps)
self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False)
@@ -100,7 +95,6 @@ class DriverStateRenderer(Widget):
rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
if self.effective_active:
active_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0)
source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height)
dest_rect = rl.Rectangle(
self._rect.x + self._rect.width / 2,
@@ -110,16 +104,13 @@ class DriverStateRenderer(Widget):
)
if not self._lines:
r = int(round(ACTIVE_ACCENT.r * active_amount + CONE_COLOR_ORANGE[0] * (1 - active_amount)))
g = int(round(ACTIVE_ACCENT.g * active_amount + CONE_COLOR_ORANGE[1] * (1 - active_amount)))
b = int(round(ACTIVE_ACCENT.b * active_amount + CONE_COLOR_ORANGE[2] * (1 - active_amount)))
rl.draw_texture_pro(
self._dm_cone,
source_rect,
dest_rect,
rl.Vector2(dest_rect.width / 2, dest_rect.height / 2),
self._rotation_filter.x - 90,
rl.Color(r, g, b, int(255 * self._fade_filter.x)),
rl.Color(ACTIVE_ACCENT.r, ACTIVE_ACCENT.g, ACTIVE_ACCENT.b, int(255 * self._fade_filter.x)),
)
else:
@@ -159,12 +150,9 @@ class DriverStateRenderer(Widget):
sm = ui_state.sm
dm_state = sm["driverMonitoringState"]
self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
self._is_active = dm_state.isActiveMode
self._is_rhd = dm_state.isRHD
self._face_detected = dm_state.visionPolicyState.faceDetected
self._awareness_unfull = self.effective_active and dm_state.visionPolicyState.awarenessPercent < self.AWARENESS_UNFULL_PERCENT
self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6)
self._face_yaw = -dm_state.visionPolicyState.pose.yaw
self._face_detected = dm_state.faceDetected
driverstate = sm["driverStateV2"]
driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
@@ -172,9 +160,24 @@ class DriverStateRenderer(Widget):
def _update_state(self):
# Get monitoring state
_ = self.get_driver_data()
pitch = self._pitch_filter.update(self._face_pitch)
yaw = self._yaw_filter.update(self._face_yaw)
driver_data = self.get_driver_data()
driver_orient = driver_data.faceOrientation
if len(driver_orient) != 3:
return
# Calibrate orientation so looking straight ahead at the road (instead of at the device) reads
# (0, 0), using live calibration. Makes the cone point in the correct direction. (stock PR #37149)
sm = ui_state.sm
if sm.valid['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) == 3:
cal_rpy = sm['liveCalibration'].rpyCalib
else:
cal_rpy = [0.0, 0.0, 0.0]
_, pitch, yaw = face_orientation_from_net(driver_orient, driver_data.facePosition, cal_rpy)
yaw = -yaw # undo sign flip in face_orientation_from_net to match UI convention
pitch = self._pitch_filter.update(pitch)
yaw = self._yaw_filter.update(yaw)
# hysteresis on looking center
if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER:
@@ -195,8 +198,9 @@ class DriverStateRenderer(Widget):
rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN)
rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN)
# filter head rotation, handling wrap-around
rotation = math.degrees(math.atan2(pitch * 2, yaw))
# filter head rotation, handling wrap-around (bias pitch up since calib/DM pose isn't exact,
# and halve yaw sensitivity)
rotation = math.degrees(math.atan2((pitch + math.radians(6)) * 2, yaw))
angle_diff = rotation - self._rotation_filter.x
angle_diff = ((angle_diff + 180) % 360) - 180
self._rotation_filter.update(self._rotation_filter.x + angle_diff)
selfdrive/ui/onroad/driver_state.py
+1 -1
View File
@@ -114,7 +114,7 @@ class DriverStateRenderer(Widget):
# Get monitoring state
dm_state = sm["driverMonitoringState"]
self.is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
self.is_active = dm_state.isActiveMode
self.is_rhd = dm_state.isRHD
# Update fade state (smoother transition between active/inactive)
selfdrive/ui/soundd.py
+18 -20
View File
@@ -45,28 +45,26 @@ sound_list_iq: dict[int, tuple[str, int | None, float]] = {
AudibleAlertIQ.promptSingleHigh: ("prompt_single_high.wav", 1, MAX_VOLUME),
}
def get_sound_list(device_type: str) -> dict[int, tuple[str, int | None, float]]:
sounds = {
sound_list: dict[int, tuple[str, int | None, float]] = {
# AudibleAlert, file name, play count (none for infinite)
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME),
AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME),
AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME),
AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME),
AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME),
AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME),
**sound_list_iq,
}
if HARDWARE.get_device_type() in ("tizi", "tici"):
sound_list.update({
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME),
AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME),
AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME),
AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME),
AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME),
AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME),
**sound_list_iq,
}
if device_type in ("tizi", "tici"):
sounds.update({
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
})
return sounds
sound_list = get_sound_list(HARDWARE.get_device_type())
})
def check_selfdrive_timeout_alert(sm):
ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
selfdrive/ui/tests/test_soundd.py
+1 -6
View File
@@ -2,19 +2,13 @@ from cereal import car
from cereal import messaging
from cereal.messaging import SubMaster, PubMaster
from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
from openpilot.selfdrive.ui.soundd import get_sound_list
import pytest
import time
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
class TestSoundd:
@pytest.mark.parametrize("device_type", ["mici", "tici", "tizi"])
def test_prompt_distracted_sound(self, device_type):
assert get_sound_list(device_type)[AudibleAlert.promptDistracted][0] == "prompt_distracted.wav"
def test_check_selfdrive_timeout_alert(self):
sm = SubMaster(['selfdriveState'])
pm = PubMaster(['selfdriveState'])
@@ -38,3 +32,4 @@ class TestSoundd:
assert check_selfdrive_timeout_alert(sm)
# TODO: add test with micd for checking that soundd actually outputs sounds
system/loggerd/loggerd.h
+9 -2
View File
@@ -28,6 +28,13 @@ const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH"))
constexpr char PRESERVE_ATTR_NAME[] = "user.preserve";
constexpr char PRESERVE_ATTR_VALUE = '1';
// 2.5x the stock 526x330 qcamera, rounded up to even. The msm_vidc encoder rejects
// VIDIOC_S_FMT with ENOTSUPP (524) on an odd width or height, which throws out of
// encoder_thread and SIGABRTs all of encoderd -- taking fcamera/dcamera/ecamera with it.
constexpr int QCAM_WIDTH = 1316;
constexpr int QCAM_HEIGHT = 826;
static_assert(QCAM_WIDTH % 2 == 0 && QCAM_HEIGHT % 2 == 0, "qcamera dimensions must be even");
struct EncoderSettings {
cereal::EncodeIndex::Type encode_type;
int bitrate;
@@ -140,8 +147,8 @@ const EncoderInfo qcam_encoder_info = {
.filename = "qcamera.ts",
.cbr = true, // enforce the bitrate so upload size stays predictable (no VBR overshoot)
.get_settings = [](int){return EncoderSettings::QcamEncoderSettings();},
.frame_width = 1315, // 2.5x the stock 526x330, same road-cam aspect ratio
.frame_height = 825,
.frame_width = QCAM_WIDTH,
.frame_height = QCAM_HEIGHT,
.include_audio = Params().getBool("RecordAudio"),
INIT_ENCODE_FUNCTIONS(QRoadEncode),
};
tools/sim/lib/simulated_sensors.py
+5 -6
View File
@@ -92,12 +92,11 @@ class SimulatedSensors:
# dmonitoringd output
dat = messaging.new_message('driverMonitoringState', valid=True)
dm = dat.driverMonitoringState
dm.alertLevel = log.DriverMonitoringState.AlertLevel.none
dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision
dm.visionPolicyState.faceDetected = True
dm.visionPolicyState.isDistracted = False
dm.visionPolicyState.awarenessPercent = 100
dat.driverMonitoringState = {
"faceDetected": True,
"isDistracted": False,
"awarenessStatus": 1.,
}
self.pm.send('driverMonitoringState', dat)
def send_camera_images(self, world: 'World'):