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.
28 lines
972 B
28 lines
972 B
from cereal import log
|
|
from openpilot.common.numpy_fast import clip
|
|
from openpilot.common.realtime import DT_CTRL
|
|
|
|
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
|
|
|