IQ.Pilot Release Commit @ b6534c0

iqpilot/selfdrive/selfdrived/helpers.py

@@ -0,0 +1,55 @@
+import math
+from enum import StrEnum, auto
+
+from iqpilot.cereal import car, messaging
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.locationd.helpers import Pose
+from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY
+from iqdbc.car.lateral import ISO_LATERAL_ACCEL
+from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+
+MIN_EXCESSIVE_ACTUATION_COUNT = int(0.25 / DT_CTRL)
+MIN_LATERAL_ENGAGE_BUFFER = int(1 / DT_CTRL)
+
+
+class ExcessiveActuationType(StrEnum):
+  LONGITUDINAL = auto()
+  LATERAL = auto()
+
+
+class ExcessiveActuationCheck:
+  def __init__(self):
+    self._excessive_counter = 0
+    self._engaged_counter = 0
+
+  def update(self, sm: messaging.SubMaster, CS: car.CarState, calibrated_pose: Pose) -> ExcessiveActuationType | None:
+    # CS.aEgo can be noisy to bumps in the road, transitioning from standstill, losing traction, etc.
+    # longitudinal
+    accel_calibrated = calibrated_pose.acceleration.x
+    excessive_long_actuation = sm['carControl'].longActive and ((not CS.gasPressed and accel_calibrated > ACCEL_MAX * 2) or
+                                                                accel_calibrated < ACCEL_MIN * 2)
+
+    # lateral
+    yaw_rate = calibrated_pose.angular_velocity.yaw
+    roll = sm['vehicleParameters'].roll
+    roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
+
+    # Prevent false positives after overriding
+    excessive_lat_actuation = False
+    self._engaged_counter = self._engaged_counter + 1 if sm['carControl'].latActive and not CS.steeringPressed else 0
+    if self._engaged_counter > MIN_LATERAL_ENGAGE_BUFFER:
+      if abs(roll_compensated_lateral_accel) > ISO_LATERAL_ACCEL * 2:
+        excessive_lat_actuation = True
+
+    # deviceMotion acceleration can be noisy due to bad mounting or aliased deviceMotion measurements
+    livepose_valid = abs(CS.aEgo - accel_calibrated) < 2
+    self._excessive_counter = self._excessive_counter + 1 if livepose_valid and (excessive_long_actuation or excessive_lat_actuation) else 0
+
+    excessive_type = None
+    if self._excessive_counter > MIN_EXCESSIVE_ACTUATION_COUNT:
+      if excessive_long_actuation:
+        excessive_type = ExcessiveActuationType.LONGITUDINAL
+      else:
+        excessive_type = ExcessiveActuationType.LATERAL
+
+    return excessive_type