You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
48 lines
1.5 KiB
48 lines
1.5 KiB
from cereal import car, log
|
|
from openpilot.common.numpy_fast import clip
|
|
from openpilot.common.realtime import DT_CTRL
|
|
from openpilot.system.version import get_build_metadata
|
|
|
|
EventName = car.OnroadEvent.EventName
|
|
|
|
MIN_SPEED = 1.0
|
|
CONTROL_N = 17
|
|
CAR_ROTATION_RADIUS = 0.0
|
|
|
|
# EU guidelines
|
|
MAX_LATERAL_JERK = 5.0
|
|
MAX_VEL_ERR = 5.0
|
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature):
|
|
v_ego = max(MIN_SPEED, v_ego)
|
|
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
|
|
safe_desired_curvature = clip(new_curvature,
|
|
prev_curvature - max_curvature_rate * DT_CTRL,
|
|
prev_curvature + max_curvature_rate * DT_CTRL)
|
|
|
|
return safe_desired_curvature
|
|
|
|
|
|
def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
|
|
# ToDo: Try relative error, and absolute speed
|
|
if len(modelV2.temporalPose.trans):
|
|
vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
|
|
return float(vel_err)
|
|
return 0.0
|
|
|
|
|
|
def get_startup_event(car_recognized, controller_available, fw_seen):
|
|
build_metadata = get_build_metadata()
|
|
if build_metadata.openpilot.comma_remote and build_metadata.tested_channel:
|
|
event = EventName.startup
|
|
else:
|
|
event = EventName.startupMaster
|
|
|
|
if not car_recognized:
|
|
if fw_seen:
|
|
event = EventName.startupNoCar
|
|
else:
|
|
event = EventName.startupNoFw
|
|
elif car_recognized and not controller_available:
|
|
event = EventName.startupNoControl
|
|
return event
|
|
|