mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-03-04 13:03:54 +08:00
8743bc4fe2
* e8cb7f27-e448-4c15-90c2-ac440cd5a042/400 * 0078ad07-4d46-4086-820f-23d61c90e07f/400 * 4bd74082-70af-47da-8156-e84ebf4d4812/400 * 2a074022-5c2c-4628-97f9-f54849a936a6/400 * 0660aa81-93c5-41b7-9cc2-dc8816a512cd/400 * Clip curvature to reasonable limits * Better curvature and speed clips * typo * typo * 31aa62c3-b373-4878-8f2e-5107305de187/400 * 384690ca-9b8a-41fe-9bcd-389b20fc6aa4/400 * ref commit --------- Co-authored-by: Yassine <yassine.y10@gmail.com>
32 lines
1.1 KiB
Python
32 lines
1.1 KiB
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
|
|
# This is a turn radius smaller than most cars can achieve
|
|
MAX_CURVATURE = 0.2
|
|
|
|
# EU guidelines
|
|
MAX_LATERAL_JERK = 5.0
|
|
MAX_VEL_ERR = 5.0
|
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature):
|
|
new_curvature = clip(new_curvature, -MAX_CURVATURE, MAX_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
|