diff --git a/frogpilot/tinygrad_modeld/dmonitoringmodeld.py b/frogpilot/tinygrad_modeld/dmonitoringmodeld.py index 6a1f0dbd9..56b071db0 100644 --- a/frogpilot/tinygrad_modeld/dmonitoringmodeld.py +++ b/frogpilot/tinygrad_modeld/dmonitoringmodeld.py @@ -4,10 +4,8 @@ from openpilot.system.hardware import TICI os.environ['DEV'] = 'QCOM' if TICI else 'CPU' from tinygrad.tensor import Tensor from tinygrad.dtype import dtypes -import math import time import pickle -import ctypes import numpy as np from pathlib import Path @@ -16,114 +14,99 @@ from cereal.messaging import PubMaster, SubMaster from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.swaglog import cloudlog from openpilot.common.realtime import config_realtime_process -from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, DM_INPUT_SIZE +from openpilot.common.transformations.model import dmonitoringmodel_intrinsics from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye -try: - from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame -except ImportError: - from openpilot.frogpilot.tinygrad_modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame -from openpilot.frogpilot.tinygrad_modeld.parse_model_outputs import sigmoid -from openpilot.frogpilot.tinygrad_modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address - -MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE -CALIB_LEN = 3 -FEATURE_LEN = 512 -OUTPUT_SIZE = 84 + FEATURE_LEN +from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp +from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame +from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address PROCESS_NAME = "frogpilot.tinygrad_modeld.dmonitoringmodeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') MODEL_PKL_PATH = Path(__file__).resolve().parents[2] / "selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl" - - -class DriverStateResult(ctypes.Structure): - _fields_ = [ - ("face_orientation", ctypes.c_float*3), - ("face_position", ctypes.c_float*3), - ("face_orientation_std", ctypes.c_float*3), - ("face_position_std", ctypes.c_float*3), - ("face_prob", ctypes.c_float), - ("_unused_a", ctypes.c_float*8), - ("left_eye_prob", ctypes.c_float), - ("_unused_b", ctypes.c_float*8), - ("right_eye_prob", ctypes.c_float), - ("left_blink_prob", ctypes.c_float), - ("right_blink_prob", ctypes.c_float), - ("sunglasses_prob", ctypes.c_float), - ("occluded_prob", ctypes.c_float), - ("ready_prob", ctypes.c_float*4), - ("not_ready_prob", ctypes.c_float*2)] - - -class DMonitoringModelResult(ctypes.Structure): - _fields_ = [ - ("driver_state_lhd", DriverStateResult), - ("driver_state_rhd", DriverStateResult), - ("poor_vision_prob", ctypes.c_float), - ("wheel_on_right_prob", ctypes.c_float), - ("features", ctypes.c_float*FEATURE_LEN)] +METADATA_PATH = Path(__file__).parent / "models/dmonitoring_model_metadata.pkl" class ModelState: inputs: dict[str, np.ndarray] output: np.ndarray - def __init__(self, cl_ctx): - assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float) + def __init__(self, cl_ctx: CLContext): + with open(METADATA_PATH, 'rb') as f: + model_metadata = pickle.load(f) + self.input_shapes = model_metadata['input_shapes'] + self.output_slices = model_metadata['output_slices'] self.frame = MonitoringModelFrame(cl_ctx) self.numpy_inputs = { - 'calib': np.zeros((1, CALIB_LEN), dtype=np.float32), + 'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32), } - 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_PKL_PATH, "rb") as f: self.model_run = pickle.load(f) def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]: - self.numpy_inputs['calib'][0,:] = calib + self.numpy_inputs['calib'][0, :] = calib t1 = time.perf_counter() input_img_cl = self.frame.prepare(buf, transform.flatten()) if TICI: - # The imgs tensors are backed by opencl memory, only need init once if 'input_img' not in self.tensor_inputs: - self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, (1, MODEL_WIDTH*MODEL_HEIGHT), dtype=dtypes.uint8) + self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address( + input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8 + ) else: - self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape((1, MODEL_WIDTH*MODEL_HEIGHT)), dtype=dtypes.uint8).realize() + self.tensor_inputs['input_img'] = Tensor( + self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8 + ).realize() - - output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy() + output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy().flatten() t2 = time.perf_counter() return output, t2 - t1 -def fill_driver_state(msg, ds_result: DriverStateResult): - msg.faceOrientation = list(ds_result.face_orientation) - msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std] - msg.facePosition = list(ds_result.face_position[:2]) - msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]] - msg.faceProb = float(sigmoid(ds_result.face_prob)) - msg.leftEyeProb = float(sigmoid(ds_result.left_eye_prob)) - msg.rightEyeProb = float(sigmoid(ds_result.right_eye_prob)) - msg.leftBlinkProb = float(sigmoid(ds_result.left_blink_prob)) - msg.rightBlinkProb = float(sigmoid(ds_result.right_blink_prob)) - msg.sunglassesProb = float(sigmoid(ds_result.sunglasses_prob)) - msg.phoneProb = 0.0 +def slice_outputs(model_outputs, output_slices): + return {k: model_outputs[np.newaxis, v] for k, v in output_slices.items()} -def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, gpu_execution_time: float): - model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents +def parse_model_output(model_output): + parsed = {} + parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right']) + for ds_suffix in ['lhd', 'rhd']: + face_descs = model_output[f'face_descs_{ds_suffix}'] + parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6] + parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:]) + for key in ['face_prob', 'left_eye_prob', 'right_eye_prob', 'left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']: + parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}']) + return parsed + + +def fill_driver_data(msg, model_output, ds_suffix): + msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist() + msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist() + msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist() + msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist() + msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item() + msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item() + msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item() + msg.leftBlinkProb = model_output[f'left_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.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item() + + +def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float): msg = messaging.new_message('driverStateV2', valid=True) ds = msg.driverStateV2 ds.frameId = frame_id - ds.modelExecutionTime = execution_time - ds.gpuExecutionTime = gpu_execution_time - ds.wheelOnRightProb = float(sigmoid(model_result.wheel_on_right_prob)) - ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b'' - fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd) - fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd) + ds.modelExecutionTime = exec_time + ds.gpuExecutionTime = gpu_exec_time + ds.rawPredictions = model_output['raw_pred'] + ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item() + fill_driver_data(ds.leftDriverData, model_output, 'lhd') + fill_driver_data(ds.rightDriverData, model_output, 'rhd') return msg @@ -144,7 +127,7 @@ def main(): sm = SubMaster(["liveCalibration"]) pm = PubMaster(["driverStateV2"]) - calib = np.zeros(CALIB_LEN, dtype=np.float32) + calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32) model_transform = None while True: @@ -163,8 +146,12 @@ def main(): t1 = time.perf_counter() model_output, gpu_execution_time = model.run(buf, calib, model_transform) t2 = time.perf_counter() - - pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time)) + raw_pred = model_output.tobytes() if SEND_RAW_PRED else b'' + model_output = slice_outputs(model_output, model.output_slices) + model_output = parse_model_output(model_output) + model_output['raw_pred'] = raw_pred + msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time) + pm.send("driverStateV2", msg) if __name__ == "__main__": diff --git a/frogpilot/tinygrad_modeld/tinygrad_modeld.py b/frogpilot/tinygrad_modeld/tinygrad_modeld.py index 004829508..544df28c2 100755 --- a/frogpilot/tinygrad_modeld/tinygrad_modeld.py +++ b/frogpilot/tinygrad_modeld/tinygrad_modeld.py @@ -139,21 +139,14 @@ class ModelState: model_id_raw = _get_param_str(params, "Model") if not model_id_raw: model_id_raw = _get_param_str(params, "DrivingModel", "sc") - model_id = model_id_raw.strip() or "sc" - model_id_lower = model_id.lower() - if model_id_lower == "sc2": - model_id = "sc" - elif model_id_lower == "sc": - model_id = "sc" - elif model_id_lower == "bd2": - model_id = "bd2" + model_id = (model_id_raw.strip() or "sc").lower() model_version = _get_param_str(params, "ModelVersion") if not model_version: model_version = _get_param_str(params, "DrivingModelVersion") model_dir = MODELS_PATH - use_builtin_model = model_id in ("sc", "bd2") + use_builtin_model = model_id == "sc" model_download_id = model_id if use_builtin_model and (model_id_raw != model_id or _get_param_str(params, "DrivingModel") != model_id): params.put("Model", model_id) diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 3e357ee09..28190db3e 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -25,7 +25,6 @@ MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl' METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl' MODELS_DIR = Path(__file__).parent / 'models' - class ModelState: inputs: dict[str, np.ndarray] output: np.ndarray @@ -46,42 +45,8 @@ class ModelState: self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} self._blob_cache : dict[int, Tensor] = {} self.image_warp = None - self._warp_rebuild_attempted: set[tuple[int, int]] = set() - self._warp_backend_rebuild_attempted: set[tuple[int, int]] = set() self.model_run = pickle.loads(read_file_chunked(str(MODEL_PKL_PATH))) - def _load_or_rebuild_dm_warp(self, width: int, height: int): - warp_path = MODELS_DIR / f'dm_warp_{width}x{height}_tinygrad.pkl' - resolution_key = (width, height) - - def load_warp(): - with open(warp_path, "rb") as f: - return pickle.load(f) - - try: - return load_warp() - except Exception as error: - if resolution_key in self._warp_rebuild_attempted: - raise - - self._warp_rebuild_attempted.add(resolution_key) - cloudlog.exception(f"Failed to load DM warp artifact {warp_path}: {error}") - cloudlog.warning(f"Rebuilding DM warp artifact for {width}x{height}") - - try: - warp_path.unlink(missing_ok=True) - except Exception: - pass - - from openpilot.selfdrive.modeld.compile_warp import compile_dm_warp - compile_dm_warp(width, height) - - try: - return load_warp() - except Exception as retry_error: - cloudlog.exception(f"Reload failed after rebuilding {warp_path}: {retry_error}") - raise - def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]: self.numpy_inputs['calib'][0,:] = calib @@ -89,33 +54,16 @@ class ModelState: if self.image_warp is None: self.frame_buf_params = get_nv12_info(buf.width, buf.height) - self.image_warp = self._load_or_rebuild_dm_warp(buf.width, buf.height) + warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl' + with open(warp_path, "rb") as f: + self.image_warp = pickle.load(f) ptr = buf.data.ctypes.data # There is a ringbuffer of imgs, just cache tensors pointing to all of them if ptr not in self._blob_cache: self._blob_cache[ptr] = Tensor.from_blob(ptr, (self.frame_buf_params[3],), dtype='uint8') self.warp_inputs_np['transform'][:] = transform[:] - resolution_key = (buf.width, buf.height) - try: - self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform']).realize() - except AssertionError as error: - # Handle runtime backend mismatch (e.g. CPU-captured warp artifact on QCOM device). - if "args mismatch in JIT" not in str(error) or resolution_key in self._warp_backend_rebuild_attempted: - raise - - self._warp_backend_rebuild_attempted.add(resolution_key) - cloudlog.warning(f"DM warp JIT backend mismatch for {buf.width}x{buf.height}; rebuilding artifact for active backend") - warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl' - try: - warp_path.unlink(missing_ok=True) - except Exception: - pass - - from openpilot.selfdrive.modeld.compile_warp import compile_dm_warp - compile_dm_warp(buf.width, buf.height) - self.image_warp = self._load_or_rebuild_dm_warp(buf.width, buf.height) - self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform']).realize() + self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform']).realize() output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy().flatten() diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index 0e08fe94a..7d59ce24e 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model.onnx and b/selfdrive/modeld/models/dmonitoring_model.onnx differ diff --git a/selfdrive/modeld/models/dmonitoring_model_metadata.pkl b/selfdrive/modeld/models/dmonitoring_model_metadata.pkl index efcdd4c37..71e4b289c 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_metadata.pkl and b/selfdrive/modeld/models/dmonitoring_model_metadata.pkl differ diff --git a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl index f80ca5a2d..299065b59 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl and b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl differ diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index fe51af1c6..d2a6163a7 100644 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -48,16 +48,11 @@ def dmonitoringd_thread(): DM.always_on = params.get_bool("AlwaysOnDM") demo_mode = params.get_bool("IsDriverViewEnabled") and sm["carState"].gearShifter != GearShifter.reverse - # save rhd virtual toggle every 5 mins, but only with clear confidence. + # save rhd virtual toggle every 5 mins if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and - DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT): - wheelpos_mean = DM.wheelpos.prob_offseter.filtered_stat.M - save_rhd = DM.settings._WHEELPOS_THRESHOLD_ENTER_RHD + DM.settings._WHEELPOS_SAVE_MARGIN - save_lhd = DM.settings._WHEELPOS_THRESHOLD_ENTER_LHD - DM.settings._WHEELPOS_SAVE_MARGIN - if wheelpos_mean >= save_rhd: - params.put_bool_nonblocking("IsRhdDetected", True) - elif wheelpos_mean <= save_lhd: - params.put_bool_nonblocking("IsRhdDetected", False) + DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and + DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): + params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right) def main(): dmonitoringd_thread() diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 70945f8cd..a1d1a5cb2 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -35,11 +35,7 @@ class DRIVER_MONITOR_SETTINGS: self._EYE_THRESHOLD = 0.65 self._SG_THRESHOLD = 0.9 self._BLINK_THRESHOLD = 0.865 - - self._PHONE_THRESH = 0.75 if device_type == 'mici' else 0.4 - self._PHONE_THRESH2 = 15.0 - self._PHONE_MAX_OFFSET = 0.06 - self._PHONE_MIN_OFFSET = 0.025 + self._PHONE_THRESH = 0.5 self._POSE_PITCH_THRESHOLD = 0.3133 self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 @@ -50,6 +46,8 @@ class DRIVER_MONITOR_SETTINGS: 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 @@ -70,18 +68,9 @@ class DRIVER_MONITOR_SETTINGS: self._WHEELPOS_CALIB_MIN_SPEED = 11 self._WHEELPOS_THRESHOLD = 0.5 self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side - self._WHEELPOS_THRESHOLD_ENTER_RHD = self._WHEELPOS_THRESHOLD - self._WHEELPOS_THRESHOLD_ENTER_LHD = self._WHEELPOS_THRESHOLD - self._WHEELPOS_SAVE_MARGIN = 0.0 - self._WHEELPOS_STARTUP_OVERRIDE_RHD = 0.55 - self._WHEELPOS_STARTUP_OVERRIDE_LHD = 0.45 - - # C4 (mici) has shown borderline wheel-side probabilities around 0.5x. - # Use hysteresis and stricter persistence thresholds to avoid false RHD latching. - if device_type == 'mici': - self._WHEELPOS_THRESHOLD_ENTER_RHD = 0.65 - self._WHEELPOS_THRESHOLD_ENTER_LHD = 0.35 - self._WHEELPOS_SAVE_MARGIN = 0.05 + self._WHEELPOS_DATA_AVG = 0.03 + self._WHEELPOS_DATA_VAR = 3*5.5e-5 + self._WHEELPOS_MAX_COUNT = -1 self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change @@ -96,24 +85,26 @@ class DistractedType: DISTRACTED_PHONE = 1 << 2 class DriverPose: - def __init__(self, max_trackable): + def __init__(self, settings): + pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2) + yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2) self.yaw = 0. self.pitch = 0. self.roll = 0. self.yaw_std = 0. self.pitch_std = 0. self.roll_std = 0. - self.pitch_offseter = RunningStatFilter(max_trackable=max_trackable) - self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable) + self.pitch_offseter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) + self.yaw_offseter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) self.calibrated = False self.low_std = True self.cfactor_pitch = 1. self.cfactor_yaw = 1. class DriverProb: - def __init__(self, max_trackable): + def __init__(self, raw_priors, max_trackable): self.prob = 0. - self.prob_offseter = RunningStatFilter(max_trackable=max_trackable) + self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable) self.prob_calibrated = False class DriverBlink: @@ -152,10 +143,11 @@ class DriverMonitoring: self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type()) # init driver status - self.wheelpos = DriverProb(-1) - self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) - self.phone = DriverProb(self.settings._POSE_OFFSET_MAX_COUNT) + wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2) + self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) + self.pose = DriverPose(settings=self.settings) self.blink = DriverBlink() + self.phone_prob = 0. self.always_on = always_on self.distracted_types = [] @@ -256,54 +248,38 @@ class DriverMonitoring: if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD: distracted_types.append(DistractedType.DISTRACTED_BLINK) - if self.phone.prob_calibrated: - using_phone = self.phone.prob > max(min(self.phone.prob_offseter.filtered_stat.M, self.settings._PHONE_MAX_OFFSET), self.settings._PHONE_MIN_OFFSET) \ - * self.settings._PHONE_THRESH2 - else: - using_phone = self.phone.prob > self.settings._PHONE_THRESH - if using_phone: + if self.phone_prob > self.settings._PHONE_THRESH: distracted_types.append(DistractedType.DISTRACTED_PHONE) return distracted_types def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False): rhd_pred = driver_state.wheelOnRightProb + left_face_prob = driver_state.leftDriverData.faceProb + right_face_prob = driver_state.rightDriverData.faceProb # 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): + if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (left_face_prob > self.settings._FACE_THRESHOLD or + right_face_prob > self.settings._FACE_THRESHOLD): self.wheelpos.prob_offseter.push_and_update(rhd_pred) self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT - startup_override = None - if not self.wheelpos.prob_calibrated and not demo_mode and not op_engaged: - left_face_detected = driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD - right_face_detected = driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD - if rhd_pred <= self.settings._WHEELPOS_STARTUP_OVERRIDE_LHD and left_face_detected and not right_face_detected: - startup_override = False - elif rhd_pred >= self.settings._WHEELPOS_STARTUP_OVERRIDE_RHD and right_face_detected and not left_face_detected: - startup_override = True - if self.wheelpos.prob_calibrated or demo_mode: - wheelpos_mean = self.wheelpos.prob_offseter.filtered_stat.M - enter_rhd = self.settings._WHEELPOS_THRESHOLD_ENTER_RHD - enter_lhd = self.settings._WHEELPOS_THRESHOLD_ENTER_LHD - - # Hysteresis: avoid side flapping near 0.5 and preserve last stable side. - if self.wheel_on_right_last is None: - if wheelpos_mean >= enter_rhd: - self.wheel_on_right = True - elif wheelpos_mean <= enter_lhd: - self.wheel_on_right = False - else: - self.wheel_on_right = self.wheel_on_right_default - elif self.wheel_on_right_last: - self.wheel_on_right = wheelpos_mean > enter_lhd - else: - self.wheel_on_right = wheelpos_mean >= enter_rhd - elif startup_override is not None: - self.wheel_on_right = startup_override + self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD else: self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished + + # On mici/C4, wheel-side inference can hover around 0.5 during startup or after off-car testing. + # If one face side is clearly valid and the other is below threshold, prefer the obvious face side + # when the wheel-side signal is still weak instead of latching the wrong side and reporting no face. + left_face_detected = left_face_prob > self.settings._FACE_THRESHOLD + right_face_detected = right_face_prob > self.settings._FACE_THRESHOLD + weak_wheelside_signal = abs(rhd_pred - self.settings._WHEELPOS_THRESHOLD) < 0.1 + if weak_wheelside_signal or not self.wheelpos.prob_calibrated: + if left_face_detected and not right_face_detected: + self.wheel_on_right = False + elif right_face_detected and not left_face_detected: + self.wheel_on_right = True + # 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 @@ -325,7 +301,7 @@ class DriverMonitoring: * (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.phone_prob = driver_data.phoneProb self.distracted_types = self._get_distracted_types() self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types @@ -339,11 +315,9 @@ class DriverMonitoring: if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted): self.pose.pitch_offseter.push_and_update(self.pose.pitch) self.pose.yaw_offseter.push_and_update(self.pose.yaw) - self.phone.prob_offseter.push_and_update(self.phone.prob) self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT - self.phone.prob_calibrated = self.phone.prob_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT if self.face_detected and not self.driver_distracted: if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD: @@ -449,8 +423,8 @@ class DriverMonitoring: "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n, "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(), "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n, - "phoneProbOffset": self.phone.prob_offseter.filtered_stat.mean(), - "phoneProbValidCount": self.phone.prob_offseter.filtered_stat.n, + "phoneProbOffset": 0., + "phoneProbValidCount": 0, "stepChange": self.step_change, "awarenessActive": self.awareness_active, "awarenessPassive": self.awareness_passive, diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 46fe72b12..6ea9b8028 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -30,23 +30,6 @@ def make_msg(face_detected, distracted=False, model_uncertain=False): return ds -def make_dual_msg(left_face_prob, right_face_prob, wheel_on_right_prob=0., model_uncertain=False): - ds = log.DriverStateV2.new_message() - ds.wheelOnRightProb = wheel_on_right_prob - for side, face_prob in ((ds.leftDriverData, left_face_prob), (ds.rightDriverData, right_face_prob)): - side.faceOrientation = [0., 0., 0.] - side.facePosition = [0., 0.] - side.faceProb = face_prob - side.leftEyeProb = 1. - side.rightEyeProb = 1. - side.leftBlinkProb = 0. - side.rightBlinkProb = 0. - side.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] - side.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] - side.phoneProb = 0. - return ds - - # driver state from neural net, 10Hz msg_NO_FACE_DETECTED = make_msg(False) msg_ATTENTIVE = make_msg(True) @@ -88,16 +71,6 @@ class TestMonitoring: events, _ = self._run_seq(always_attentive, always_false, always_true, always_false) self._assert_no_events(events) - def test_saved_rhd_recovers_to_lhd_with_strong_left_face(self): - settings = DRIVER_MONITOR_SETTINGS(device_type='mici') - DM = DriverMonitoring(rhd_saved=True, settings=settings) - msg = make_dual_msg(left_face_prob=0.95, right_face_prob=0.2, wheel_on_right_prob=0.35) - - DM._update_states(msg, [0, 0, 0], 0, False, False) - - assert not DM.wheel_on_right - assert DM.face_detected - # engaged, driver is distracted and does nothing def test_fully_distracted_driver(self): events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) @@ -230,3 +203,4 @@ class TestMonitoring: 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 +