mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-20 10:12:07 +08:00
Harald Schäfer
e8bea2c78f
* 69acff08-5383-4103-beea-822f0d228c76/160 * 53f39907-4763-4d19-ba26-e757527c2b61/200 * 21afb89f-1397-4652-b423-abc2da32417a/200 * 21afb89f-1397-4652-b423-abc2da32417a/400 * 53f39907-4763-4d19-ba26-e757527c2b61/400 * 3fb967fd-16a8-4569-ba57-359e54deeab3/395 * Try other policy again * 3fb967fd-16a8-4569-ba57-359e54deeab3/400 * 0136cabf-539a-4a43-bc7f-06c3654a493c/4400 * 473c0686-1ac9-4c05-9b0b-d1f1afdb6cc3/400 * 83b7993d-51a9-4e3f-904e-3fcd5763c231/400 * 021566c7-cff3-431d-8da2-17a96c888c5f/400 * Fix bugs * Update longitudinal_planner.py * Update fill_model_msg.py * 8be6b59d-4449-42fd-b8e0-93c48387070f/400 * f670a748-3591-4489-a0b3-215118ddad01/400 * 790a2950-c713-4eec-838b-4f55f4fe0ccb/400 * Update model ref * Plans start at ego * Update ref
29 lines
956 B
Python
29 lines
956 B
Python
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.velocity.x):
|
|
vel_err = clip(modelV2.velocity.x[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
|
|
return float(vel_err)
|
|
return 0.0
|