mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-27 17:42:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

that's alot

Browse Source
This commit is contained in:
firestar5683
2026-06-26 15:29:52 -05:00
parent 836b0278eb
commit 284bb45c0c
35 changed files with 958 additions and 701 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+1 -1
View File
@@ -278,7 +278,7 @@ struct CustomReserved9 @0xa1680744031fdb2d {
wallTimeNanos @5 :UInt64;
}
struct LateralManeuverPlan @0xcb9fd56c7057593a {
struct StarPilotLateralManeuverPlanDEPRECATED @0xcb9fd56c7057593a {
desiredCurvature @0 :Float32; # 1/m
}
cereal/libcereal.a
BIN
View File
Binary file not shown.
cereal/log.capnp
+91 -14
View File
@@ -68,12 +68,12 @@ struct OnroadEvent @0xc4fa6047f024e718 {
longitudinalManeuver @30;
steerTempUnavailableSilent @31;
resumeRequired @32;
preDriverDistracted @33;
promptDriverDistracted @34;
driverDistracted @35;
preDriverUnresponsive @36;
promptDriverUnresponsive @37;
driverUnresponsive @38;
driverDistracted1 @33;
driverDistracted2 @34;
driverDistracted3 @35;
driverUnresponsive1 @36;
driverUnresponsive2 @37;
driverUnresponsive3 @38;
belowSteerSpeed @39;
lowBattery @40;
accFaulted @41;
@@ -1238,6 +1238,10 @@ struct DriverAssistance {
# FCW, AEB, etc. will go here
}
struct LateralManeuverPlan {
desiredCurvature @0 :Float32; # 1/m
}
struct LongitudinalPlan @0xe00b5b3eba12876c {
modelMonoTime @9 :UInt64;
hasLead @7 :Bool;
@@ -2208,7 +2212,7 @@ struct DriverStateDEPRECATED @0xb83c6cc593ed0a00 {
stdDEPRECATED @2 :Float32;
}
struct DriverMonitoringState @0xb83cda094a1da284 {
struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 {
events @18 :List(OnroadEvent);
faceDetected @1 :Bool;
isDistracted @2 :Bool;
@@ -2226,12 +2230,83 @@ struct DriverMonitoringState @0xb83cda094a1da284 {
isActiveMode @16 :Bool;
isRHD @4 :Bool;
uncertainCount @19 :UInt32;
phoneProbOffset @20 :Float32;
phoneProbValidCount @21 :UInt32;
isPreviewDEPRECATED @15 :Bool;
rhdCheckedDEPRECATED @5 :Bool;
eventsDEPRECATED @0 :List(Car.OnroadEventDEPRECATED);
deprecated :group {
phoneProbOffset @20 :Float32;
phoneProbValidCount @21 :UInt32;
isPreview @15 :Bool;
rhdChecked @5 :Bool;
events @0 :List(Car.OnroadEventDEPRECATED);
}
}
struct DriverMonitoringState {
lockout @0 :Bool;
alertCountLockoutPercent @1 :Int8;
alertTimeLockoutPercent @2 :Int8;
alwaysOn @3 :Bool;
alwaysOnLockout @4 :Bool;
alertLevel @5 :AlertLevel;
activePolicy @6 :MonitoringPolicy;
isRHD @7 :Bool;
rhdCalibration @8 :CalibrationState;
visionPolicyState @9 :VisionPolicyState;
wheeltouchPolicyState @10 :WheeltouchPolicyState;
enum AlertLevel {
# ordinal must match the name to prevent bugs
# comparing against the raw ordinal value
none @0;
one @1;
two @2;
three @3;
}
enum MonitoringPolicy {
wheeltouch @0;
vision @1;
}
struct VisionPolicyState {
awarenessPercent @0 :Int8;
awarenessStep @1 :Float32;
isDistracted @2 :Bool;
distractedTypes @3 :DistractedTypes;
faceDetected @4 :Bool;
pose @5 :Pose;
wheeltouchFallbackPercent @6 :Int8;
uncertainOffroadAlertPercent @7 :Int8;
struct DistractedTypes {
pose @0: Bool;
eye @1: Bool;
phone @2: Bool;
}
struct Pose {
pitch @0 :Float32;
yaw @1 :Float32;
pitchCalib @2 :CalibrationState;
yawCalib @3 :CalibrationState;
calibrated @4 :Bool;
uncertainty @5 :Float32;
}
}
struct WheeltouchPolicyState {
awarenessPercent @0 :Int8;
awarenessStep @1 :Float32;
driverInteracting @2 :Bool;
}
struct CalibrationState {
calibratedPercent @0 :Int8;
offset @1 :Float32;
}
}
struct Boot {
@@ -2552,7 +2627,7 @@ struct Event {
thumbnail @66: Thumbnail;
onroadEvents @134: List(OnroadEvent);
carParams @69: Car.CarParams;
driverMonitoringState @71: DriverMonitoringState;
driverMonitoringState @151 :DriverMonitoringState;
livePose @129 :LivePose;
modelV2 @75 :ModelDataV2;
drivingModelData @128 :DrivingModelData;
@@ -2602,6 +2677,7 @@ struct Event {
bookmarkButton @148 :UserBookmark;
audioFeedback @149 :AudioFeedback;
lateralManeuverPlan @150 :LateralManeuverPlan;
# *********** debug ***********
testJoystick @52 :Joystick;
roadEncodeData @86 :EncodeData;
@@ -2635,7 +2711,7 @@ struct Event {
starpilotRadarState @114 :Custom.StarPilotRadarState;
starpilotSelfdriveState @115 :Custom.StarPilotSelfdriveState;
customReserved9 @116 :Custom.CustomReserved9;
lateralManeuverPlan @136 :Custom.LateralManeuverPlan;
starpilotLateralManeuverPlanDEPRECATED @136 :Custom.StarPilotLateralManeuverPlanDEPRECATED;
customReserved11 @137 :Custom.CustomReserved11;
customReserved12 @138 :Custom.CustomReserved12;
customReserved13 @139 :Custom.CustomReserved13;
@@ -2693,5 +2769,6 @@ struct Event {
gyroscope2DEPRECATED @100 :SensorEventData;
accelerometer2DEPRECATED @101 :SensorEventData;
temperatureSensor2DEPRECATED @123 :SensorEventData;
driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED;
}
}
common/libcommon.a
BIN
View File
Binary file not shown.
common/params_keys.h
+1
View File
@@ -116,6 +116,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"PandaSomResetTriggered", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
{"PandaSignatures", {CLEAR_ON_MANAGER_START, BYTES}},
{"PrimeType", {PERSISTENT, INT}},
{"PriusClusterOffsetMigrated", {PERSISTENT, BOOL, "0", "0"}},
{"RecordAudio", {PERSISTENT, BOOL}},
{"RecordAudioFeedback", {PERSISTENT, BOOL, "0"}},
{"RecordFront", {PERSISTENT, BOOL}},
common/params_pyx.so
BIN
View File
Binary file not shown.
opendbc_repo/opendbc/car/gm/interface.py
+4
View File
@@ -527,6 +527,10 @@ class CarInterface(CarInterfaceBase):
if not ret.openpilotLongitudinalControl:
ret.minEnableSpeed = -1.
if candidate == CAR.CHEVROLET_BLAZER:
# The Blazer builds brake torque noticeably later than the rest of the GM set.
# A slightly larger planner delay estimate starts the request earlier and keeps
# stopped-lead approaches from turning into a late, harsh max-brake catch-up.
ret.longitudinalActuatorDelay = 0.7
ret.longitudinalTuning.kpBP = [0.0, 4.0, 12.0, 35.0]
ret.longitudinalTuning.kpV = [0.09, 0.075, 0.055, 0.040]
ret.longitudinalTuning.kiBP = [0.0, 4.0, 12.0, 35.0]
opendbc_repo/opendbc/car/gm/tests/test_gm.py
+1
View File
@@ -180,6 +180,7 @@ class TestGMInterface:
assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.09, 0.075, 0.055, 0.04])
assert list(car_params.longitudinalTuning.kiBP) == pytest.approx([0.0, 4.0, 12.0, 35.0])
assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.03, 0.04, 0.055, 0.07])
assert car_params.longitudinalActuatorDelay == pytest.approx(0.7)
assert car_params.minEnableSpeed == pytest.approx(5 * CV.KPH_TO_MS)
assert car_params.stoppingDecelRate == pytest.approx(1.0)
assert car_params.vEgoStopping == pytest.approx(0.35)
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+7 -1
View File
@@ -614,8 +614,14 @@ class CarController(CarControllerBase):
# steering control
preserve_stock_lkas = bool(self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING) and not self.long_active_ecu
steering_msg_active = apply_steer_req
if self.CP.carFingerprint == CAR.KIA_EV9 and self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
# EV9 faults if the angle-steering status drops inactive during torque limiting.
# Hold the angle path active while lateral is active; gain/angle are already limited above.
steering_msg_active = CC.latActive
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled,
apply_steer_req, apply_torque, apply_angle,
steering_msg_active, apply_torque, apply_angle,
CS.stock_lfa_msg,
CS.stock_lkas_msg if preserve_stock_lkas else None,
lka_icon=lka_icon))
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+23 -13
View File
@@ -131,19 +131,29 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
lkas_values["LKAS_ANGLE_ACTIVE"] = 2 if lat_active else 1
lkas_values["ADAS_ACIAnglTqRedcGainVal"] = apply_torque if lat_active else 0.0
if ev9_angle_lkas_alt:
lkas_values.update({
"LKA_MODE": 0,
"LKA_AVAILABLE": 3 if lat_active else 0,
"LKA_WARNING": 0,
"LKA_ICON": lka_icon,
"FCA_SYSWARN": 0,
"TORQUE_REQUEST": 0,
"STEER_REQ": 0,
"LFA_BUTTON": 0,
"LKA_ASSIST": 0,
"DAMP_FACTOR": 100,
"HAS_LANE_SAFETY": 0,
})
if lat_active:
lkas_values.update({
"LKA_MODE": 0,
"LKA_AVAILABLE": 3,
"LKA_WARNING": 0,
"LKA_ICON": lka_icon,
"FCA_SYSWARN": 0,
"TORQUE_REQUEST": 0,
"STEER_REQ": 0,
"LFA_BUTTON": 0,
"LKA_ASSIST": 0,
"DAMP_FACTOR": 100,
"HAS_LANE_SAFETY": 0,
})
elif lkas_base_values:
for signal in ("LKA_MODE", "LKA_AVAILABLE", "LKA_WARNING", "LKA_ICON", "FCA_SYSWARN",
"LFA_BUTTON", "LKA_ASSIST", "DAMP_FACTOR", "HAS_LANE_SAFETY"):
if signal in lkas_base_values:
lkas_values[signal] = lkas_base_values[signal]
lkas_values.update({
"TORQUE_REQUEST": 0,
"STEER_REQ": 0,
})
# These signals overlap DAMP_FACTOR in the local DBC naming; omitting them
# preserves the stock angle-steering damping byte expected by the ADAS ECU.
lkas_values.pop("STEER_MODE", None)
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+104 -2
View File
@@ -235,10 +235,11 @@ class TestHyundaiFingerprint:
fingerprint[ev9_radar_config.bus][ev9_radar_config.start_addr] = ev9_radar_config.expected_length
ev9_car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
CP = CarInterface.get_params(CAR.KIA_EV9, fingerprint, ev9_car_fw, True, False, False, None)
assert CP.openpilotLongitudinalControl
assert not CP.alphaLongitudinalAvailable
assert not CP.openpilotLongitudinalControl
assert not CP.radarUnavailable
assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
assert not (CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG)
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_ANGLE_STEERING
CP = CarInterface.get_params(CAR.KIA_EV9, fingerprint, [], True, False, False, None)
@@ -1293,6 +1294,53 @@ class TestHyundaiFingerprint:
assert lead_distance == pytest.approx(20.0)
assert lead_rel_speed == pytest.approx(0.0)
def test_ev9_angle_status_stays_active_when_gain_is_zero(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV9
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_ANGLE_STEERING |
HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
CP.openpilotLongitudinalControl = False
controller = CarController(DBC[CP.carFingerprint], CP)
controller.frame = 1
can_bus = CanBus(CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
stock_lkas = {
"CHECKSUM": 1234,
"COUNTER": 42,
"LKA_MODE": 2,
"LKA_AVAILABLE": 3,
"LKA_WARNING": 1,
"LKA_ICON": 1,
"FCA_SYSWARN": 1,
"TORQUE_REQUEST": 17,
"STEER_REQ": 1,
"LFA_BUTTON": 1,
"LKA_ASSIST": 1,
"STEER_MODE": 5,
"NEW_SIGNAL_2": 0,
"LKAS_ANGLE_ACTIVE": 1,
"HAS_LANE_SAFETY": 1,
"ADAS_StrAnglReqVal": 12.3,
"ADAS_ACIAnglTqRedcGainVal": 0.42,
"DAMP_FACTOR": 0,
}
cc = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(longControlState=LongCtrlState.off),
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace())
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas)
msgs = controller.create_canfd_msgs(0, False, 0.0, 8.5, 0.0, 0.0, False, cc.hudControl, cs, cc,
get_test_toggles(), lka_icon=2, lfa_icon=2)
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
assert len(lkas_msgs) == 1
parser.update([(1, lkas_msgs)])
assert parser.can_valid
assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 2
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0)
assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(8.5)
def test_can_acc_commands_use_default_values(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.GENESIS_G90
@@ -1642,6 +1690,60 @@ class TestHyundaiFingerprint:
assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(-31.5)
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.44)
def test_ev9_angle_lkas_alt_preserves_stock_status_when_inactive(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV9
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_ANGLE_STEERING |
HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
can_bus = CanBus(CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
stock_lkas = {
"CHECKSUM": 1234,
"COUNTER": 42,
"LKA_MODE": 2,
"LKA_AVAILABLE": 3,
"LKA_WARNING": 1,
"LKA_ICON": 1,
"FCA_SYSWARN": 1,
"TORQUE_REQUEST": 17,
"STEER_REQ": 1,
"LFA_BUTTON": 1,
"LKA_ASSIST": 1,
"STEER_MODE": 5,
"NEW_SIGNAL_2": 0,
"LKAS_ANGLE_ACTIVE": 2,
"HAS_LANE_SAFETY": 1,
"ADAS_StrAnglReqVal": 12.3,
"ADAS_ACIAnglTqRedcGainVal": 0.42,
"DAMP_FACTOR": 0,
}
msgs = hyundaicanfd.create_steering_messages(packer, CP, can_bus, False, False, 0.44, -31.5,
lkas_base_values=stock_lkas, lka_icon=1)
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
assert len(lkas_msgs) == 1
parser.update([(1, lkas_msgs)])
assert parser.can_valid
assert parser.vl["LKAS_ALT"]["LKA_MODE"] == 2
assert parser.vl["LKAS_ALT"]["LKA_AVAILABLE"] == 3
assert parser.vl["LKAS_ALT"]["LKA_WARNING"] == 1
assert parser.vl["LKAS_ALT"]["LKA_ICON"] == 1
assert parser.vl["LKAS_ALT"]["FCA_SYSWARN"] == 1
assert parser.vl["LKAS_ALT"]["TORQUE_REQUEST"] == 0
assert parser.vl["LKAS_ALT"]["STEER_REQ"] == 0
assert parser.vl["LKAS_ALT"]["LFA_BUTTON"] == 1
assert parser.vl["LKAS_ALT"]["LKA_ASSIST"] == 1
assert parser.vl["LKAS_ALT"]["DAMP_FACTOR"] == 0
assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 1
assert parser.vl["LKAS_ALT"]["HAS_LANE_SAFETY"] == 1
assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(-31.5)
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0)
def test_ev9_accelerator_brake_alt_spoof_matches_route_template(self):
msg = hyundaicanfd.create_accelerator_brake_alt_spoof(0, 0x66, True, False, CAR.KIA_EV9)
opendbc_repo/opendbc/car/hyundai/values.py
+2 -3
View File
@@ -1129,9 +1129,8 @@ CANFD_RADAR_SCC_CAR = CAR.with_flags(HyundaiFlags.RADAR_SCC) # TODO: merge with
# CAN-FD cars with ADAS ECUs that work with the communication-control path.
CANFD_SECURITYACCESS_CAR = {CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_KONA_EV_2ND_GEN}
CANFD_UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.CANFD_NO_RADAR_DISABLE) - CANFD_SECURITYACCESS_CAR # TODO: merge with UNSUPPORTED_LONGITUDINAL_CAR
CANFD_ANGLE_LONGITUDINAL_CAR = {CAR.KIA_EV9}
CANFD_RADAR_LIVE_LONGITUDINAL_CAR = {CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV6, CAR.GENESIS_GV60_EV_1ST_GEN,
CAR.KIA_EV9}
CANFD_ANGLE_LONGITUDINAL_CAR = set()
CANFD_RADAR_LIVE_LONGITUDINAL_CAR = {CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV6, CAR.GENESIS_GV60_EV_1ST_GEN}
RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | {
CAR.HYUNDAI_IONIQ,
CAR.HYUNDAI_KONA_EV_2022,
selfdrive/controls/controlsd.py
+1 -1
View File
@@ -342,7 +342,7 @@ class Controls:
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'].alertLevel == log.DriverMonitoringState.AlertLevel.three) or
(self.sm['selfdriveState'].state == State.softDisabling) or self.sm["starpilotCarState"].forceCoast)
lat_tuning = self.CP.lateralTuning.which()
selfdrive/controls/lib/latcontrol_torque.py
+32 -16
View File
@@ -649,28 +649,33 @@ VOLT_PLEXY_FRICTION_SCALE_MULT_RIGHT = 1.05
VOLT_PLEXY_FRICTION_THRESHOLD_MULT_LEFT = 1.00
VOLT_PLEXY_FRICTION_THRESHOLD_MULT_RIGHT = 0.96
VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT = 0.98
PRIUS_TRANSITION_SPEED = 8.5
PRIUS_TRANSITION_SPEED = 11.0
PRIUS_PHASE_SCALE = 0.09
PRIUS_FF_GAIN_LEFT = 0.08
PRIUS_FF_GAIN_RIGHT = 0.09
PRIUS_FF_ONSET = 0.15
PRIUS_FF_ONSET_WIDTH = 0.08
PRIUS_FF_CUTOFF = 1.30
PRIUS_FF_CUTOFF_WIDTH = 0.32
PRIUS_FF_CUTOFF = 1.45
PRIUS_FF_CUTOFF_WIDTH = 0.36
PRIUS_FRICTION_LAT_RISE = 0.18
PRIUS_FRICTION_JERK_RISE = 0.22
PRIUS_TURN_IN_BOOST_LEFT = 0.28
PRIUS_TURN_IN_BOOST_RIGHT = 0.25
PRIUS_UNWIND_TAPER_LEFT = 0.22
PRIUS_UNWIND_TAPER_RIGHT = 0.34
PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16
PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.16
PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT = 0.22
PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.32
PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.07
PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.07
PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.12
PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.18
PRIUS_TURN_IN_BOOST_LEFT = 0.38
PRIUS_TURN_IN_BOOST_RIGHT = 0.34
PRIUS_UNWIND_TAPER_LEFT = 0.28
PRIUS_UNWIND_TAPER_RIGHT = 0.42
PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.22
PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.22
PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28
PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.38
PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.10
PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.10
PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.16
PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22
PRIUS_CENTER_TAPER_MAX = 0.12
PRIUS_CENTER_TAPER_LAT = 0.14
PRIUS_CENTER_TAPER_LAT_WIDTH = 0.035
PRIUS_CENTER_TAPER_SPEED = 20.0
PRIUS_CENTER_TAPER_SPEED_WIDTH = 2.5
TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG
TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS
@@ -777,6 +782,13 @@ def get_prius_friction_scale(v_ego: float, desired_lateral_accel: float, desired
return min(max(friction_scale, 0.90), 1.14)
def get_prius_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _prius_sigmoid((v_ego - PRIUS_CENTER_TAPER_SPEED) / PRIUS_CENTER_TAPER_SPEED_WIDTH)
center_weight = _prius_sigmoid((PRIUS_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / PRIUS_CENTER_TAPER_LAT_WIDTH)
reduction = PRIUS_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def civic_bosch_modified_lateral_testing_ground_active() -> bool:
return testing_ground.use("8", "B")
@@ -2243,6 +2255,7 @@ class LatControlTorque(LatControl):
kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active()
volt_plexy_test_active = self.is_volt_standard and volt_plexy_lateral_testing_ground_active()
ioniq_5_center_taper = get_ioniq_5_center_taper_scale(setpoint, CS.vEgo) if ioniq_5_active else 1.0
prius_center_taper = get_prius_center_taper_scale(setpoint, CS.vEgo) if prius_active else 1.0
volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0
volt_plexy_center_taper = get_volt_plexy_center_taper_scale(setpoint, CS.vEgo) if volt_plexy_test_active else 1.0
ioniq_ev_old_center_taper = get_ioniq_ev_old_center_taper_scale(setpoint, CS.vEgo) if ioniq_ev_old_active else 1.0
@@ -2281,9 +2294,10 @@ class LatControlTorque(LatControl):
friction_threshold = get_palisade_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = get_palisade_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
elif prius_active:
ff *= get_prius_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
ff *= get_prius_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * prius_center_taper
friction_threshold = get_prius_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = get_prius_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = 1.0 + ((friction_scale - 1.0) * prius_center_taper)
elif ioniq_5_active:
ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper
friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
@@ -2355,6 +2369,8 @@ class LatControlTorque(LatControl):
if ioniq_6_active:
output_torque *= get_ioniq_6_highway_output_taper_scale(setpoint, CS.vEgo)
output_torque *= get_ioniq_6_highway_transition_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif prius_active:
output_torque *= prius_center_taper
elif volt_standard_test_active:
output_torque *= volt_standard_center_taper
elif volt_plexy_test_active:
selfdrive/debug/cycle_alerts.py
+3 -3
View File
@@ -30,9 +30,9 @@ def cycle_alerts(duration=200, is_metric=False):
(EventName.accFaulted, ET.IMMEDIATE_DISABLE),
# DM sequence
(EventName.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
selfdrive/monitoring/dmonitoringd.py
+8 -17
View File
@@ -1,11 +1,8 @@
#!/usr/bin/env python3
import cereal.messaging as messaging
from opendbc.car import structs
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process
from openpilot.selfdrive.monitoring.helpers import DriverMonitoring
GearShifter = structs.CarState.GearShifter
from openpilot.selfdrive.monitoring.policy import DriverMonitoring
def get_rhd_override(params):
@@ -26,10 +23,6 @@ def dmonitoringd_thread():
)
demo_mode=False
sm = sm.extend(['starpilotCarState'])
driver_view_enabled = params.get_bool("IsDriverViewEnabled")
# 20Hz <- dmonitoringmodeld
while True:
sm.update()
@@ -39,14 +32,12 @@ def dmonitoringd_thread():
valid = sm.all_checks()
if demo_mode and sm.valid['driverStateV2']:
DM.run_step(sm, demo=demo_mode)
DM.run_step(sm, demo=True)
elif valid:
DM.run_step(sm, demo=demo_mode)
elif driver_view_enabled:
DM.face_detected = sm['driverStateV2'].leftDriverData.faceProb > DM.settings._FACE_THRESHOLD or sm['driverStateV2'].rightDriverData.faceProb > DM.settings._FACE_THRESHOLD
# publish
dat = DM.get_state_packet(valid=valid or driver_view_enabled)
dat = DM.get_state_packet(valid=valid)
pm.send('driverMonitoringState', dat)
# load live always-on toggle
@@ -54,14 +45,14 @@ def dmonitoringd_thread():
DM.always_on = params.get_bool("AlwaysOnDM")
DM.wheel_on_right_default = params.get_bool("IsRhdDetected")
DM.wheel_on_right_override = get_rhd_override(params)
demo_mode = params.get_bool("IsDriverViewEnabled") and sm["carState"].gearShifter != GearShifter.reverse
demo_mode = params.get_bool("IsDriverViewEnabled")
# save rhd virtual toggle every 5 mins
if (DM.wheel_on_right_override is None and 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)
params.put_bool_nonblocking("IsRHD", 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)
params.put_bool("IsRHD", DM.wheel_on_right)
def main():
dmonitoringd_thread()
selfdrive/monitoring/helpers.py
+4 -468
View File
@@ -1,471 +1,7 @@
from math import atan2, radians
import numpy as np
from openpilot.selfdrive.monitoring.policy import DRIVER_MONITOR_SETTINGS, DriverMonitoring, face_orientation_from_model
from cereal import car, log
import cereal.messaging as messaging
from openpilot.selfdrive.selfdrived.events import Events
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
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.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.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_RESET_COUNT = int(2 / 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._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.
self.steer_yaw_offset = 0.
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, rhd_override=None):
# 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)
self.wheelpos = DriverProb(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.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.wheel_on_right_override = rhd_override
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")
set_offroad_alert("Offroad_DriverMonitoringUncertain", False)
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
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
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 > self.settings._PHONE_THRESH:
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, 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.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.wheel_on_right_override is not None and not demo_mode:
self.wheel_on_right = self.wheel_on_right_override
elif 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 (self.wheel_on_right_override is None and 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)
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.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.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
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
awareness_prev = self.awareness
_reaching_pre = self.awareness - self.step_change <= self.threshold_pre
_reaching_terminal = self.awareness - self.step_change <= 0
standstill_orange_exemption = standstill and _reaching_pre
always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal
if self.awareness > 0 and \
((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_orange_exemption):
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
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 and reaching green
# also will not be reaching 0 if DM is active when not engaged
if not (standstill_orange_exemption or always_on_red_exemption):
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:
# 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": 0.,
"phoneProbValidCount": 0,
"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['starpilotCarState'].alwaysOnLateralEnabled
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
rpyCalib = sm['liveCalibration'].rpyCalib
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,
steering_angle_deg=sm['carState'].steeringAngleDeg,
)
# Update distraction events
self._update_events(
driver_engaged=driver_engaged,
op_engaged=enabled,
standstill=standstill,
wrong_gear=wrong_gear,
car_speed=highway_speed
)
pitch, yaw = face_orientation_from_model(angles_desc, pos_desc, rpy_calib)
roll = angles_desc[2] if len(angles_desc) > 2 else 0.
return roll, pitch, yaw
selfdrive/monitoring/policy.py
+440
View File
@@ -0,0 +1,440 @@
from collections import defaultdict
from math import atan2, radians
import numpy as np
from cereal import log
from cereal import car
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=CELEX:42018X1947&rid=2
self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 15.
self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 24.
self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 30.
# https://cdn.euroncap.com/cars/assets/euro_ncap_protocol_safe_driving_driver_engagement_v11_a30e874152.pdf
self._VISION_POLICY_ALERT_1_TIMEOUT = 3.
self._VISION_POLICY_ALERT_2_TIMEOUT = 5.
self._VISION_POLICY_ALERT_3_TIMEOUT = 11.
self._TIMEOUT_RECOVERY_FACTOR_MAX = 5.
self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25
self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
self._MAX_TERMINAL_DURATION = int(30 / DT_DMON) # not allowed to engage after 30s of terminal alerts
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, rhd_override=None):
# 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.wheel_on_right_override = rhd_override
self.face_detected = False
self.terminal_alert_cnt = 0
self.terminal_time = 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.too_distracted = Params().get_bool("DriverTooDistracted")
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, standstill, 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 self.wheel_on_right_override is not None:
self.wheel_on_right = self.wheel_on_right_override
elif 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 (self.wheel_on_right_override is None and 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 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_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, standstill, wrong_gear):
self.alert_level = AlertLevel.none
self.driver_interacting = driver_engaged
if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \
self.terminal_time >= self.settings._MAX_TERMINAL_DURATION:
self.too_distracted = True
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
standstill_exemption = standstill 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 standstill_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 standstill and reaching green
# also will not be reaching 0 if DM is active when not engaged
if not (standstill_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
self.terminal_time += 1
if awareness_prev > 0.:
self.terminal_alert_cnt += 1
elif 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.too_distracted
dm.alertCountLockoutPercent = to_percent(self.terminal_alert_cnt / self.settings._MAX_TERMINAL_ALERTS)
dm.alertTimeLockoutPercent = to_percent(self.terminal_time / self.settings._MAX_TERMINAL_DURATION)
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
standstill = 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)
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
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,
standstill=standstill,
demo_mode=demo,
steering_angle_deg=steering_angle_deg,
)
# Update distraction events
self._update_events(
driver_engaged=driver_engaged,
op_engaged=enabled,
standstill=standstill,
wrong_gear=wrong_gear,
)
selfdrive/monitoring/test_monitoring.py
+78 -79
View File
@@ -2,17 +2,16 @@ import numpy as np
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()
@@ -36,7 +35,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
@@ -52,19 +51,17 @@ always_false = [False] * int(TEST_TIMESPAN / DT_DMON)
class TestMonitoring:
def _run_seq(self, msgs, interaction, engaged, standstill):
DM = DriverMonitoring()
events = []
alert_lvls = []
for idx in range(len(msgs)):
DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx])
# cal_rpy and car_speed don't matter here
# evaluate events at 10Hz for tests
DM._update_events(interaction[idx], engaged[idx], 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], standstill[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)
def test_rhd_manual_override_beats_saved_default(self):
DM = DriverMonitoring(rhd_saved=False, rhd_override=True)
@@ -75,35 +72,38 @@ class TestMonitoring:
DM._update_states(msg_ATTENTIVE, [0, 0, 0], 0, False, False)
assert not DM.wheel_on_right
# 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, 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
@@ -114,13 +114,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
@@ -138,11 +138,11 @@ 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
@@ -153,16 +153,16 @@ class TestMonitoring:
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
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/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 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
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)] == 1
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
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
@@ -174,19 +174,19 @@ 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(INVISIBLE_SECONDS_TO_ORANGE*0.5/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
@@ -194,11 +194,12 @@ class TestMonitoring:
_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 len(events[int((_redlight_time-0.1)/DT_DMON)]) == 0
_pre_to_prompt = d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL - d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL
assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.preDriverDistracted
assert events[int((_redlight_time+_pre_to_prompt+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_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 standstill
@@ -206,20 +207,18 @@ class TestMonitoring:
_stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange
standstill_vector = always_false[:]
standstill_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON)
events, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
# just before and briefly after stopping: orange alert; goes away quickly after stopped
assert events[int((_stop_time+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert len(events[int((_stop_time+0.5)/DT_DMON)]) == 0
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
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
selfdrive/selfdrived/events.py
+8 -8
View File
@@ -610,7 +610,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8),
},
EventName.preDriverDistracted: {
EventName.driverDistracted1: {
ET.PERMANENT: Alert(
"Pay Attention",
"",
@@ -618,7 +618,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.promptDriverDistracted: {
EventName.driverDistracted2: {
ET.PERMANENT: Alert(
"Pay Attention",
"Driver Distracted",
@@ -626,7 +626,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",
@@ -634,7 +634,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",
"",
@@ -642,7 +642,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",
@@ -650,7 +650,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",
@@ -1418,14 +1418,14 @@ STARPILOT_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),
},
EventName.promptDriverDistracted: {
EventName.driverDistracted2: {
ET.PERMANENT: Alert(
"Pay Attention",
"Driver Distracted",
selfdrive/selfdrived/selfdrived.py
+23 -1
View File
@@ -47,6 +47,8 @@ LaneChangeDirection = log.LaneChangeDirection
EventName = log.OnroadEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type
SafetyModel = car.CarParams.SafetyModel
AlertLevel = log.DriverMonitoringState.AlertLevel
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
StarPilotEventName = custom.StarPilotOnroadEvent.EventName
@@ -209,6 +211,8 @@ class SelfdriveD:
self.safe_mode = self.params.get_bool("SafeMode")
self.personality = log.LongitudinalPersonality.relaxed if self.safe_mode else self.params.get("LongitudinalPersonality", return_default=True)
self.recalibrating_seen = False
self.dm_lockout_set = False
self.dm_uncertain_alerted = False
self.state_machine = StateMachine()
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prev_pedal_long_active = False
@@ -331,7 +335,25 @@ class SelfdriveD:
self.events.add(EventName.resumeBlocked)
if not self.CP.notCar:
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Block engaging until ignition cycle after max number or time of distractions
if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set:
self.params.put_bool("DriverTooDistracted", True)
self.dm_lockout_set = True
# No entry conditions
if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout:
self.events.add(EventName.tooDistracted)
# Alerts
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)
# Warn consistent DM uncertainty
if self.sm['driverMonitoringState'].visionPolicyState.uncertainOffroadAlertPercent >= 100 and not self.dm_uncertain_alerted:
set_offroad_alert("Offroad_DriverMonitoringUncertain", True)
self.dm_uncertain_alerted = True
# Add car events, ignore if CAN isn't valid
if CS.canValid:
selfdrive/test/process_replay/README.md
+1 -1
View File
@@ -30,7 +30,7 @@ optional arguments:
--whitelist-cars WHITELIST_CARS Whitelist given cars from the test (e.g. HONDA)
--blacklist-procs BLACKLIST_PROCS Blacklist given processes from the test (e.g. controlsd)
--blacklist-cars BLACKLIST_CARS Blacklist given cars from the test (e.g. HONDA)
--ignore-fields IGNORE_FIELDS Extra fields or msgs to ignore (e.g. driverMonitoringState.events)
--ignore-fields IGNORE_FIELDS Extra fields or msgs to ignore (e.g. driverMonitoringState.alertLevel)
--ignore-msgs IGNORE_MSGS Msgs to ignore (e.g. onroadEvents)
--update-refs Updates reference logs using current commit
--upload-only Skips testing processes and uploads logs from previous test run
selfdrive/test/process_replay/migration.py
+48 -12
View File
@@ -39,6 +39,7 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo
migrate_liveLocationKalman,
migrate_liveTracks,
migrate_driverAssistance,
migrate_lateralManeuverPlan,
migrate_drivingModelData,
migrate_onroadEvents,
migrate_driverMonitoringState,
@@ -126,6 +127,16 @@ def migrate_driverAssistance(msgs):
return [], add_ops, []
@migration(inputs=["starpilotLateralManeuverPlanDEPRECATED"], product="lateralManeuverPlan")
def migrate_lateralManeuverPlan(msgs):
ops = []
for index, msg in msgs:
new_msg = messaging.new_message('lateralManeuverPlan', valid=msg.valid, logMonoTime=msg.logMonoTime)
new_msg.lateralManeuverPlan.desiredCurvature = msg.starpilotLateralManeuverPlanDEPRECATED.desiredCurvature
ops.append((index, new_msg.as_reader()))
return ops, [], []
@migration(inputs=["modelV2"], product="drivingModelData")
def migrate_drivingModelData(msgs):
add_ops = []
@@ -454,21 +465,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, [], []
selfdrive/test/process_replay/test_processes.py
+1 -1
View File
@@ -137,7 +137,7 @@ if __name__ == "__main__":
parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[],
help="Blacklist given cars from the test (e.g. HONDA)")
parser.add_argument("--ignore-fields", type=str, nargs="*", default=[],
help="Extra fields or msgs to ignore (e.g. driverMonitoringState.events)")
help="Extra fields or msgs to ignore (e.g. driverMonitoringState.alertLevel)")
parser.add_argument("--ignore-msgs", type=str, nargs="*", default=[],
help="Msgs to ignore (e.g. carEvents)")
parser.add_argument("--update-refs", action="store_true",
selfdrive/ui/mici/layouts/onboarding.py
+1 -1
View File
@@ -138,7 +138,7 @@ class TrainingGuideDMTutorial(NavWidget):
# stay at 100% once reached
in_bad_face = gui_app.get_active_widget() == self._bad_face_page
if ((dm_state.faceDetected and looking_center) or self._progress.x > 0.99) and not in_bad_face:
if ((dm_state.visionPolicyState.faceDetected and looking_center) or self._progress.x > 0.99) and not in_bad_face:
slow = self._progress.x < 0.25
duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION
self._progress.x += 1.0 / (duration * gui_app.target_fps)
selfdrive/ui/mici/onroad/driver_camera_dialog.py
+18 -19
View File
@@ -1,21 +1,15 @@
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 import Widget
from openpilot.system.ui.widgets.nav_widget import NavWidget
from openpilot.system.ui.widgets.label import gui_label
EventName = log.OnroadEvent.EventName
EVENT_TO_INT = EventName.schema.enumerants
class DriverCameraView(CameraView):
def _calc_frame_matrix(self, rect: rl.Rectangle):
base = super()._calc_frame_matrix(rect)
@@ -110,10 +104,13 @@ class BaseDriverCameraDialog(Widget):
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 = {
'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):
@@ -121,29 +118,31 @@ class BaseDriverCameraDialog(Widget):
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]
# Show alert level
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)))
@@ -160,7 +159,7 @@ class BaseDriverCameraDialog(Widget):
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
selfdrive/ui/mici/onroad/driver_state.py
+11 -25
View File
@@ -3,7 +3,6 @@ import numpy as np
import math
from cereal import log
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net
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
@@ -24,7 +23,7 @@ class DriverStateRenderer(Widget):
BASE_SIZE = 60
LINES_ANGLE_INCREMENT = 5
LINES_STALE_ANGLES = 3.0 # seconds
AWARENESS_UNFULL_THRESHOLD = 0.95 # ~0.5s
AWARENESS_UNFULL_PERCENT = 95 # ~0.5s
def __init__(self, lines: bool = False, inset: bool = False):
super().__init__()
@@ -39,6 +38,8 @@ 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
@@ -163,10 +164,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._awareness_unfull = self.effective_active and dm_state.awarenessStatus < self.AWARENESS_UNFULL_THRESHOLD
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) # calib or DM pose is not accurate, add a fake upward pitch to bias forward
self._face_yaw = -dm_state.visionPolicyState.pose.yaw # undo sign flip in face_orientation_from_model to match UI convention
driverstate = sm["driverStateV2"]
driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
@@ -174,26 +177,9 @@ class DriverStateRenderer(Widget):
def _update_state(self):
# Get monitoring state
driver_data = self.get_driver_data()
driver_orient = driver_data.faceOrientation
driver_position = driver_data.facePosition
if len(driver_orient) != 3:
return
# Calibrate orientation so looking straight ahead at road (instead of at device) is (0, 0, 0)
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_position, cal_rpy)
pitch += math.radians(6) # calib or DM pose is not accurate, add a fake upward pitch to bias forward
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:
selfdrive/ui/onroad/driver_state.py
+1 -1
View File
@@ -117,7 +117,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)
selfdrive/ui/qt/onroad/driver_monitoring.cc
+1 -1
View File
@@ -31,7 +31,7 @@ void DriverMonitorRenderer::updateState(const UIState &s) {
if (!is_visible) return;
auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState();
is_active = dm_state.getIsActiveMode();
is_active = dm_state.getActivePolicy() == cereal::DriverMonitoringState::MonitoringPolicy::VISION;
is_rhd = dm_state.getIsRHD();
dm_fade_state = std::clamp(dm_fade_state + 0.2f * (0.5f - is_active), 0.0f, 1.0f);
starpilot/common/starpilot_utilities.py
+1 -1
View File
@@ -396,7 +396,7 @@ def wait_for_no_driver(params, sm, door_checks=False, time_threshold=60):
if any(ps.ignitionLine or ps.ignitionCan for ps in sm["pandaStates"] if ps.pandaType != log.PandaState.PandaType.unknown):
break
if sm["driverMonitoringState"].faceDetected or not sm.alive["driverMonitoringState"]:
if sm["driverMonitoringState"].visionPolicyState.faceDetected or not sm.alive["driverMonitoringState"]:
start_time = time.monotonic()
if door_checks:
starpilot/common/starpilot_variables.py
+25 -1
View File
@@ -20,7 +20,7 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, EV_CAR as HYUNDAI_EV_
from opendbc.car.interfaces import TORQUE_SUBSTITUTE_PATH, CarInterfaceBase, GearShifter
from opendbc.car.mock.values import CAR as MOCK
from opendbc.car.subaru.values import SubaruFlags
from opendbc.car.toyota.values import ToyotaStarPilotFlags
from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaStarPilotFlags
from openpilot.common.basedir import BASEDIR
from openpilot.common.constants import CV
from openpilot.common.params import Params
@@ -85,6 +85,14 @@ LEGACY_VOLT_STOCK_ACC_CARS = {
GM_CAR.CHEVROLET_VOLT_CAMERA,
}
PRIUS_CLUSTER_OFFSET_DEFAULT = 1.015
PRIUS_CLUSTER_OFFSET_MIGRATION_KEY = "PriusClusterOffsetMigrated"
PRIUS_CLUSTER_OFFSET_CARS = {
str(TOYOTA_CAR.TOYOTA_PRIUS),
str(TOYOTA_CAR.TOYOTA_PRIUS_V),
str(TOYOTA_CAR.TOYOTA_PRIUS_TSS2),
}
RESOURCES_REPO = os.getenv("STARPILOT_RESOURCES_REPO", "firestar5683/StarPilot-Resources")
ACTIVE_THEME_PATH = Path(BASEDIR) / "starpilot/assets/active_theme"
@@ -487,6 +495,21 @@ class StarPilotVariables:
# runtime behavior to defaults after the driver has configured them.
return self.get_value(key, cast=float, condition=condition, respect_tuning_level=False)
def migrate_prius_cluster_offset(self, car_model):
if car_model not in PRIUS_CLUSTER_OFFSET_CARS or self.params_raw.get_bool(PRIUS_CLUSTER_OFFSET_MIGRATION_KEY):
return
cluster_offset_raw = self.params_raw.get("ClusterOffset")
try:
cluster_offset = float(cluster_offset_raw) if cluster_offset_raw not in (None, b"") else 1.0
except (TypeError, ValueError):
cluster_offset = 1.0
if math.isclose(cluster_offset, 1.0, abs_tol=1e-6):
self.params.put_float("ClusterOffset", PRIUS_CLUSTER_OFFSET_DEFAULT)
self.params.put_bool(PRIUS_CLUSTER_OFFSET_MIGRATION_KEY, True)
@staticmethod
def set_favorite_button_flags(toggle, suffix, button_control):
for slot_number in range(1, 4):
@@ -727,6 +750,7 @@ class StarPilotVariables:
if toggle.force_fingerprint:
toggle.car_model = car_model
self.migrate_prius_cluster_offset(str(toggle.car_model))
toggle.cluster_offset = self.get_value("ClusterOffset", cast=float, condition=toggle.car_make == "toyota")
toggle.conditional_experimental_mode = toggle.openpilot_longitudinal and self.get_value("ConditionalExperimental")
starpilot/controls/lib/starpilot_vcruise.py
+1 -1
View File
@@ -34,7 +34,7 @@ NAV_TURN_TARGET_SPEEDS = {
# Force-stop kinematic profile. The user tunes one signed knob (ForceStopDistanceOffset,
# in feet); positive = stop later/longer, negative = stop sooner/shorter.
# Smaller values pull speed down earlier on approach.
FORCE_STOP_MODEL_APPROACH_DECEL = 0.8
FORCE_STOP_MODEL_APPROACH_DECEL = 0.65
FORCE_STOP_DASH_APPROACH_DECEL = 1.0
ACTIVATION_M = 75.0 # m — CEM/model path activates when model_length < this
MPC_HANDOFF_M = 6.0 # m — below this, command 0 and let MPC finish the stop
starpilot/system/the_galaxy/tests/test_dashboard_stats.py
+5 -5
View File
@@ -288,12 +288,12 @@ def test_route_metrics_count_selected_attention_events():
msg("initData", 0, SimpleNamespace(params={"DrivingModelName": b"North Star"})),
msg("carState", 0, SimpleNamespace(vEgo=10.0)),
msg("selfdriveState", 0, SimpleNamespace(enabled=True)),
msg("onroadEvents", 1, events("promptDriverDistracted")),
msg("onroadEvents", 2, events("promptDriverDistracted")),
msg("onroadEvents", 1, events("driverDistracted2")),
msg("onroadEvents", 2, events("driverDistracted2")),
msg("onroadEvents", 3, events()),
msg("onroadEvents", 4, events("promptDriverDistracted")),
msg("onroadEvents", 5, events("driverUnresponsive")),
msg("onroadEvents", 6, events("driverUnresponsive")),
msg("onroadEvents", 4, events("driverDistracted2")),
msg("onroadEvents", 5, events("driverUnresponsive3")),
msg("onroadEvents", 6, events("driverUnresponsive3")),
msg("carState", 10, SimpleNamespace(vEgo=10.0)),
msg("selfdriveState", 10, SimpleNamespace(enabled=False)),
]
starpilot/system/the_galaxy/utilities.py
+2 -2
View File
@@ -75,8 +75,8 @@ DASHBOARD_ANALYZER_LOG_PATH = "/tmp/galaxy_dashboard_analyzer.log"
DASHBOARD_ANALYZER_STATUS_PATH = Path("/tmp/galaxy_dashboard_analyzer_status.json")
DASHBOARD_ANALYZER_STATUS_MAX_AGE_SECONDS = 30 * 60
DASHBOARD_TOP_MODEL_LIMIT = 3
DASHBOARD_EVENT_DISTRACTED = "promptDriverDistracted"
DASHBOARD_EVENT_UNRESPONSIVE = "driverUnresponsive"
DASHBOARD_EVENT_DISTRACTED = "driverDistracted2"
DASHBOARD_EVENT_UNRESPONSIVE = "driverUnresponsive3"
DASHBOARD_TIME_SOURCE_LOG = "log"
DASHBOARD_TIME_SOURCE_FILESYSTEM = "filesystem"
DASHBOARD_MIN_VALID_ROUTE_TIME = datetime(2026, 1, 1)
tools/sim/lib/simulated_sensors.py
+11 -3
View File
@@ -93,9 +93,17 @@ class SimulatedSensors:
# dmonitoringd output
dat = messaging.new_message('driverMonitoringState', valid=True)
dat.driverMonitoringState = {
"faceDetected": True,
"isDistracted": False,
"awarenessStatus": 1.,
"alertLevel": log.DriverMonitoringState.AlertLevel.none,
"activePolicy": log.DriverMonitoringState.MonitoringPolicy.vision,
"isRHD": False,
"visionPolicyState": {
"awarenessPercent": 100,
"isDistracted": False,
"faceDetected": True,
},
"wheeltouchPolicyState": {
"awarenessPercent": 100,
},
}
self.pm.send('driverMonitoringState', dat)