From a35382505e77e11e8f91873b94b4c37b7869de35 Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Sun, 22 Mar 2026 13:42:34 -0500
Subject: [PATCH] Lock driver monitoring Side

---
 selfdrive/monitoring/dmonitoringd.py | 15 ++++++++++-----
 selfdrive/monitoring/helpers.py      | 27 ++++++++++++++++++++++++++-
 2 files changed, 36 insertions(+), 6 deletions(-)

diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py
index d2a6163a70..0da7f33968 100644
--- a/selfdrive/monitoring/dmonitoringd.py
+++ b/selfdrive/monitoring/dmonitoringd.py
@@ -48,11 +48,16 @@ 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
-    if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
-     DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
-     DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
-      params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right)
+  # save rhd virtual toggle every 5 mins, but only with clear confidence.
+  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)
 
 def main():
   dmonitoringd_thread()
diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py
index e0788c59e7..b9c0d33c0c 100644
--- a/selfdrive/monitoring/helpers.py
+++ b/selfdrive/monitoring/helpers.py
@@ -70,6 +70,16 @@ 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
+
+    # 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._RECOVERY_FACTOR_MAX = 5.  # relative to minus step change
     self._RECOVERY_FACTOR_MIN = 1.25  # relative to minus step change
@@ -264,7 +274,22 @@ class DriverMonitoring:
     self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT
 
     if self.wheelpos.prob_calibrated or demo_mode:
-      self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
+      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
     else:
       self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
     # make sure no switching when engaged