mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 19:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Lock driver monitoring Side

Browse Source
This commit is contained in:
firestar5683
2026-03-22 13:42:34 -05:00
parent 7000063c41
commit a35382505e
2 changed files with 36 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/monitoring/dmonitoringd.py
+10 -5
View File
@@ -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()
selfdrive/monitoring/helpers.py
+26 -1
View File
@@ -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