forked from IQ.Lvbs/IQ.Pilot
56 lines
2.3 KiB
Python
56 lines
2.3 KiB
Python
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
|