IQ.Pilot Release Commit @ 773dae9

This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-27 18:08:12 -05:00
parent 15c14e1369
commit 1a98a22c7f
74 changed files with 705 additions and 1286 deletions

View File

@@ -21,6 +21,10 @@ SLC can now also raise your cruise speed automatically when the speed limit incr
IQ.Pilot now detects upcoming speed cameras, red light cameras, and ALPR/surveillance cameras (including Flock Safety cameras) sourced from OpenStreetMap's and alerts you before you reach them. Each camera type has its own toggle so you can pick what you want to be warned about. Speed cameras can also trigger a speed reduction to the limit when detected if enabled. Camera data is sourced from OSM and is updated periodically. IQ.Pilot now detects upcoming speed cameras, red light cameras, and ALPR/surveillance cameras (including Flock Safety cameras) sourced from OpenStreetMap's and alerts you before you reach them. Each camera type has its own toggle so you can pick what you want to be warned about. Speed cameras can also trigger a speed reduction to the limit when detected if enabled. Camera data is sourced from OSM and is updated periodically.
**Direct Flock / ALPR Camera Detection (Bluetooth & WiFi)**
Beyond map data, IQ.Pilot can now spot Flock Safety and similar ALPR cameras directly over the air by their Bluetooth and WiFi signatures as you approach them. Because it's sensing the actual hardware rather than relying on a map, this works anywhere, including fully offline and even for cameras that haven't been mapped yet, so you get a heads-up the moment one is nearby. When IQ.Pilot picks up a camera directly, that live detection takes priority over map data, so you see a single clear "Flock Camera Detected" alert instead of a duplicate. It shares the same Flock camera alert toggle, runs quietly in the background only when that's enabled, and is built to stay out of the way of your Bluetooth (phone link, game controllers) and WiFi connections.
**IQ.Dynamic and Driving Behavior** **IQ.Dynamic and Driving Behavior**
In IQ.Dynamic blended mode, when IQ.Pilot sees a stop light ahead, and the model agrees you need to stop, and there's no lead car to track, it will now commit (force) to stopping on its own, no lead car required. Gas pedal overrides it instantly. The stop prediction horizon is adjustable in IQ.Dynamic settings. Behavior for curves, low-speed driving, stopped leads, speed-limit fallbacks, and vision-based stops is now configurable. On-device IQ.Dynamic tuning is accessible by double-tapping IQ.Dynamic in longitudinal mode selection. In IQ.Dynamic blended mode, when IQ.Pilot sees a stop light ahead, and the model agrees you need to stop, and there's no lead car to track, it will now commit (force) to stopping on its own, no lead car required. Gas pedal overrides it instantly. The stop prediction horizon is adjustable in IQ.Dynamic settings. Behavior for curves, low-speed driving, stopped leads, speed-limit fallbacks, and vision-based stops is now configurable. On-device IQ.Dynamic tuning is accessible by double-tapping IQ.Dynamic in longitudinal mode selection.

View File

@@ -16,13 +16,13 @@
}, },
"python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": { "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": {
"mode": 493, "mode": 493,
"sha256": "77a601a12a498f09f01c4b1618e164638d5d9702913b6d5bb30a3c729a1c7d54", "sha256": "16990383c7ba1d2f3d33e5e4c1746019101bc79bed867721b51de853a68002ed",
"size": 401688 "size": 401688
}, },
"python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": { "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": {
"mode": 493, "mode": 493,
"sha256": "ca91f99d94c8bb7842332a69896d4b3a31c109c027426d1d7386876f11710d80", "sha256": "bd060a8527d21501ce866f24d055f1c8bb03209b2730eb745b786780aace0aae",
"size": 69696 "size": 69688
}, },
"runtime": { "runtime": {
"entries": { "entries": {
@@ -37,7 +37,7 @@
} }
}, },
"signatures": { "signatures": {
"python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": "4KY001fMJhdkiQNoUXbvtOKQ2nWocmg9ZL0umWnDjAztueIstXBoc1iqW83plm2Xq0jR9NLk1Jd4yl3AydWFBA==", "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": "IVz01J4HpdUHDrjPP9yB+9ksGFberB9MeFiCNLcv8hMzXCLIjIATl3JXTwpBqU8Kad/q683nkhgL5IJ3tCKdCw==",
"python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": "JqSaxsDJT9q0UYgVJUo9Pqm0J0Aqwy5gIa2C8n9/ioOg/fV6PLn2KiVGn6470U4F5oNaaOgAmm+AM9jwYrZXAA==" "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": "/aW/StEMRpge+zzihQDK7VPLmKhgwCucX8JielvPnPTud/mk0pmCy5pE8fB+TbwtWZCBfVhEd1bidRgsBqZfBA=="
} }
} }

View File

@@ -315,6 +315,9 @@ struct IQOnroadEvent @0xf4621d3ee9233bc9 {
# construction zone assist # construction zone assist
constructionZoneDetected @30; constructionZoneDetected @30;
# model management
modelUpdating @31;
} }
} }

View File

@@ -70,12 +70,12 @@ struct OnroadEvent @0xc4fa6047f024e718 {
longitudinalManeuver @30; longitudinalManeuver @30;
steerTempUnavailableSilent @31; steerTempUnavailableSilent @31;
resumeRequired @32; resumeRequired @32;
driverDistracted1 @33; preDriverDistracted @33;
driverDistracted2 @34; promptDriverDistracted @34;
driverDistracted3 @35; driverDistracted @35;
driverUnresponsive1 @36; preDriverUnresponsive @36;
driverUnresponsive2 @37; promptDriverUnresponsive @37;
driverUnresponsive3 @38; driverUnresponsive @38;
belowSteerSpeed @39; belowSteerSpeed @39;
lowBattery @40; lowBattery @40;
accFaulted @41; accFaulted @41;
@@ -2183,7 +2183,6 @@ struct DriverStateV2 {
rightBlinkProb @8 :Float32; rightBlinkProb @8 :Float32;
sunglassesProb @9 :Float32; sunglassesProb @9 :Float32;
phoneProb @13 :Float32; phoneProb @13 :Float32;
sleepProb @14 :Float32;
notReadyProbDEPRECATED @12 :List(Float32); notReadyProbDEPRECATED @12 :List(Float32);
occludedProbDEPRECATED @10 :Float32; occludedProbDEPRECATED @10 :Float32;
readyProbDEPRECATED @11 :List(Float32); readyProbDEPRECATED @11 :List(Float32);
@@ -2225,7 +2224,7 @@ struct DriverStateDEPRECATED @0xb83c6cc593ed0a00 {
stdDEPRECATED @2 :Float32; stdDEPRECATED @2 :Float32;
} }
struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 { struct DriverMonitoringState @0xb83cda094a1da284 {
events @18 :List(OnroadEvent); events @18 :List(OnroadEvent);
faceDetected @1 :Bool; faceDetected @1 :Bool;
isDistracted @2 :Bool; isDistracted @2 :Bool;
@@ -2251,81 +2250,6 @@ struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 {
eventsDEPRECATED @0 :List(Car.OnroadEventDEPRECATED); eventsDEPRECATED @0 :List(Car.OnroadEventDEPRECATED);
} }
struct DriverMonitoringState {
lockout @0 :Bool;
lockoutCount @15 :Int8;
lockoutMinutesRemaining @11 :Int8;
alert3Count @12 :Int8;
noResponseCount @13 :Int8;
noResponseForceDecel @14 :Bool;
alwaysOn @3 :Bool;
alwaysOnLockout @4 :Bool;
alertLevel @5 :AlertLevel;
activePolicy @6 :MonitoringPolicy;
isRHD @7 :Bool;
rhdCalibration @8 :CalibrationState;
visionPolicyState @9 :VisionPolicyState;
wheeltouchPolicyState @10 :WheeltouchPolicyState;
enum AlertLevel {
none @0;
one @1;
two @2;
three @3;
}
enum MonitoringPolicy {
wheeltouch @0;
vision @1;
}
struct VisionPolicyState {
awarenessPercent @0 :Int8;
awarenessStep @1 :Float32;
isDistracted @2 :Bool;
distractedTypes @3 :DistractedTypes;
faceDetected @4 :Bool;
pose @5 :Pose;
wheeltouchFallbackPercent @6 :Int8;
uncertainOffroadAlertPercent @7 :Int8;
struct DistractedTypes {
pose @0: Bool;
eye @1: Bool;
phone @2: Bool;
}
struct Pose {
pitch @0 :Float32;
yaw @1 :Float32;
pitchCalib @2 :CalibrationState;
yawCalib @3 :CalibrationState;
calibrated @4 :Bool;
uncertainty @5 :Float32;
}
}
struct WheeltouchPolicyState {
awarenessPercent @0 :Int8;
awarenessStep @1 :Float32;
driverInteracting @2 :Bool;
}
struct CalibrationState {
calibratedPercent @0 :Int8;
offset @1 :Float32;
}
deprecated :group {
alertCountLockoutPercent @1 :Int8;
alertTimeLockoutPercent @2 :Int8;
}
}
struct Boot { struct Boot {
wallTimeNanos @0 :UInt64; wallTimeNanos @0 :UInt64;
pstore @4 :Map(Text, Data); pstore @4 :Map(Text, Data);
@@ -2646,7 +2570,7 @@ struct Event {
thumbnail @66: Thumbnail; thumbnail @66: Thumbnail;
onroadEvents @134: List(OnroadEvent); onroadEvents @134: List(OnroadEvent);
carParams @69: Car.CarParams; carParams @69: Car.CarParams;
driverMonitoringState @165: DriverMonitoringState; driverMonitoringState @71: DriverMonitoringState;
livePose @129 :LivePose; livePose @129 :LivePose;
modelV2 @75 :ModelDataV2; modelV2 @75 :ModelDataV2;
drivingModelData @128 :DrivingModelData; drivingModelData @128 :DrivingModelData;
@@ -2769,7 +2693,6 @@ struct Event {
wifiScanDEPRECATED @29 :List(Legacy.WifiScan); wifiScanDEPRECATED @29 :List(Legacy.WifiScan);
uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent; uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent;
liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED; liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED;
driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED;
gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints; gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints;
gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan; gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan;
applanixRawDEPRECATED @42 :Data; applanixRawDEPRECATED @42 :Data;

View File

@@ -41,7 +41,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}}, {"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}},
{"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}}, {"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}},
{"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}}, {"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}},
{"DriverLockoutCount", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, INT, "0"}},
{"AlphaLongitudinalEnabled", {PERSISTENT, BOOL}}, {"AlphaLongitudinalEnabled", {PERSISTENT, BOOL}},
{"ExperimentalMode", {PERSISTENT, BOOL}}, {"ExperimentalMode", {PERSISTENT, BOOL}},
{"ExperimentalModeConfirmed", {PERSISTENT, BOOL}}, {"ExperimentalModeConfirmed", {PERSISTENT, BOOL}},
@@ -132,7 +131,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"SnoozeUpdate", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, {"SnoozeUpdate", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
{"SshEnabled", {PERSISTENT, BOOL}}, {"SshEnabled", {PERSISTENT, BOOL}},
{"TermsVersion", {PERSISTENT, STRING}}, {"TermsVersion", {PERSISTENT, STRING}},
{"TorqueBar", {PERSISTENT, BOOL, "0"}}, {"IQSteerEffortArc", {PERSISTENT, BOOL, "0"}},
{"TrainingVersion", {PERSISTENT, STRING}}, {"TrainingVersion", {PERSISTENT, STRING}},
{"UbloxAvailable", {PERSISTENT, BOOL}}, {"UbloxAvailable", {PERSISTENT, BOOL}},
{"UsbStorageEnabled", {PERSISTENT, BOOL}}, {"UsbStorageEnabled", {PERSISTENT, BOOL}},
@@ -167,7 +166,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"CarPlatformBundle", {PERSISTENT, JSON}}, {"CarPlatformBundle", {PERSISTENT, JSON}},
{"Konn3ktVwOdometers", {PERSISTENT, JSON}}, {"Konn3ktVwOdometers", {PERSISTENT, JSON}},
{"Konn3ktVehicleOdometers", {PERSISTENT, JSON}}, {"Konn3ktVehicleOdometers", {PERSISTENT, JSON}},
{"ChevronInfo", {PERSISTENT, INT, "4"}}, {"IQLeadReadouts", {PERSISTENT, INT, "4"}},
{"DeviceBootMode", {PERSISTENT, INT, "0"}}, {"DeviceBootMode", {PERSISTENT, INT, "0"}},
{"IQDevUIInfo", {PERSISTENT, INT, "0"}}, {"IQDevUIInfo", {PERSISTENT, INT, "0"}},
{"EnableEsimProvisioning", {PERSISTENT, BOOL, "1"}}, {"EnableEsimProvisioning", {PERSISTENT, BOOL, "1"}},
@@ -201,9 +200,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"OfflineTilesBaseUrl", {PERSISTENT, STRING}}, {"OfflineTilesBaseUrl", {PERSISTENT, STRING}},
{"OnroadUploads", {PERSISTENT, BOOL, "1"}}, {"OnroadUploads", {PERSISTENT, BOOL, "1"}},
{"IQAlertSilence", {PERSISTENT, BOOL, "0"}}, {"IQAlertSilence", {PERSISTENT, BOOL, "0"}},
{"RainbowMode", {PERSISTENT, BOOL, "0"}}, {"IQAccelMeter", {PERSISTENT, BOOL, "0"}},
{"RocketFuel", {PERSISTENT, BOOL, "0"}}, {"IQBlinkerIndicators", {PERSISTENT, BOOL, "0"}},
{"ShowTurnSignals", {PERSISTENT, BOOL, "0"}},
{"StandstillTimer", {PERSISTENT, BOOL, "0"}}, {"StandstillTimer", {PERSISTENT, BOOL, "0"}},
// AOL (Always On Lateral) params // AOL (Always On Lateral) params
{"AolEnabled", {PERSISTENT, BOOL, "1"}}, {"AolEnabled", {PERSISTENT, BOOL, "1"}},
@@ -215,7 +213,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ModelManager_ActiveBundle", {PERSISTENT, JSON}}, {"ModelManager_ActiveBundle", {PERSISTENT, JSON}},
{"ModelManager_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}}, {"ModelManager_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}},
{"ModelManager_DownloadIndex", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, INT, "-1"}}, {"ModelManager_DownloadIndex", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, INT, "-1"}},
{"ModelManager_Favs", {PERSISTENT, STRING}}, {"IQModelFavorites", {PERSISTENT, STRING}},
{"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}}, {"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
{"ModelManager_ModelsCache", {PERSISTENT, JSON}}, {"ModelManager_ModelsCache", {PERSISTENT, JSON}},
@@ -227,7 +225,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"BackupManager_RestoreVersion", {PERSISTENT, STRING}}, {"BackupManager_RestoreVersion", {PERSISTENT, STRING}},
// iqpilot car specific params // iqpilot car specific params
{"HyundaiLongitudinalTuning", {PERSISTENT, INT, "0"}}, {"IQHyundaiLongTune", {PERSISTENT, INT, "0"}},
{"AutoCruiseControl", {PERSISTENT, INT, "0"}}, {"AutoCruiseControl", {PERSISTENT, INT, "0"}},
{"AutoEngage", {PERSISTENT, INT, "0"}}, {"AutoEngage", {PERSISTENT, INT, "0"}},
{"CanfdDebug", {PERSISTENT, INT, "0"}}, {"CanfdDebug", {PERSISTENT, INT, "0"}},
@@ -254,10 +252,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"LongitudinalPersonalityMax", {PERSISTENT, INT, "3"}}, {"LongitudinalPersonalityMax", {PERSISTENT, INT, "3"}},
{"MaxAngleFrames", {PERSISTENT, INT, "89"}}, {"MaxAngleFrames", {PERSISTENT, INT, "89"}},
{"SpeedFromPCM", {PERSISTENT, INT, "2"}}, {"SpeedFromPCM", {PERSISTENT, INT, "2"}},
{"SubaruStopAndGo", {PERSISTENT, BOOL, "0"}}, {"IQSubaruCreepAssist", {PERSISTENT, BOOL, "0"}},
{"SubaruStopAndGoManualParkingBrake", {PERSISTENT, BOOL, "0"}}, {"IQSubaruCreepAssistManualBrake", {PERSISTENT, BOOL, "0"}},
{"TeslaCoopSteering", {PERSISTENT, BOOL, "0"}}, {"IQTeslaTorqueBlend", {PERSISTENT, BOOL, "0"}},
{"ToyotaEnforceStockLongitudinal", {PERSISTENT, BOOL, "0"}}, {"IQToyotaFactoryLong", {PERSISTENT, BOOL, "0"}},
{"VwPqEpsPatched", {PERSISTENT, BOOL}}, {"VwPqEpsPatched", {PERSISTENT, BOOL}},
{"ToyotaSnGHack", {PERSISTENT, BOOL, "0"}}, {"ToyotaSnGHack", {PERSISTENT, BOOL, "0"}},
{"pqhca5or7Toggle", {PERSISTENT, BOOL}}, {"pqhca5or7Toggle", {PERSISTENT, BOOL}},
@@ -278,7 +276,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"IQDynamicMinimumForceStopLength", {PERSISTENT, FLOAT, "0.0"}}, {"IQDynamicMinimumForceStopLength", {PERSISTENT, FLOAT, "0.0"}},
{"IQForceStops", {PERSISTENT, BOOL, "1"}}, {"IQForceStops", {PERSISTENT, BOOL, "1"}},
{"IQCustomStopDistance", {PERSISTENT, INT, "0"}}, // meters, -2..2; negative = stop closer, positive = stop further back; independent of IQForceStops {"IQCustomStopDistance", {PERSISTENT, INT, "0"}}, // meters, -2..2; negative = stop closer, positive = stop further back; independent of IQForceStops
{"BlindSpot", {PERSISTENT, BOOL, "0"}}, {"IQBlindSpotAlerts", {PERSISTENT, BOOL, "0"}},
{"IQExpandedStatus", {PERSISTENT, BOOL, "0"}}, {"IQExpandedStatus", {PERSISTENT, BOOL, "0"}},
{"HomePanelWidget", {PERSISTENT, STRING, "changelog"}}, {"HomePanelWidget", {PERSISTENT, STRING, "changelog"}},
@@ -342,7 +340,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"OsmStateNames", {PERSISTENT, JSON}}, {"OsmStateNames", {PERSISTENT, JSON}},
{"OsmWayTest", {PERSISTENT, STRING}}, {"OsmWayTest", {PERSISTENT, STRING}},
{"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
{"RoadNameToggle", {PERSISTENT, BOOL, "0"}}, {"IQRoadNameOverlay", {PERSISTENT, BOOL, "0"}},
{"IQSpeedAssistMode", {PERSISTENT, INT, "1"}}, {"IQSpeedAssistMode", {PERSISTENT, INT, "1"}},
{"IQSpeedAssistOffsetType", {PERSISTENT, INT, "0"}}, {"IQSpeedAssistOffsetType", {PERSISTENT, INT, "0"}},
{"IQSpeedAssistPolicy", {PERSISTENT, INT, "3"}}, {"IQSpeedAssistPolicy", {PERSISTENT, INT, "3"}},

View File

@@ -6,7 +6,7 @@ from iqdbc.car.interfaces import CarControllerBase
from iqdbc.car.subaru import subarucan from iqdbc.car.subaru import subarucan
from iqdbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags from iqdbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
from iqdbc.lvbs.car.subaru.stop_and_go import IQStopAndGoController from iqdbc.lvbs.car.subaru.creep_assist import IQStopAndGoController
# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
# involves the total steering angle change rather than rate, but these limits work well for now # involves the total steering angle change rather than rate, but these limits work well for now
@@ -139,7 +139,7 @@ class CarController(CarControllerBase, IQStopAndGoController):
if self.frame % 2 == 0: if self.frame % 2 == 0:
can_sends.append(subarucan.create_es_static_2(self.packer)) can_sends.append(subarucan.create_es_static_2(self.packer))
can_sends.extend(IQStopAndGoController.create_stop_and_go(self, self.packer, CC, CS, self.frame)) can_sends.extend(IQStopAndGoController.create_creep_assist(self, self.packer, CC, CS, self.frame))
new_actuators = actuators.as_builder() new_actuators = actuators.as_builder()
new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX

View File

@@ -7,7 +7,7 @@ from iqdbc.car.subaru.values import DBC, CanBus, SubaruFlags
from iqdbc.car import CanSignalRateCalculator from iqdbc.car import CanSignalRateCalculator
from iqdbc.lvbs.car.subaru.aol import AolCarState from iqdbc.lvbs.car.subaru.aol import AolCarState
from iqdbc.lvbs.car.subaru.stop_and_go import IQStopAndGoState from iqdbc.lvbs.car.subaru.creep_assist import IQStopAndGoState
class CarState(CarStateBase, AolCarState, IQStopAndGoState): class CarState(CarStateBase, AolCarState, IQStopAndGoState):

View File

@@ -8,7 +8,7 @@ from iqdbc.car.tesla.teslacan import TeslaCAN
from iqdbc.car.tesla.values import CarControllerParams from iqdbc.car.tesla.values import CarControllerParams
from iqdbc.car.vehicle_model import VehicleModel from iqdbc.car.vehicle_model import VehicleModel
from openpilot.iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import get_set_speed_kph_from_params from openpilot.iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import get_set_speed_kph_from_params
from iqdbc.lvbs.car.tesla.coop_steering import CoopSteeringCarController from iqdbc.lvbs.car.tesla.torque_blend import TorqueBlendController
from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
@@ -22,7 +22,7 @@ def get_safety_CP():
class CarController(CarControllerBase): class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_IQ): def __init__(self, dbc_names, CP, CP_IQ):
CarControllerBase.__init__(self, dbc_names, CP, CP_IQ) CarControllerBase.__init__(self, dbc_names, CP, CP_IQ)
self.coop_steer = CoopSteeringCarController() self.coop_steer = TorqueBlendController()
self.apply_angle_last = 0 self.apply_angle_last = 0
self.packer = CANPacker(dbc_names[Bus.party]) self.packer = CANPacker(dbc_names[Bus.party])
self.tesla_can = TeslaCAN(CP, self.packer) self.tesla_can = TeslaCAN(CP, self.packer)
@@ -99,7 +99,7 @@ class CarController(CarControllerBase):
# TODO: HUD control # TODO: HUD control
new_actuators = actuators.as_builder() new_actuators = actuators.as_builder()
new_actuators.steeringAngleDeg = self.apply_angle_last new_actuators.steeringAngleDeg = self.apply_angle_last
new_actuators.accel = self.coop_steer.coop_apply_angle_last_sat # debug new_actuators.accel = self.coop_steer.blend_apply_angle_last_sat # debug
new_actuators.curvature = float(self.coop_steer.debug_angle_desired_limited) # debug new_actuators.curvature = float(self.coop_steer.debug_angle_desired_limited) # debug
new_actuators.torque = float(self.coop_steer.override_angle_accu) # debug new_actuators.torque = float(self.coop_steer.override_angle_accu) # debug

View File

@@ -10,7 +10,7 @@ class TeslaCAN:
self.l_jerk = 0.0 self.l_jerk = 0.0
def create_steering_control(self, angle, enabled, control_type): def create_steering_control(self, angle, enabled, control_type):
# control_type comes from coop_steering: ANGLE_CONTROL (1) normally, LANE_KEEP_ASSIST (2) when cooperative steering is enabled # control_type comes from torque_blend: ANGLE_CONTROL (1) normally, LANE_KEEP_ASSIST (2) when cooperative steering is enabled
control_type = control_type if enabled else 0 control_type = control_type if enabled else 0
if self.CP.flags & TeslaFlags.LEGACY_DAS_STEERING: if self.CP.flags & TeslaFlags.LEGACY_DAS_STEERING:
control_type <<= 1 # legacy firmware uses a 2-bit field, one bit up from the 3-bit signal control_type <<= 1 # legacy firmware uses a 2-bit field, one bit up from the 3-bit signal

View File

@@ -5,7 +5,7 @@ from iqdbc.car.docs import get_all_footnotes, get_params_for_docs
from iqdbc.car.values import PLATFORMS from iqdbc.car.values import PLATFORMS
def get_car_list() -> dict[str, dict[str, list[str] | str]]: def build_car_catalog() -> dict[str, dict[str, list[str] | str]]:
collected_footnote = get_all_footnotes() collected_footnote = get_all_footnotes()
sorted_list: dict[str, dict[str, list[str] | str]] = collect_car_docs(PLATFORMS, collected_footnote) sorted_list: dict[str, dict[str, list[str] | str]] = collect_car_docs(PLATFORMS, collected_footnote)
return sorted_list return sorted_list
@@ -57,6 +57,6 @@ def collect_car_docs(platforms, footnotes) -> dict[str, dict[str, list[str] | st
if __name__ == "__main__": if __name__ == "__main__":
# get_car_list() is the raw platform source; the shipped catalog is generated # build_car_catalog() is the raw platform source; the shipped catalog is generated
# (and encoded to its on-disk envelope) by the main-repo entry point: # (and encoded to its on-disk envelope) by the main-repo entry point:
print("run: python -m openpilot.iqpilot.selfdrive.car.vehicle_catalog") print("run: python -m openpilot.iqpilot.selfdrive.car.vehicle_catalog")

View File

@@ -75,43 +75,43 @@ def apply_iq_car_config(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
params_dict = {k: v for param in params_list for k, v in param.items()} params_dict = {k: v for param in params_list for k, v in param.items()}
_initialize_custom_longitudinal_tuning(CI, CP, CP_IQ, params_dict) _apply_long_tuning(CI, CP, CP_IQ, params_dict)
_initialize_coop_steering(CP, CP_IQ, params_dict) _apply_torque_blend(CP, CP_IQ, params_dict)
_initialize_stop_and_go(CP, CP_IQ, params_dict) _apply_creep_assist(CP, CP_IQ, params_dict)
_initialize_toyota(CP, CP_IQ, params_dict) _apply_toyota_options(CP, CP_IQ, params_dict)
def _initialize_custom_longitudinal_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams, def _apply_long_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
params_dict: dict[str, str]) -> None: params_dict: dict[str, str]) -> None:
_ = CI.get_longitudinal_tuning_iq(CP, CP_IQ) _ = CI.get_longitudinal_tuning_iq(CP, CP_IQ)
def _initialize_coop_steering(CP: structs.CarParams, CP_IQ: structs.IQCarParams, def _apply_torque_blend(CP: structs.CarParams, CP_IQ: structs.IQCarParams,
params_dict: dict[str, str]) -> None: params_dict: dict[str, str]) -> None:
if CP.brand == 'tesla': if CP.brand == 'tesla':
coop_steering = int(params_dict.get("TeslaCoopSteering", 0)) == 1 torque_blend = int(params_dict.get("IQTeslaTorqueBlend", 0)) == 1
if coop_steering: if torque_blend:
CP_IQ.flags |= TeslaFlagsIQ.COOP_STEERING.value CP_IQ.flags |= TeslaFlagsIQ.COOP_STEERING.value
def _initialize_stop_and_go(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None: def _apply_creep_assist(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None:
# Subaru stop-and-go; unsupported on gen2-global and hybrid platforms. # Subaru stop-and-go; unsupported on gen2-global and hybrid platforms.
if CP.brand != 'subaru' or CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID): if CP.brand != 'subaru' or CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID):
return return
if int(params_dict.get("SubaruStopAndGo", 0)) == 1: if int(params_dict.get("IQSubaruCreepAssist", 0)) == 1:
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO.value CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO.value
if int(params_dict.get("SubaruStopAndGoManualParkingBrake", 0)) == 1: if int(params_dict.get("IQSubaruCreepAssistManualBrake", 0)) == 1:
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE.value CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE.value
if CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE): if CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE):
CP_IQ.iqSafetyFlags |= SubaruSafetyFlagsIQ.STOP_AND_GO CP_IQ.iqSafetyFlags |= SubaruSafetyFlagsIQ.STOP_AND_GO
def _initialize_toyota(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None: def _apply_toyota_options(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None:
if CP.brand == 'toyota': if CP.brand == 'toyota':
toyota_stock_long = int(params_dict.get("ToyotaEnforceStockLongitudinal", 0)) == 1 toyota_stock_long = int(params_dict.get("IQToyotaFactoryLong", 0)) == 1
toyota_sng_hack = int(params_dict.get("ToyotaSnGHack", 0)) == 1 toyota_sng_hack = int(params_dict.get("ToyotaSnGHack", 0)) == 1
if toyota_stock_long: if toyota_stock_long:

View File

@@ -68,7 +68,7 @@ class IQStopAndGoController:
return held_long_enough return held_long_enough
return self._epb_pulse(standing and lead_pulling_away) return self._epb_pulse(standing and lead_pulling_away)
def create_stop_and_go(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]: def create_creep_assist(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]:
if not self.enabled: if not self.enabled:
return [] return []

View File

@@ -15,7 +15,7 @@ from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
DT_LAT_CTRL = DT_CTRL * CarControllerParams.STEER_STEP DT_LAT_CTRL = DT_CTRL * CarControllerParams.STEER_STEP
class CoopSteeringCarControllerParams(CarControllerParams): class TorqueBlendParams(CarControllerParams):
ANGLE_LIMITS = replace(CarControllerParams.ANGLE_LIMITS, MAX_ANGLE_RATE=5) ANGLE_LIMITS = replace(CarControllerParams.ANGLE_LIMITS, MAX_ANGLE_RATE=5)
STEERING_DEG_PHASE_LEAD_COEFF = 8.0 STEERING_DEG_PHASE_LEAD_COEFF = 8.0
@@ -28,7 +28,7 @@ STEER_OVERRIDE_LAT_ACCEL_GAIN_LIMIT = 10 # deg/Nm stability and smoothness for a
# angle ramping # angle ramping
STEER_OVERRIDE_MAX_LAT_JERK = 2.0 # m/s^3 - determines angle ramping rate - speed dependent STEER_OVERRIDE_MAX_LAT_JERK = 2.0 # m/s^3 - determines angle ramping rate - speed dependent
STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_LATERAL_JERK # m/s^3 - for low speed angle ramp down STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = TorqueBlendParams.ANGLE_LIMITS.MAX_LATERAL_JERK # m/s^3 - for low speed angle ramp down
# stability and smoothness for angle ramp control - at very low speeds this takes precedence over jerk settings # stability and smoothness for angle ramp control - at very low speeds this takes precedence over jerk settings
STEER_OVERRIDE_LAT_JERK_GAIN_LIMIT = 100 # deg/s/Nm - should be less than CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE / DT_CTRL / STEER_OVERRIDE_TORQUE_RANGE STEER_OVERRIDE_LAT_JERK_GAIN_LIMIT = 100 # deg/s/Nm - should be less than CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE / DT_CTRL / STEER_OVERRIDE_TORQUE_RANGE
STEER_OVERRIDE_TORQUE_RANGE = STEER_OVERRIDE_MAX_TORQUE - STEER_OVERRIDE_MIN_TORQUE STEER_OVERRIDE_TORQUE_RANGE = STEER_OVERRIDE_MAX_TORQUE - STEER_OVERRIDE_MIN_TORQUE
@@ -42,7 +42,7 @@ STEER_DESIRED_LIMITER_OVERRIDE_ACTIVE_COUNTER = 0.7 # second
STEER_RESUME_RATE_LIMIT_RAMP_RATE = 500 # deg/s^2 - controls rate of rise of angle rate limit, not angle directly STEER_RESUME_RATE_LIMIT_RAMP_RATE = 500 # deg/s^2 - controls rate of rise of angle rate limit, not angle directly
CoopSteeringDataIQ = namedtuple("CoopSteeringDataIQ", TorqueBlendDataIQ = namedtuple("TorqueBlendDataIQ",
["steeringAngleDeg", "lat_active", "control_type"]) ["steeringAngleDeg", "lat_active", "control_type"])
def get_steer_from_lat_accel(lat_accel, v_ego: float, VM: VehicleModel): def get_steer_from_lat_accel(lat_accel, v_ego: float, VM: VehicleModel):
@@ -84,7 +84,7 @@ def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleMod
""" """
# prevents windup in carcontroller rate limiter # prevents windup in carcontroller rate limiter
lat_jerk = min(lat_jerk, CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_LATERAL_JERK) lat_jerk = min(lat_jerk, TorqueBlendParams.ANGLE_LIMITS.MAX_LATERAL_JERK)
# lateral accel is linear in respect to angle so it's fine to interpolate it with torque # lateral accel is linear in respect to angle so it's fine to interpolate it with torque
torque_to_angle = get_steer_from_lat_accel(lat_jerk, vEgo, VM) / STEER_OVERRIDE_TORQUE_RANGE torque_to_angle = get_steer_from_lat_accel(lat_jerk, vEgo, VM) / STEER_OVERRIDE_TORQUE_RANGE
@@ -93,7 +93,7 @@ def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleMod
override_angle_rate = torque * min(torque_to_angle, gain_limit) override_angle_rate = torque * min(torque_to_angle, gain_limit)
# prevent windup in angle rate limiter # prevent windup in angle rate limiter
return apply_bounds(override_angle_rate * DT_LAT_CTRL, CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE) return apply_bounds(override_angle_rate * DT_LAT_CTRL, TorqueBlendParams.ANGLE_LIMITS.MAX_ANGLE_RATE)
class SteerRateLimiter: class SteerRateLimiter:
@@ -160,10 +160,10 @@ class SteerAccelLimiter:
return angle_out return angle_out
class CoopSteeringCarController: class TorqueBlendController:
def __init__(self): def __init__(self):
self.coop_apply_angle_last = 0 self.coop_apply_angle_last = 0
self.coop_apply_angle_last_sat = 0 self.blend_apply_angle_last_sat = 0
self.override_angle_accu = 0 self.override_angle_accu = 0
self.override_active_counter = 0 # Counter for how many cycles torque is below threshold self.override_active_counter = 0 # Counter for how many cycles torque is below threshold
self.resume_rate_limiter_delta = SteerRateLimiter() self.resume_rate_limiter_delta = SteerRateLimiter()
@@ -201,7 +201,7 @@ class CoopSteeringCarController:
return 0 return 0
# unwind accumulator toward zero if the previous loop saturated (apply_steer_angle_limits_vm) # unwind accumulator toward zero if the previous loop saturated (apply_steer_angle_limits_vm)
unwind = (self.coop_apply_angle_last - self.coop_apply_angle_last_sat) * unwind_weight unwind = (self.coop_apply_angle_last - self.blend_apply_angle_last_sat) * unwind_weight
if self.override_angle_accu * unwind > 0: if self.override_angle_accu * unwind > 0:
unwind = apply_bounds(unwind, abs(self.override_angle_accu)) unwind = apply_bounds(unwind, abs(self.override_angle_accu))
self.override_angle_accu -= unwind self.override_angle_accu -= unwind
@@ -289,7 +289,7 @@ class CoopSteeringCarController:
apply_angle_lim = self.resume_rate_limiter.update(apply_angle, angle_rate_delta_lim) apply_angle_lim = self.resume_rate_limiter.update(apply_angle, angle_rate_delta_lim)
return apply_angle_lim return apply_angle_lim
def update(self, apply_angle, lat_active, CP_IQ: structs.IQCarParams, CS: structs.CarState, VM: VehicleModel) -> CoopSteeringDataIQ: def update(self, apply_angle, lat_active, CP_IQ: structs.IQCarParams, CS: structs.CarState, VM: VehicleModel) -> TorqueBlendDataIQ:
# estimate real steering angle by adding rate to the tesla filtered angle # estimate real steering angle by adding rate to the tesla filtered angle
steeringAngleDegPhaseLead = CS.out.steeringAngleDeg + CS.out.steeringRateDeg / STEERING_DEG_PHASE_LEAD_COEFF steeringAngleDegPhaseLead = CS.out.steeringAngleDeg + CS.out.steeringRateDeg / STEERING_DEG_PHASE_LEAD_COEFF
@@ -306,7 +306,7 @@ class CoopSteeringCarController:
# final rate limit - matching panda safety # final rate limit - matching panda safety
self.coop_apply_angle_last = apply_angle self.coop_apply_angle_last = apply_angle
self.coop_apply_angle_last_sat = apply_steer_angle_limits_vm(apply_angle, self.coop_apply_angle_last_sat, CS.out.vEgoRaw, self.blend_apply_angle_last_sat = apply_steer_angle_limits_vm(apply_angle, self.blend_apply_angle_last_sat, CS.out.vEgoRaw,
CS.out.steeringAngleDeg, lat_active, CoopSteeringCarControllerParams, VM) CS.out.steeringAngleDeg, lat_active, TorqueBlendParams, VM)
return CoopSteeringDataIQ(self.coop_apply_angle_last_sat, lat_active, 1) # 1 = angle control return TorqueBlendDataIQ(self.blend_apply_angle_last_sat, lat_active, 1) # 1 = angle control

View File

@@ -2,7 +2,7 @@ import json
import os import os
from iqdbc.car.common.basedir import BASEDIR from iqdbc.car.common.basedir import BASEDIR
from iqdbc.lvbs.car.platform_list import get_car_list from iqdbc.lvbs.car.car_catalog import build_car_catalog
CATALOG_JSON = os.path.join(BASEDIR, "..", "..", "iqpilot", "selfdrive", "car", "vehicle_catalog.json") CATALOG_JSON = os.path.join(BASEDIR, "..", "..", "iqpilot", "selfdrive", "car", "vehicle_catalog.json")
@@ -18,7 +18,7 @@ def _decode(envelope) -> dict:
class TestCarList: class TestCarList:
def test_generator(self): def test_generator(self):
generated = get_car_list() generated = build_car_catalog()
with open(CATALOG_JSON) as f: with open(CATALOG_JSON) as f:
shipped = _decode(json.load(f)) shipped = _decode(json.load(f))

View File

@@ -214,10 +214,9 @@ class NoEntryCard(AlertCard):
def __init__(self, def __init__(self,
alert_text_2: str, alert_text_2: str,
alert_text_1: str = "openpilot Unavailable", alert_text_1: str = "openpilot Unavailable",
visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none, visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none):
priority: Tier = Tier.LOW):
primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2) primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2)
super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0) super().__init__(primary, secondary, AlertStatus.normal, size, Tier.LOW, visual_alert, AudibleAlert.refuse, 3.0)
class GentleDisableCard(AlertCard): class GentleDisableCard(AlertCard):

View File

@@ -12,11 +12,11 @@ _ANGLE = _dbc.CarParams.SteerControlType.angle
# Port tunables surfaced to the fingerprint step, flat so the read is one pass. # Port tunables surfaced to the fingerprint step, flat so the read is one pass.
_TUNABLES = ( _TUNABLES = (
"HyundaiLongitudinalTuning", "IQHyundaiLongTune",
"SubaruStopAndGo", "IQSubaruCreepAssist",
"SubaruStopAndGoManualParkingBrake", "IQSubaruCreepAssistManualBrake",
"TeslaCoopSteering", "IQTeslaTorqueBlend",
"ToyotaEnforceStockLongitudinal", "IQToyotaFactoryLong",
"ToyotaSnGHack", "ToyotaSnGHack",
) )

View File

@@ -81,5 +81,5 @@ def _write(vehicles: dict[str, dict], basedir: str = BASEDIR) -> str:
if __name__ == "__main__": if __name__ == "__main__":
from iqdbc.lvbs.car.platform_list import get_car_list from iqdbc.lvbs.car.car_catalog import build_car_catalog
print("wrote", _write(get_car_list())) print("wrote", _write(build_car_catalog()))

View File

@@ -43,6 +43,7 @@ from openpilot.iqpilot.selfdrive.iqmodeld.models.helpers import (
bundle_files_ready, bundle_files_ready,
get_active_bundle, get_active_bundle,
get_runtime_bundle_upgrade, get_runtime_bundle_upgrade,
is_default_bundle,
persist_active_bundle, persist_active_bundle,
) )
@@ -56,6 +57,7 @@ class IQModelManager(_BaseIQModelManager):
def __init__(self): def __init__(self):
super().__init__() super().__init__()
self._validated_active_key: tuple[tuple[str, str], ...] | None = None self._validated_active_key: tuple[tuple[str, str], ...] | None = None
self._manifest_refresh_key: tuple[tuple[str, str], ...] | None = None
@staticmethod @staticmethod
def _bundle_index(bundle) -> int | None: def _bundle_index(bundle) -> int | None:
@@ -190,6 +192,58 @@ class IQModelManager(_BaseIQModelManager):
if bundle_index is not None and self._download_index() is None: if bundle_index is not None and self._download_index() is None:
self.params.put(_DOWNLOAD_INDEX_KEY, bundle_index) self.params.put(_DOWNLOAD_INDEX_KEY, bundle_index)
def _find_manifest_counterpart(self, target):
# never match by index: indexes shift between manifest generations, and a
# positional match could redownload a different model than the user selected
for attr in ("ref", "internalName", "displayName"):
value = getattr(target, attr, None)
if not value:
continue
for bundle in self.available_models:
if getattr(bundle, attr, None) == value:
return bundle
return None
def _queue_active_manifest_refresh(self) -> None:
active = self.active_bundle
if active is None or is_default_bundle(active):
return
if self._download_index() is not None:
return
counterpart = self._find_manifest_counterpart(active)
if counterpart is None:
return
counterpart_index = self._bundle_index(counterpart)
if counterpart_index is None:
return
active_files = dict(self._bundle_files(active))
stale = False
for filename, sha in self._bundle_files(counterpart):
if not sha:
continue
active_sha = active_files.get(filename)
# an empty recorded hash can't prove a mismatch, so it never triggers a redownload
if active_sha is None or (active_sha and active_sha.lower() != sha.lower()):
stale = True
break
if not stale:
self._manifest_refresh_key = None
return
# the manifest may be an expired offline cache, so keep the active bundle and its
# files in place: the download flow replaces artifacts atomically and only persists
# the counterpart as active once everything landed. One attempt per bundle per run
# so a dead network doesn't turn the 1Hz loop into a download-retry storm.
key = self._bundle_validation_key(active)
if key == self._manifest_refresh_key:
return
self._manifest_refresh_key = key
cloudlog.warning(f"Active model {_display_bundle_name(active)} artifacts are stale vs current manifest; queueing redownload")
self.params.put(_DOWNLOAD_INDEX_KEY, counterpart_index)
async def _download_file(self, url: str, path: str, model) -> None: async def _download_file(self, url: str, path: str, model) -> None:
temp_path = f"{path}.download" temp_path = f"{path}.download"
self._download_start_times[model.fileName] = time.monotonic() self._download_start_times[model.fileName] = time.monotonic()
@@ -302,6 +356,7 @@ class IQModelManager(_BaseIQModelManager):
self.active_bundle = get_active_bundle(self.params) self.active_bundle = get_active_bundle(self.params)
self._queue_active_redownload_if_invalid() self._queue_active_redownload_if_invalid()
self._queue_tinygrad_upgrade() self._queue_tinygrad_upgrade()
self._queue_active_manifest_refresh()
if (index_to_download := self._download_index()) is not None: if (index_to_download := self._download_index()) is not None:
if model_to_download := next((model for model in self.available_models if model.index == index_to_download), None): if model_to_download := next((model for model in self.available_models if model.index == index_to_download), None):

View File

@@ -0,0 +1,150 @@
"""
Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from dataclasses import dataclass, field
from openpilot.iqpilot.selfdrive.iqmodeld.models.manager import IQModelManager, _DOWNLOAD_INDEX_KEY
@dataclass
class _DownloadUri:
sha256: str = ""
uri: str = ""
@dataclass
class _Artifact:
fileName: str = ""
downloadUri: _DownloadUri = field(default_factory=_DownloadUri)
@dataclass
class _Model:
artifact: _Artifact = field(default_factory=_Artifact)
metadata: _Artifact | None = None
@dataclass
class _Bundle:
index: int = 0
ref: str = ""
internalName: str = ""
displayName: str = ""
models: list = field(default_factory=list)
class _FakeParams:
def __init__(self):
self.store = {}
def get(self, key):
return self.store.get(key)
def put(self, key, value):
self.store[key] = value
def remove(self, key):
self.store.pop(key, None)
def _bundle(index, name, sha, filename="driving_vision_test_tinygrad.pkl"):
return _Bundle(
index=index,
ref=f"ref-{name}",
internalName=name,
displayName=f"{name} display",
models=[_Model(artifact=_Artifact(fileName=filename, downloadUri=_DownloadUri(sha256=sha)))],
)
def _manager(active, available):
mgr = IQModelManager.__new__(IQModelManager)
mgr.params = _FakeParams()
mgr.active_bundle = active
mgr.available_models = available
mgr._validated_active_key = None
mgr._manifest_refresh_key = None
return mgr
def test_stale_active_bundle_queues_redownload_at_current_index():
active = _bundle(55, "WMIV12", "a" * 64)
counterpart = _bundle(12, "WMIV12", "b" * 64)
mgr = _manager(active, [counterpart])
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) == 12
assert mgr.active_bundle is active
def test_matching_shas_do_not_queue():
active = _bundle(55, "WMIV12", "a" * 64)
counterpart = _bundle(12, "WMIV12", "A" * 64)
mgr = _manager(active, [counterpart])
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None
def test_retired_bundle_is_left_alone():
active = _bundle(55, "WMIV12", "a" * 64)
mgr = _manager(active, [_bundle(12, "OtherModel", "b" * 64)])
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None
assert mgr.active_bundle is active
def test_default_bundle_is_never_refreshed():
active = _bundle(0, "Default", "a" * 64)
active.ref = "default"
mgr = _manager(active, [_bundle(0, "Default", "b" * 64)])
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None
def test_pending_download_blocks_refresh():
active = _bundle(55, "WMIV12", "a" * 64)
mgr = _manager(active, [_bundle(12, "WMIV12", "b" * 64)])
mgr.params.put(_DOWNLOAD_INDEX_KEY, 3)
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) == 3
def test_empty_manifest_hash_never_triggers():
active = _bundle(55, "WMIV12", "a" * 64)
mgr = _manager(active, [_bundle(12, "WMIV12", "")])
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None
def test_refresh_queued_once_per_run():
active = _bundle(55, "WMIV12", "a" * 64)
mgr = _manager(active, [_bundle(12, "WMIV12", "b" * 64)])
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) == 12
mgr.params.remove(_DOWNLOAD_INDEX_KEY)
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None
def test_counterpart_matched_by_name_not_index():
active = _bundle(55, "WMIV12", "a" * 64)
imposter = _bundle(55, "OtherModel", "c" * 64)
counterpart = _bundle(12, "WMIV12", "b" * 64)
mgr = _manager(active, [imposter, counterpart])
mgr._queue_active_manifest_refresh()
assert mgr.params.get(_DOWNLOAD_INDEX_KEY) == 12

View File

@@ -337,6 +337,13 @@ _NOTICE_EVENTS: EVENTS_IQ_TYPE = {
Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
}, },
# outranks the generic processNotRunning alert so the driver sees why engagement is blocked
EventNameIQ.modelUpdating: {
ET.NO_ENTRY: NoEntryAlert("Update finishes while parked with internet",
alert_text_1="Driving Model Updating",
priority=Priority.MID),
},
} }
EVENTS_IQ: EVENTS_IQ_TYPE = {**_GUIDANCE_EVENTS, **_ENGAGE_EVENTS, **_CABIN_BLOCK_EVENTS, **_NOTICE_EVENTS} EVENTS_IQ: EVENTS_IQ_TYPE = {**_GUIDANCE_EVENTS, **_ENGAGE_EVENTS, **_CABIN_BLOCK_EVENTS, **_NOTICE_EVENTS}

View File

@@ -194,7 +194,7 @@ CHEVRON_INFO_DESCRIPTION = {
# param key -> (title fn, description fn) # param key -> (title fn, description fn)
_HUD_TOGGLES = { _HUD_TOGGLES = {
"BlindSpot": ( "IQBlindSpotAlerts": (
lambda: tr("Blind Spot Alerts"), lambda: tr("Blind Spot Alerts"),
lambda: tr("Flashes a side warning whenever the car reports something sitting in your blind spot (BSM-equipped cars only)."), lambda: tr("Flashes a side warning whenever the car reports something sitting in your blind spot (BSM-equipped cars only)."),
), ),
@@ -202,20 +202,20 @@ _HUD_TOGGLES = {
lambda: tr("Expanded Status Bar"), lambda: tr("Expanded Status Bar"),
lambda: tr("Bring back the classic UI's wide offroad status strip: temperature, vehicle, and Konn3kt state at a glance."), lambda: tr("Bring back the classic UI's wide offroad status strip: temperature, vehicle, and Konn3kt state at a glance."),
), ),
"TorqueBar": ( "IQSteerEffortArc": (
lambda: tr("Steering Effort Arc"), lambda: tr("Steering Effort Arc"),
lambda: tr("Trace an arc over the road view showing how much steering IQ.Pilot is applying while lateral control runs."), lambda: tr("Trace an arc over the road view showing how much steering IQ.Pilot is applying while lateral control runs."),
), ),
"RoadNameToggle": ( "IQRoadNameOverlay": (
lambda: tr("Road Name Overlay"), lambda: tr("Road Name Overlay"),
lambda: tr("Show the current road's name over the driving view." lambda: tr("Show the current road's name over the driving view."
"<br>Requires offline map data for your region to be installed."), "<br>Requires offline map data for your region to be installed."),
), ),
"ShowTurnSignals": ( "IQBlinkerIndicators": (
lambda: tr("Blinker Indicators"), lambda: tr("Blinker Indicators"),
lambda: tr("Mirror the car's blinkers as arrows on the driving screen."), lambda: tr("Mirror the car's blinkers as arrows on the driving screen."),
), ),
"RocketFuel": ( "IQAccelMeter": (
lambda: tr("Acceleration Meter"), lambda: tr("Acceleration Meter"),
lambda: tr("Draw a bar along the left edge tracking measured acceleration and braking — what the car is actually " lambda: tr("Draw a bar along the left edge tracking measured acceleration and braking — what the car is actually "
"doing right now, not the planner's request."), "doing right now, not the planner's request."),
@@ -243,7 +243,7 @@ class VisualsLayout(Widget):
title=lambda: tr("Lead Vehicle Readouts"), title=lambda: tr("Lead Vehicle Readouts"),
description="", description="",
buttons=[lambda: tr("Off"), lambda: tr("Distance"), lambda: tr("Speed"), lambda: tr("Time"), lambda: tr("All")], buttons=[lambda: tr("Off"), lambda: tr("Distance"), lambda: tr("Speed"), lambda: tr("Time"), lambda: tr("All")],
param="ChevronInfo", param="IQLeadReadouts",
inline=False, inline=False,
) )
self._dev_ui_info = toggle_item( self._dev_ui_info = toggle_item(
@@ -270,12 +270,12 @@ class VisualsLayout(Widget):
def _sync_chevron_row(self): def _sync_chevron_row(self):
if ui_state.has_longitudinal_control: if ui_state.has_longitudinal_control:
self._chevron_info.set_description(tr(CHEVRON_INFO_DESCRIPTION["enabled"])) self._chevron_info.set_description(tr(CHEVRON_INFO_DESCRIPTION["enabled"]))
self._chevron_info.action_item.set_selected_button(ui_state.params.get("ChevronInfo", return_default=True)) self._chevron_info.action_item.set_selected_button(ui_state.params.get("IQLeadReadouts", return_default=True))
self._chevron_info.action_item.set_enabled(True) self._chevron_info.action_item.set_enabled(True)
else: else:
self._chevron_info.set_description(tr(CHEVRON_INFO_DESCRIPTION["disabled"])) self._chevron_info.set_description(tr(CHEVRON_INFO_DESCRIPTION["disabled"]))
self._chevron_info.action_item.set_enabled(False) self._chevron_info.action_item.set_enabled(False)
ui_state.params.put("ChevronInfo", 0) ui_state.params.put("IQLeadReadouts", 0)
def _update_state(self): def _update_state(self):
super()._update_state() super()._update_state()
@@ -1388,7 +1388,7 @@ class IQDeviceLayout(DeviceLayout):
DeviceLayout._initialize_items(self) DeviceLayout._initialize_items(self)
# Using dual button with no right button for better alignment # Using dual button with no right button for better alignment
self._always_offroad_btn = self._left_button(lambda: tr("Force Offroad Mode"), self._handle_always_offroad) self._always_offroad_btn = self._left_button(lambda: tr("Keep Device Offroad"), self._handle_always_offroad)
self._force_onroad_btn = self._left_button(lambda: tr("Force On-Road (10 min)"), self._handle_force_onroad) self._force_onroad_btn = self._left_button(lambda: tr("Force On-Road (10 min)"), self._handle_force_onroad)
self._max_time_offroad = option_item( self._max_time_offroad = option_item(
@@ -1592,7 +1592,7 @@ class IQDeviceLayout(DeviceLayout):
force_onroad_active = force_onroad_until > now force_onroad_active = force_onroad_until > now
# Text & Color # Text & Color
offroad_mode_btn_text = tr("Exit Offroad Mode") if always_offroad else tr("Force Offroad Mode") offroad_mode_btn_text = tr("Exit Offroad Mode") if always_offroad else tr("Keep Device Offroad")
offroad_mode_btn_style = ButtonStyle.PRIMARY if always_offroad else ButtonStyle.DANGER offroad_mode_btn_style = ButtonStyle.PRIMARY if always_offroad else ButtonStyle.DANGER
self._always_offroad_btn.action_item.left_button.set_text(offroad_mode_btn_text) self._always_offroad_btn.action_item.left_button.set_text(offroad_mode_btn_text)
self._always_offroad_btn.action_item.left_button.set_button_style(offroad_mode_btn_style) self._always_offroad_btn.action_item.left_button.set_button_style(offroad_mode_btn_style)
@@ -1957,12 +1957,12 @@ class ModelsLayout(Widget):
return folders_list return folders_list
def _handle_current_model_clicked(self): def _handle_current_model_clicked(self):
favs = ui_state.params.get("ModelManager_Favs") favs = ui_state.params.get("IQModelFavorites")
favorites = set(favs.split(';')) if favs else set() favorites = set(favs.split(';')) if favs else set()
folders_list = self._get_folders(favorites) folders_list = self._get_folders(favorites)
active_ref = self.model_manager.activeBundle.ref if self._has_active_bundle_param() and self.model_manager.activeBundle else "Default" active_ref = self.model_manager.activeBundle.ref if self._has_active_bundle_param() and self.model_manager.activeBundle else "Default"
self.model_dialog = PickerDialog(tr("Choose a Model"), folders_list, active_ref, "ModelManager_Favs", self.model_dialog = PickerDialog(tr("Choose a Model"), folders_list, active_ref, "IQModelFavorites",
get_folders_fn=self._get_folders, on_exit=self._on_model_selected) get_folders_fn=self._get_folders, on_exit=self._on_model_selected)
gui_app.set_modal_overlay(self.model_dialog, callback=self._on_model_selected) gui_app.set_modal_overlay(self.model_dialog, callback=self._on_model_selected)
@@ -2191,8 +2191,8 @@ class HyundaiSettings(BrandPanel):
self.longitudinal_tuning_item = multiple_button_item( self.longitudinal_tuning_item = multiple_button_item(
tr("Longitudinal Tune Profile"), "", tr("Longitudinal Tune Profile"), "",
[tr("Off"), tr("Dynamic"), tr("Predictive")], [tr("Off"), tr("Dynamic"), tr("Predictive")],
button_width=300, param="HyundaiLongitudinalTuning", inline=False, button_width=300, param="IQHyundaiLongTune", inline=False,
callback=lambda index: ui_state.params.put("HyundaiLongitudinalTuning", index)) callback=lambda index: ui_state.params.put("IQHyundaiLongTune", index))
self.items = [self.longitudinal_tuning_item] self.items = [self.longitudinal_tuning_item]
def _alpha_long_supported(self) -> bool: def _alpha_long_supported(self) -> bool:
@@ -2204,7 +2204,7 @@ class HyundaiSettings(BrandPanel):
def update_settings(self): def update_settings(self):
self.alpha_long_available = self._alpha_long_supported() self.alpha_long_available = self._alpha_long_supported()
selected = int(ui_state.params.get("HyundaiLongitudinalTuning") or "0") selected = int(ui_state.params.get("IQHyundaiLongTune") or "0")
if not ui_state.is_offroad(): if not ui_state.is_offroad():
desc, usable = tr("Unavailable while the car is onroad."), False desc, usable = tr("Unavailable while the car is onroad."), False
@@ -2231,11 +2231,11 @@ class SubaruSettings(BrandPanel):
def __init__(self): def __init__(self):
super().__init__() super().__init__()
self._supported = False self._supported = False
self.stop_and_go_toggle = toggle_item(tr("Creep from Standstill (Beta)"), "", param="SubaruStopAndGo", self.stop_and_go_toggle = toggle_item(tr("Creep from Standstill (Beta)"), "", param="IQSubaruCreepAssist",
callback=lambda _: self.update_settings()) callback=lambda _: self.update_settings())
self.stop_and_go_manual_parking_brake_toggle = toggle_item( self.stop_and_go_manual_parking_brake_toggle = toggle_item(
tr("Creep from Standstill — Manual Handbrake (Beta)"), "", tr("Creep from Standstill — Manual Handbrake (Beta)"), "",
param="SubaruStopAndGoManualParkingBrake", callback=lambda _: self.update_settings()) param="IQSubaruCreepAssistManualBrake", callback=lambda _: self.update_settings())
self.items = [self.stop_and_go_toggle, self.stop_and_go_manual_parking_brake_toggle] self.items = [self.stop_and_go_toggle, self.stop_and_go_manual_parking_brake_toggle]
def _platform_flags(self) -> int: def _platform_flags(self) -> int:
@@ -2286,8 +2286,8 @@ def _speed_text(kmh: int) -> str:
class TeslaSettings(BrandPanel): class TeslaSettings(BrandPanel):
def __init__(self): def __init__(self):
super().__init__() super().__init__()
self.coop_steering_toggle = toggle_item(tr("VTB (Virtual Torque Blending)"), "", param="TeslaCoopSteering") self.torque_blend_toggle = toggle_item(tr("VTB (Virtual Torque Blending)"), "", param="IQTeslaTorqueBlend")
self.items = [self.coop_steering_toggle] self.items = [self.torque_blend_toggle]
def update_settings(self): def update_settings(self):
caution = tr("Warning: steering may oscillate in turns below {}; turn this off if you feel it.").format( caution = tr("Warning: steering may oscillate in turns below {}; turn this off if you feel it.").format(
@@ -2300,8 +2300,8 @@ class TeslaSettings(BrandPanel):
blocker = tr("Flip on Always Offroad from the Device panel, or power the car down, to change this.") blocker = tr("Flip on Always Offroad from the Device panel, or power the car down, to change this.")
body = f"<b>{blocker}</b><br><br>{body}" body = f"<b>{blocker}</b><br><br>{body}"
self.coop_steering_toggle.set_description(body) self.torque_blend_toggle.set_description(body)
self.coop_steering_toggle.action_item.set_enabled(ui_state.is_offroad()) self.torque_blend_toggle.action_item.set_enabled(ui_state.is_offroad())
# ===== vehicle_brands_toyota ===== # ===== vehicle_brands_toyota =====
@@ -2312,7 +2312,7 @@ class ToyotaSettings(BrandPanel):
self.enforce_stock_longitudinal = toggle_item( self.enforce_stock_longitudinal = toggle_item(
lambda: tr("Keep Factory Gas and Brake"), lambda: tr("Keep Factory Gas and Brake"),
description=lambda: tr("Keeps gas and brakes with the factory Toyota system; IQ.Pilot steers only."), description=lambda: tr("Keeps gas and brakes with the factory Toyota system; IQ.Pilot steers only."),
initial_state=ui_state.params.get_bool("ToyotaEnforceStockLongitudinal"), initial_state=ui_state.params.get_bool("IQToyotaFactoryLong"),
callback=self._on_toggled, callback=self._on_toggled,
enabled=lambda: not ui_state.engaged, enabled=lambda: not ui_state.engaged,
) )
@@ -2320,7 +2320,7 @@ class ToyotaSettings(BrandPanel):
@staticmethod @staticmethod
def _apply(enabled: bool): def _apply(enabled: bool):
ui_state.params.put_bool("ToyotaEnforceStockLongitudinal", enabled) ui_state.params.put_bool("IQToyotaFactoryLong", enabled)
if enabled and ui_state.params.get_bool("AlphaLongitudinalEnabled"): if enabled and ui_state.params.get_bool("AlphaLongitudinalEnabled"):
ui_state.params.put_bool("AlphaLongitudinalEnabled", False) ui_state.params.put_bool("AlphaLongitudinalEnabled", False)
ui_state.params.put_bool("OnroadCycleRequested", True) ui_state.params.put_bool("OnroadCycleRequested", True)

View File

@@ -1,10 +0,0 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from openpilot.iqpilot.ui.onroad.hud_overlays import ChevronMetrics
from openpilot.iqpilot.ui.onroad.rainbow_path import RainbowPath
class IQModelRenderer:
def __init__(self):
self.rainbow_path = RainbowPath()

View File

@@ -1,41 +0,0 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import colorsys
import time
import pyray as rl
from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
# Scrolling spectrum along the driving path: a fixed set of stops from the
# bottom (1.0) to the top (0.0) of the path, each a full-saturation swatch whose
# hue advances with time and whose opacity thins toward the horizon.
_SEGMENTS = 8
_SCROLL_DEG_PER_S = 50.0
_SATURATION = 0.9
_LIGHTNESS = 0.6
_ALPHA_NEAR = 0.8 # opacity at the bottom of the path
_ALPHA_HORIZON_FRACTION = 0.3 # how much of that opacity is shed by the top
# Stop offsets are constant, so resolve them once.
_STOP_OFFSETS = tuple(i / (_SEGMENTS - 1) for i in range(_SEGMENTS))
def _swatch(hue_turns: float, alpha: float) -> rl.Color:
r, g, b = colorsys.hls_to_rgb(hue_turns, _LIGHTNESS, _SATURATION)
return rl.Color(int(r * 255), int(g * 255), int(b * 255), int(alpha * 255))
def _spectrum_gradient() -> Gradient:
scroll_deg = (time.monotonic() * _SCROLL_DEG_PER_S) % 360.0
colors = []
for offset in _STOP_OFFSETS:
hue_deg = (scroll_deg + offset * 360.0) % 360.0
alpha = _ALPHA_NEAR * (1.0 - offset * _ALPHA_HORIZON_FRACTION)
colors.append(_swatch(hue_deg / 360.0, alpha))
return Gradient(start=(0.0, 1.0), end=(0.0, 0.0), colors=colors, stops=list(_STOP_OFFSETS))
class RainbowPath:
def draw_rainbow_path(self, rect, path):
draw_polygon(rect, path.projected_points, gradient=_spectrum_gradient())

View File

@@ -260,7 +260,7 @@ class Controls(IQControlsLayer):
hudControl.leadFollowTime = 1.45 hudControl.leadFollowTime = 1.45
hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
hudControl.audibleAlert = self.sm['selfdriveState'].alertSound hudControl.audibleAlert = self.sm['selfdriveState'].alertSound
hudControl.driverUnresponsive = self.sm['driverMonitoringState'].noResponseForceDecel hudControl.driverUnresponsive = self.sm['selfdriveState'].alertType.split('/', 1)[0] == 'driverUnresponsive'
hudControl.rightLaneVisible = True hudControl.rightLaneVisible = True
hudControl.leftLaneVisible = True hudControl.leftLaneVisible = True
@@ -292,7 +292,7 @@ class Controls(IQControlsLayer):
cs.upAccelCmd = float(self.LoC.pid.p) cs.upAccelCmd = float(self.LoC.pid.p)
cs.uiAccelCmd = float(self.LoC.pid.i) cs.uiAccelCmd = float(self.LoC.pid.i)
cs.ufAccelCmd = float(self.LoC.pid.f) cs.ufAccelCmd = float(self.LoC.pid.f)
cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
(self.sm['selfdriveState'].state == State.softDisabling)) (self.sm['selfdriveState'].state == State.softDisabling))
lat_tuning = self.CP.lateralTuning.which() lat_tuning = self.CP.lateralTuning.which()

View File

@@ -30,9 +30,9 @@ def cycle_alerts(duration=200, is_metric=False):
(EventName.accFaulted, ET.IMMEDIATE_DISABLE), (EventName.accFaulted, ET.IMMEDIATE_DISABLE),
# DM sequence # DM sequence
(EventName.driverDistracted1, ET.WARNING), (EventName.preDriverDistracted, ET.WARNING),
(EventName.driverDistracted2, ET.WARNING), (EventName.promptDriverDistracted, ET.WARNING),
(EventName.driverDistracted3, ET.WARNING), (EventName.driverDistracted, ET.WARNING),
] ]
# debug alerts # debug alerts

View File

@@ -60,7 +60,7 @@ def host_tinygrad_flags(*, float16=False):
return f"{base} FLOAT16=1" if float16 else base return f"{base} FLOAT16=1" if float16 else base
# Compile small models # Compile small models
for model_name in ['dmonitoring_model', 'dmonitoring_model_mici']: for model_name in ['dmonitoring_model']:
# The optimization flags are mandatory on QCOM: without FLOAT16/NOLOCALS/JIT_BATCH_SIZE/OPENPILOT_HACKS these # The optimization flags are mandatory on QCOM: without FLOAT16/NOLOCALS/JIT_BATCH_SIZE/OPENPILOT_HACKS these
# models compile to unoptimized QCOM kernels and run ~20x slower (dmonitoring_model: ~300ms -> ~14ms), # models compile to unoptimized QCOM kernels and run ~20x slower (dmonitoring_model: ~300ms -> ~14ms),
# which starves the driving model on the shared Adreno. IMAGE=2 (not upstream's IMAGE=1) because the # which starves the driving model on the shared Adreno. IMAGE=2 (not upstream's IMAGE=1) because the

View File

@@ -20,25 +20,19 @@ from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
from openpilot.system.hardware import HARDWARE
PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld" PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld"
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
MODELS_PATH = Path(__file__).parent / 'models' MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl'
def get_model_paths(device_type: str) -> tuple[Path, Path]:
model_name = 'dmonitoring_model_mici' if device_type == 'mici' else 'dmonitoring_model'
return MODELS_PATH / f'{model_name}_tinygrad.pkl', MODELS_PATH / f'{model_name}_metadata.pkl'
class ModelState: class ModelState:
inputs: dict[str, np.ndarray] inputs: dict[str, np.ndarray]
output: np.ndarray output: np.ndarray
def __init__(self, cl_ctx, device_type=None): def __init__(self, cl_ctx):
model_path, metadata_path = get_model_paths(device_type or HARDWARE.get_device_type()) with open(METADATA_PATH, 'rb') as f:
with open(metadata_path, 'rb') as f:
model_metadata = pickle.load(f) model_metadata = pickle.load(f)
self.input_shapes = model_metadata['input_shapes'] self.input_shapes = model_metadata['input_shapes']
self.output_slices = model_metadata['output_slices'] self.output_slices = model_metadata['output_slices']
@@ -49,7 +43,7 @@ class ModelState:
} }
self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
with open(model_path, "rb") as f: with open(MODEL_PKL_PATH, "rb") as f:
self.model_run = pickle.load(f) self.model_run = pickle.load(f)
def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]: def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
@@ -81,10 +75,8 @@ def parse_model_output(model_output):
face_descs = model_output[f'face_descs_{ds_suffix}'] face_descs = model_output[f'face_descs_{ds_suffix}']
parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6] parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:]) parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob', 'sleep_prob']: for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
output_key = f'{key}_{ds_suffix}' parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
if output_key in model_output:
parsed[output_key] = sigmoid(model_output[output_key])
return parsed return parsed
def fill_driver_data(msg, model_output, ds_suffix): def fill_driver_data(msg, model_output, ds_suffix):
@@ -99,8 +91,6 @@ def fill_driver_data(msg, model_output, ds_suffix):
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item() msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item() msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item() msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
sleep_prob = model_output.get(f'sleep_prob_{ds_suffix}')
msg.sleepProb = sleep_prob[0, 0].item() if sleep_prob is not None else 0.
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float): def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
msg = messaging.new_message('driverStateV2', valid=True) msg = messaging.new_message('driverStateV2', valid=True)

View File

@@ -1,15 +1,8 @@
{ {
"dmonitoring_model": { "dmonitoring_model": {
"outputs": { "outputs": {
"dmonitoring_model_metadata.pkl": "5999c262b1c25c62e485fb4ced5806d20a8ca59e3ae94e0ff499c0fe3497fedc", "dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3",
"dmonitoring_model_tinygrad.pkl": "5aca89a35b42376d56f67ccd28a1080806706d546dd8c63f6e1b4c681f8e2c01" "dmonitoring_model_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52"
},
"signature": "2364ebd4bb95c4b4e539c9b1ba68324b713cbb56617396b73262accf9cdcfbfc"
},
"dmonitoring_model_mici": {
"outputs": {
"dmonitoring_model_mici_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3",
"dmonitoring_model_mici_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52"
}, },
"signature": "e1eeb5ce45774a816c8da2394e6ee35ebf700b71dff345e0141dbab8ff592349" "signature": "e1eeb5ce45774a816c8da2394e6ee35ebf700b71dff345e0141dbab8ff592349"
} }

View File

@@ -11,7 +11,7 @@ BASEDIR = MODELD_DIR.parents[1]
TINYGRAD_DIR = BASEDIR / 'tinygrad_repo' TINYGRAD_DIR = BASEDIR / 'tinygrad_repo'
METADATA_SCRIPT = MODELD_DIR / 'get_model_metadata.py' METADATA_SCRIPT = MODELD_DIR / 'get_model_metadata.py'
MODEL_NAMES = ['dmonitoring_model', 'dmonitoring_model_mici'] MODEL_NAMES = ['dmonitoring_model']
def _hash_file(h, path: Path) -> None: def _hash_file(h, path: Path) -> None:

View File

@@ -1,37 +0,0 @@
import pickle
import numpy as np
import pytest
from openpilot.selfdrive.modeld.dmonitoringmodeld import get_driverstate_packet, get_model_paths, parse_model_output, slice_outputs
@pytest.mark.parametrize("device_type, model_name", [
("mici", "dmonitoring_model_mici"),
("tici", "dmonitoring_model"),
("tizi", "dmonitoring_model"),
])
def test_model_paths(device_type, model_name):
model_path, metadata_path = get_model_paths(device_type)
assert model_path.name == f"{model_name}_tinygrad.pkl"
assert metadata_path.name == f"{model_name}_metadata.pkl"
@pytest.mark.parametrize("device_type, expected_sleep_prob", [
("mici", 0.),
("tici", 0.5),
("tizi", 0.5),
])
def test_sleep_probability_output(device_type, expected_sleep_prob):
_, metadata_path = get_model_paths(device_type)
with open(metadata_path, 'rb') as f:
metadata = pickle.load(f)
output = np.zeros(metadata['output_shapes']['outputs'][1], dtype=np.float32)
parsed = parse_model_output(slice_outputs(output, metadata['output_slices']))
parsed['raw_pred'] = b''
msg = get_driverstate_packet(parsed, 1, 0, 0., 0.)
assert msg.driverStateV2.leftDriverData.sleepProb == expected_sleep_prob
assert msg.driverStateV2.rightDriverData.sleepProb == expected_sleep_prob

View File

@@ -1,52 +1,51 @@
import hashlib import hashlib
import json import json
import pytest
from openpilot.selfdrive.modeld import prebuilt_models from openpilot.selfdrive.modeld import prebuilt_models
def write_outputs(models_dir, check_path, model_names): def write_outputs(models_dir, check_path):
checks = {} outputs = {}
for model_name in model_names: for name, contents in {
outputs = {} 'dmonitoring_model_tinygrad.pkl': b'tinygrad',
for suffix, contents in { 'dmonitoring_model_metadata.pkl': b'metadata',
'tinygrad.pkl': b'tinygrad', }.items():
'metadata.pkl': b'metadata', (models_dir / name).write_bytes(contents)
}.items(): outputs[name] = hashlib.sha256(contents).hexdigest()
name = f'{model_name}_{suffix}' check_path.write_text(json.dumps({'dmonitoring_model': {'outputs': outputs}}))
(models_dir / name).write_bytes(contents)
outputs[name] = hashlib.sha256(contents).hexdigest()
checks[model_name] = {'outputs': outputs}
check_path.write_text(json.dumps(checks))
@pytest.fixture def test_packaged_prebuilt_without_onnx(tmp_path, monkeypatch):
def packaged_models(tmp_path, monkeypatch):
models_dir = tmp_path / 'models' models_dir = tmp_path / 'models'
models_dir.mkdir() models_dir.mkdir()
check_path = models_dir / 'prebuilt_check.json' check_path = models_dir / 'prebuilt_check.json'
write_outputs(models_dir, check_path, prebuilt_models.MODEL_NAMES) write_outputs(models_dir, check_path)
monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir) monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path) monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
return models_dir
assert prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
assert not prebuilt_models.verify_prebuilt('dmonitoring_model', 'flags')
@pytest.mark.parametrize("model_name", prebuilt_models.MODEL_NAMES) def test_packaged_prebuilt_rejects_corrupt_output(tmp_path, monkeypatch):
def test_packaged_prebuilt_without_onnx(packaged_models, model_name): models_dir = tmp_path / 'models'
assert prebuilt_models.packaged_prebuilt_matches(model_name) models_dir.mkdir()
assert not prebuilt_models.verify_prebuilt(model_name, 'flags') check_path = models_dir / 'prebuilt_check.json'
write_outputs(models_dir, check_path)
(models_dir / 'dmonitoring_model_tinygrad.pkl').write_bytes(b'corrupt')
monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
@pytest.mark.parametrize("model_name", prebuilt_models.MODEL_NAMES) def test_source_checkout_is_not_packaged_prebuilt(tmp_path, monkeypatch):
def test_packaged_prebuilt_rejects_corrupt_output(packaged_models, model_name): models_dir = tmp_path / 'models'
(packaged_models / f'{model_name}_tinygrad.pkl').write_bytes(b'corrupt') models_dir.mkdir()
check_path = models_dir / 'prebuilt_check.json'
write_outputs(models_dir, check_path)
(models_dir / 'dmonitoring_model.onnx').write_bytes(b'onnx')
monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
assert not prebuilt_models.packaged_prebuilt_matches(model_name) assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
@pytest.mark.parametrize("model_name", prebuilt_models.MODEL_NAMES)
def test_source_checkout_is_not_packaged_prebuilt(packaged_models, model_name):
(packaged_models / f'{model_name}.onnx').write_bytes(b'onnx')
assert not prebuilt_models.packaged_prebuilt_matches(model_name)

View File

@@ -0,0 +1,15 @@
# driver monitoring (DM)
Uploading driver-facing camera footage is opt-in, but it is encouraged to opt-in to improve the DM model. You can always change your preference using the "Record and Upload Driver Camera" toggle.
## Troubleshooting
Before creating a bug report, go through these troubleshooting steps.
* Ensure the driver-facing camera has a good view of the driver in normal driving positions.
* This can be checked in Settings -> Device -> Preview Driver Camera (when car is off).
* If the camera can't see the driver, the device should be re-mounted.
## Bug report
In order for us to look into DM bug reports, we'll need the driver-facing camera footage. If you don't normally have this enabled, simply enable the toggle for a single drive. Also ensure the "Upload Raw Logs" toggle is enabled before going for a drive.

View File

@@ -1,33 +1,8 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from types import SimpleNamespace
import cereal.messaging as messaging import cereal.messaging as messaging
from openpilot.common.params import Params from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process from openpilot.common.realtime import config_realtime_process
from openpilot.selfdrive.monitoring.legacy_policy import DRIVER_MONITOR_SETTINGS as LegacySettings from openpilot.selfdrive.monitoring.helpers import DriverMonitoring
from openpilot.selfdrive.monitoring.legacy_policy import DriverMonitoring as LegacyDriverMonitoring
from openpilot.selfdrive.monitoring.policy import DriverMonitoring as UpstreamDriverMonitoring
from openpilot.system.hardware import HARDWARE
def use_legacy_dm(device_type: str) -> bool:
return device_type == 'mici'
def create_driver_monitoring(device_type: str, rhd_saved: bool, always_on: bool):
if use_legacy_dm(device_type):
return LegacyDriverMonitoring(rhd_saved=rhd_saved, settings=LegacySettings(device_type), always_on=always_on)
return UpstreamDriverMonitoring(rhd_saved=rhd_saved, always_on=always_on)
def get_dm_inputs(sm):
return {
'driverStateV2': sm['driverStateV2'],
'liveCalibration': sm['liveCalibration'],
'carState': sm['carState'],
'selfdriveState': SimpleNamespace(enabled=sm['selfdriveState'].enabled or sm['carControl'].latActive),
'modelV2': sm['modelV2'],
}
def dmonitoringd_thread(): def dmonitoringd_thread():
@@ -38,9 +13,7 @@ def dmonitoringd_thread():
sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2', sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2',
'carControl'], poll='driverStateV2') 'carControl'], poll='driverStateV2')
device_type = HARDWARE.get_device_type() DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"))
legacy_dm = use_legacy_dm(device_type)
DM = create_driver_monitoring(device_type, params.get_bool("IsRhdDetected"), params.get_bool("AlwaysOnDM"))
demo_mode=False demo_mode=False
# 20Hz <- dmonitoringmodeld # 20Hz <- dmonitoringmodeld
@@ -52,9 +25,9 @@ def dmonitoringd_thread():
valid = sm.all_checks() valid = sm.all_checks()
if demo_mode and sm.valid['driverStateV2']: if demo_mode and sm.valid['driverStateV2']:
DM.run_step(sm if legacy_dm else get_dm_inputs(sm), demo=True) DM.run_step(sm, demo=demo_mode)
elif valid: elif valid:
DM.run_step(sm if legacy_dm else get_dm_inputs(sm), demo=demo_mode) DM.run_step(sm, demo=demo_mode)
# publish # publish
dat = DM.get_state_packet(valid=valid) dat = DM.get_state_packet(valid=valid)
@@ -66,11 +39,10 @@ def dmonitoringd_thread():
demo_mode = params.get_bool("IsDriverViewEnabled") demo_mode = params.get_bool("IsDriverViewEnabled")
# save rhd virtual toggle every 5 mins # save rhd virtual toggle every 5 mins
wheelpos_offsetter = DM.wheelpos.prob_offseter if legacy_dm else DM.wheelpos_offsetter
if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
wheelpos_offsetter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
DM.wheel_on_right == (wheelpos_offsetter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
params.put_bool("IsRhdDetected", DM.wheel_on_right) params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right)
def main(): def main():
dmonitoringd_thread() dmonitoringd_thread()

View File

@@ -13,12 +13,6 @@ from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
from openpilot.system.hardware import HARDWARE from openpilot.system.hardware import HARDWARE
EventName = log.OnroadEvent.EventName EventName = log.OnroadEvent.EventName
AlertLevel = log.DriverMonitoringState.AlertLevel
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
def to_percent(v):
return int(min(max(v * 100., 0.), 100.))
# ****************************************************************************************** # ******************************************************************************************
# NOTE: To fork maintainers. # NOTE: To fork maintainers.
@@ -397,61 +391,44 @@ class DriverMonitoring:
alert = None alert = None
if self.awareness <= 0.: if self.awareness <= 0.:
# terminal red alert: disengagement required # terminal red alert: disengagement required
alert = EventName.driverDistracted3 if self.active_monitoring_mode else EventName.driverUnresponsive3 alert = EventName.driverDistracted if self.active_monitoring_mode else EventName.driverUnresponsive
self.terminal_time += 1 self.terminal_time += 1
if awareness_prev > 0.: if awareness_prev > 0.:
self.terminal_alert_cnt += 1 self.terminal_alert_cnt += 1
elif self.awareness <= self.threshold_prompt: elif self.awareness <= self.threshold_prompt:
# prompt orange alert # prompt orange alert
alert = EventName.driverDistracted2 if self.active_monitoring_mode else EventName.driverUnresponsive2 alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive
elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption: elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption:
# pre green alert # pre green alert
alert = EventName.driverDistracted1 if self.active_monitoring_mode else EventName.driverUnresponsive1 alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive
if alert is not None: if alert is not None:
self.current_events.add(alert) self.current_events.add(alert)
def get_state_packet(self, valid=True): def get_state_packet(self, valid=True):
# build driverMonitoringState packet
dat = messaging.new_message('driverMonitoringState', valid=valid) dat = messaging.new_message('driverMonitoringState', valid=valid)
dm = dat.driverMonitoringState dat.driverMonitoringState = {
"events": self.current_events.to_msg(),
dm.lockout = self.too_distracted "faceDetected": self.face_detected,
dm.alert3Count = self.terminal_alert_cnt "isDistracted": self.driver_distracted,
dm.noResponseCount = int(self.terminal_time >= self.settings._MAX_TERMINAL_DURATION) "distractedType": sum(self.distracted_types),
dm.noResponseForceDecel = self.awareness <= 0. "awarenessStatus": self.awareness,
dm.alwaysOn = self.always_on "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(),
dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_prompt "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n,
if self.awareness <= 0.: "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(),
dm.alertLevel = AlertLevel.three "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n,
elif self.awareness <= self.threshold_prompt: "phoneProbOffset": self.phone.prob_offseter.filtered_stat.mean(),
dm.alertLevel = AlertLevel.two "phoneProbValidCount": self.phone.prob_offseter.filtered_stat.n,
elif self.awareness <= self.threshold_pre: "stepChange": self.step_change,
dm.alertLevel = AlertLevel.one "awarenessActive": self.awareness_active,
dm.activePolicy = MonitoringPolicy.vision if self.active_monitoring_mode else MonitoringPolicy.wheeltouch "awarenessPassive": self.awareness_passive,
dm.isRHD = self.wheel_on_right "isLowStd": self.pose.low_std,
dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos.prob_offseter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT) "hiStdCount": self.hi_stds,
dm.rhdCalibration.offset = self.wheelpos.prob_offseter.filtered_stat.M "isActiveMode": self.active_monitoring_mode,
"isRHD": self.wheel_on_right,
dm.visionPolicyState.awarenessPercent = to_percent(self.awareness if self.active_monitoring_mode else self.awareness_active) "uncertainCount": self.dcam_uncertain_cnt,
dm.visionPolicyState.awarenessStep = self.step_change if self.active_monitoring_mode else 0. }
dm.visionPolicyState.isDistracted = self.driver_distracted
dm.visionPolicyState.distractedTypes.pose = DistractedType.DISTRACTED_POSE in self.distracted_types
dm.visionPolicyState.distractedTypes.eye = DistractedType.DISTRACTED_BLINK in self.distracted_types
dm.visionPolicyState.distractedTypes.phone = DistractedType.DISTRACTED_PHONE in self.distracted_types
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_offseter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offseter.filtered_stat.M
dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offseter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offseter.filtered_stat.M
dm.visionPolicyState.pose.uncertainty = max(self.pose.pitch_std, self.pose.yaw_std)
dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME)
dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / int(60 / self.settings._DT_DMON))
dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.awareness if not self.active_monitoring_mode else self.awareness_passive)
dm.wheeltouchPolicyState.awarenessStep = self.step_change if not self.active_monitoring_mode else 0.
return dat return dat
def run_step(self, sm, demo=False): def run_step(self, sm, demo=False):

View File

@@ -1,467 +0,0 @@
from collections import defaultdict
from math import atan2, radians
import numpy as np
from cereal import car, log
import cereal.messaging as messaging
from openpilot.common.realtime import DT_DMON
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.common.stat_live import RunningStatFilter
from openpilot.common.transformations.camera import DEVICE_CAMERAS
AlertLevel = log.DriverMonitoringState.AlertLevel
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
def to_percent(v):
return int(min(max(v * 100., 0.), 100.))
# ******************************************************************************************
# NOTE: To fork maintainers.
# Disabling or nerfing safety features will get you and your users banned from our servers.
# We recommend that you do not change these numbers from the defaults.
# ******************************************************************************************
class DRIVER_MONITOR_SETTINGS:
def __init__(self):
# https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=OJ:L_202501899
self._ALERT_MIN_SPEED = 2.8 # 10 km/h
self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 5.
self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 15.
self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 25.
self._VISION_POLICY_ALERT_1_TIMEOUT = 5.
self._VISION_POLICY_ALERT_2_TIMEOUT = 8.
self._VISION_POLICY_ALERT_3_TIMEOUT = 13.
# no response = alert_3 sustained for certain amount of time
self._NO_RESPONSE_TIMEOUT = 5.
# lockout specs
self._MAX_ALERT_3 = 2
self._MAX_NO_RESPONSE = 1
self._LOCKOUT_TIMES = [int(60 * n_min / DT_DMON) for n_min in [1, 5, 15, 30]]
self._TIMEOUT_RECOVERY_FACTOR_MAX = 5.
self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25
self._FACE_THRESHOLD = 0.7
self._EYE_THRESHOLD = 0.65
self._SG_THRESHOLD = 0.9
self._BLINK_THRESHOLD = 0.865
self._PHONE_THRESH = 0.5
self._POSE_PITCH_THRESHOLD = 0.3133
self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
self._POSE_YAW_THRESHOLD = 0.4020
self._POSE_YAW_THRESHOLD_SLACK = 0.5042
self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
self._POSE_YAW_MIN_STEER_DEG = 30
self._POSE_YAW_STEER_FACTOR = 0.15
self._POSE_YAW_STEER_MAX_OFFSET = 0.3927
self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
self._PITCH_NATURAL_THRESHOLD = 0.449
self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
self._PITCH_NATURAL_VAR = 3*0.01
self._YAW_NATURAL_VAR = 3*0.05
self._PITCH_MAX_OFFSET = 0.124
self._PITCH_MIN_OFFSET = -0.0881
self._YAW_MAX_OFFSET = 0.289
self._YAW_MIN_OFFSET = -0.0246
self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / DT_DMON)
self._DCAM_UNCERTAIN_RESET_COUNT = int(2 / DT_DMON)
self._HI_STD_THRESHOLD = 0.3
self._HI_STD_FALLBACK_TIME = int(10 / DT_DMON) # fall back to wheel touch if model is uncertain for 10s
self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
self._POSE_CALIB_MIN_SPEED = 13 # 30 mph
self._POSE_OFFSET_MIN_COUNT = int(60 / DT_DMON) # valid data counts before calibration completes, 1min cumulative
self._POSE_OFFSET_MAX_COUNT = int(360 / DT_DMON) # stop deweighting new data after 6 min, aka "short term memory"
self._WHEELPOS_CALIB_MIN_SPEED = 11
self._WHEELPOS_THRESHOLD = 0.5
self._WHEELPOS_FILTER_MIN_COUNT = int(15 / DT_DMON) # allow 15 seconds to converge wheel side
self._WHEELPOS_DATA_AVG = 0.03
self._WHEELPOS_DATA_VAR = 3*5.5e-5
self._WHEELPOS_MAX_COUNT = -1
class DriverPose:
def __init__(self, settings):
pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2)
yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2)
self.yaw = 0.
self.pitch = 0.
self.pitch_offsetter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
self.yaw_offsetter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
self.calibrated = False
self.low_std = True
self.cfactor_pitch = 1.
self.cfactor_yaw = 1.
self.steer_yaw_offset = 0.
class DriverBlink:
def __init__(self):
self.left = 0.
self.right = 0.
# model output refers to center of undistorted+leveled image
ref_undistorted_cam = DEVICE_CAMERAS[("tici", "ar0231")].dcam
dcam_undistorted_FL = 598.0
dcam_undistorted_W, dcam_undistorted_H = (ref_undistorted_cam.width, ref_undistorted_cam.height)
def face_orientation_from_model(orient_model, pos_model, rpy_calib):
pitch_model = orient_model[0]
yaw_model = orient_model[1]
face_pixel_position = ((pos_model[0]+0.5)*dcam_undistorted_W, (pos_model[1]+0.5)*dcam_undistorted_H)
yaw_focal_angle = atan2(face_pixel_position[0] - dcam_undistorted_W//2, dcam_undistorted_FL)
pitch_focal_angle = atan2(face_pixel_position[1] - dcam_undistorted_H//2, dcam_undistorted_FL)
pitch = pitch_model + pitch_focal_angle
yaw = -yaw_model + yaw_focal_angle
pitch -= rpy_calib[1]
yaw -= rpy_calib[2]
return pitch, yaw
class DriverMonitoring:
def __init__(self, rhd_saved=False, settings=None, always_on=False):
# init policy settings
self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS()
# init driver status
wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
self.wheelpos_offsetter = RunningStatFilter(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
self.pose = DriverPose(settings=self.settings)
self.blink = DriverBlink()
self.phone_prob = 0.
self.alert_level = AlertLevel.none
self.always_on = always_on
self.distracted_types = defaultdict(bool)
self.driver_distracted = False
self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, DT_DMON)
self.wheel_on_right = False
self.wheel_on_right_last = None
self.wheel_on_right_default = rhd_saved
self.face_detected = False
self.alert_3_cnt = 0
self.cnt_since_alert_3 = 0
self.no_response_timeout = int(self.settings._NO_RESPONSE_TIMEOUT / DT_DMON)
self.no_response_cnt = 0
self.lockout_active = Params().get_bool("DriverTooDistracted")
self.lockout_count = Params().get("DriverLockoutCount") or 0
self.lockout_duration = self.settings._LOCKOUT_TIMES[min(max(self.lockout_count - 1, 0), len(self.settings._LOCKOUT_TIMES) - 1)]
self.lockout_time_elapsed = 0
self.step_change = 0.
self.active_policy = MonitoringPolicy.vision
self.driver_interacting = False
self.is_model_uncertain = False
self.hi_stds = 0
self.model_std_max = 0.
self.threshold_alert_1 = 0.
self.threshold_alert_2 = 0.
self.dcam_uncertain_cnt = 0
self.dcam_reset_cnt = 0
self._reset_awareness()
self._set_policy(MonitoringPolicy.vision)
def _reset_awareness(self):
self.awareness = 1.
self.last_vision_awareness = 1.
self.last_wheeltouch_awareness = 1.
def _set_policy(self, target_policy):
if self.active_policy == MonitoringPolicy.vision and self.awareness <= self.threshold_alert_2:
if target_policy == MonitoringPolicy.vision:
self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
else:
self.step_change = 0.
return # no exploit after orange alert
elif self.awareness <= 0.:
return
if target_policy == MonitoringPolicy.vision:
# when falling back from passive mode to active mode, reset awareness to avoid false alert
if self.active_policy != MonitoringPolicy.vision:
self.last_wheeltouch_awareness = self.awareness
self.awareness = self.last_vision_awareness
self.threshold_alert_1 = 1. - self.settings._VISION_POLICY_ALERT_1_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
self.threshold_alert_2 = 1. - self.settings._VISION_POLICY_ALERT_2_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
self.active_policy = MonitoringPolicy.vision
else:
if self.active_policy == MonitoringPolicy.vision:
self.last_vision_awareness = self.awareness
self.awareness = self.last_wheeltouch_awareness
self.threshold_alert_1 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
self.threshold_alert_2 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
self.step_change = DT_DMON / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
self.active_policy = MonitoringPolicy.wheeltouch
def _set_pose_strictness(self, brake_disengage_prob, car_speed):
bp = brake_disengage_prob
k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
bp_normal = max(min(bp / k1, 0.5),0)
self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_PITCH_THRESHOLD_SLACK,
self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
[self.settings._POSE_YAW_THRESHOLD_SLACK,
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
def _get_distracted_types(self):
self.distracted_types = defaultdict(bool)
if not self.pose.calibrated:
pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
else:
pitch_error = self.pose.pitch - min(max(self.pose.pitch_offsetter.filtered_stat.mean(),
self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
yaw_error = self.pose.yaw - min(max(self.pose.yaw_offsetter.filtered_stat.mean(),
self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional
yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.)
else:
yaw_error = abs(yaw_error)
pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD
yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw
self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold))
self.distracted_types['eye'] = bool((self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD)
self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH)
def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, lowspeed, demo_mode=False, steering_angle_deg=0.):
rhd_pred = driver_state.wheelOnRightProb
# calibrates only when there's movement and either face detected
if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
self.wheelpos_offsetter.push_and_update(rhd_pred)
wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT
if wheelpos_calibrated or demo_mode:
self.wheel_on_right = self.wheelpos_offsetter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
else:
self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
# make sure no switching when engaged
if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
self.wheel_on_right = self.wheel_on_right_last
driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
driver_data.faceOrientationStd, driver_data.facePositionStd)):
return
self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
self.pose.pitch, self.pose.yaw = face_orientation_from_model(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.)
self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR
if self.wheel_on_right:
self.pose.yaw *= -1
self.pose.steer_yaw_offset *= -1
self.wheel_on_right_last = self.wheel_on_right
self.model_std_max = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1])
self.pose.low_std = self.model_std_max < self.settings._HI_STD_THRESHOLD
self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.phone_prob = driver_data.phoneProb
self._get_distracted_types()
self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
self.driver_distraction_filter.update(self.driver_distracted)
# only update offsetter when driver is actively driving the car above a certain speed
if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
self.pose.pitch_offsetter.push_and_update(self.pose.pitch)
self.pose.yaw_offsetter.push_and_update(self.pose.yaw)
self.pose.calibrated = self.pose.pitch_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT and \
self.pose.yaw_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT
if self.face_detected and not self.driver_distracted:
dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD
if dcam_uncertain and not lowspeed:
self.dcam_uncertain_cnt += 1
self.dcam_reset_cnt = 0
else:
self.dcam_reset_cnt += 1
if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT:
self.dcam_uncertain_cnt = 0
self.is_model_uncertain = self.hi_stds >= self.settings._HI_STD_FALLBACK_TIME
self._set_policy(MonitoringPolicy.vision if self.face_detected and not self.is_model_uncertain else MonitoringPolicy.wheeltouch)
if self.face_detected and not self.pose.low_std and not self.driver_distracted:
self.hi_stds += 1
elif self.face_detected and self.pose.low_std:
self.hi_stds = 0
def _update_events(self, driver_engaged, op_engaged, lowspeed, wrong_gear):
self.alert_level = AlertLevel.none
self.driver_interacting = driver_engaged
if self.alert_3_cnt >= self.settings._MAX_ALERT_3 or self.no_response_cnt >= self.settings._MAX_NO_RESPONSE:
if not self.lockout_active:
self.lockout_count += 1
self.lockout_duration = self.settings._LOCKOUT_TIMES[min(self.lockout_count - 1, len(self.settings._LOCKOUT_TIMES) - 1)]
Params().put("DriverLockoutCount", self.lockout_count)
self.lockout_active = True
if self.lockout_active:
self.lockout_time_elapsed += 1
if self.lockout_time_elapsed > self.lockout_duration:
self.lockout_active = False
self.alert_3_cnt = 0
self.cnt_since_alert_3 = 0
self.no_response_cnt = 0
self.lockout_time_elapsed = 0
always_on_valid = self.always_on and not wrong_gear
if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \
(not always_on_valid and not op_engaged) or \
(always_on_valid and not op_engaged and self.awareness <= 0):
# always reset on disengage with normal mode; disengage resets only on red if always on
self._reset_awareness()
return
awareness_prev = self.awareness
_reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1
_reaching_alert_3 = self.awareness - self.step_change <= 0
lowspeed_exemption = lowspeed and _reaching_alert_1
always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3
if self.awareness > 0 and \
((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or lowspeed_exemption):
if self.driver_interacting:
self._reset_awareness()
return
# only restore awareness when paying attention and alert is not red
self.awareness = min(self.awareness + ((self.settings._TIMEOUT_RECOVERY_FACTOR_MAX-self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*
(1.-self.awareness)+self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
if self.awareness == 1.:
self.last_wheeltouch_awareness = min(self.last_wheeltouch_awareness + self.step_change, 1.)
# don't display alert banner when awareness is recovering and has cleared orange
if self.awareness > self.threshold_alert_2:
return
certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected
maybe_distracted = self.is_model_uncertain or not self.face_detected
if certainly_distracted or maybe_distracted:
# should always be counting if distracted unless at low speed and reaching green
# also will not be reaching 0 if DM is active when not engaged
if not (lowspeed_exemption or always_on_exemption):
self.awareness = max(self.awareness - self.step_change, -0.1)
if self.awareness <= 0.:
# terminal alert: disengagement required
self.alert_level = AlertLevel.three
if awareness_prev > 0.:
self.alert_3_cnt += 1
self.cnt_since_alert_3 = 0
else:
self.cnt_since_alert_3 += 1
if self.cnt_since_alert_3 == self.no_response_timeout:
self.no_response_cnt += 1
else:
if self.awareness <= self.threshold_alert_2:
self.alert_level = AlertLevel.two
elif self.awareness <= self.threshold_alert_1:
self.alert_level = AlertLevel.one
def get_state_packet(self, valid=True):
# build driverMonitoringState packet
dat = messaging.new_message('driverMonitoringState', valid=valid)
dm = dat.driverMonitoringState
dm.lockout = self.lockout_active
dm.lockoutCount = self.lockout_count
if self.lockout_active:
dm.lockoutMinutesRemaining = max(1, round((self.lockout_duration - self.lockout_time_elapsed) * DT_DMON / 60.))
dm.alert3Count = self.alert_3_cnt
dm.noResponseCount = self.no_response_cnt
dm.noResponseForceDecel = self.alert_level == AlertLevel.three and self.cnt_since_alert_3 >= self.no_response_timeout
dm.alwaysOn = self.always_on
dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2
dm.alertLevel = self.alert_level
dm.activePolicy = self.active_policy
dm.isRHD = self.wheel_on_right
dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos_offsetter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT)
dm.rhdCalibration.offset = self.wheelpos_offsetter.filtered_stat.M
dm.visionPolicyState.awarenessPercent = to_percent(self.last_vision_awareness if self.active_policy != MonitoringPolicy.vision else self.awareness)
dm.visionPolicyState.awarenessStep = self.step_change if self.active_policy == MonitoringPolicy.vision else 0.
dm.visionPolicyState.isDistracted = self.driver_distracted
dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose']
dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye']
dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone']
dm.visionPolicyState.faceDetected = self.face_detected
dm.visionPolicyState.pose.pitch = self.pose.pitch
dm.visionPolicyState.pose.yaw = self.pose.yaw
dm.visionPolicyState.pose.calibrated = self.pose.calibrated
dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offsetter.filtered_stat.M
dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offsetter.filtered_stat.M
dm.visionPolicyState.pose.uncertainty = self.model_std_max
dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME)
dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / self.settings._DCAM_UNCERTAIN_ALERT_COUNT)
dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.last_wheeltouch_awareness if self.active_policy == MonitoringPolicy.vision else self.awareness)
dm.wheeltouchPolicyState.awarenessStep = 0. if self.active_policy == MonitoringPolicy.vision else self.step_change
dm.wheeltouchPolicyState.driverInteracting = self.driver_interacting
return dat
def run_step(self, sm, demo=False):
if demo:
car_speed = 30
enabled = True
wrong_gear = False
lowspeed = False
driver_engaged = False
brake_disengage_prob = 1.0
steering_angle_deg = 0.0
rpyCalib = [0., 0., 0.]
else:
car_speed = sm['carState'].vEgo
enabled = sm['selfdriveState'].enabled
wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
lowspeed = car_speed < self.settings._ALERT_MIN_SPEED
driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
steering_angle_deg = sm['carState'].steeringAngleDeg
rpyCalib = sm['liveCalibration'].rpyCalib
self._set_pose_strictness(
brake_disengage_prob=brake_disengage_prob,
car_speed=car_speed,
)
# Parse data from dmonitoringmodeld
self._update_states(
driver_state=sm['driverStateV2'],
cal_rpy=rpyCalib,
car_speed=car_speed,
op_engaged=enabled,
lowspeed=lowspeed,
demo_mode=demo,
steering_angle_deg=steering_angle_deg,
)
# Update distraction events
self._update_events(
driver_engaged=driver_engaged,
op_engaged=enabled,
lowspeed=lowspeed,
wrong_gear=wrong_gear,
)

View File

@@ -1,61 +0,0 @@
from types import SimpleNamespace
import pytest
from cereal import log
from openpilot.selfdrive.monitoring.dmonitoringd import create_driver_monitoring, get_dm_inputs, use_legacy_dm
from openpilot.selfdrive.monitoring.legacy_policy import DriverMonitoring as LegacyDriverMonitoring
from openpilot.selfdrive.monitoring.policy import DriverMonitoring as UpstreamDriverMonitoring
@pytest.mark.parametrize("device_type, expected_type", [
("mici", LegacyDriverMonitoring),
("tici", UpstreamDriverMonitoring),
("tizi", UpstreamDriverMonitoring),
])
def test_policy_selection(device_type, expected_type):
assert use_legacy_dm(device_type) == (device_type == "mici")
assert isinstance(create_driver_monitoring(device_type, False, False), expected_type)
def test_mici_uses_legacy_thresholds():
dm = create_driver_monitoring("mici", False, False)
assert dm.settings._DISTRACTED_TIME == 11.
assert dm.settings._AWARENESS_TIME == 30.
assert dm.settings._PHONE_THRESH == 0.75
@pytest.mark.parametrize("enabled, lat_active, expected", [
(False, False, False),
(True, False, True),
(False, True, True),
(True, True, True),
])
def test_iq_enabled_adapter(enabled, lat_active, expected):
sm = {
'driverStateV2': object(),
'liveCalibration': object(),
'carState': object(),
'selfdriveState': SimpleNamespace(enabled=enabled),
'modelV2': object(),
'carControl': SimpleNamespace(latActive=lat_active),
}
assert get_dm_inputs(sm)['selfdriveState'].enabled == expected
def test_legacy_policy_packet_uses_shared_schema():
dm = create_driver_monitoring("mici", False, False)
dm.awareness = 0.
dm.terminal_alert_cnt = 2
dm.terminal_time = dm.settings._MAX_TERMINAL_DURATION
dm.too_distracted = True
state = dm.get_state_packet().driverMonitoringState
assert state.lockout
assert state.alert3Count == 2
assert state.noResponseCount == 1
assert state.noResponseForceDecel
assert state.alertLevel == log.DriverMonitoringState.AlertLevel.three

View File

@@ -1,15 +1,19 @@
from cereal import log import numpy as np
import pytest
from cereal import log, car
from openpilot.common.realtime import DT_DMON from openpilot.common.realtime import DT_DMON
from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS
from openpilot.system.hardware import HARDWARE
EventName = log.OnroadEvent.EventName EventName = log.OnroadEvent.EventName
dm_settings = DRIVER_MONITOR_SETTINGS() dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
TEST_TIMESPAN = 120 # seconds TEST_TIMESPAN = 120 # seconds
DISTRACTED_SECONDS_TO_ORANGE = dm_settings._VISION_POLICY_ALERT_2_TIMEOUT + 1 DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
DISTRACTED_SECONDS_TO_RED = dm_settings._VISION_POLICY_ALERT_3_TIMEOUT + 1 DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1
INVISIBLE_SECONDS_TO_ORANGE = dm_settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + 1 INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
INVISIBLE_SECONDS_TO_RED = dm_settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + 1 INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1
def make_msg(face_detected, distracted=False, model_uncertain=False): def make_msg(face_detected, distracted=False, model_uncertain=False):
ds = log.DriverStateV2.new_message() ds = log.DriverStateV2.new_message()
@@ -33,7 +37,7 @@ msg_ATTENTIVE = make_msg(True)
msg_DISTRACTED = make_msg(True, distracted=True) msg_DISTRACTED = make_msg(True, distracted=True)
msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True) msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True)
msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True) msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True)
msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._HI_STD_THRESHOLD*1.5) msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5)
# driver interaction with car # driver interaction with car
car_interaction_DETECTED = True car_interaction_DETECTED = True
@@ -47,66 +51,51 @@ always_true = [True] * int(TEST_TIMESPAN / DT_DMON)
always_false = [False] * int(TEST_TIMESPAN / DT_DMON) always_false = [False] * int(TEST_TIMESPAN / DT_DMON)
class TestMonitoring: class TestMonitoring:
def _run_seq(self, msgs, interaction, engaged, lowspeed): def _run_seq(self, msgs, interaction, engaged, standstill):
DM = DriverMonitoring() DM = DriverMonitoring()
alert_lvls = [] events = []
for idx in range(len(msgs)): for idx in range(len(msgs)):
DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], lowspeed[idx]) DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx])
# cal_rpy and car_speed don't matter here # cal_rpy and car_speed don't matter here
# evaluate events at 10Hz for tests # evaluate events at 10Hz for tests
DM._update_events(interaction[idx], engaged[idx], lowspeed[idx], 0) DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0)
alert_lvls.append(DM.alert_level) events.append(DM.current_events)
assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs" assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs"
return alert_lvls, DM return events, DM
def _assert_no_events(self, events):
assert all(not len(e) for e in events)
# engaged, driver is attentive all the time # engaged, driver is attentive all the time
def test_fully_aware_driver(self): def test_fully_aware_driver(self):
alert_lvls, d_status = self._run_seq(always_attentive, always_false, always_true, always_false) events, _ = self._run_seq(always_attentive, always_false, always_true, always_false)
assert all(a == 0 for a in alert_lvls) self._assert_no_events(events)
assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.vision
# engaged, driver is distracted and does nothing # engaged, driver is distracted and does nothing
def test_fully_distracted_driver(self): def test_fully_distracted_driver(self):
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
s = d_status.settings assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0
assert alert_lvls[int(s._VISION_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0 assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \
assert alert_lvls[int((s._VISION_POLICY_ALERT_1_TIMEOUT + \ ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
(s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1 EventName.preDriverDistracted
assert alert_lvls[int((s._VISION_POLICY_ALERT_2_TIMEOUT + \ assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \
(s._VISION_POLICY_ALERT_3_TIMEOUT - s._VISION_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2 ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert alert_lvls[int((s._VISION_POLICY_ALERT_3_TIMEOUT + \ assert events[int((d_status.settings._DISTRACTED_TIME + \
(TEST_TIMESPAN - 10 - s._VISION_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted
assert isinstance(d_status.awareness, float) assert isinstance(d_status.awareness, float)
# engaged, distracted past red and beyond the no-response window -> unavailability response + lockout
def test_distracted_lockout(self):
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
assert alert_lvls[int(DISTRACTED_SECONDS_TO_RED / DT_DMON)] == 3
assert d_status.lockout_active
assert d_status.lockout_time_elapsed > 0
assert d_status.lockout_count >= 1
# no face -> wheeltouch red, sustained past the no-response timeout -> unavailability response + lockout
def test_invisible_lockout(self):
_, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch
assert d_status.lockout_active
assert d_status.lockout_count >= 1
# engaged, no face detected the whole time, no action # engaged, no face detected the whole time, no action
def test_fully_invisible_driver(self): def test_fully_invisible_driver(self):
alert_lvls, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
s = d_status.settings assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0
assert alert_lvls[int(s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0 assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT + \ ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
(s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1 EventName.preDriverUnresponsive
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + \ assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \
(s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2 ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + \ assert events[int((d_status.settings._AWARENESS_TIME + \
(TEST_TIMESPAN - 10 - s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive
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 # 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 # - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention
@@ -117,13 +106,13 @@ class TestMonitoring:
[msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON)) [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) + \ 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)) [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON))
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) events, _ = 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 len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)] == 0 assert len(events[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 events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)] == 2 assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)] == 0 assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0
# engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ # 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 # driver dodges, and then touches wheel to no avail, disengages and reengages
@@ -141,31 +130,31 @@ class TestMonitoring:
= [True] * int(1/DT_DMON) = [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)] \ 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) = [False] * int(0.5/DT_DMON)
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) events, _ = 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 events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)] == 3 assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] == 3 assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)] == 0 assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0
# engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears # 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 # - both actions should clear the alert, but momentary appearance should not
def test_sometimes_transparent_commuter(self): def test_sometimes_transparent_commuter(self):
for _visible_time in (0.5, 10): _visible_time = np.random.choice([0.5, 10])
ds_vector = always_no_face[:]*2 ds_vector = always_no_face[:]*2
interaction_vector = always_false[:]*2 interaction_vector = always_false[:]*2
ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ 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) [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) interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0 assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0 assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0
if _visible_time == 0.5: if _visible_time == 0.5:
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 2 assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.preDriverUnresponsive
elif _visible_time == 10: elif _visible_time == 10:
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0 assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0
# engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
# - only disengage will clear the alert # - only disengage will clear the alert
@@ -177,51 +166,105 @@ class TestMonitoring:
ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) 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) 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) op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON)
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0 assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3 assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3 assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3 assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)] == 0 assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0
# disengaged, always distracted driver # disengaged, always distracted driver
# - dm should stay quiet when not engaged # - dm should stay quiet when not engaged
def test_pure_dashcam_user(self): def test_pure_dashcam_user(self):
alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false) events, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
assert all(a == 0 for a in alert_lvls) assert sum(len(event) for event in events) == 0
# engaged, car stops at traffic light, down to orange, no action, then car starts moving # 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 # - should only reach green when stopped, but continues counting down on launch
def test_long_traffic_light_victim(self): def test_long_traffic_light_victim(self):
_redlight_time = 60 # seconds _redlight_time = 60 # seconds
lowspeed_vector = always_true[:] standstill_vector = always_true[:]
lowspeed_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON)
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector) events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
s = d_status.settings assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \
assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0 EventName.preDriverDistracted
_alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted
assert alert_lvls[int((_redlight_time+0.5)/DT_DMON)] == 1 assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted
assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2
# engaged, distracted while moving, then car stops after reaching orange
# - should reset timer to pre green at low speed
def test_distracted_then_stops(self):
_stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange
lowspeed_vector = always_false[:]
lowspeed_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON)
alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector)
# just before and briefly after stopping: orange alert; goes away quickly after stopped
assert alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2
assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0
# engaged, model is somehow uncertain and driver is distracted # engaged, model is somehow uncertain and driver is distracted
# - should fall back to wheel touch after uncertain alert # - should fall back to wheel touch after uncertain alert
def test_somehow_indecisive_model(self): def test_somehow_indecisive_model(self):
ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON) ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON)
interaction_vector = always_false[:] interaction_vector = always_false[:]
alert_lvls, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
s = d_status.settings assert EventName.preDriverUnresponsive in \
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)] == 1 events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 2 assert EventName.promptDriverUnresponsive in \
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 3 events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
assert EventName.driverUnresponsive in \
events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
@pytest.mark.parametrize("enabled_state, lat_active_state, expected", [
(False, False, False), # Both Disabled
(True, False, True), # OP Enabled, Lat Inactive
(False, True, True), # OP Disabled, Lat Active (e.g. AOL)
(True, True, True) # Both Active
])
def test_enabled_states(enabled_state, lat_active_state, expected):
"""
Test DriverMonitoring.run_step with all 4 combinations of:
- selfdriveState.enabled (True/False)
- carControl.latActive (True/False)
"""
cs = car.CarState.new_message()
cs.vEgo = 30.0
cs.gearShifter = car.CarState.GearShifter.drive
cs.standstill = False
cs.steeringPressed = False
cs.gasPressed = False
ss = log.SelfdriveState.new_message()
ss.enabled = enabled_state
cc = car.CarControl.new_message()
cc.latActive = lat_active_state
mv2 = log.ModelDataV2.new_message()
mv2.meta.disengagePredictions.brakeDisengageProbs = [0.0]
lc = log.LiveCalibrationData.new_message()
lc.rpyCalib = [0.0, 0.0, 0.0]
ds = make_msg(False)
sm = {
'carState': cs,
'selfdriveState': ss,
'carControl': cc,
'modelV2': mv2,
'liveCalibration': lc,
'driverStateV2': ds
}
driver_monitoring = DriverMonitoring()
# run_test doesn't assign enabled to a variable, so we need to spy on _update_events to see its value
captured_args = []
original_update_events = driver_monitoring._update_events
def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
captured_args.append(op_engaged)
return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed)
driver_monitoring._update_events = spy_update_events
driver_monitoring.run_step(sm, demo=False)
# Assertion
assert len(captured_args) == 1, "Expected _update_events to be called exactly once"
actual_enabled = captured_args[0]
assert actual_enabled == expected, f"Expected op_engaged={expected}, but got {actual_enabled}"

View File

@@ -79,15 +79,6 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4) Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4)
def too_distracted_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
if sm['driverMonitoringState'].lockout:
mins_left = sm['driverMonitoringState'].lockoutMinutesRemaining
if mins_left <= 0:
return NoEntryAlert("Distraction Level Too High", priority=Priority.HIGH)
return NoEntryAlert("Too Distracted", f"{mins_left} minute{'s' if mins_left != 1 else ''} Left", priority=Priority.HIGH)
return NoEntryAlert("Pay Attention to Engage", priority=Priority.HIGH)
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating' first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating'
return Alert( return Alert(
@@ -235,8 +226,7 @@ def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messagin
return NormalPermanentAlert(title, text) return NormalPermanentAlert(title, text)
def invalid_lkas_setting_no_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, def invalid_lkas_setting_no_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
metric: bool, soft_disable_time: int, personality) -> Alert:
if CP.brand == "tesla": if CP.brand == "tesla":
return NoEntryAlert("FSD / Autosteer is active", alert_text_1="Dashcam Mode") return NoEntryAlert("FSD / Autosteer is active", alert_text_1="Dashcam Mode")
return NoEntryAlert("Invalid LKAS setting") return NoEntryAlert("Invalid LKAS setting")
@@ -370,15 +360,15 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8), Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8),
}, },
EventName.driverDistracted1: { EventName.preDriverDistracted: {
ET.PERMANENT: Alert( ET.PERMANENT: Alert(
"Pay Attention", "Pay Attention",
"", "",
AlertStatus.normal, AlertSize.small, AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, .1), Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
}, },
EventName.driverDistracted2: { EventName.promptDriverDistracted: {
ET.PERMANENT: Alert( ET.PERMANENT: Alert(
"Pay Attention", "Pay Attention",
"Driver Distracted", "Driver Distracted",
@@ -386,7 +376,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
}, },
EventName.driverDistracted3: { EventName.driverDistracted: {
ET.PERMANENT: Alert( ET.PERMANENT: Alert(
"DISENGAGE IMMEDIATELY", "DISENGAGE IMMEDIATELY",
"Driver Distracted", "Driver Distracted",
@@ -394,7 +384,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
}, },
EventName.driverUnresponsive1: { EventName.preDriverUnresponsive: {
ET.PERMANENT: Alert( ET.PERMANENT: Alert(
"Touch Steering Wheel: No Face Detected", "Touch Steering Wheel: No Face Detected",
"", "",
@@ -402,7 +392,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1), Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1),
}, },
EventName.driverUnresponsive2: { EventName.promptDriverUnresponsive: {
ET.PERMANENT: Alert( ET.PERMANENT: Alert(
"Touch Steering Wheel", "Touch Steering Wheel",
"Driver Unresponsive", "Driver Unresponsive",
@@ -410,7 +400,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
}, },
EventName.driverUnresponsive3: { EventName.driverUnresponsive: {
ET.PERMANENT: Alert( ET.PERMANENT: Alert(
"DISENGAGE IMMEDIATELY", "DISENGAGE IMMEDIATELY",
"Driver Unresponsive", "Driver Unresponsive",
@@ -642,7 +632,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
}, },
EventName.tooDistracted: { EventName.tooDistracted: {
ET.NO_ENTRY: too_distracted_alert, ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"),
}, },
EventName.excessiveActuation: { EventName.excessiveActuation: {
@@ -888,14 +878,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
if HARDWARE.get_device_type() == 'mici': if HARDWARE.get_device_type() == 'mici':
EVENTS.update({ EVENTS.update({
EventName.driverDistracted1: { EventName.preDriverDistracted: {
ET.PERMANENT: Alert( ET.PERMANENT: Alert(
"Pay Attention", "Pay Attention",
"", "",
AlertStatus.normal, AlertSize.small, AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, 2), Priority.LOW, VisualAlert.none, AudibleAlert.none, 2),
}, },
EventName.driverDistracted2: { EventName.promptDriverDistracted: {
ET.PERMANENT: Alert( ET.PERMANENT: Alert(
"Pay Attention", "Pay Attention",
"Driver Distracted", "Driver Distracted",

View File

@@ -54,8 +54,6 @@ EventName = log.OnroadEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type ButtonType = car.CarState.ButtonEvent.Type
SafetyModel = car.CarParams.SafetyModel SafetyModel = car.CarParams.SafetyModel
TurnDirection = custom.IQTurnSignalDirection TurnDirection = custom.IQTurnSignalDirection
AlertLevel = log.DriverMonitoringState.AlertLevel
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
@@ -129,6 +127,7 @@ class SelfdriveD(GapButtonActions):
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
self.nav_exit_lane_change = self._read_nav_exit_lane_change() self.nav_exit_lane_change = self._read_nav_exit_lane_change()
self.model_download_pending = self.params.get("ModelManager_DownloadIndex") is not None
car_recognized = self.CP.brand != 'mock' car_recognized = self.CP.brand != 'mock'
@@ -149,7 +148,6 @@ class SelfdriveD(GapButtonActions):
self.events_prev = [] self.events_prev = []
self.logged_comm_issue = None self.logged_comm_issue = None
self.not_running_prev = None self.not_running_prev = None
self.dm_lockout_set = False
self.experimental_mode = False self.experimental_mode = False
self.personality = get_sanitize_int_param( self.personality = get_sanitize_int_param(
"LongitudinalPersonality", "LongitudinalPersonality",
@@ -186,6 +184,7 @@ class SelfdriveD(GapButtonActions):
self.events_iq = IQEvents() self.events_iq = IQEvents()
self.events_iq_prev = [] self.events_iq_prev = []
self._cached_dm_event_names: tuple[int, ...] = ()
self._cached_plan_event_names: tuple[int, ...] = () self._cached_plan_event_names: tuple[int, ...] = ()
self._cached_model_event_names: tuple[int, ...] = () self._cached_model_event_names: tuple[int, ...] = ()
self._cached_nav_event_names: tuple[int, ...] = () self._cached_nav_event_names: tuple[int, ...] = ()
@@ -207,6 +206,10 @@ class SelfdriveD(GapButtonActions):
if self.sm.updated['iqPlan']: if self.sm.updated['iqPlan']:
self._cached_plan_event_names = tuple(event.name.raw for event in self._get_longitudinal_plan_ext().events) self._cached_plan_event_names = tuple(event.name.raw for event in self._get_longitudinal_plan_ext().events)
def _refresh_cached_dm_events(self) -> None:
if self.sm.updated['driverMonitoringState']:
self._cached_dm_event_names = tuple(event.name.raw for event in self.sm['driverMonitoringState'].events)
def _refresh_cached_model_events(self) -> None: def _refresh_cached_model_events(self) -> None:
if not self.sm.updated['iqDriveModelData']: if not self.sm.updated['iqDriveModelData']:
return return
@@ -295,24 +298,8 @@ class SelfdriveD(GapButtonActions):
self.events.add(EventName.resumeBlocked) self.events.add(EventName.resumeBlocked)
if not self.CP.notCar: if not self.CP.notCar:
if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set: self._refresh_cached_dm_events()
self.params.put_bool("DriverTooDistracted", True) self._add_event_names(self._cached_dm_event_names)
self.dm_lockout_set = True
elif not self.sm['driverMonitoringState'].lockout and self.dm_lockout_set:
self.params.remove("DriverTooDistracted")
self.dm_lockout_set = False
if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout:
self.events.add(EventName.tooDistracted)
vision_dm = self.sm['driverMonitoringState'].activePolicy == MonitoringPolicy.vision
if self.sm['driverMonitoringState'].alertLevel == AlertLevel.one:
self.events.add(EventName.driverDistracted1 if vision_dm else EventName.driverUnresponsive1)
elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.two:
self.events.add(EventName.driverDistracted2 if vision_dm else EventName.driverUnresponsive2)
elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.three:
self.events.add(EventName.driverDistracted3 if vision_dm else EventName.driverUnresponsive3)
self._refresh_cached_plan_events() self._refresh_cached_plan_events()
self._add_iq_event_names(self._cached_plan_event_names) self._add_iq_event_names(self._cached_plan_event_names)
@@ -443,6 +430,8 @@ class SelfdriveD(GapButtonActions):
self.not_running_prev = not_running self.not_running_prev = not_running
if self.sm.recv_frame['managerState'] and (not_running - self.ignored_processes): if self.sm.recv_frame['managerState'] and (not_running - self.ignored_processes):
self.events.add(EventName.processNotRunning) self.events.add(EventName.processNotRunning)
if 'iqmodeld' in not_running and self.model_download_pending:
self.events_iq.add(custom.IQOnroadEvent.EventName.modelUpdating)
else: else:
if not SIMULATION and not self.rk.lagging: if not SIMULATION and not self.rk.lagging:
if not self.sm.all_alive(self.camera_packets): if not self.sm.all_alive(self.camera_packets):
@@ -756,6 +745,7 @@ class SelfdriveD(GapButtonActions):
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
self.personality = self.params.get("LongitudinalPersonality", return_default=True) self.personality = self.params.get("LongitudinalPersonality", return_default=True)
self.nav_exit_lane_change = self._read_nav_exit_lane_change() self.nav_exit_lane_change = self._read_nav_exit_lane_change()
self.model_download_pending = self.params.get("ModelManager_DownloadIndex") is not None
self.aol.read_params() self.aol.read_params()
time.sleep(0.1) time.sleep(0.1)

View File

@@ -1,40 +0,0 @@
from types import SimpleNamespace
from cereal import car, log
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
EventName = log.OnroadEvent.EventName
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
def test_driver_monitoring_alert_stages():
expected_sounds = {
EventName.driverDistracted1: AudibleAlert.preAlert,
EventName.driverDistracted2: AudibleAlert.promptDistracted,
EventName.driverDistracted3: AudibleAlert.warningImmediate,
EventName.driverUnresponsive1: AudibleAlert.none,
EventName.driverUnresponsive2: AudibleAlert.promptDistracted,
EventName.driverUnresponsive3: AudibleAlert.warningImmediate,
}
for event_name, audible_alert in expected_sounds.items():
assert EVENTS[event_name][ET.PERMANENT].audible_alert == audible_alert
def test_driver_monitoring_lockout_alert():
callback = EVENTS[EventName.tooDistracted][ET.NO_ENTRY]
sm = {'driverMonitoringState': SimpleNamespace(lockout=True, lockoutMinutesRemaining=5)}
alert = callback(None, None, sm, False, 0, None)
assert alert.alert_text_1 == "5 minutes Left"
assert alert.alert_text_2 == "Too Distracted"
def test_legacy_driver_monitoring_lockout_alert():
callback = EVENTS[EventName.tooDistracted][ET.NO_ENTRY]
sm = {'driverMonitoringState': SimpleNamespace(lockout=True, lockoutMinutesRemaining=0)}
alert = callback(None, None, sm, False, 0, None)
assert alert.alert_text_2 == "Distraction Level Too High"

View File

@@ -455,46 +455,21 @@ def migrate_onroadEvents(msgs):
return ops, [], [] return ops, [], []
@migration(inputs=["driverMonitoringStateDEPRECATED"]) @migration(inputs=["driverMonitoringState"])
def migrate_driverMonitoringState(msgs): def migrate_driverMonitoringState(msgs):
ops = [] ops = []
for index, msg in msgs: for index, msg in msgs:
old = msg.driverMonitoringStateDEPRECATED msg = msg.as_builder()
new_msg = messaging.new_message('driverMonitoringState', valid=msg.valid, logMonoTime=msg.logMonoTime) events = []
dm = new_msg.driverMonitoringState for event in msg.driverMonitoringState.eventsDEPRECATED:
dm.isRHD = old.isRHD try:
dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision if old.isActiveMode else \ if not str(event.name).endswith('DEPRECATED'):
log.DriverMonitoringState.MonitoringPolicy.wheeltouch # dict converts name enum into string representation
events.append(log.OnroadEvent(**event.to_dict()))
except RuntimeError: # Member was null
traceback.print_exc()
AlertLevel = log.DriverMonitoringState.AlertLevel msg.driverMonitoringState.events = events
event_to_alert_level = { ops.append((index, msg.as_reader()))
'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, [], [] return ops, [], []

View File

@@ -10,7 +10,7 @@ from openpilot.selfdrive.ui.widgets.offroad_alerts import UpdateAlert, OffroadAl
from openpilot.selfdrive.ui.widgets.setup import SetupWidget from openpilot.selfdrive.ui.widgets.setup import SetupWidget
from openpilot.selfdrive.ui.widgets.inspire_widget import InspireWidget from openpilot.selfdrive.ui.widgets.inspire_widget import InspireWidget
from openpilot.selfdrive.ui.widgets.map_panel_widget import MapPanelWidget from openpilot.selfdrive.ui.widgets.map_panel_widget import MapPanelWidget
from openpilot.iqpilot.ui.layouts.settings.trips import TripsLayout from openpilot.iqpilot.ui.layouts.settings.drive_history import TripsLayout
from openpilot.selfdrive.ui.layouts.sidebar import NETWORK_TYPES from openpilot.selfdrive.ui.layouts.sidebar import NETWORK_TYPES
from openpilot.selfdrive.ui.lib.wifi_ssid import current_ssid from openpilot.selfdrive.ui.lib.wifi_ssid import current_ssid
from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.selfdrive.ui.ui_state import ui_state

View File

@@ -214,7 +214,7 @@ class TogglesLayout(Widget):
toyota_stock_long_forced = bool( toyota_stock_long_forced = bool(
ui_state.CP is not None and ui_state.CP is not None and
ui_state.CP.brand == "toyota" and ui_state.CP.brand == "toyota" and
self._params.get_bool("ToyotaEnforceStockLongitudinal") self._params.get_bool("IQToyotaFactoryLong")
) )
iq_modes_selectable = alpha_available or alpha_requested or toyota_stock_long_forced iq_modes_selectable = alpha_available or alpha_requested or toyota_stock_long_forced
availability_note = "" availability_note = ""
@@ -268,7 +268,7 @@ class TogglesLayout(Widget):
def _apply_longitudinal_control_mode(self, button_index: int): def _apply_longitudinal_control_mode(self, button_index: int):
# 0 = Stock ACC, 1 = IQ.Standard, 2 = IQ.Dynamic, 3 = IQ.Pilot # 0 = Stock ACC, 1 = IQ.Standard, 2 = IQ.Dynamic, 3 = IQ.Pilot
previous_alpha = self._params.get_bool("AlphaLongitudinalEnabled") previous_alpha = self._params.get_bool("AlphaLongitudinalEnabled")
previous_toyota_stock_long = self._params.get_bool("ToyotaEnforceStockLongitudinal") previous_toyota_stock_long = self._params.get_bool("IQToyotaFactoryLong")
if button_index == 0: if button_index == 0:
self._params.put_bool("AlphaLongitudinalEnabled", False) self._params.put_bool("AlphaLongitudinalEnabled", False)
@@ -289,9 +289,9 @@ class TogglesLayout(Widget):
self._params.put_bool("IQDynamicMode", False) self._params.put_bool("IQDynamicMode", False)
if button_index != 0 and previous_toyota_stock_long: if button_index != 0 and previous_toyota_stock_long:
self._params.put_bool("ToyotaEnforceStockLongitudinal", False) self._params.put_bool("IQToyotaFactoryLong", False)
if previous_alpha != self._params.get_bool("AlphaLongitudinalEnabled") or previous_toyota_stock_long != self._params.get_bool("ToyotaEnforceStockLongitudinal"): if previous_alpha != self._params.get_bool("AlphaLongitudinalEnabled") or previous_toyota_stock_long != self._params.get_bool("IQToyotaFactoryLong"):
self._params.put_bool("OnroadCycleRequested", True) self._params.put_bool("OnroadCycleRequested", True)
def _toggle_callback(self, state: bool, param: str): def _toggle_callback(self, state: bool, param: str):

View File

@@ -1,7 +1,7 @@
import pyray as rl import pyray as rl
from collections.abc import Callable from collections.abc import Callable
from openpilot.iqpilot.ui.layouts.settings.trips import TripsLayout from openpilot.iqpilot.ui.layouts.settings.drive_history import TripsLayout
from openpilot.selfdrive.ui.widgets.screen_header import ScreenHeader, HEADER_HEIGHT from openpilot.selfdrive.ui.widgets.screen_header import ScreenHeader, HEADER_HEIGHT
from openpilot.system.ui.lib.multilang import tr from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets import Widget

View File

@@ -200,7 +200,7 @@ class TrainingGuideDMTutorial(Widget):
looking_center = False looking_center = False
# stay at 100% once reached # stay at 100% once reached
if (dm_state.visionPolicyState.faceDetected and looking_center) or self._progress.x > 0.99: if (dm_state.faceDetected and looking_center) or self._progress.x > 0.99:
slow = self._progress.x < 0.25 slow = self._progress.x < 0.25
duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION
self._progress.x += 1.0 / (duration * gui_app.target_fps) self._progress.x += 1.0 / (duration * gui_app.target_fps)

View File

@@ -223,7 +223,7 @@ class ModelsLayoutMici(NavScroller):
@staticmethod @staticmethod
def _read_favorites() -> set: def _read_favorites() -> set:
favs = ui_state.params.get("ModelManager_Favs") favs = ui_state.params.get("IQModelFavorites")
return set(favs.split(';')) if favs else set() return set(favs.split(';')) if favs else set()
def _toggle_favorite(self, bundle) -> bool: def _toggle_favorite(self, bundle) -> bool:
@@ -232,7 +232,7 @@ class ModelsLayoutMici(NavScroller):
favs.discard(bundle.ref) favs.discard(bundle.ref)
else: else:
favs.add(bundle.ref) favs.add(bundle.ref)
ui_state.params.put("ModelManager_Favs", ';'.join(sorted(favs))) ui_state.params.put("IQModelFavorites", ';'.join(sorted(favs)))
return bundle.ref in favs return bundle.ref in favs
def _confirm_clear_cache(self): def _confirm_clear_cache(self):

View File

@@ -11,7 +11,7 @@ from openpilot.selfdrive.ui.mici.layouts.settings.cruise import CruiseLayoutMici
from openpilot.selfdrive.ui.mici.layouts.settings.visuals import VisualsLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.visuals import VisualsLayoutMici
from openpilot.selfdrive.ui.mici.layouts.settings.models import ModelsLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.models import ModelsLayoutMici
from openpilot.selfdrive.ui.mici.layouts.settings.display import DisplayLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.display import DisplayLayoutMici
from openpilot.selfdrive.ui.mici.layouts.settings.trips import TripsLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.drive_history import TripsLayoutMici
from openpilot.selfdrive.ui.mici.layouts.settings.vehicle import VehicleLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.vehicle import VehicleLayoutMici
from openpilot.selfdrive.ui.mici.layouts.settings.dashcam import DashcamLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.dashcam import DashcamLayoutMici
from openpilot.selfdrive.ui.mici.layouts.settings.network.network_layout import NetworkLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.network.network_layout import NetworkLayoutMici

View File

@@ -69,17 +69,17 @@ class VehicleLayoutMici(NavScroller):
self._vehicle_btn = BigButton("vehicle") self._vehicle_btn = BigButton("vehicle")
self._vehicle_btn.set_click_callback(self._on_vehicle_clicked) self._vehicle_btn.set_click_callback(self._on_vehicle_clicked)
self._toyota_long = BigParamControl("enforce factory long.", "ToyotaEnforceStockLongitudinal", self._toyota_long = BigParamControl("enforce factory long.", "IQToyotaFactoryLong",
toggle_callback=self._on_toyota_long) toggle_callback=self._on_toyota_long)
self._hyundai_tuning = MappedParamToggle("hyundai long. tuning", "HyundaiLongitudinalTuning", self._hyundai_tuning = MappedParamToggle("hyundai long. tuning", "IQHyundaiLongTune",
["off", "dynamic", "predictive"], [0, 1, 2]) ["off", "dynamic", "predictive"], [0, 1, 2])
self._subaru_snag = BigParamControl("creep from standstill (beta)", "SubaruStopAndGo") self._subaru_snag = BigParamControl("creep from standstill (beta)", "IQSubaruCreepAssist")
self._subaru_manual = BigParamControl("stop and go manual brake", "SubaruStopAndGoManualParkingBrake") self._subaru_manual = BigParamControl("stop and go manual brake", "IQSubaruCreepAssistManualBrake")
self._vw_pq_hca = BigParamControl("PQ HCA status 7 mode", "pqhca5or7Toggle") self._vw_pq_hca = BigParamControl("PQ HCA status 7 mode", "pqhca5or7Toggle")
self._vw_lateral = BigParamControl("lateral when cruise faulted", "AllowLateralWhenLongUnavailable") self._vw_lateral = BigParamControl("lateral when cruise faulted", "AllowLateralWhenLongUnavailable")
self._vw_mqb_acc_resume = BigParamControl("MQB ACC resume", "iqMqbAccResume") self._vw_mqb_acc_resume = BigParamControl("MQB ACC resume", "iqMqbAccResume")
self._vw_mqb_steering_lockout = BigParamControl("MQB steering lockout", "iqMqbSteeringLockout") self._vw_mqb_steering_lockout = BigParamControl("MQB steering lockout", "iqMqbSteeringLockout")
self._tesla_vtb = BigParamControl("virtual torque blending", "TeslaCoopSteering") self._tesla_vtb = BigParamControl("virtual torque blending", "IQTeslaTorqueBlend")
self._brand_widgets = { self._brand_widgets = {
"toyota": [self._toyota_long], "toyota": [self._toyota_long],

View File

@@ -9,11 +9,11 @@ from openpilot.system.ui.widgets.scroller import NavScroller
class VisualsLayoutMici(NavScroller): class VisualsLayoutMici(NavScroller):
def __init__(self): def __init__(self):
super().__init__() super().__init__()
self._blind_spot = BigParamControl("Blind Spot Warnings", "BlindSpot") self._blind_spot = BigParamControl("Blind Spot Warnings", "IQBlindSpotAlerts")
self._steering_arc = BigParamControl("Steering Effort Arc", "TorqueBar") self._steering_arc = BigParamControl("Steering Effort Arc", "IQSteerEffortArc")
self._road_name = BigParamControl("Road Name", "RoadNameToggle") self._road_name = BigParamControl("Road Name", "IQRoadNameOverlay")
self._turn_signals = BigParamControl("Turn Signals", "ShowTurnSignals") self._turn_signals = BigParamControl("Turn Signals", "IQBlinkerIndicators")
self._accel_bar = BigParamControl("Acceleration Bar", "RocketFuel") self._accel_bar = BigParamControl("Acceleration Bar", "IQAccelMeter")
self._toggles = [self._blind_spot, self._steering_arc, self._road_name, self._toggles = [self._blind_spot, self._steering_arc, self._road_name,
self._turn_signals, self._accel_bar] self._turn_signals, self._accel_bar]

View File

@@ -29,7 +29,7 @@ from openpilot.iqpilot.ui.onroad.augmented_road_view import BORDER_COLORS_IQ
if gui_app.iqpilot_ui(): if gui_app.iqpilot_ui():
from openpilot.iqpilot.ui.mici.onroad.hud_renderer import IQMiciHudRenderer as HudRenderer from openpilot.iqpilot.ui.mici.onroad.hud_renderer import IQMiciHudRenderer as HudRenderer
from openpilot.iqpilot.ui.mici.onroad.road_name import RoadNameRendererMici from openpilot.iqpilot.ui.mici.onroad.road_label import RoadNameRendererMici
from openpilot.selfdrive.ui.ui_state import OnroadTimerStatus from openpilot.selfdrive.ui.ui_state import OnroadTimerStatus
OpState = log.SelfdriveState.OpenpilotState OpState = log.SelfdriveState.OpenpilotState

View File

@@ -1,14 +1,20 @@
import pyray as rl import pyray as rl
from cereal import car, log, messaging from cereal import log, messaging
from msgq.visionipc import VisionStreamType from msgq.visionipc import VisionStreamType
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
from openpilot.selfdrive.ui.ui_state import ui_state, device 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.application import gui_app, FontWeight
from openpilot.system.ui.lib.multilang import tr from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.widgets.nav_widget import NavWidget from openpilot.system.ui.widgets.nav_widget import NavWidget
from openpilot.system.ui.widgets.label import gui_label from openpilot.system.ui.widgets.label import gui_label
EventName = log.OnroadEvent.EventName
EVENT_TO_INT = EventName.schema.enumerants
class DriverCameraView(CameraView): class DriverCameraView(CameraView):
def _calc_frame_matrix(self, rect: rl.Rectangle): def _calc_frame_matrix(self, rect: rl.Rectangle):
base = super()._calc_frame_matrix(rect) base = super()._calc_frame_matrix(rect)
@@ -110,14 +116,10 @@ class DriverCameraDialog(NavWidget):
return return
msg = messaging.new_message('selfdriveState') msg = messaging.new_message('selfdriveState')
if dm_state is not None: if dm_state is not None and len(dm_state.events):
AudibleAlert = car.CarControl.HUDControl.AudibleAlert event_name = EVENT_TO_INT[dm_state.events[0].name]
alert_sounds = { if event_name is not None and event_name in EVENTS and ET.PERMANENT in EVENTS[event_name]:
'one': AudibleAlert.preAlert, msg.selfdriveState.alertSound = EVENTS[event_name][ET.PERMANENT].audible_alert
'two': AudibleAlert.promptDistracted,
'three': AudibleAlert.warningImmediate,
}
msg.selfdriveState.alertSound = alert_sounds.get(str(dm_state.alertLevel), AudibleAlert.none)
self._pm.send('selfdriveState', msg) self._pm.send('selfdriveState', msg)
def _render_dm_alerts(self, rect: rl.Rectangle): def _render_dm_alerts(self, rect: rl.Rectangle):
@@ -125,30 +127,29 @@ class DriverCameraDialog(NavWidget):
dm_state = ui_state.sm["driverMonitoringState"] dm_state = ui_state.sm["driverMonitoringState"]
self._publish_alert_sound(dm_state) 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), gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height),
f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
color=rl.Color(0, 0, 0, 180)) color=rl.Color(0, 0, 0, 180))
gui_label(rect, f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, gui_label(rect, f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
color=rl.Color(255, 255, 255, int(255 * 0.9))) color=rl.Color(255, 255, 255, int(255 * 0.9)))
if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none: if not dm_state.events:
return return
alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}" # Show first event (only one should be active at a time)
event_name_str = str(dm_state.events[0].name).split('.')[-1]
alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT 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) shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height)
gui_label(shadow_rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD, gui_label(shadow_rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
alignment=alignment, alignment=alignment,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
color=rl.Color(0, 0, 0, 180)) color=rl.Color(0, 0, 0, 180))
gui_label(rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD, gui_label(rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
alignment=alignment, alignment=alignment,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
color=rl.Color(255, 255, 255, int(255 * 0.9))) color=rl.Color(255, 255, 255, int(255 * 0.9)))
@@ -165,7 +166,7 @@ class DriverCameraDialog(NavWidget):
def _draw_face_detection(self, rect: rl.Rectangle): def _draw_face_detection(self, rect: rl.Rectangle):
dm_state = ui_state.sm["driverMonitoringState"] dm_state = ui_state.sm["driverMonitoringState"]
driver_data = self.driver_state_renderer.get_driver_data() driver_data = self.driver_state_renderer.get_driver_data()
if not dm_state.visionPolicyState.faceDetected: if not dm_state.faceDetected:
return return
# Get face position and orientation # Get face position and orientation

View File

@@ -6,12 +6,12 @@ from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets import Widget
from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net
AlertSize = log.SelfdriveState.AlertSize AlertSize = log.SelfdriveState.AlertSize
DEBUG = False DEBUG = False
ACTIVE_ACCENT = rl.Color(0x0C, 0x94, 0x96, 0xFF) ACTIVE_ACCENT = rl.Color(0x0C, 0x94, 0x96, 0xFF)
CONE_COLOR_ORANGE = (255, 115, 0)
LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6) LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6)
LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3) LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3)
@@ -21,7 +21,6 @@ class DriverStateRenderer(Widget):
BASE_SIZE = 60 BASE_SIZE = 60
LINES_ANGLE_INCREMENT = 5 LINES_ANGLE_INCREMENT = 5
LINES_STALE_ANGLES = 3.0 # seconds LINES_STALE_ANGLES = 3.0 # seconds
AWARENESS_UNFULL_PERCENT = 95
def __init__(self, lines: bool = False, inset: bool = False): def __init__(self, lines: bool = False, inset: bool = False):
super().__init__() super().__init__()
@@ -36,15 +35,11 @@ class DriverStateRenderer(Widget):
self._is_active = False self._is_active = False
self._is_rhd = False self._is_rhd = False
self._face_detected = False self._face_detected = False
self._face_pitch = 0.
self._face_yaw = 0.
self._should_draw = False self._should_draw = False
self._force_active = False self._force_active = False
self._looking_center = False self._looking_center = False
self._awareness_unfull = False
self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps)
self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps)
self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False) self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False)
@@ -100,7 +95,6 @@ class DriverStateRenderer(Widget):
rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x))) rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
if self.effective_active: if self.effective_active:
active_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0)
source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height) source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height)
dest_rect = rl.Rectangle( dest_rect = rl.Rectangle(
self._rect.x + self._rect.width / 2, self._rect.x + self._rect.width / 2,
@@ -110,16 +104,13 @@ class DriverStateRenderer(Widget):
) )
if not self._lines: if not self._lines:
r = int(round(ACTIVE_ACCENT.r * active_amount + CONE_COLOR_ORANGE[0] * (1 - active_amount)))
g = int(round(ACTIVE_ACCENT.g * active_amount + CONE_COLOR_ORANGE[1] * (1 - active_amount)))
b = int(round(ACTIVE_ACCENT.b * active_amount + CONE_COLOR_ORANGE[2] * (1 - active_amount)))
rl.draw_texture_pro( rl.draw_texture_pro(
self._dm_cone, self._dm_cone,
source_rect, source_rect,
dest_rect, dest_rect,
rl.Vector2(dest_rect.width / 2, dest_rect.height / 2), rl.Vector2(dest_rect.width / 2, dest_rect.height / 2),
self._rotation_filter.x - 90, self._rotation_filter.x - 90,
rl.Color(r, g, b, int(255 * self._fade_filter.x)), rl.Color(ACTIVE_ACCENT.r, ACTIVE_ACCENT.g, ACTIVE_ACCENT.b, int(255 * self._fade_filter.x)),
) )
else: else:
@@ -159,12 +150,9 @@ class DriverStateRenderer(Widget):
sm = ui_state.sm sm = ui_state.sm
dm_state = sm["driverMonitoringState"] dm_state = sm["driverMonitoringState"]
self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision self._is_active = dm_state.isActiveMode
self._is_rhd = dm_state.isRHD self._is_rhd = dm_state.isRHD
self._face_detected = dm_state.visionPolicyState.faceDetected self._face_detected = dm_state.faceDetected
self._awareness_unfull = self.effective_active and dm_state.visionPolicyState.awarenessPercent < self.AWARENESS_UNFULL_PERCENT
self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6)
self._face_yaw = -dm_state.visionPolicyState.pose.yaw
driverstate = sm["driverStateV2"] driverstate = sm["driverStateV2"]
driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
@@ -172,9 +160,24 @@ class DriverStateRenderer(Widget):
def _update_state(self): def _update_state(self):
# Get monitoring state # Get monitoring state
_ = self.get_driver_data() driver_data = self.get_driver_data()
pitch = self._pitch_filter.update(self._face_pitch) driver_orient = driver_data.faceOrientation
yaw = self._yaw_filter.update(self._face_yaw)
if len(driver_orient) != 3:
return
# Calibrate orientation so looking straight ahead at the road (instead of at the device) reads
# (0, 0), using live calibration. Makes the cone point in the correct direction. (stock PR #37149)
sm = ui_state.sm
if sm.valid['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) == 3:
cal_rpy = sm['liveCalibration'].rpyCalib
else:
cal_rpy = [0.0, 0.0, 0.0]
_, pitch, yaw = face_orientation_from_net(driver_orient, driver_data.facePosition, cal_rpy)
yaw = -yaw # undo sign flip in face_orientation_from_net to match UI convention
pitch = self._pitch_filter.update(pitch)
yaw = self._yaw_filter.update(yaw)
# hysteresis on looking center # hysteresis on looking center
if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER: if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER:
@@ -195,8 +198,9 @@ class DriverStateRenderer(Widget):
rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN) rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN)
rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN) rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN)
# filter head rotation, handling wrap-around # filter head rotation, handling wrap-around (bias pitch up since calib/DM pose isn't exact,
rotation = math.degrees(math.atan2(pitch * 2, yaw)) # and halve yaw sensitivity)
rotation = math.degrees(math.atan2((pitch + math.radians(6)) * 2, yaw))
angle_diff = rotation - self._rotation_filter.x angle_diff = rotation - self._rotation_filter.x
angle_diff = ((angle_diff + 180) % 360) - 180 angle_diff = ((angle_diff + 180) % 360) - 180
self._rotation_filter.update(self._rotation_filter.x + angle_diff) self._rotation_filter.update(self._rotation_filter.x + angle_diff)

View File

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

View File

@@ -12,7 +12,7 @@ from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets import Widget
from openpilot.iqpilot.ui.onroad.model_renderer import ChevronMetrics, IQModelRenderer from openpilot.iqpilot.ui.onroad.hud_overlays import ChevronMetrics
from openpilot.iqpilot.ui.onroad.lead_confidence import driving_confidence from openpilot.iqpilot.ui.onroad.lead_confidence import driving_confidence
CLIP_MARGIN = 500 CLIP_MARGIN = 500
@@ -68,10 +68,9 @@ class VisionDot:
_VD_EASE = 0.4 _VD_EASE = 0.4
class ModelRenderer(Widget, IQModelRenderer): class ModelRenderer(Widget):
def __init__(self): def __init__(self):
Widget.__init__(self) Widget.__init__(self)
IQModelRenderer.__init__(self)
self.chevron_metrics = ChevronMetrics() self.chevron_metrics = ChevronMetrics()
self._lead_orb = gui_app.texture("icons/lead_orb.png", 256, 256) self._lead_orb = gui_app.texture("icons/lead_orb.png", 256, 256)
self._longitudinal_control = False self._longitudinal_control = False
@@ -433,9 +432,6 @@ class ModelRenderer(Widget, IQModelRenderer):
allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control
self._blend_filter.update(int(allow_throttle)) self._blend_filter.update(int(allow_throttle))
if ui_state.rainbow_path:
self.rainbow_path.draw_rainbow_path(self._rect, self._path)
return
if self._experimental_mode: if self._experimental_mode:
# Draw with acceleration coloring # Draw with acceleration coloring

View File

@@ -45,28 +45,26 @@ sound_list_iq: dict[int, tuple[str, int | None, float]] = {
AudibleAlertIQ.promptSingleHigh: ("prompt_single_high.wav", 1, MAX_VOLUME), AudibleAlertIQ.promptSingleHigh: ("prompt_single_high.wav", 1, MAX_VOLUME),
} }
def get_sound_list(device_type: str) -> dict[int, tuple[str, int | None, float]]: sound_list: dict[int, tuple[str, int | None, float]] = {
sounds = { # AudibleAlert, file name, play count (none for infinite)
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME),
AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME),
AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME),
AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME),
AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME),
AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME),
**sound_list_iq,
}
if HARDWARE.get_device_type() in ("tizi", "tici"):
sound_list.update({
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), })
AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME),
AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME),
AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME),
AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME),
AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME),
**sound_list_iq,
}
if device_type in ("tizi", "tici"):
sounds.update({
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
})
return sounds
sound_list = get_sound_list(HARDWARE.get_device_type())
def check_selfdrive_timeout_alert(sm): def check_selfdrive_timeout_alert(sm):
ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] ss_missing = time.monotonic() - sm.recv_time['selfdriveState']

View File

@@ -2,19 +2,13 @@ from cereal import car
from cereal import messaging from cereal import messaging
from cereal.messaging import SubMaster, PubMaster from cereal.messaging import SubMaster, PubMaster
from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
from openpilot.selfdrive.ui.soundd import get_sound_list
import pytest
import time import time
AudibleAlert = car.CarControl.HUDControl.AudibleAlert AudibleAlert = car.CarControl.HUDControl.AudibleAlert
class TestSoundd: class TestSoundd:
@pytest.mark.parametrize("device_type", ["mici", "tici", "tizi"])
def test_prompt_distracted_sound(self, device_type):
assert get_sound_list(device_type)[AudibleAlert.promptDistracted][0] == "prompt_distracted.wav"
def test_check_selfdrive_timeout_alert(self): def test_check_selfdrive_timeout_alert(self):
sm = SubMaster(['selfdriveState']) sm = SubMaster(['selfdriveState'])
pm = PubMaster(['selfdriveState']) pm = PubMaster(['selfdriveState'])
@@ -38,3 +32,4 @@ class TestSoundd:
assert check_selfdrive_timeout_alert(sm) assert check_selfdrive_timeout_alert(sm)
# TODO: add test with micd for checking that soundd actually outputs sounds # TODO: add test with micd for checking that soundd actually outputs sounds

View File

@@ -142,15 +142,14 @@ class IQUIState:
_PARAM_MIRROR = { _PARAM_MIRROR = {
"active_bundle": ("ModelManager_ActiveBundle", "raw"), "active_bundle": ("ModelManager_ActiveBundle", "raw"),
"blindspot": ("BlindSpot", "bool"), "blindspot": ("IQBlindSpotAlerts", "bool"),
"chevron_metrics": ("ChevronInfo", "raw"), "chevron_metrics": ("IQLeadReadouts", "raw"),
"developer_ui": ("IQDevUIInfo", "raw"), "developer_ui": ("IQDevUIInfo", "raw"),
"night_mode": ("NightMode", "bool"), "night_mode": ("NightMode", "bool"),
"rainbow_path": ("RainbowMode", "bool"), "road_name_toggle": ("IQRoadNameOverlay", "bool"),
"road_name_toggle": ("RoadNameToggle", "bool"), "rocket_fuel": ("IQAccelMeter", "bool"),
"rocket_fuel": ("RocketFuel", "bool"), "torque_bar": ("IQSteerEffortArc", "bool"),
"torque_bar": ("TorqueBar", "bool"), "turn_signals": ("IQBlinkerIndicators", "bool"),
"turn_signals": ("ShowTurnSignals", "bool"),
"custom_interactive_timeout": ("InteractivityTimeout", "default"), "custom_interactive_timeout": ("InteractivityTimeout", "default"),
"onroad_brightness_timer_param": ("OnroadScreenOffTimer", "default"), "onroad_brightness_timer_param": ("OnroadScreenOffTimer", "default"),
"speed_limit_mode": ("IQSpeedAssistMode", "default"), "speed_limit_mode": ("IQSpeedAssistMode", "default"),

View File

@@ -79,7 +79,7 @@ def navrenderd_onroad(started: bool, params: Params, CP: car.CarParams) -> bool:
def iqmapd_needed(params: Params) -> bool: def iqmapd_needed(params: Params) -> bool:
return ( return (
params.get_bool("RoadNameToggle") params.get_bool("IQRoadNameOverlay")
or params.get_bool("ShowSpeedLimits") or params.get_bool("ShowSpeedLimits")
or params.get_bool("SpeedLimitController") or params.get_bool("SpeedLimitController")
or params.get_bool("EnableSpeedLimitControl") or params.get_bool("EnableSpeedLimitControl")

View File

@@ -145,12 +145,12 @@ def _patch_mock_state():
mp.put("IQLaneChangeBsmDelay", False) mp.put("IQLaneChangeBsmDelay", False)
# ── Visuals (correct param keys matching visuals.py) ───────────────────── # ── Visuals (correct param keys matching visuals.py) ─────────────────────
mp.put("BlindSpot", True) mp.put("IQBlindSpotAlerts", True)
mp.put("TorqueBar", True) mp.put("IQSteerEffortArc", True)
mp.put("RoadNameToggle", True) mp.put("IQRoadNameOverlay", True)
mp.put("ShowTurnSignals", True) mp.put("IQBlinkerIndicators", True)
mp.put("RocketFuel", False) mp.put("IQAccelMeter", False)
mp.put("ChevronInfo", 0) # 0=off mp.put("IQLeadReadouts", 0) # 0=off
mp.put("IQDevUIInfo", 0) # 0=off mp.put("IQDevUIInfo", 0) # 0=off
mp.put("AlphaLongitudinalEnabled", False) # real param; gates ChevronInfo mp.put("AlphaLongitudinalEnabled", False) # real param; gates ChevronInfo

View File

@@ -92,12 +92,11 @@ class SimulatedSensors:
# dmonitoringd output # dmonitoringd output
dat = messaging.new_message('driverMonitoringState', valid=True) dat = messaging.new_message('driverMonitoringState', valid=True)
dm = dat.driverMonitoringState dat.driverMonitoringState = {
dm.alertLevel = log.DriverMonitoringState.AlertLevel.none "faceDetected": True,
dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision "isDistracted": False,
dm.visionPolicyState.faceDetected = True "awarenessStatus": 1.,
dm.visionPolicyState.isDistracted = False }
dm.visionPolicyState.awarenessPercent = 100
self.pm.send('driverMonitoringState', dat) self.pm.send('driverMonitoringState', dat)
def send_camera_images(self, world: 'World'): def send_camera_images(self, world: 'World'):