46 lines
1.7 KiB
Python
46 lines
1.7 KiB
Python
import numpy as np
|
|
from cereal import log
|
|
from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
|
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
|
|
MAX_VEL_ERR = 5.0 # m/s
|
|
|
|
# EU guidelines
|
|
MAX_LATERAL_JERK = 5.0 # m/s^3
|
|
MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2
|
|
|
|
|
|
def clamp(val, min_val, max_val):
|
|
clamped_val = float(np.clip(val, min_val, max_val))
|
|
return clamped_val, clamped_val != val
|
|
|
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature, roll):
|
|
# This function respects ISO lateral jerk and acceleration limits + a max curvature
|
|
v_ego = max(v_ego, MIN_SPEED)
|
|
max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
|
|
new_curvature = np.clip(new_curvature,
|
|
prev_curvature - max_curvature_rate * DT_CTRL,
|
|
prev_curvature + max_curvature_rate * DT_CTRL)
|
|
|
|
roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
|
|
max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
|
|
min_lat_accel = -MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
|
|
new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
|
|
|
|
new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
|
return float(new_curvature), limited_accel or limited_max_curv
|
|
|
|
|
|
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 = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
|
|
return float(vel_err)
|
|
return 0.0
|