mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 16:23:46 +08:00
Shane Smiskol
c35494c19f
* here? * nah card shouldn't become bloated * better * import * actually selfdrived is probably best since it already manages alerts card is car interfacing, controlsd is for calculating control input, selfdrived is rest * consistent name * add to params * ai * maybe better? * shorter * build out lockout * do * check active * descriptive * this is a terrible experience just to get lat accel * just pass sm * not iso * type * rm * math * use calibrated roll * fix * fix borkenness * cmt * compare some methods * rolling window * 1 and 2 are the same * rm it * stuff * plot * plot kf * generic implementation * adjust limits * fix from merge * clean up * revert filter to master * and here * and * run_process_on_route imps * clean up * why not * extrapolate * this doesn't generically work for angle/curvature cars Revert "extrapolate" This reverts commit 556f0c3a92b82f07ceb6422f0e39322e79a10dcd. * cmt * move * rm debug rm debug and * others use helpers * two counters might be too much to return * turn into class * clean up * cmt * kinda obvious * impossible for this not to be true, but make it explicit * clean up
54 lines
2.1 KiB
Python
54 lines
2.1 KiB
Python
import math
|
|
from enum import StrEnum, auto
|
|
|
|
from cereal import car, messaging
|
|
from openpilot.common.realtime import DT_CTRL
|
|
from openpilot.selfdrive.locationd.helpers import Pose
|
|
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
|
|
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX, ISO_LATERAL_ACCEL
|
|
|
|
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 (accel_calibrated > ACCEL_MAX * 2 or accel_calibrated < ACCEL_MIN * 2)
|
|
|
|
# lateral
|
|
yaw_rate = calibrated_pose.angular_velocity.yaw
|
|
roll = sm['liveParameters'].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
|
|
|
|
# livePose acceleration can be noisy due to bad mounting or aliased livePose 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
|