mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-15 22:32:11 +08:00
whoisdomi
7e1b3769d7
1. Reduced jerk 2. Smoother 3. Improved arrest, reducing chances of overshooting if setting is too low
88 lines
3.9 KiB
Python
88 lines
3.9 KiB
Python
import numpy as np
|
|
from openpilot.common.constants 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
|
|
|
|
def smooth_value(val, prev_val, tau, dt=DT_MDL):
|
|
alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
|
|
return alpha * val + (1 - alpha) * prev_val
|
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature, roll, jerk_factor=1.0, lat_accel_factor=1.0) -> tuple[float, bool]:
|
|
# 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 * jerk_factor) / (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)
|
|
|
|
effective_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL * lat_accel_factor
|
|
roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
|
|
min_curvature = (-effective_lat_accel + roll_compensation) / v_ego ** 2
|
|
max_curvature = (effective_lat_accel + roll_compensation) / v_ego ** 2
|
|
if lat_accel_factor < 1.0:
|
|
# A tightened maneuver clamp must not clip curvature already being commanded
|
|
# (e.g. lane change on a curve); it only limits further growth.
|
|
min_curvature = min(min_curvature, prev_curvature)
|
|
max_curvature = max(max_curvature, prev_curvature)
|
|
# Saturation is reported against the stock envelope only: riding an intentionally
|
|
# tightened lane-change ceiling is comfort shaping, not steering saturation, and
|
|
# must not trip the "Turn Exceeds Steering Limit" alert.
|
|
stock_min_curvature = (-MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation) / v_ego ** 2
|
|
stock_max_curvature = (MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation) / v_ego ** 2
|
|
limited_accel = bool(new_curvature < stock_min_curvature or new_curvature > stock_max_curvature)
|
|
new_curvature, _ = clamp(new_curvature, min_curvature, max_curvature)
|
|
|
|
new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
|
return float(new_curvature), limited_accel or limited_max_curv
|
|
|
|
|
|
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.3):
|
|
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
|
|
else:
|
|
v_now = 0.0
|
|
v_target = 0.0
|
|
a_target = 0.0
|
|
should_stop = (v_now < vEgoStopping and a_target < 0.1)
|
|
return a_target, should_stop
|
|
|
|
|
|
# Backward-compatible alias used by tinygrad_modeld.
|
|
get_accel_from_plan_tomb_raider = get_accel_from_plan
|
|
|
|
|
|
def get_lateral_active(enabled: bool, active: bool, always_on_lateral_enabled: bool,
|
|
steer_fault_temporary: bool, steer_fault_permanent: bool,
|
|
standstill: bool, steer_at_standstill: bool, lateral_check: bool) -> bool:
|
|
lateral_allowed = (enabled and active) or always_on_lateral_enabled
|
|
return lateral_allowed and not steer_fault_temporary and not steer_fault_permanent and \
|
|
(not standstill or steer_at_standstill) and lateral_check
|
|
|
|
def curv_from_psis(psi_target, psi_rate, vego, action_t):
|
|
vego = np.clip(vego, MIN_SPEED, np.inf)
|
|
curv_from_psi = psi_target / (vego * action_t)
|
|
return 2*curv_from_psi - psi_rate / vego
|
|
|
|
def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
|
|
psi_target = np.interp(action_t, t_idxs, yaws)
|
|
psi_rate = yaw_rates[0]
|
|
return curv_from_psis(psi_target, psi_rate, vego, action_t)
|