openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.

65 lines
2.4 KiB

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, DT_MDL
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
2 weeks ago
def smooth_value(val, prev_val, tau):
alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1
return alpha * val + (1 - alpha) * prev_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
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
if len(speeds) == len(t_idxs):
v_now = speeds[0]
a_now = accels[0]
v_target = np.interp(action_t, t_idxs, speeds)
a_target = 2 * (v_target - v_now) / (action_t) - a_now
v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
else:
v_target = 0.0
v_target_1sec = 0.0
a_target = 0.0
should_stop = (v_target < vEgoStopping and
v_target_1sec < vEgoStopping)
return a_target, should_stop