sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,54 @@
|
||||
# **How custom longitudinal tuning works for Hyundai/Kia/Genesis vehicles in sunnypilot.**
|
||||
|
||||
This document explains the custom longitudinal control tuning implemented in sunnypilot for Hyundai, Kia, and Genesis vehicles.
|
||||
|
||||
**Safety First: ISO 15622:2018**
|
||||
|
||||
Our primary safety guideline is [ISO 15622:2018](https://www.iso.org/obp/ui/en/#iso:std:iso:15622:ed-3:v1:en).
|
||||
This standard provides the foundation for the safety limits enforced by this tuning, particularly concerning maximum allowable jerk based on speed.
|
||||
|
||||
**Core Concepts**
|
||||
|
||||
1. **Jerk-Limited Acceleration:** Instead of directly applying the planner's target acceleration (`CC.actuators.accel`), we use a jerk-limited integrator. This ensures smoother transitions by limiting the rate of change of acceleration (jerk).
|
||||
* `desired_accel`: The target acceleration from the planner, clipped within safe limits (`CarControllerParams.ACCEL_MIN`, `CarControllerParams.ACCEL_MAX`). During stopping, this is forced to 0.
|
||||
* `actual_accel`: The acceleration command sent to the car. It's calculated by integrating `desired_accel` but limiting the change from the previous step (`state.accel_last`) based on calculated `jerk_upper` and `jerk_lower` limits.
|
||||
* `jerk_limited_integrator(desired_accel, last_accel, jerk_upper, jerk_lower)`: This function calculates the maximum allowable acceleration change per control step (`DT_CTRL * 2`) based on the upper and lower jerk limits and applies it using `rate_limit`.
|
||||
|
||||
2. **Predictive Jerk Limits:** The upper and lower jerk limits (`jerk_upper`, `jerk_lower`) are not fixed but calculated dynamically based on several factors:
|
||||
* **Actual Acceleration (`aego`):** The car's current acceleration (`CS.out.aEgo`, potentially blended with `CS.aBasis` at low speeds) is smoothed using a `FirstOrderFilter` (`self.aego`).
|
||||
* **Acceleration Error:** The difference between the *commanded* acceleration (`self.accel_cmd`) and the *actual* smoothed acceleration (`self.aego.x`).
|
||||
* **Lookahead Jerk:** We estimate the jerk required to reach the target acceleration (`accel_cmd`) within a future time window. This window (`future_t_upper`, `future_t_lower`) varies with speed, defined in `config.py` (`lookahead_jerk_bp`, `lookahead_jerk_upper_v`, `lookahead_jerk_lower_v`).
|
||||
* `j_ego_upper = accel_error / future_t_upper`
|
||||
* `j_ego_lower = accel_error / future_t_lower`
|
||||
* **ISO 15622 Speed Factors:** Maximum allowed jerk decreases with speed according to ISO 15622. We interpolate values based on current velocity (`CS.out.vEgo`) to get `upper_speed_factor` and `lower_speed_factor`.
|
||||
* **Minimum Jerk (`MIN_JERK`):** A baseline minimum jerk (currently 0.5 m/s³) is enforced to ensure responsiveness.
|
||||
* **Combining Factors:**
|
||||
* `desired_jerk_upper = min(max(j_ego_upper, MIN_JERK), upper_speed_factor)`
|
||||
* `desired_jerk_lower = min(max(-j_ego_lower, MIN_JERK), lower_speed_factor)` (Note: `j_ego_lower` is negated for the lower limit calculation).
|
||||
|
||||
3. **Dynamic Braking Tune:** For use of the tune without the `LONG_TUNING_PREDICTIVE` flag, a specific negative jerk profile is applied during braking (`gen1_accel_error < 0`).
|
||||
* `gen1_accel_error = a_ego_blended - self.state.accel_last`
|
||||
* A dynamic lower jerk limit (`gen1_lower_jerk`) is interpolated based on this error using `DYNAMIC_LOWER_JERK_BP` and a dynamically scaled `DYNAMIC_LOWER_JERK_V` (scaled by `car_config.jerk_limits`).
|
||||
* `gen1_desired_jerk_lower = min(gen1_lower_jerk, lower_speed_factor)`
|
||||
|
||||
4. **Applying Jerk Limits:**
|
||||
* The `jerk_upper` limit is always smoothed using `ramp_update` to prevent abrupt changes.
|
||||
* The `jerk_lower` limit is applied differently based on the `LONG_TUNING_PREDICTIVE` flag:
|
||||
* **If Flag Set (predictive tune):** `jerk_lower` is set directly to `desired_jerk_lower`.
|
||||
* **If Flag Not Set (dynamic tune):** `jerk_lower` is smoothed using `ramp_update` towards `gen1_desired_jerk_lower`.
|
||||
|
||||
**Configuration (`config.py`)**
|
||||
|
||||
* The `CarTuningConfig` dataclass holds tuning parameters like stopping/starting speeds, lookahead jerk profiles, actuator delay, and base jerk limits.
|
||||
* `TUNING_CONFIGS` provides default configurations based on car type (CANFD, EV, HYBRID, DEFAULT).
|
||||
* `CAR_SPECIFIC_CONFIGS` allows overriding defaults for specific car models (e.g., `KIA_NIRO_EV`, `HYUNDAI_IONIQ`).
|
||||
* The `get_car_config` helper function selects the appropriate configuration based on the car fingerprint and flags.
|
||||
* `get_longitudinal_tune` applies relevant config values (like stopping/starting speeds, delay) to the main `CarParams` (CP).
|
||||
|
||||
**Summary**
|
||||
|
||||
This tuning aims for smoother and more responsive longitudinal control by:
|
||||
* Using a jerk-limited integrator for acceleration commands.
|
||||
* Dynamically calculating jerk limits based on lookahead predictions, actual acceleration, and ISO 15622 speed constraints.
|
||||
* Providing specific tuning profiles via a configuration system based on car type and model.
|
||||
* Applying special braking logic for use of predictive or dynamic tune.
|
||||
Whitespace-only changes.
@@ -0,0 +1,55 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from opendbc.car.hyundai.values import CAR
|
||||
|
||||
|
||||
@dataclass
|
||||
class CarTuningConfig:
|
||||
v_ego_stopping: float = 0.25
|
||||
v_ego_starting: float = 0.10
|
||||
stopping_decel_rate: float = 0.40
|
||||
lookahead_jerk_bp: list[float] = field(default_factory=lambda: [2., 5., 20.])
|
||||
lookahead_jerk_upper_v: list[float] = field(default_factory=lambda: [0.3, 0.45, 0.6])
|
||||
lookahead_jerk_lower_v: list[float] = field(default_factory=lambda: [0.3, 0.45, 0.6])
|
||||
longitudinal_actuator_delay: float = 0.50
|
||||
jerk_limits: float = 4.0
|
||||
|
||||
|
||||
# Default configurations for different car types
|
||||
TUNING_CONFIGS = {
|
||||
"CANFD": CarTuningConfig(
|
||||
v_ego_stopping=0.30,
|
||||
),
|
||||
"EV": CarTuningConfig(
|
||||
stopping_decel_rate=0.45,
|
||||
v_ego_stopping=0.35,
|
||||
),
|
||||
"HYBRID": CarTuningConfig(
|
||||
v_ego_starting=0.15,
|
||||
stopping_decel_rate=0.45,
|
||||
v_ego_stopping=0.4,
|
||||
),
|
||||
"DEFAULT": CarTuningConfig(
|
||||
v_ego_stopping=0.3,
|
||||
)
|
||||
}
|
||||
|
||||
# Car-specific configs
|
||||
CAR_SPECIFIC_CONFIGS = {
|
||||
CAR.KIA_NIRO_EV: CarTuningConfig(
|
||||
v_ego_stopping=0.1,
|
||||
stopping_decel_rate=0.3,
|
||||
jerk_limits=3.3,
|
||||
),
|
||||
CAR.KIA_NIRO_PHEV_2022: CarTuningConfig(
|
||||
stopping_decel_rate=0.8,
|
||||
jerk_limits=5.0,
|
||||
),
|
||||
}
|
||||
@@ -0,0 +1,266 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
from opendbc.car import structs, DT_CTRL
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.hyundai.values import CarControllerParams
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import get_car_config, jerk_limited_integrator
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
MIN_JERK = 0.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class LongitudinalState:
|
||||
desired_accel: float = 0.0
|
||||
actual_accel: float = 0.0
|
||||
accel_last: float = 0.0
|
||||
jerk_upper: float = 0.0
|
||||
jerk_lower: float = 0.0
|
||||
comfort_band_upper: float = 0.0
|
||||
comfort_band_lower: float = 0.0
|
||||
stopping: bool = False
|
||||
|
||||
|
||||
class LongitudinalController:
|
||||
"""Longitudinal controller which gets injected into CarControllerParams."""
|
||||
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None:
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.tuning = LongitudinalState()
|
||||
self.car_config = get_car_config(CP)
|
||||
self.long_control_state_last = LongCtrlState.off
|
||||
self.stopping_count = 0
|
||||
|
||||
self.accel_cmd = 0.0
|
||||
self.desired_accel = 0.0
|
||||
self.actual_accel = 0.0
|
||||
self.accel_last = 0.0
|
||||
self.jerk_upper = 0.0
|
||||
self.jerk_lower = 0.0
|
||||
self.comfort_band_upper = 0.0
|
||||
self.comfort_band_lower = 0.0
|
||||
self.stopping = False
|
||||
|
||||
@property
|
||||
def enabled(self) -> bool:
|
||||
return bool(self.CP_SP.flags & (HyundaiFlagsSP.LONG_TUNING_DYNAMIC | HyundaiFlagsSP.LONG_TUNING_PREDICTIVE))
|
||||
|
||||
def get_stopping_state(self, actuators: structs.CarControl.Actuators) -> None:
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
|
||||
# If custom tuning is not enabled, use upstream stopping logic
|
||||
if not self.enabled:
|
||||
self.stopping = stopping
|
||||
self.stopping_count = 0
|
||||
return
|
||||
|
||||
# Reset stopping state when not in stopping mode
|
||||
if not stopping:
|
||||
self.stopping = False
|
||||
self.stopping_count = 0
|
||||
return
|
||||
|
||||
# When transitioning from off state to stopping
|
||||
if self.long_control_state_last == LongCtrlState.off:
|
||||
self.stopping = True
|
||||
return
|
||||
|
||||
# Keep track of time in stopping state (in control cycles)
|
||||
if self.stopping_count > 1 / (DT_CTRL * 5):
|
||||
self.stopping = True
|
||||
|
||||
self.stopping_count += 1
|
||||
|
||||
def _calculate_speed_based_jerk_limits(self, velocity: float, long_control_state: LongCtrlState) -> tuple[float, float]:
|
||||
"""Calculate jerk limits based on vehicle speed according to ISO 15622:2018.
|
||||
|
||||
Args:
|
||||
velocity: Current vehicle speed (m/s)
|
||||
long_control_state: Current longitudinal control state
|
||||
|
||||
Returns:
|
||||
Tuple of (upper_limit, lower_limit) in m/s³
|
||||
"""
|
||||
|
||||
if long_control_state == LongCtrlState.pid:
|
||||
upper_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0]))
|
||||
else:
|
||||
upper_limit = 0.5 # Default for non-PID states
|
||||
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
|
||||
else:
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [3.5, 3.5, 3.0]))
|
||||
|
||||
return upper_limit, lower_limit
|
||||
|
||||
def _calculate_lookahead_jerk(self, accel_error: float, velocity: float) -> tuple[float, float]:
|
||||
"""Calculate lookahead jerk needed to reach target acceleration.
|
||||
|
||||
Args:
|
||||
accel_error: Difference between target and current acceleration (m/s²)
|
||||
velocity: Current vehicle speed (m/s)
|
||||
|
||||
Returns:
|
||||
Tuple of (upper_jerk, lower_jerk) in m/s³
|
||||
"""
|
||||
|
||||
# Time window to reach target acceleration, varies with speed
|
||||
future_t_upper = float(np.interp(velocity, self.car_config.lookahead_jerk_bp, self.car_config.lookahead_jerk_upper_v))
|
||||
future_t_lower = float(np.interp(velocity, self.car_config.lookahead_jerk_bp, self.car_config.lookahead_jerk_lower_v))
|
||||
|
||||
# Required jerk to reach target acceleration in lookahead window
|
||||
j_ego_upper = accel_error / future_t_upper
|
||||
j_ego_lower = accel_error / future_t_lower
|
||||
|
||||
jerk_limit = self.car_config.jerk_limits
|
||||
j_ego_upper = np.clip(j_ego_upper, -jerk_limit, jerk_limit)
|
||||
j_ego_lower = np.clip(j_ego_lower, -jerk_limit, jerk_limit)
|
||||
|
||||
return float(j_ego_upper), float(j_ego_lower)
|
||||
|
||||
def calculate_jerk(self, CC: structs.CarControl, CS: CarStateBase, long_control_state: LongCtrlState) -> None:
|
||||
"""Calculate appropriate jerk limits for smooth acceleration/deceleration.
|
||||
|
||||
Args:
|
||||
CC: Car control signals
|
||||
CS: Car state
|
||||
long_control_state: Current longitudinal control state
|
||||
"""
|
||||
|
||||
# If custom tuning is disabled, use upstream fixed values
|
||||
if not self.enabled:
|
||||
jerk_limit = 3.0 if long_control_state == LongCtrlState.pid else 1.0
|
||||
self.jerk_upper = jerk_limit
|
||||
self.jerk_lower = 5.0
|
||||
return
|
||||
|
||||
velocity = CS.out.vEgo
|
||||
accel_error = self.accel_cmd - self.accel_last
|
||||
|
||||
# Calculate jerk limits based on speed
|
||||
upper_speed_factor, lower_speed_factor = self._calculate_speed_based_jerk_limits(velocity, long_control_state)
|
||||
|
||||
# Apply jerk limits based on tuning approach
|
||||
# Predictive tuning uses calculated desired jerk directly
|
||||
# Minimal Dynamic tuning applies only a speed based approach
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
|
||||
j_ego_upper, j_ego_lower = self._calculate_lookahead_jerk(accel_error, velocity)
|
||||
desired_jerk_upper = min(max(j_ego_upper, MIN_JERK), upper_speed_factor)
|
||||
desired_jerk_lower = min(max(-j_ego_lower, MIN_JERK), lower_speed_factor)
|
||||
|
||||
self.jerk_upper = desired_jerk_upper
|
||||
self.jerk_lower = desired_jerk_lower if not self.CP.radarUnavailable else 5.0
|
||||
else:
|
||||
self.jerk_upper = upper_speed_factor
|
||||
self.jerk_lower = lower_speed_factor
|
||||
|
||||
# Disable jerk when longitudinal control is inactive
|
||||
if not CC.longActive:
|
||||
self.jerk_upper = 0.0
|
||||
self.jerk_lower = 0.0
|
||||
|
||||
def calculate_accel(self, CC: structs.CarControl) -> None:
|
||||
"""Calculate commanded acceleration using jerk-limited approach.
|
||||
|
||||
Args:
|
||||
CC: Car control signals
|
||||
"""
|
||||
|
||||
# Skip custom processing if tuning is disabled or radar unavailable
|
||||
if not self.enabled:
|
||||
self.desired_accel = self.accel_cmd
|
||||
self.actual_accel = self.accel_cmd
|
||||
return
|
||||
|
||||
# Reset acceleration when control is inactive
|
||||
if not CC.longActive:
|
||||
self.desired_accel = 0.0
|
||||
self.actual_accel = 0.0
|
||||
self.accel_last = 0.0
|
||||
return
|
||||
|
||||
# Force zero acceleration during stopping
|
||||
if self.stopping:
|
||||
self.desired_accel = 0.0
|
||||
else:
|
||||
self.desired_accel = float(np.clip(self.accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
# Apply jerk-limited integration to get smooth acceleration
|
||||
if not self.CP.radarUnavailable:
|
||||
self.actual_accel = jerk_limited_integrator(self.desired_accel, self.accel_last, self.jerk_upper, self.jerk_lower)
|
||||
else:
|
||||
self.actual_accel = self.desired_accel
|
||||
|
||||
self.accel_last = self.actual_accel
|
||||
|
||||
def calculate_comfort_band(self, CC: structs.CarControl, CS: CarStateBase) -> None:
|
||||
if not self.enabled or not CC.longActive:
|
||||
self.comfort_band_upper = 0.0
|
||||
self.comfort_band_lower = 0.0
|
||||
return
|
||||
|
||||
accel = CS.out.aEgo
|
||||
accel_vals = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0]
|
||||
decel_vals = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05]
|
||||
comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
|
||||
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
|
||||
self.comfort_band_upper = float(np.interp(accel, accel_vals, comfort_band_vals))
|
||||
self.comfort_band_lower = float(np.interp(accel, decel_vals, comfort_band_vals[::-1]))
|
||||
else:
|
||||
self.comfort_band_upper = float(np.interp(accel, [0.0, 2.0], [0.0, 0.04]))
|
||||
self.comfort_band_lower = float(np.interp(accel, [-3.5, -0.05], [0.04, 0.0]))
|
||||
|
||||
def get_tuning_state(self) -> None:
|
||||
"""Update the tuning state object with current control values.
|
||||
|
||||
External components depend on this state for longitudinal control.
|
||||
"""
|
||||
|
||||
self.tuning = LongitudinalState(
|
||||
desired_accel=self.desired_accel,
|
||||
actual_accel=self.actual_accel,
|
||||
accel_last=self.accel_last,
|
||||
jerk_upper=self.jerk_upper,
|
||||
jerk_lower=self.jerk_lower,
|
||||
comfort_band_upper=self.comfort_band_upper,
|
||||
comfort_band_lower=self.comfort_band_lower,
|
||||
stopping=self.stopping,
|
||||
)
|
||||
|
||||
def update(self, CC: structs.CarControl, CS: CarStateBase) -> None:
|
||||
"""Update longitudinal control calculations.
|
||||
|
||||
This is the main entry point called externally.
|
||||
|
||||
Args:
|
||||
CC: Car control signals including actuators
|
||||
CS: Car state information
|
||||
"""
|
||||
|
||||
actuators = CC.actuators
|
||||
long_control_state = actuators.longControlState
|
||||
self.accel_cmd = CC.actuators.accel
|
||||
|
||||
self.get_stopping_state(actuators)
|
||||
self.calculate_jerk(CC, CS, long_control_state)
|
||||
self.calculate_accel(CC)
|
||||
self.calculate_comfort_band(CC, CS)
|
||||
self.get_tuning_state()
|
||||
|
||||
self.long_control_state_last = long_control_state
|
||||
|
||||
# TODO-SP Why were we calculating both jerks at once lol
|
||||
@@ -0,0 +1,53 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.car import structs, DT_CTRL, rate_limit
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.config import CarTuningConfig, TUNING_CONFIGS, CAR_SPECIFIC_CONFIGS
|
||||
|
||||
JERK_THRESHOLD = 0.1
|
||||
JERK_STEP = 0.1
|
||||
|
||||
|
||||
class LongitudinalTuningType:
|
||||
OFF = 0
|
||||
DYNAMIC = 1
|
||||
PREDICTIVE = 2
|
||||
|
||||
|
||||
def get_car_config(CP: structs.CarParams) -> CarTuningConfig:
|
||||
# Get car type flags from specific configs or determine from car flags
|
||||
car_config = CAR_SPECIFIC_CONFIGS.get(CP.carFingerprint)
|
||||
# If car is not in specific configs, determine from flags
|
||||
if car_config is None:
|
||||
if CP.flags & HyundaiFlags.CANFD:
|
||||
car_config = TUNING_CONFIGS["CANFD"]
|
||||
elif CP.flags & HyundaiFlags.EV:
|
||||
car_config = TUNING_CONFIGS["EV"]
|
||||
elif CP.flags & HyundaiFlags.HYBRID:
|
||||
car_config = TUNING_CONFIGS["HYBRID"]
|
||||
else:
|
||||
car_config = TUNING_CONFIGS["DEFAULT"]
|
||||
|
||||
return car_config
|
||||
|
||||
|
||||
def get_longitudinal_tune(CP: structs.CarParams) -> None:
|
||||
config = get_car_config(CP)
|
||||
CP.vEgoStopping = config.v_ego_stopping
|
||||
CP.vEgoStarting = config.v_ego_starting
|
||||
CP.stoppingDecelRate = config.stopping_decel_rate
|
||||
CP.startingState = False
|
||||
CP.longitudinalActuatorDelay = config.longitudinal_actuator_delay
|
||||
|
||||
|
||||
def jerk_limited_integrator(desired_accel, last_accel, jerk_upper, jerk_lower) -> float:
|
||||
if desired_accel >= last_accel:
|
||||
val = jerk_upper * DT_CTRL * 5
|
||||
else:
|
||||
val = jerk_lower * DT_CTRL * 5
|
||||
|
||||
return rate_limit(desired_accel, last_accel, -val, val)
|
||||
Reference in new issue
Block a user