mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-27 09:53:45 +08:00
AngusBell97
c51b96879a
Move the fork-specific diagnostics out of the stock CarOutput schema and into StarPilot reserved messaging. Preserve the legacy saturation fallback when custom diagnostics are unavailable or stale. Co-authored-by: AngusBell97 <124716116+AngusBell97@users.noreply.github.com>
53 lines
1.7 KiB
Python
53 lines
1.7 KiB
Python
import math
|
|
|
|
from opendbc.car.tesla.values import CAR, TeslaSafetyFlags
|
|
|
|
|
|
STEER_ANGLE_SATURATION_THRESHOLD = 2.5
|
|
_MAX_STEERING_LIMIT_INFO_AGE_NANOS = 100_000_000
|
|
|
|
|
|
def _legacy_angle_steering_limited(requested_angle: float, car_output) -> bool:
|
|
return abs(requested_angle - car_output.actuatorsOutput.steeringAngleDeg) > \
|
|
STEER_ANGLE_SATURATION_THRESHOLD
|
|
|
|
|
|
def is_angle_steering_limited(CP, requested_angle: float, car_output, starpilot_car_control,
|
|
output_healthy: bool, now_nanos: int) -> bool:
|
|
legacy_limited = _legacy_angle_steering_limited(requested_angle, car_output)
|
|
|
|
try:
|
|
cooperative_enabled = any(
|
|
safety_config.safetyParam & TeslaSafetyFlags.COOP_STEERING.value
|
|
for safety_config in CP.safetyConfigs
|
|
)
|
|
if CP.carFingerprint != CAR.TESLA_MODEL_3 or not cooperative_enabled or not output_healthy:
|
|
return legacy_limited
|
|
|
|
if not starpilot_car_control.valid:
|
|
return legacy_limited
|
|
|
|
info = starpilot_car_control.starpilotCarControl.steeringLimitInfo
|
|
errors = (
|
|
info.modelLimitErrorDeg,
|
|
info.resumeLimitErrorDeg,
|
|
info.cooperativeLimitErrorDeg,
|
|
info.combinedLimitErrorDeg,
|
|
)
|
|
numeric_values = errors + (info.cooperativeOffsetDeg,)
|
|
mono_time = info.monoTime
|
|
|
|
if not info.valid or mono_time <= 0:
|
|
return legacy_limited
|
|
|
|
age_nanos = now_nanos - mono_time
|
|
if age_nanos < 0 or age_nanos > _MAX_STEERING_LIMIT_INFO_AGE_NANOS:
|
|
return legacy_limited
|
|
|
|
if not all(math.isfinite(value) for value in numeric_values) or any(error < 0 for error in errors):
|
|
return legacy_limited
|
|
|
|
return max(errors) > STEER_ANGLE_SATURATION_THRESHOLD
|
|
except (AttributeError, TypeError, ValueError, OverflowError):
|
|
return legacy_limited
|