IQ.Lvbs/IQ.Pilot
8
7
Fork 3
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ f82ff4d

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-21 13:43:46 -05:00
parent 7b20edda67
commit b712a31728
717 changed files with 4347 additions and 3060 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
selfdrive/assets/sounds/pre_alert.wav Normal file
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/prompt_distracted.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/prompt_single_high.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/prompt_single_low.wav
View File

Binary file not shown.

BIN
selfdrive/assets/sounds/warning_immediate.wav
View File

Binary file not shown.

8
selfdrive/car/car_specific.py
View File

@@ -1,8 +1,8 @@
from cereal import car, log
from opendbc.car import DT_CTRL, structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.interfaces import MAX_CTRL_SPEED
from opendbc.car.toyota.values import ToyotaFlags
from iqdbc.car import DT_CTRL, structs
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.interfaces import MAX_CTRL_SPEED
from iqdbc.car.toyota.values import ToyotaFlags
from openpilot.selfdrive.selfdrived.events import Events

12
selfdrive/car/card.py
View File

@@ -15,12 +15,12 @@ from openpilot.common.params import Params, UnknownKeyName
from openpilot.common.realtime import config_realtime_process, lock_memory, Priority, Ratekeeper
from openpilot.common.swaglog import cloudlog, ForwardingHandler
from opendbc.car import DT_CTRL, structs
from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
from opendbc.car.carlog import carlog
from opendbc.car.fw_versions import ObdCallback
from opendbc.car.car_helpers import get_car, interfaces
from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
from iqdbc.car import DT_CTRL, structs
from iqdbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
from iqdbc.car.carlog import carlog
from iqdbc.car.fw_versions import ObdCallback
from iqdbc.car.car_helpers import get_car, interfaces
from iqdbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
from openpilot.selfdrive.car.cruise import VCruiseHelper
from openpilot.selfdrive.car.helpers import convert_iq_car_control_compact, convert_to_capnp

46
selfdrive/car/cruise.py
View File

@@ -4,7 +4,7 @@ import numpy as np
from cereal import car
from openpilot.common.constants import CV
from cereal import car, custom
from opendbc.car import structs
from iqdbc.car import structs
from openpilot.common.params import Params
from openpilot.iqpilot.selfdrive.car.long_increments import LongIncrementConfig, read_long_increment_config, resolve_button_step
@@ -44,15 +44,16 @@ def get_minimum_set_speed_kph(_is_metric: bool) -> float:
def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None:
# increment timer for buttons still pressed
for k in button_timers:
if button_timers[k] > 0:
button_timers[k] += 1
# age any button that's currently held (nonzero timer)
for btn, held_frames in button_timers.items():
if held_frames > 0:
button_timers[btn] = held_frames + 1
for b in CS.buttonEvents:
if b.type.raw in button_timers:
# Start/end timer and store current state on change of button pressed
button_timers[b.type.raw] = 1 if b.pressed else 0
# a press/release edge (re)starts the timer at 1 or clears it to 0
for event in CS.buttonEvents:
raw = event.type.raw
if raw in button_timers:
button_timers[raw] = 1 if event.pressed else 0
class VCruiseHelperIQ:
@@ -136,20 +137,21 @@ class VCruiseHelperIQ:
self.v_cruise_min = get_minimum_set_speed_kph(is_metric)
def update_enabled_state(self, CS: car.CarState, enabled: bool) -> bool:
# special enabled state for non pcmCruiseSpeed, unchanged for non pcmCruise
if not self.CP_IQ.pcmCruiseSpeed:
update_manual_button_timers(CS, self.enable_button_timers)
button_pressed = any(self.enable_button_timers[k] > 0 for k in self.enable_button_timers)
# pcmCruiseSpeed cars keep the stock enabled flag; others gate engagement on button release
if self.CP_IQ.pcmCruiseSpeed:
return enabled
if enabled and not self.enabled_prev:
self.enabled_prev = not button_pressed
enabled = False
elif not enabled:
self.enabled_prev = enabled
update_manual_button_timers(CS, self.enable_button_timers)
button_pressed = any(t > 0 for t in self.enable_button_timers.values())
return enabled and self.enabled_prev
if enabled and not self.enabled_prev:
# first engage frame while the button is still down: hold off until it's let go
self.enabled_prev = not button_pressed
return False
if not enabled:
self.enabled_prev = False
return enabled
return enabled and self.enabled_prev
def update_speed_limit_assist(self, is_metric, LP_IQ: custom.IQPlan) -> None:
resolver = LP_IQ.speedLimit.resolver
@@ -162,7 +164,9 @@ class VCruiseHelperIQ:
@property
def update_speed_limit_final_last_changed(self) -> bool:
return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph)
if not self.has_speed_limit:
return False
return self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph
def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
if self.set_speed_to_limit and \

2
selfdrive/car/docs.py
View File

@@ -3,7 +3,7 @@ import argparse
import os
from openpilot.common.basedir import BASEDIR
from opendbc.car.docs import get_all_car_docs, generate_cars_md
from iqdbc.car.docs import get_all_car_docs, generate_cars_md
CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md")
CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md")

2
selfdrive/car/helpers.py
View File

@@ -2,7 +2,7 @@ import capnp
from typing import Any
from cereal import custom
from opendbc.car import structs
from iqdbc.car import structs
_FIELDS = '__dataclass_fields__' # copy of dataclasses._FIELDS

10
selfdrive/car/tests/test_car_interfaces.py
View File

@@ -5,11 +5,11 @@ from parameterized import parameterized
from types import SimpleNamespace
from cereal import car, custom
from opendbc.car import DT_CTRL
from opendbc.car.structs import CarParams
from opendbc.car.tests.test_car_interfaces import get_fuzzy_car_interface
from opendbc.car.mock.values import CAR as MOCK
from opendbc.car.values import PLATFORMS
from iqdbc.car import DT_CTRL
from iqdbc.car.structs import CarParams
from iqdbc.car.tests.test_car_interfaces import get_fuzzy_car_interface
from iqdbc.car.mock.values import CAR as MOCK
from iqdbc.car.values import PLATFORMS
from openpilot.selfdrive.car.card import run_optional_pre_init
from openpilot.selfdrive.car.helpers import convert_iq_car_control, convert_iq_car_control_compact
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle

2
selfdrive/car/tests/test_car_specific_events.py
View File

@@ -1,7 +1,7 @@
from types import SimpleNamespace
from cereal import log
from opendbc.car import structs
from iqdbc.car import structs
from openpilot.selfdrive.car.car_specific import CarSpecificEvents

2
selfdrive/car/tests/test_cruise_speed.py
View File

@@ -41,7 +41,7 @@ class TestCruiseSpeed:
cruise_speed = float(self.speed)
simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality)
assert simulation_steady_state == pytest.approx(cruise_speed, abs=1.5), f'Did not reach {self.speed} m/s'
assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s'
# TODO: test pcmCruise and pcmCruiseSpeed

2
selfdrive/car/tests/test_docs.py
View File

@@ -1,7 +1,7 @@
import os
from openpilot.common.basedir import BASEDIR
from opendbc.car.docs import generate_cars_md, get_all_car_docs
from iqdbc.car.docs import generate_cars_md, get_all_car_docs
from openpilot.selfdrive.debug.dump_car_docs import dump_car_docs
from openpilot.selfdrive.debug.print_docs_diff import print_car_docs_diff
from openpilot.selfdrive.car.docs import CARS_MD_TEMPLATE

18
selfdrive/car/tests/test_models.py
View File

@@ -9,15 +9,15 @@ import hypothesis.strategies as st
from hypothesis import Phase, given, settings
from parameterized import parameterized_class
from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
from opendbc.car.can_definitions import CanData
from opendbc.car.car_helpers import FRAME_FINGERPRINT, interfaces
from opendbc.car.fingerprints import MIGRATION
from opendbc.car.honda.values import CAR as HONDA, HondaFlags
from opendbc.car.structs import car
from opendbc.car.tests.routes import non_tested_cars, routes, CarTestRoute
from opendbc.car.values import Platform, PLATFORMS
from opendbc.safety.tests.libsafety import libsafety_py
from iqdbc.car import DT_CTRL, gen_empty_fingerprint, structs
from iqdbc.car.can_definitions import CanData
from iqdbc.car.car_helpers import FRAME_FINGERPRINT, interfaces
from iqdbc.car.fingerprints import MIGRATION
from iqdbc.car.honda.values import CAR as HONDA, HondaFlags
from iqdbc.car.structs import car
from iqdbc.car.tests.routes import non_tested_cars, routes, CarTestRoute
from iqdbc.car.values import Platform, PLATFORMS
from iqdbc.safety.tests.libsafety import libsafety_py
from openpilot.common.basedir import BASEDIR
from openpilot.selfdrive.pandad import can_capnp_to_list
from openpilot.selfdrive.test.helpers import read_segment_list

10
selfdrive/controls/controlsd.py
View File

@@ -11,8 +11,8 @@ from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, lock_memory, DT_CTRL, Priority, Ratekeeper
from openpilot.common.swaglog import cloudlog
from opendbc.car.car_helpers import interfaces
from opendbc.car.vehicle_model import VehicleModel
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -99,7 +99,7 @@ class Controls(IQControlsLayer):
try:
if self.CP.brand != 'volkswagen':
return False
from opendbc.car.volkswagen.values import VolkswagenFlags
from iqdbc.car.volkswagen.values import VolkswagenFlags
return bool(self.CP.flags & VolkswagenFlags.PQ)
except Exception:
cloudlog.exception("pq torque selection failed; using generic torque")
@@ -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['selfdriveState'].alertType.split('/', 1)[0] == 'driverUnresponsive'
hudControl.driverUnresponsive = self.sm['driverMonitoringState'].noResponseForceDecel
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'].awarenessStatus < 0.) or
cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or
(self.sm['selfdriveState'].state == State.softDisabling))
lat_tuning = self.CP.lateralTuning.which()

10
selfdrive/controls/lib/drive_helpers.py
View File

@@ -12,6 +12,11 @@ MAX_VEL_ERR = 5.0 # m/s
MAX_LATERAL_JERK = 5.0 # m/s^3
MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2
MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE = 5.0 # m/s^2
DEFAULT_STOPPING_SPEED = 0.25 # m/s
def should_stop(v_ego: float, a_target: float, stopping_speed: float = DEFAULT_STOPPING_SPEED) -> bool:
return bool(v_ego < stopping_speed and a_target < 0.1)
def clamp(val, min_val, max_val):
@@ -52,7 +57,7 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll, override=False) -
return float(new_curvature), limited_accel or limited_max_curv
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.3):
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, stopping_speed=DEFAULT_STOPPING_SPEED):
if len(speeds) == len(t_idxs):
v_now = speeds[0]
a_now = accels[0]
@@ -62,8 +67,7 @@ def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.
v_now = 0.0
v_target = 0.0
a_target = 0.0
should_stop = (v_now < vEgoStopping and a_target < 0.1)
return a_target, should_stop
return a_target, should_stop(v_now, a_target, stopping_speed)
def curv_from_psis(psi_target, psi_rate, vego, action_t):
vego = np.clip(vego, MIN_SPEED, np.inf)

10
selfdrive/controls/lib/latcontrol_torque.py
View File

@@ -8,10 +8,10 @@ from difflib import SequenceMatcher
import numpy as np
from cereal import log, custom # noqa: F401 (custom kept available for downstream imports)
from opendbc.car import structs
from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
from opendbc.iqpilot.car.interfaces import LatControlInputs
from opendbc.iqpilot.car.lateral_ext import get_friction as get_friction_in_torque_space
from iqdbc.car import structs
from iqdbc.car.lateral import FRICTION_THRESHOLD, get_friction
from iqdbc.iqpilot.car.interfaces import LatControlInputs
from iqdbc.iqpilot.car.lateral_ext import get_friction as get_friction_in_torque_space
from openpilot.common.basedir import BASEDIR
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
@@ -27,7 +27,7 @@ from openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import Na
# ===== locator =====
TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "iqpilot", "iqpilot_iq_nnff_models", "neural_network_lateral_control")
TORQUE_NN_MODEL_SUBSTITUTE_PATH = os.path.join(BASEDIR, "opendbc", "car", "torque_data", "substitute.toml")
TORQUE_NN_MODEL_SUBSTITUTE_PATH = os.path.join(BASEDIR, "iqdbc", "car", "torque_data", "substitute.toml")
MOCK_MODEL_PATH = os.path.join(TORQUE_NN_MODEL_PATH, "MOCK.json")
# A candidate must reach this score for the fingerprint(+fw) match to count as exact.

2
selfdrive/controls/lib/latcontrol_torque_pq.py
View File

@@ -3,7 +3,7 @@ import numpy as np
from collections import deque
from cereal import log
from opendbc.car.lateral import get_friction
from iqdbc.car.lateral import get_friction
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.latcontrol import LatControl

31
selfdrive/controls/lib/longcontrol.py
View File

@@ -11,16 +11,13 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
LongCtrlState = car.CarControl.Actuators.LongControlState
def long_control_state_trans(CP, CP_IQ, active, long_control_state, v_ego,
should_stop, brake_pressed, cruise_standstill):
def long_control_state_trans(CP_IQ, active, long_control_state, should_stop, brake_pressed, cruise_standstill):
# Gas Interceptor
cruise_standstill = cruise_standstill and not CP_IQ.enableGasInterceptor
stopping_condition = should_stop
starting_condition = (not should_stop and
not cruise_standstill and
not brake_pressed)
started_condition = v_ego > CP.vEgoStarting
if not active:
long_control_state = LongCtrlState.off
@@ -30,22 +27,15 @@ def long_control_state_trans(CP, CP_IQ, active, long_control_state, v_ego,
if not starting_condition:
long_control_state = LongCtrlState.stopping
else:
if starting_condition and CP.startingState:
long_control_state = LongCtrlState.starting
else:
long_control_state = LongCtrlState.pid
long_control_state = LongCtrlState.pid
elif long_control_state == LongCtrlState.stopping:
if starting_condition and CP.startingState:
long_control_state = LongCtrlState.starting
elif starting_condition:
if starting_condition:
long_control_state = LongCtrlState.pid
elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]:
if stopping_condition:
elif long_control_state == LongCtrlState.pid:
if should_stop:
long_control_state = LongCtrlState.stopping
elif started_condition:
long_control_state = LongCtrlState.pid
return long_control_state
class LongControl:
@@ -73,8 +63,7 @@ class LongControl:
else:
stop_now = should_stop
self.long_control_state = long_control_state_trans(self.CP, self.CP_IQ, active, self.long_control_state, CS.vEgo,
stop_now, CS.brakePressed,
self.long_control_state = long_control_state_trans(self.CP_IQ, active, self.long_control_state, stop_now, CS.brakePressed,
CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off:
self.reset()
@@ -85,12 +74,8 @@ class LongControl:
output_accel = self.last_output_accel
if output_accel > self.CP.stopAccel:
output_accel = min(output_accel, 0.0)
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
self.reset()
self.smooth.reset()
elif self.long_control_state == LongCtrlState.starting:
output_accel = self.CP.startAccel
# TODO: can we just go straight to stopAccel?
output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop
self.reset()
self.smooth.reset()

55
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -3,7 +3,7 @@ import os
import time
import numpy as np
from cereal import log
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.realtime import DT_MDL
from openpilot.common.swaglog import cloudlog
# WARNING: imports outside of constants will not trigger a rebuild
@@ -26,15 +26,12 @@ EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1, LongitudinalPlanSource.cruise)
CRUISE_MIN_ACCEL = -1.2
CRUISE_MAX_ACCEL = 1.6
MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
X_DIM = 3
U_DIM = 1
PARAM_DIM = 6
COST_E_DIM = 5
PARAM_DIM = 5
COST_E_DIM = 4
COST_DIM = COST_E_DIM + 1
CONSTR_DIM = 4
@@ -42,8 +39,7 @@ X_EGO_OBSTACLE_COST = 3.
X_EGO_COST = 0.
V_EGO_COST = 0.
A_EGO_COST = 0.
J_EGO_COST = 5.
A_CHANGE_COST = 200.
J_EGO_COST = 20.
DANGER_ZONE_COST = 100.
CRASH_DISTANCE = .25
LEAD_DANGER_FACTOR = 0.75
@@ -114,10 +110,9 @@ def gen_long_model():
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
x_obstacle = SX.sym('x_obstacle')
prev_a = SX.sym('prev_a')
lead_t_follow = SX.sym('lead_t_follow')
lead_danger_factor = SX.sym('lead_danger_factor')
model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor)
model.p = vertcat(a_min, a_max, x_obstacle, lead_t_follow, lead_danger_factor)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
@@ -149,9 +144,8 @@ def gen_long_ocp():
a_min, a_max = ocp.model.p[0], ocp.model.p[1]
x_obstacle = ocp.model.p[2]
prev_a = ocp.model.p[3]
lead_t_follow = ocp.model.p[4]
lead_danger_factor = ocp.model.p[5]
lead_t_follow = ocp.model.p[3]
lead_danger_factor = ocp.model.p[4]
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
@@ -166,7 +160,6 @@ def gen_long_ocp():
x_ego,
v_ego,
a_ego,
a_ego - prev_a,
j_ego]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@@ -182,7 +175,7 @@ def gen_long_ocp():
x0 = np.zeros(X_DIM)
ocp.constraints.x0 = x0
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
# We put all constraint cost weights to 0 and only set them at runtime
@@ -242,7 +235,6 @@ class LongitudinalMpc:
self.v_solution = np.zeros(N+1)
self.a_solution = np.zeros(N+1)
self.j_solution = np.zeros(N)
self.prev_a = np.array(self.a_solution)
self.yref = np.zeros((N+1, COST_DIM))
for i in range(N):
@@ -270,9 +262,6 @@ class LongitudinalMpc:
def set_cost_weights(self, cost_weights, constraint_cost_weights):
W = np.asfortranarray(np.diag(cost_weights))
for i in range(N):
# TODO don't hardcode A_CHANGE_COST idx
# reduce the cost on (a-a_prev) later in the horizon.
W[4,4] = cost_weights[4] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
self.solver.cost_set(i, 'W', W)
# Setting the slice without the copy make the array not contiguous,
# causing issues with the C interface.
@@ -283,10 +272,9 @@ class LongitudinalMpc:
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
def set_weights(self, personality=log.LongitudinalPersonality.standard):
jerk_factor = get_jerk_factor(personality)
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * J_EGO_COST]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
self.set_cost_weights(cost_weights, constraint_cost_weights)
@@ -361,8 +349,7 @@ class LongitudinalMpc:
x_lead_mpc = np.maximum(x_lead_mpc, radar_distance_floor)
return np.column_stack((x_lead_mpc, v_lead_mpc))
def update(self, modelV2, radarstate, v_cruise, cruise_accel_limits=(CRUISE_MIN_ACCEL, CRUISE_MAX_ACCEL),
personality=log.LongitudinalPersonality.standard):
def update(self, modelV2, radarstate, personality=log.LongitudinalPersonality.standard):
self.new_lead_mpc = self._read_new_lead_mpc()
t_follow = get_T_FOLLOW(personality)
model_leads = modelV2.leadsV3
@@ -386,16 +373,7 @@ class LongitudinalMpc:
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
# v_cruise: scalar or per-timestep (N+1) envelope
v_ego = self.x0[1]
min_a, max_a = cruise_accel_limits
v_lower = np.maximum(v_ego + (T_IDXS * min(min_a, -0.1) * 1.05), 0.0)
# max_a can be negative (coast/turn clip); the reachable band must stay ordered
v_upper = np.maximum(v_ego + (T_IDXS * max_a * 1.05), v_lower)
v_cruise_clipped = np.clip(np.broadcast_to(v_cruise, (N+1,)), v_lower, v_upper)
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
self.yref[:,:] = 0.0
@@ -406,9 +384,8 @@ class LongitudinalMpc:
self.params[:,0] = ACCEL_MIN
self.params[:,1] = ACCEL_MAX
self.params[:,2] = np.min(x_obstacles, axis=1)
self.params[:,3] = np.copy(self.prev_a)
self.params[:,4] = t_follow
self.params[:,5] = LEAD_DANGER_FACTOR
self.params[:,3] = t_follow
self.params[:,4] = LEAD_DANGER_FACTOR
self.run()
lead_crash_prob = model_leads[0].prob if self.new_lead_mpc else radarstate.leadOne.modelProb
@@ -439,8 +416,6 @@ class LongitudinalMpc:
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]
self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution)
t = time.monotonic()
if self.solution_status != 0:
if t > self.last_cloudlog_t + 5.0:

110
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -3,17 +3,15 @@ import math
import numpy as np
import cereal.messaging as messaging
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.constants import CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from cereal import log
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import CRUISE_MIN_ACCEL, CRUISE_MAX_ACCEL
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, DEFAULT_STOPPING_SPEED, get_accel_from_plan
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
from openpilot.common.swaglog import cloudlog
from openpilot.common.issue_debug import log_issue_limited
@@ -22,15 +20,11 @@ from openpilot.iqpilot.selfdrive.controls.lib.longitudinal_planner import Longit
A_CRUISE_MAX_VALS = [2.0, 1.6, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
A_CRUISE_MIN = -1.2
J_CRUISE = 1.0
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
ALLOW_THROTTLE_THRESHOLD = 0.4
MIN_ALLOW_THROTTLE_SPEED = 2.5
# scales the in-MPC cruise envelope's decel bound per drive personality
PERSONALITY_CRUISE_DECEL_SCALE = {
log.LongitudinalPersonality.relaxed: 0.85,
log.LongitudinalPersonality.standard: 1.0,
log.LongitudinalPersonality.aggressive: 1.15,
}
LAUNCH_DISARM_SPEED = 2.0
LAUNCH_COMMIT_T = 3.5
@@ -54,23 +48,31 @@ def get_lead_distance(radarState):
return radarState.leadTwo.dRel
return 0
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
"""
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
this should avoid accelerating when losing the target in turns
"""
# FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
# The lookup table for turns should also be updated if we do this
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
return [a_target[0], min(a_target[1], a_x_allowed)]
if not e2e:
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
max_accel = min(max_accel, a_x_allowed)
if not allow_throttle:
clipped_accel_coast = max(accel_coast, ACCEL_MIN)
coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast])
max_accel = min(max_accel, coast_limit)
target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel)
if not e2e:
target_accel = float(np.clip(target_accel, a_cruise_prev - J_CRUISE * dt, a_cruise_prev + J_CRUISE * dt))
cruise_should_stop = v_cruise == 0.0
return target_accel, cruise_should_stop
class LongitudinalPlanner(LongitudinalPlannerIQ):
def __init__(self, CP, CP_IQ, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.CP = CP
self.stopping_speed = CP_IQ.longitudinalStoppingSpeedOverride or DEFAULT_STOPPING_SPEED
self.mpc = LongitudinalMpc(dt=dt)
LongitudinalPlannerIQ.__init__(self, self.CP, CP_IQ, self.mpc)
self.fcw = False
@@ -79,7 +81,7 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX]
self.a_cruise = 0.0
self.output_a_target = 0.0
self.output_should_stop = False
self.launch_armed = False
@@ -119,27 +121,21 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
v_ego = sm['carState'].vEgo
v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
v_cruise = v_cruise_kph * CV.KPH_TO_MS
v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
if sm['controlsState'].forceDecel:
v_cruise = 0.0
long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
force_slow_decel = sm['controlsState'].forceDecel
# Reset current state when not engaged, or user is controlling the speed
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
# PCM cruise speed may be updated a few cycles later, check if initialized
v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
reset_state = reset_state or not v_cruise_initialized
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
if reset_state:
self.v_desired_filter.x = v_ego
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1])
self.a_desired = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX)
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -147,25 +143,16 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
# Don't clip at low speeds since throttle_prob doesn't account for creep
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
if not self.allow_throttle:
clipped_accel_coast = max(accel_coast, accel_clip[0])
clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
# Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
v_cruise, self.a_desired = LongitudinalPlannerIQ.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
if force_slow_decel:
if sm['controlsState'].forceDecel:
v_cruise = 0.0
personality = sm['selfdriveState'].personality
v_cruise_envelope = LongitudinalPlannerIQ.cruise_envelope(self, v_cruise, v_ego, T_IDXS_MPC)
decel_scale = PERSONALITY_CRUISE_DECEL_SCALE.get(personality, 1.0)
cruise_accel_limits = (CRUISE_MIN_ACCEL * decel_scale, min(CRUISE_MAX_ACCEL, accel_clip[1]))
self.mpc.set_weights(prev_accel_constraint, personality=personality)
self.mpc.set_weights(personality=personality)
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
self.mpc.update(sm['modelV2'], sm['radarState'], v_cruise_envelope, cruise_accel_limits, personality=personality)
self.mpc.update(sm['modelV2'], sm['radarState'], personality=personality)
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -176,14 +163,12 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
if self.fcw:
cloudlog.info("FCW triggered")
# Interpolate 0.05 seconds and save as starting point for next iteration
# Save starting point for next iteration
a_prev = self.a_desired
self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
action_t=action_t, stopping_speed=self.stopping_speed)
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop
@@ -199,25 +184,28 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
t_shifted = T_IDXS_MPC + t_cut
v_shifted = np.interp(t_shifted, T_IDXS_MPC, model_v)
a_shifted = np.interp(t_shifted, T_IDXS_MPC, model_a)
a_launch = get_accel_from_plan(v_shifted, a_shifted, T_IDXS_MPC, action_t=action_t, vEgoStopping=self.CP.vEgoStopping)[0]
a_launch = get_accel_from_plan(v_shifted, a_shifted, T_IDXS_MPC, action_t=action_t)[0]
a_launch_max = np.interp(v_ego, [LAUNCH_MOVING_SPEED, LAUNCH_DISARM_SPEED], [LAUNCH_MAX_ACCEL, 0.])
output_a_target_e2e = max(output_a_target_e2e, min(a_launch, a_launch_max))
if self.is_e2e(sm):
output_a_target = min(output_a_target_e2e, output_a_target_mpc)
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
if output_a_target < output_a_target_mpc:
self.mpc.source = LongitudinalPlanSource.e2e
else:
output_a_target = output_a_target_mpc
self.output_should_stop = output_should_stop_mpc
e2e = self.is_e2e(sm)
self.a_cruise, cruise_should_stop = get_cruise_accel(e2e, v_cruise, v_ego, self.a_cruise,
steer_angle_without_offset, self.CP, self.dt,
accel_coast, self.allow_throttle)
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
(self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
if e2e:
candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
self.output_should_stop = self.output_should_stop or self.forcing_stop
self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
for idx in range(2):
accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
self.prev_accel_clip = accel_clip
self.a_desired = float(self.output_a_target)
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')

6
selfdrive/controls/radard.py
View File

@@ -12,9 +12,9 @@ from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
from openpilot.common.swaglog import cloudlog
from openpilot.common.simple_kalman import KF1D
from opendbc.car import structs
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.iqpilot.car.hyundai.values import HyundaiFlagsIQ
from iqdbc.car import structs
from iqdbc.car.hyundai.values import HyundaiFlags
from iqdbc.iqpilot.car.hyundai.values import HyundaiFlagsIQ
from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance

20
selfdrive/controls/tests/test_drive_helpers.py Normal file
View File

@@ -0,0 +1,20 @@
import pytest
from openpilot.selfdrive.controls.lib.drive_helpers import DEFAULT_STOPPING_SPEED, should_stop
class TestShouldStop:
@pytest.mark.parametrize("v_ego, expected", [
(DEFAULT_STOPPING_SPEED - 0.01, True),
(DEFAULT_STOPPING_SPEED, False),
])
def test_upstream_default(self, v_ego, expected):
assert should_stop(v_ego, -0.1) == expected
@pytest.mark.parametrize("stopping_speed", [0.55 / 3.6, 1.5 / 3.6])
def test_car_override(self, stopping_speed):
assert should_stop(stopping_speed - 0.01, -0.1, stopping_speed)
assert not should_stop(stopping_speed, -0.1, stopping_speed)
def test_requires_deceleration(self):
assert not should_stop(0.0, 0.1, 1.0)

12
selfdrive/controls/tests/test_latcontrol.py
View File

@@ -1,12 +1,12 @@
from parameterized import parameterized
from cereal import car, log
from opendbc.car.car_helpers import interfaces
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.nissan.values import CAR as NISSAN
from opendbc.car.gm.values import CAR as GM
from opendbc.car.vehicle_model import VehicleModel
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.honda.values import CAR as HONDA
from iqdbc.car.toyota.values import CAR as TOYOTA
from iqdbc.car.nissan.values import CAR as NISSAN
from iqdbc.car.gm.values import CAR as GM
from iqdbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car.helpers import convert_to_capnp
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID

6
selfdrive/controls/tests/test_latcontrol_torque_buffer.py
View File

@@ -1,9 +1,9 @@
from parameterized import parameterized
from cereal import car, log
from opendbc.car.car_helpers import interfaces
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.vehicle_model import VehicleModel
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.toyota.values import CAR as TOYOTA
from iqdbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque, LAT_ACCEL_REQUEST_BUFFER_SECONDS

2
selfdrive/controls/tests/test_leads.py
View File

@@ -1,6 +1,6 @@
import cereal.messaging as messaging
from opendbc.car.toyota.values import CAR as TOYOTA
from iqdbc.car.toyota.values import CAR as TOYOTA
from openpilot.selfdrive.test.process_replay import replay_process_with_name

36
selfdrive/controls/tests/test_longcontrol.py
View File

@@ -1,59 +1,43 @@
from cereal import car, custom
from cereal import custom
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
class TestLongControlStateTransition:
def test_stay_stopped(self):
CP = car.CarParams.new_message()
CP_IQ = custom.IQCarParams.new_message()
active = True
current_state = LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=True, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=True, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=True)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=1.0,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid
active = False
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=1.0,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.off
def test_engage():
CP = car.CarParams.new_message()
CP_IQ = custom.IQCarParams.new_message()
active = True
current_state = LongCtrlState.off
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=True, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=True, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=True)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid
def test_starting():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP_IQ = custom.IQCarParams.new_message()
active = True
current_state = LongCtrlState.starting
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.starting
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=1.0,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid

6
selfdrive/controls/tests/test_torqued_lat_accel_offset.py
View File

@@ -1,8 +1,8 @@
import numpy as np
from cereal import car, messaging
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
from opendbc.car import structs
from opendbc.car.lateral import get_friction, FRICTION_THRESHOLD
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY
from iqdbc.car import structs
from iqdbc.car.lateral import get_friction, FRICTION_THRESHOLD
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS

6
selfdrive/debug/car/clear_dtc.py
View File

@@ -2,9 +2,9 @@
import sys
import argparse
from subprocess import check_output, CalledProcessError
from opendbc.car.carlog import carlog
from opendbc.car.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE
from opendbc.car.structs import CarParams
from iqdbc.car.carlog import carlog
from iqdbc.car.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE
from iqdbc.car.structs import CarParams
from panda import Panda
parser = argparse.ArgumentParser(description="clear DTC status")

2
selfdrive/debug/car/disable_ecu.py
View File

@@ -1,7 +1,7 @@
#!/usr/bin/env python3
import time
import cereal.messaging as messaging
from opendbc.car.disable_ecu import disable_ecu
from iqdbc.car.disable_ecu import disable_ecu
from openpilot.selfdrive.car.card import can_comm_callbacks
if __name__ == "__main__":

4
selfdrive/debug/car/ecu_addrs.py
View File

@@ -2,8 +2,8 @@
import argparse
import time
import cereal.messaging as messaging
from opendbc.car.carlog import carlog
from opendbc.car.ecu_addrs import get_all_ecu_addrs
from iqdbc.car.carlog import carlog
from iqdbc.car.ecu_addrs import get_all_ecu_addrs
from openpilot.common.params import Params
from openpilot.selfdrive.car.card import can_comm_callbacks, obd_callback

6
selfdrive/debug/car/fw_versions.py
View File

@@ -3,9 +3,9 @@ import time
import argparse
import cereal.messaging as messaging
from cereal import car
from opendbc.car.carlog import carlog
from opendbc.car.fw_versions import get_fw_versions, match_fw_to_car
from opendbc.car.vin import get_vin
from iqdbc.car.carlog import carlog
from iqdbc.car.fw_versions import get_fw_versions, match_fw_to_car
from iqdbc.car.vin import get_vin
from openpilot.common.params import Params
from openpilot.selfdrive.car.card import can_comm_callbacks, obd_callback
from typing import Any

6
selfdrive/debug/car/hyundai_enable_radar_points.py
View File

@@ -16,9 +16,9 @@ import argparse
from typing import NamedTuple
from subprocess import check_output, CalledProcessError
from opendbc.car.carlog import carlog
from opendbc.car.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
from opendbc.car.structs import CarParams
from iqdbc.car.carlog import carlog
from iqdbc.car.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
from iqdbc.car.structs import CarParams
from panda.python import Panda
class ConfigValues(NamedTuple):

2
selfdrive/debug/car/toyota_eps_factor.py
View File

@@ -3,7 +3,7 @@ import sys
import numpy as np
import matplotlib.pyplot as plt
from sklearn import linear_model
from opendbc.car.toyota.values import STEER_THRESHOLD
from iqdbc.car.toyota.values import STEER_THRESHOLD
from openpilot.tools.lib.logreader import LogReader

4
selfdrive/debug/car/vin.py
View File

@@ -2,8 +2,8 @@
import argparse
import time
import cereal.messaging as messaging
from opendbc.car.carlog import carlog
from opendbc.car.vin import get_vin
from iqdbc.car.carlog import carlog
from iqdbc.car.vin import get_vin
from openpilot.selfdrive.car.card import can_comm_callbacks
if __name__ == "__main__":

6
selfdrive/debug/car/vw_mqb_config.py
View File

@@ -3,10 +3,10 @@
import argparse
import struct
from enum import IntEnum
from opendbc.car.carlog import carlog
from opendbc.car.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\
from iqdbc.car.carlog import carlog
from iqdbc.car.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\
DATA_IDENTIFIER_TYPE, ACCESS_TYPE
from opendbc.car.structs import CarParams
from iqdbc.car.structs import CarParams
from panda import Panda
from datetime import date

2
selfdrive/debug/check_can_parser_performance.py
View File

@@ -4,7 +4,7 @@ import time
from tqdm import tqdm
from cereal import car
from opendbc.car.tests.routes import CarTestRoute
from iqdbc.car.tests.routes import CarTestRoute
from openpilot.selfdrive.car.tests.test_models import TestCarModelBase
from openpilot.tools.plotjuggler.juggle import DEMO_ROUTE

8
selfdrive/debug/cycle_alerts.py
View File

@@ -4,7 +4,7 @@ import random
from cereal import car, log
import cereal.messaging as messaging
from opendbc.car.honda.interface import CarInterface
from iqdbc.car.honda.interface import CarInterface
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.selfdrived.events import ET, Events
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
@@ -30,9 +30,9 @@ def cycle_alerts(duration=200, is_metric=False):
(EventName.accFaulted, ET.IMMEDIATE_DISABLE),
# DM sequence
(EventName.preDriverDistracted, ET.WARNING),
(EventName.promptDriverDistracted, ET.WARNING),
(EventName.driverDistracted, ET.WARNING),
(EventName.driverDistracted1, ET.WARNING),
(EventName.driverDistracted2, ET.WARNING),
(EventName.driverDistracted3, ET.WARNING),
]
# debug alerts

2
selfdrive/debug/debug_fw_fingerprinting_offline.py
View File

@@ -1,7 +1,7 @@
#!/usr/bin/env python3
import argparse
from opendbc.car import uds
from iqdbc.car import uds
from openpilot.tools.lib.live_logreader import live_logreader
from openpilot.tools.lib.logreader import LogReader, ReadMode

2
selfdrive/debug/dump_car_docs.py
View File

@@ -2,7 +2,7 @@
import argparse
import pickle
from opendbc.car.docs import get_all_car_docs
from iqdbc.car.docs import get_all_car_docs
def dump_car_docs(path):

10
selfdrive/debug/fuzz_fw_fingerprint.py
View File

@@ -4,11 +4,11 @@ from collections import defaultdict
from tqdm import tqdm
from opendbc.car.fw_versions import match_fw_to_car_fuzzy
from opendbc.car.toyota.values import FW_VERSIONS as TOYOTA_FW_VERSIONS
from opendbc.car.honda.values import FW_VERSIONS as HONDA_FW_VERSIONS
from opendbc.car.hyundai.values import FW_VERSIONS as HYUNDAI_FW_VERSIONS
from opendbc.car.volkswagen.values import FW_VERSIONS as VW_FW_VERSIONS
from iqdbc.car.fw_versions import match_fw_to_car_fuzzy
from iqdbc.car.toyota.values import FW_VERSIONS as TOYOTA_FW_VERSIONS
from iqdbc.car.honda.values import FW_VERSIONS as HONDA_FW_VERSIONS
from iqdbc.car.hyundai.values import FW_VERSIONS as HYUNDAI_FW_VERSIONS
from iqdbc.car.volkswagen.values import FW_VERSIONS as VW_FW_VERSIONS
FWS = {}

4
selfdrive/debug/print_docs_diff.py
View File

@@ -4,8 +4,8 @@ from collections import defaultdict
import difflib
import pickle
from opendbc.car.docs import get_all_car_docs
from opendbc.car.docs_definitions import Column
from iqdbc.car.docs import get_all_car_docs
from iqdbc.car.docs_definitions import Column
FOOTNOTE_TAG = "<sup>{}</sup>"
STAR_ICON = '<a href="##"><img valign="top" ' + \

2
selfdrive/debug/print_flags.py
View File

@@ -1,5 +1,5 @@
#!/usr/bin/env python3
from opendbc.car.values import BRANDS
from iqdbc.car.values import BRANDS
for brand in BRANDS:
all_flags = set()

6
selfdrive/debug/read_dtc_status.py
View File

@@ -2,9 +2,9 @@
import sys
import argparse
from subprocess import check_output, CalledProcessError
from opendbc.car.carlog import carlog
from opendbc.car.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE, get_dtc_num_as_str, get_dtc_status_names
from opendbc.car.structs import CarParams
from iqdbc.car.carlog import carlog
from iqdbc.car.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE, get_dtc_num_as_str, get_dtc_status_names
from iqdbc.car.structs import CarParams
from panda import Panda
parser = argparse.ArgumentParser(description="read DTC status")

6
selfdrive/debug/test_fw_query_on_routes.py
View File

@@ -5,9 +5,9 @@ import argparse
import os
import traceback
from tqdm import tqdm
from opendbc.car.car_helpers import interface_names
from opendbc.car.fingerprints import MIGRATION
from opendbc.car.fw_versions import VERSIONS, match_fw_to_car
from iqdbc.car.car_helpers import interface_names
from iqdbc.car.fingerprints import MIGRATION
from iqdbc.car.fw_versions import VERSIONS, match_fw_to_car
from openpilot.tools.lib.logreader import LogReader, ReadMode
from openpilot.tools.lib.route import SegmentRange

3
selfdrive/modeld/dmonitoringmodeld.py
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']:
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob', 'sleep_prob']:
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
return parsed
@@ -91,6 +91,7 @@ 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)

BIN
selfdrive/modeld/models/dmonitoring_model.onnx
View File

Binary file not shown.

BIN
selfdrive/modeld/models/dmonitoring_model_metadata.pkl
View File

Binary file not shown.

BIN
selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl
View File

Binary file not shown.

6
selfdrive/modeld/models/prebuilt_check.json
View File

@@ -1,9 +1,9 @@
{
"dmonitoring_model": {
"outputs": {
"dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3",
"dmonitoring_model_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52"
"dmonitoring_model_metadata.pkl": "5999c262b1c25c62e485fb4ced5806d20a8ca59e3ae94e0ff499c0fe3497fedc",
"dmonitoring_model_tinygrad.pkl": "5aca89a35b42376d56f67ccd28a1080806706d546dd8c63f6e1b4c681f8e2c01"
},
"signature": "e1eeb5ce45774a816c8da2394e6ee35ebf700b71dff345e0141dbab8ff592349"
"signature": "1f150841aef31b85b1608ee684c776b724e397f2d805d2a837117d71c97fc314"
}
}

19
selfdrive/modeld/test_dmonitoringmodeld.py Normal file
View File

@@ -0,0 +1,19 @@
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

15
selfdrive/monitoring/README.md
View File

@@ -1,15 +0,0 @@
# 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.

24
selfdrive/monitoring/dmonitoringd.py
View File

@@ -1,8 +1,20 @@
#!/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.helpers import DriverMonitoring
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'],
}
def dmonitoringd_thread():
@@ -25,9 +37,9 @@ def dmonitoringd_thread():
valid = sm.all_checks()
if demo_mode and sm.valid['driverStateV2']:
DM.run_step(sm, demo=demo_mode)
DM.run_step(get_dm_inputs(sm), demo=True)
elif valid:
DM.run_step(sm, demo=demo_mode)
DM.run_step(get_dm_inputs(sm), demo=demo_mode)
# publish
dat = DM.get_state_packet(valid=valid)
@@ -40,9 +52,9 @@ def dmonitoringd_thread():
# save rhd virtual toggle every 5 mins
if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
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)
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)
def main():
dmonitoringd_thread()

474
selfdrive/monitoring/helpers.py
View File

@@ -1,474 +0,0 @@
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
)

467
selfdrive/monitoring/policy.py Normal file
View File

@@ -0,0 +1,467 @@
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,
)

24
selfdrive/monitoring/test_dmonitoringd.py Normal file
View File

@@ -0,0 +1,24 @@
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

269
selfdrive/monitoring/test_monitoring.py
View File

@@ -1,19 +1,15 @@
import numpy as np
import pytest
from cereal import log, car
from cereal import log
from openpilot.common.realtime import DT_DMON
from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS
from openpilot.system.hardware import HARDWARE
from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS
EventName = log.OnroadEvent.EventName
dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
dm_settings = DRIVER_MONITOR_SETTINGS()
TEST_TIMESPAN = 120 # seconds
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
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
def make_msg(face_detected, distracted=False, model_uncertain=False):
ds = log.DriverStateV2.new_message()
@@ -37,7 +33,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._POSESTD_THRESHOLD*1.5)
msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._HI_STD_THRESHOLD*1.5)
# driver interaction with car
car_interaction_DETECTED = True
@@ -51,51 +47,66 @@ 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, standstill):
def _run_seq(self, msgs, interaction, engaged, lowspeed):
DM = DriverMonitoring()
events = []
alert_lvls = []
for idx in range(len(msgs)):
DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx])
DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], lowspeed[idx])
# cal_rpy and car_speed don't matter here
# evaluate events at 10Hz for tests
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
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
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):
events, _ = self._run_seq(always_attentive, always_false, always_true, always_false)
self._assert_no_events(events)
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
# engaged, driver is distracted and does nothing
def test_fully_distracted_driver(self):
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
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
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):
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
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
# 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
@@ -106,13 +117,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))
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
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
# 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
@@ -130,31 +141,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)
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
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
# 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):
_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
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
# engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
# - only disengage will clear the alert
@@ -166,105 +177,51 @@ 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)
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
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
# disengaged, always distracted driver
# - dm should stay quiet when not engaged
def test_pure_dashcam_user(self):
events, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
assert sum(len(event) for event in events) == 0
alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
assert all(a == 0 for a in alert_lvls)
# 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
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
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
# 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[:]
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}"
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

2
selfdrive/pandad/tests/test_pandad_loopback.py
View File

@@ -8,7 +8,7 @@ from pprint import pprint
import cereal.messaging as messaging
from cereal import car, log
from opendbc.car.can_definitions import CanData
from iqdbc.car.can_definitions import CanData
from openpilot.common.utils import retry
from openpilot.common.params import Params
from openpilot.common.timeout import Timeout

29
selfdrive/selfdrived/events.py
View File

@@ -79,6 +79,13 @@ 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(
@@ -360,15 +367,15 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8),
},
EventName.preDriverDistracted: {
EventName.driverDistracted1: {
ET.PERMANENT: Alert(
"Pay Attention",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, .1),
},
EventName.promptDriverDistracted: {
EventName.driverDistracted2: {
ET.PERMANENT: Alert(
"Pay Attention",
"Driver Distracted",
@@ -376,7 +383,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverDistracted: {
EventName.driverDistracted3: {
ET.PERMANENT: Alert(
"DISENGAGE IMMEDIATELY",
"Driver Distracted",
@@ -384,7 +391,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
},
EventName.preDriverUnresponsive: {
EventName.driverUnresponsive1: {
ET.PERMANENT: Alert(
"Touch Steering Wheel: No Face Detected",
"",
@@ -392,7 +399,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1),
},
EventName.promptDriverUnresponsive: {
EventName.driverUnresponsive2: {
ET.PERMANENT: Alert(
"Touch Steering Wheel",
"Driver Unresponsive",
@@ -400,7 +407,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverUnresponsive: {
EventName.driverUnresponsive3: {
ET.PERMANENT: Alert(
"DISENGAGE IMMEDIATELY",
"Driver Unresponsive",
@@ -632,7 +639,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
},
EventName.tooDistracted: {
ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"),
ET.NO_ENTRY: too_distracted_alert,
},
EventName.excessiveActuation: {
@@ -878,14 +885,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
if HARDWARE.get_device_type() == 'mici':
EVENTS.update({
EventName.preDriverDistracted: {
EventName.driverDistracted1: {
ET.PERMANENT: Alert(
"Pay Attention",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 2),
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, 2),
},
EventName.promptDriverDistracted: {
EventName.driverDistracted2: {
ET.PERMANENT: Alert(
"Pay Attention",
"Driver Distracted",

6
selfdrive/selfdrived/helpers.py
View File

@@ -4,9 +4,9 @@ from enum import StrEnum, auto
from cereal import car, messaging
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.locationd.helpers import Pose
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
from opendbc.car.lateral import ISO_LATERAL_ACCEL
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY
from iqdbc.car.lateral import ISO_LATERAL_ACCEL
from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
MIN_EXCESSIVE_ACTUATION_COUNT = int(0.25 / DT_CTRL)
MIN_LATERAL_ENGAGE_BUFFER = int(1 / DT_CTRL)

28
selfdrive/selfdrived/selfdrived.py
View File

@@ -54,6 +54,8 @@ 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)
@@ -147,6 +149,7 @@ 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",
@@ -183,7 +186,6 @@ 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, ...] = ()
@@ -205,10 +207,6 @@ 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
@@ -297,8 +295,24 @@ class SelfdriveD(GapButtonActions):
self.events.add(EventName.resumeBlocked)
if not self.CP.notCar:
self._refresh_cached_dm_events()
self._add_event_names(self._cached_dm_event_names)
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_plan_events()
self._add_iq_event_names(self._cached_plan_event_names)

32
selfdrive/selfdrived/tests/test_driver_monitoring.py Normal file
View File

@@ -0,0 +1,32 @@
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"

8
selfdrive/test/longitudinal_maneuvers/plant.py
View File

@@ -48,8 +48,8 @@ class Plant:
time.sleep(0.1)
self.sm = messaging.SubMaster(['longitudinalPlan'])
from opendbc.car.honda.values import CAR
from opendbc.car.honda.interface import CarInterface
from iqdbc.car.honda.values import CAR
from iqdbc.car.honda.interface import CarInterface
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
CP_IQ = CarInterface.get_non_essential_params_iq(CP, CAR.HONDA_CIVIC)
@@ -70,6 +70,7 @@ class Plant:
car_control = messaging.new_message('carControl')
model = messaging.new_message('modelV2')
iq_car_state = messaging.new_message('iqCarState')
iq_nav_state = messaging.new_message('iqNavState')
live_map_data_iq = messaging.new_message('iqLiveData')
gps_data = messaging.new_message('gpsLocation')
a_lead = (v_lead - self.v_lead_prev)/self.ts
@@ -112,7 +113,7 @@ class Plant:
position = log.XYZTData.new_message()
position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
model.modelV2.position = position
model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1)
model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.5)
velocity = log.XYZTData.new_message()
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
velocity.x[0] = float(self.speed) # always start at current speed
@@ -140,6 +141,7 @@ class Plant:
'liveParameters': lp.liveParameters,
'modelV2': model.modelV2,
'iqCarState': iq_car_state.iqCarState,
'iqNavState': iq_nav_state.iqNavState,
'iqLiveData': live_map_data_iq.iqLiveData,
'gpsLocation': gps_data.gpsLocation}
self.planner.update(sm)

59
selfdrive/test/process_replay/migration.py
View File

@@ -6,11 +6,11 @@ import functools
import traceback
from cereal import messaging, car, log
from opendbc.car.fingerprints import MIGRATION
from opendbc.car.toyota.values import EPS_SCALE, ToyotaSafetyFlags
from opendbc.car.ford.values import CAR as FORD, FordFlags, FordSafetyFlags
from opendbc.car.hyundai.values import HyundaiSafetyFlags
from opendbc.car.gm.values import GMSafetyFlags
from iqdbc.car.fingerprints import MIGRATION
from iqdbc.car.toyota.values import EPS_SCALE, ToyotaSafetyFlags
from iqdbc.car.ford.values import CAR as FORD, FordFlags, FordSafetyFlags
from iqdbc.car.hyundai.values import HyundaiSafetyFlags
from iqdbc.car.gm.values import GMSafetyFlags
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta
from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index
@@ -455,21 +455,46 @@ def migrate_onroadEvents(msgs):
return ops, [], []
@migration(inputs=["driverMonitoringState"])
@migration(inputs=["driverMonitoringStateDEPRECATED"])
def migrate_driverMonitoringState(msgs):
ops = []
for index, msg in msgs:
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()
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.driverMonitoringState.events = events
ops.append((index, msg.as_reader()))
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()))
return ops, [], []

4
selfdrive/test/process_replay/process_replay.py
View File

@@ -17,8 +17,8 @@ import cereal.messaging as messaging
from cereal import car
from cereal.services import SERVICE_LIST
from msgq.visionipc import VisionIpcServer, get_endpoint_name as vipc_get_endpoint_name
from opendbc.car.can_definitions import CanData
from opendbc.car.car_helpers import get_car, interfaces
from iqdbc.car.can_definitions import CanData
from iqdbc.car.car_helpers import get_car, interfaces
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.common.timeout import Timeout

2
selfdrive/test/process_replay/test_fuzzy.py
View File

@@ -5,7 +5,7 @@ import hypothesis.strategies as st
from parameterized import parameterized
from cereal import log
from opendbc.car.toyota.values import CAR as TOYOTA
from iqdbc.car.toyota.values import CAR as TOYOTA
from openpilot.selfdrive.test.fuzzy_generation import FuzzyGenerator
import openpilot.selfdrive.test.process_replay.process_replay as pr

2
selfdrive/test/process_replay/test_processes.py
View File

@@ -7,7 +7,7 @@ from collections import defaultdict
from tqdm import tqdm
from typing import Any
from opendbc.car.car_helpers import interface_names
from iqdbc.car.car_helpers import interface_names
from openpilot.common.git import get_commit
from openpilot.tools.lib.openpilotci import get_url, upload_file
from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff

2
selfdrive/test/scons_build_test.sh
View File

@@ -11,7 +11,7 @@ cd $BASEDIR
# helpful commands:
# scons -Q --tree=derived
cd $BASEDIR/opendbc_repo/
cd $BASEDIR/iqdbc_repo/
scons --clean
scons --no-cache --random -j$(nproc)
if ! scons -q; then

1
selfdrive/test/test_onroad.py
View File

@@ -63,7 +63,6 @@ PROCS = {
"system.micd": 5.0,
"system.timed": 0,
"selfdrive.pandad.pandad": 0,
"system.statsd": 1.0,
"iqpilot.konn3kt.uploaderd.iquploaderd": 15.0,
"system.loggerd.deleter": 1.0,
"./pandad": 19.0,

2
selfdrive/test/update_ci_routes.py
View File

@@ -7,7 +7,7 @@ from collections.abc import Iterable
from tqdm import tqdm
from opendbc.car.tests.routes import routes as test_car_models_routes
from iqdbc.car.tests.routes import routes as test_car_models_routes
from openpilot.selfdrive.test.process_replay.test_processes import source_segments as replay_segments
from openpilot.tools.lib.azure_container import AzureContainer
from openpilot.tools.lib.openpilotcontainers import DataCIContainer, DataProdContainer, OpenpilotCIContainer

2
selfdrive/ui/mici/layouts/onboarding.py
View File

@@ -200,7 +200,7 @@ class TrainingGuideDMTutorial(Widget):
looking_center = False
# stay at 100% once reached
if (dm_state.faceDetected and looking_center) or self._progress.x > 0.99:
if (dm_state.visionPolicyState.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)

8
selfdrive/ui/mici/layouts/settings/vehicle.py
View File

@@ -102,7 +102,7 @@ class VehicleLayoutMici(NavScroller):
def _vw_flags(self):
try:
from opendbc.car.volkswagen.values import CAR
from iqdbc.car.volkswagen.values import CAR
bundle = ui_state.params.get("CarPlatformBundle")
if bundle and (platform := bundle.get("platform")):
return CAR[platform].config.flags
@@ -113,16 +113,16 @@ class VehicleLayoutMici(NavScroller):
return 0
def _is_vw_pq(self) -> bool:
from opendbc.car.volkswagen.values import VolkswagenFlags
from iqdbc.car.volkswagen.values import VolkswagenFlags
return bool(self._vw_flags() & VolkswagenFlags.PQ)
def _is_vw_mqb(self) -> bool:
from opendbc.car.volkswagen.values import VolkswagenFlags
from iqdbc.car.volkswagen.values import VolkswagenFlags
flags = self._vw_flags()
return not bool(flags & (VolkswagenFlags.PQ | VolkswagenFlags.MLB | VolkswagenFlags.MEB | VolkswagenFlags.MEB_GEN2 | VolkswagenFlags.MQB_EVO))
def _supports_vw_lateral_when_faulted(self) -> bool:
from opendbc.car.volkswagen.values import VolkswagenFlags
from iqdbc.car.volkswagen.values import VolkswagenFlags
# PQ, MEB, MQB_EVO and base MQB all implement cruiseFaultLateralMode in carstate.py.
# MLB does not.
return not bool(self._vw_flags() & VolkswagenFlags.MLB)

37
selfdrive/ui/mici/onroad/driver_camera_dialog.py
View File

@@ -1,20 +1,14 @@
import pyray as rl
from cereal import log, messaging
from cereal import car, 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)
@@ -116,10 +110,14 @@ class DriverCameraDialog(NavWidget):
return
msg = messaging.new_message('selfdriveState')
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
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)
self._pm.send('selfdriveState', msg)
def _render_dm_alerts(self, rect: rl.Rectangle):
@@ -127,29 +125,30 @@ 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: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
f"Awareness: {awareness_pct:.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: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
gui_label(rect, f"Awareness: {awareness_pct:.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 not dm_state.events:
if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none:
return
# Show first event (only one should be active at a time)
event_name_str = str(dm_state.events[0].name).split('.')[-1]
alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}"
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, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
gui_label(shadow_rect, alert_level_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, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
gui_label(rect, alert_level_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)))
@@ -166,7 +165,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.faceDetected:
if not dm_state.visionPolicyState.faceDetected:
return
# Get face position and orientation

46
selfdrive/ui/mici/onroad/driver_state.py
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,6 +21,7 @@ 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__()
@@ -35,11 +36,15 @@ 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)
@@ -95,6 +100,7 @@ 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,
@@ -104,13 +110,16 @@ 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(ACTIVE_ACCENT.r, ACTIVE_ACCENT.g, ACTIVE_ACCENT.b, int(255 * self._fade_filter.x)),
rl.Color(r, g, b, int(255 * self._fade_filter.x)),
)
else:
@@ -150,9 +159,12 @@ class DriverStateRenderer(Widget):
sm = ui_state.sm
dm_state = sm["driverMonitoringState"]
self._is_active = dm_state.isActiveMode
self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
self._is_rhd = dm_state.isRHD
self._face_detected = dm_state.faceDetected
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
driverstate = sm["driverStateV2"]
driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
@@ -160,24 +172,9 @@ class DriverStateRenderer(Widget):
def _update_state(self):
# Get monitoring state
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)
_ = self.get_driver_data()
pitch = self._pitch_filter.update(self._face_pitch)
yaw = self._yaw_filter.update(self._face_yaw)
# hysteresis on looking center
if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER:
@@ -198,9 +195,8 @@ 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 (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))
# filter head rotation, handling wrap-around
rotation = math.degrees(math.atan2(pitch * 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)

2
selfdrive/ui/onroad/driver_state.py
View File

@@ -114,7 +114,7 @@ class DriverStateRenderer(Widget):
# Get monitoring state
dm_state = sm["driverMonitoringState"]
self.is_active = dm_state.isActiveMode
self.is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
self.is_rhd = dm_state.isRHD
# Update fade state (smoother transition between active/inactive)

38
selfdrive/ui/soundd.py
View File

@@ -45,26 +45,28 @@ sound_list_iq: dict[int, tuple[str, int | None, float]] = {
AudibleAlertIQ.promptSingleHigh: ("prompt_single_high.wav", 1, MAX_VOLUME),
}
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({
def get_sound_list(device_type: str) -> dict[int, tuple[str, int | None, float]]:
sounds = {
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']

7
selfdrive/ui/tests/test_soundd.py
View File

@@ -2,13 +2,19 @@ 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'])
@@ -32,4 +38,3 @@ class TestSoundd:
assert check_selfdrive_timeout_alert(sm)
# TODO: add test with micd for checking that soundd actually outputs sounds

57
selfdrive/ui/ui_state.py
View File

@@ -50,32 +50,35 @@ class IQUIState:
pass
def onroad_brightness_handle_alerts(self, started: bool, alert):
has_alert = started and self.onroad_brightness != OnroadBrightness.AUTO and alert is not None
self.update_onroad_brightness(has_alert)
if has_alert:
# while an alert is on screen the dim countdown is frozen and re-armed; otherwise it ticks down
alert_showing = bool(started and alert is not None and self.onroad_brightness != OnroadBrightness.AUTO)
self.update_onroad_brightness(alert_showing)
if alert_showing:
self.reset_onroad_sleep_timer()
def update_onroad_brightness(self, has_alert: bool) -> None:
if has_alert:
return
if self.onroad_brightness_timer > 0:
if not has_alert and self.onroad_brightness_timer > 0:
self.onroad_brightness_timer -= 1
def reset_onroad_sleep_timer(self, timer_status: OnroadTimerStatus = OnroadTimerStatus.NONE) -> None:
if timer_status == OnroadTimerStatus.PAUSE and self.onroad_brightness_timer != ONROAD_BRIGHTNESS_TIMER_PAUSED:
self.onroad_brightness_timer = ONROAD_BRIGHTNESS_TIMER_PAUSED
elif (self.onroad_brightness_timer_param >= 0 and self.onroad_brightness != OnroadBrightness.AUTO and
self.onroad_brightness_timer != ONROAD_BRIGHTNESS_TIMER_PAUSED) or timer_status == OnroadTimerStatus.RESUME:
if self.onroad_brightness == OnroadBrightness.AUTO_DARK:
self.onroad_brightness_timer = 15 * gui_app.target_fps
else:
self.onroad_brightness_timer = self.onroad_brightness_timer_param * gui_app.target_fps
paused = self.onroad_brightness_timer == ONROAD_BRIGHTNESS_TIMER_PAUSED
# an explicit PAUSE latches the timer and never re-arms
if timer_status == OnroadTimerStatus.PAUSE:
if not paused:
self.onroad_brightness_timer = ONROAD_BRIGHTNESS_TIMER_PAUSED
return
dimming_active = self.onroad_brightness_timer_param >= 0 and self.onroad_brightness != OnroadBrightness.AUTO
if timer_status == OnroadTimerStatus.RESUME or (dimming_active and not paused):
seconds = 15 if self.onroad_brightness == OnroadBrightness.AUTO_DARK else self.onroad_brightness_timer_param
self.onroad_brightness_timer = seconds * gui_app.target_fps
@property
def onroad_brightness_timer_expired(self) -> bool:
return self.onroad_brightness != OnroadBrightness.AUTO and self.onroad_brightness_timer == 0
if self.onroad_brightness == OnroadBrightness.AUTO:
return False
return self.onroad_brightness_timer == 0
@property
def auto_onroad_brightness(self) -> bool:
@@ -207,18 +210,20 @@ class IQDevice:
@staticmethod
def set_min_onroad_brightness(_ui_state, min_brightness: int) -> int:
if _ui_state.onroad_brightness == OnroadBrightness.AUTO_DARK:
min_brightness = 10
return min_brightness
dark = _ui_state.onroad_brightness == OnroadBrightness.AUTO_DARK
return 10 if dark else min_brightness
@staticmethod
def wake_from_dimmed_onroad_brightness(_ui_state, evs) -> None:
if _ui_state.started and (_ui_state.onroad_brightness_timer_expired or _ui_state.onroad_brightness == OnroadBrightness.AUTO_DARK):
if any(ev.left_down for ev in evs):
if _ui_state.onroad_brightness_timer_expired:
gui_app.mouse_events.clear()
_ui_state.reset_onroad_sleep_timer()
expired = _ui_state.onroad_brightness_timer_expired
dimmed = expired or _ui_state.onroad_brightness == OnroadBrightness.AUTO_DARK
if not (_ui_state.started and dimmed):
return
if not any(ev.left_down for ev in evs):
return
if expired:
gui_app.mouse_events.clear()
_ui_state.reset_onroad_sleep_timer()
class UIStatus(Enum):