mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-03 12:32:06 +08:00
DM: compensate for non-calibrated pitch spread (#26348)
old-commit-hash: 2837e73a70b8b8faba7047db5124a582635439ff
This commit is contained in:
@@ -44,6 +44,7 @@ class DRIVER_MONITOR_SETTINGS():
|
||||
self._POSE_YAW_THRESHOLD_SLACK = 0.5042
|
||||
self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
|
||||
self._PITCH_NATURAL_OFFSET = 0.029 # initial value before offset is learned
|
||||
self._PITCH_NATURAL_THRESHOLD = 0.449
|
||||
self._YAW_NATURAL_OFFSET = 0.097 # initial value before offset is learned
|
||||
self._PITCH_MAX_OFFSET = 0.124
|
||||
self._PITCH_MIN_OFFSET = -0.0881
|
||||
@@ -197,7 +198,7 @@ class DriverStatus():
|
||||
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
|
||||
yaw_error = abs(yaw_error)
|
||||
if pitch_error > self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch or \
|
||||
if pitch_error > (self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch if self.pose_calibrated else self.settings._PITCH_NATURAL_THRESHOLD) or \
|
||||
yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw:
|
||||
distracted_types.append(DistractedType.DISTRACTED_POSE)
|
||||
|
||||
|
||||
@@ -1 +1 @@
|
||||
89641653a0fc765105dc1c6a2a995cba2fb3f344
|
||||
24a8d02b148b7f6d20f641d56a7bed71c244b6e3
|
||||
Reference in New Issue
Block a user