mirror of
https://github.com/infiniteCable2/opendbc.git
synced 2026-02-18 13:03:52 +08:00
Merge branch 'master' of https://github.com/sunnypilot/opendbc
This commit is contained in:
infiniteCable2
@@ -41,7 +41,7 @@
|
||||
|Chevrolet|Bolt EV Non-ACC 2017|No Adaptive Cruise Control (Non-ACC)|[Community](community)|
|
||||
|Chevrolet|Bolt EV Non-ACC 2018-21|No Adaptive Cruise Control (Non-ACC)|[Community](community)|
|
||||
|Chevrolet|Equinox 2019-22|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|
||||
|Chevrolet|Equinox Non-ACC 2019-22|No Adaptive Cruise Control (Non-ACC)|[Dashcam mode](#dashcam)|
|
||||
|Chevrolet|Equinox Non-ACC 2019-22|No Adaptive Cruise Control (Non-ACC)|[Community](community)|
|
||||
|Chevrolet|Malibu Non-ACC 2016-23|No Adaptive Cruise Control (Non-ACC)|[Community](community)|
|
||||
|Chevrolet|Silverado 1500 2020-21|Safety Package II|[Upstream](#upstream)|
|
||||
|Chevrolet|Suburban Non-ACC 2016-20|No Adaptive Cruise Control (Non-ACC)|[Dashcam mode](#dashcam)|
|
||||
|
||||
@@ -260,9 +260,8 @@ class CarInterface(CarInterfaceBase, CarInterfaceExt):
|
||||
stock_cp.minSteerSpeed = 3.0 # ~6 mph
|
||||
|
||||
# dashcamOnly platforms: untested platforms, need user validations
|
||||
if candidate in (CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN,
|
||||
CAR.CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN, CAR.CADILLAC_CT6_NON_ACC_1ST_GEN, CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN,
|
||||
CAR.CADILLAC_XT5_NON_ACC_1ST_GEN):
|
||||
if candidate in (CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN, CAR.CADILLAC_CT6_NON_ACC_1ST_GEN,
|
||||
CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN, CAR.CADILLAC_XT5_NON_ACC_1ST_GEN):
|
||||
stock_cp.dashcamOnly = True
|
||||
|
||||
return ret
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
""" AUTO-FORMATTED USING opendbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.tesla.values import CAR
|
||||
from opendbc.sunnypilot.car.fingerprints_ext import merge_fw_versions
|
||||
from opendbc.sunnypilot.car.tesla.fingerprints_ext import FW_VERSIONS_EXT
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
@@ -48,3 +50,5 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
FW_VERSIONS = merge_fw_versions(FW_VERSIONS, FW_VERSIONS_EXT)
|
||||
|
||||
@@ -152,6 +152,7 @@ class CarState(CarStateBase, CarStateExt):
|
||||
ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor
|
||||
|
||||
if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
|
||||
if not (self.CP_SP.flags & ToyotaFlagsSP.SMART_DSU.value):
|
||||
self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
|
||||
ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"])
|
||||
|
||||
|
||||
@@ -176,6 +176,7 @@ class CarInterface(CarInterfaceBase):
|
||||
stock_cp.flags |= ToyotaFlags.DISABLE_RADAR.value
|
||||
else:
|
||||
use_sdsu = use_sdsu and alpha_long
|
||||
stock_cp.flags &= ~ToyotaFlags.DISABLE_RADAR.value
|
||||
|
||||
# openpilot longitudinal enabled by default:
|
||||
# - non-(TSS2 radar ACC cars) w/ smartDSU installed
|
||||
@@ -195,10 +196,6 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyParam |= ToyotaSafetyFlagsSP.GAS_INTERCEPTOR
|
||||
stock_cp.minEnableSpeed = -1.
|
||||
|
||||
if ret.flags & ToyotaFlagsSP.STOCK_LONGITUDINAL:
|
||||
stock_cp.alphaLongitudinalAvailable = False
|
||||
stock_cp.openpilotLongitudinalControl = False
|
||||
|
||||
if not stock_cp.openpilotLongitudinalControl:
|
||||
stock_cp.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
|
||||
else:
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
CM_ "IMPORT _community.dbc";
|
||||
CM_ "IMPORT _honda_common.dbc";
|
||||
CM_ "IMPORT _nidec_common.dbc";
|
||||
CM_ "IMPORT _lkas_hud_5byte.dbc";
|
||||
|
||||
@@ -282,6 +282,19 @@
|
||||
],
|
||||
"package": "Adaptive Cruise Control (ACC)"
|
||||
},
|
||||
"Chevrolet Equinox Non-ACC 2019-22": {
|
||||
"platform": "CHEVROLET_EQUINOX_NON_ACC_3RD_GEN",
|
||||
"make": "Chevrolet",
|
||||
"brand": "gm",
|
||||
"model": "Equinox Non-ACC",
|
||||
"year": [
|
||||
"2019",
|
||||
"2020",
|
||||
"2021",
|
||||
"2022"
|
||||
],
|
||||
"package": "No Adaptive Cruise Control (Non-ACC)"
|
||||
},
|
||||
"Chevrolet Malibu Non-ACC 2016-23": {
|
||||
"platform": "CHEVROLET_MALIBU_NON_ACC_9TH_GEN",
|
||||
"make": "Chevrolet",
|
||||
|
||||
@@ -18,7 +18,7 @@ class CarTuningConfig:
|
||||
lookahead_jerk_bp: list[float] = field(default_factory=lambda: [5., 20.])
|
||||
lookahead_jerk_upper_v: list[float] = field(default_factory=lambda: [0.25, 0.5])
|
||||
lookahead_jerk_lower_v: list[float] = field(default_factory=lambda: [0.15, 0.3])
|
||||
longitudinal_actuator_delay: float = 0.45
|
||||
longitudinal_actuator_delay: float = 0.50
|
||||
jerk_limits: float = 4.0
|
||||
|
||||
|
||||
@@ -33,8 +33,10 @@ TUNING_CONFIGS = {
|
||||
"EV": CarTuningConfig(
|
||||
stopping_decel_rate=0.45,
|
||||
v_ego_stopping=0.35,
|
||||
lookahead_jerk_upper_v=[0.3, 0.7],
|
||||
lookahead_jerk_lower_v=[0.2, 0.4],
|
||||
lookahead_jerk_bp=[2., 5., 20.],
|
||||
lookahead_jerk_upper_v=[0.25, 0.5, 1.0],
|
||||
lookahead_jerk_lower_v=[0.05, 0.10, 0.3],
|
||||
longitudinal_actuator_delay=0.25
|
||||
),
|
||||
"HYBRID": CarTuningConfig(
|
||||
v_ego_starting=0.15,
|
||||
@@ -42,6 +44,7 @@ TUNING_CONFIGS = {
|
||||
v_ego_stopping=0.4,
|
||||
),
|
||||
"DEFAULT": CarTuningConfig(
|
||||
v_ego_stopping=0.3,
|
||||
lookahead_jerk_bp=[2., 5., 20.],
|
||||
lookahead_jerk_upper_v=[0.25, 0.5, 1.0],
|
||||
lookahead_jerk_lower_v=[0.05, 0.10, 0.3],
|
||||
@@ -51,18 +54,21 @@ TUNING_CONFIGS = {
|
||||
# Car-specific configs
|
||||
CAR_SPECIFIC_CONFIGS = {
|
||||
CAR.KIA_NIRO_EV: CarTuningConfig(
|
||||
stopping_decel_rate=0.3,
|
||||
lookahead_jerk_upper_v=[0.3, 1.0],
|
||||
lookahead_jerk_lower_v=[0.2, 0.4],
|
||||
v_ego_stopping=0.1,
|
||||
stopping_decel_rate=0.1,
|
||||
lookahead_jerk_bp=[2., 5., 20.],
|
||||
lookahead_jerk_upper_v=[0.25, 0.5, 1.0],
|
||||
lookahead_jerk_lower_v=[0.1, 0.15, 0.3],
|
||||
longitudinal_actuator_delay=0.25,
|
||||
jerk_limits=2.5,
|
||||
),
|
||||
CAR.KIA_NIRO_PHEV_2022: CarTuningConfig(
|
||||
stopping_decel_rate=0.3,
|
||||
lookahead_jerk_upper_v=[0.3, 1.0],
|
||||
lookahead_jerk_lower_v=[0.15, 0.3],
|
||||
jerk_limits=4.0,
|
||||
jerk_limits=5.0,
|
||||
),
|
||||
CAR.HYUNDAI_IONIQ: CarTuningConfig(
|
||||
jerk_limits=4.5,
|
||||
jerk_limits=5.0,
|
||||
)
|
||||
}
|
||||
|
||||
@@ -17,7 +17,6 @@ from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
MIN_JERK = 0.5
|
||||
COMFORT_BAND_VAL = 0.01
|
||||
|
||||
DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -0.005]
|
||||
DYNAMIC_LOWER_JERK_V = [3.3, 2.5, 2.0, 1.9, 1.8, 1.65, 1.15, 0.5]
|
||||
@@ -101,12 +100,12 @@ class LongitudinalController:
|
||||
|
||||
# Upper jerk limit varies based on speed and control state
|
||||
if long_control_state == LongCtrlState.pid:
|
||||
upper_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [2.0, 3.0, 1.6]))
|
||||
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
|
||||
|
||||
# Lower jerk limit varies based on speed
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 4.0, 2.5]))
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
|
||||
|
||||
return upper_limit, lower_limit
|
||||
|
||||
@@ -144,9 +143,6 @@ class LongitudinalController:
|
||||
Dynamic lower jerk limit (m/s³)
|
||||
"""
|
||||
|
||||
if self.CP.radarUnavailable:
|
||||
return 5.0
|
||||
|
||||
if accel_error < 0:
|
||||
# Scale the brake jerk values based on car config
|
||||
lower_max = self.car_config.jerk_limits
|
||||
@@ -187,8 +183,6 @@ class LongitudinalController:
|
||||
|
||||
# Calculate lower jerk limit
|
||||
lower_jerk = max(-j_ego_lower, MIN_JERK)
|
||||
if self.CP.radarUnavailable:
|
||||
lower_jerk = 5.0
|
||||
|
||||
# Final jerk limits with thresholds
|
||||
desired_jerk_upper = min(max(j_ego_upper, MIN_JERK), upper_speed_factor)
|
||||
@@ -200,6 +194,10 @@ class LongitudinalController:
|
||||
dynamic_lower_jerk = self._calculate_dynamic_lower_jerk(dynamic_accel_error, velocity)
|
||||
dynamic_desired_lower_jerk = min(dynamic_lower_jerk, lower_speed_factor)
|
||||
|
||||
if self.CP.radarUnavailable:
|
||||
desired_jerk_lower = 5.0
|
||||
dynamic_desired_lower_jerk = 5.0
|
||||
|
||||
# Apply jerk limits based on tuning approach
|
||||
self.jerk_upper = ramp_update(self.jerk_upper, desired_jerk_upper)
|
||||
|
||||
@@ -223,7 +221,7 @@ class LongitudinalController:
|
||||
"""
|
||||
|
||||
# Skip custom processing if tuning is disabled or radar unavailable
|
||||
if not self.enabled or self.CP.radarUnavailable:
|
||||
if not self.enabled:
|
||||
self.desired_accel = self.accel_cmd
|
||||
self.actual_accel = self.accel_cmd
|
||||
return
|
||||
@@ -246,14 +244,19 @@ class LongitudinalController:
|
||||
|
||||
self.accel_last = self.actual_accel
|
||||
|
||||
def calculate_comfort_band(self, CC: structs.CarControl) -> None:
|
||||
if not self.enabled or self.CP.radarUnavailable or not CC.longActive:
|
||||
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
|
||||
|
||||
self.comfort_band_upper = COMFORT_BAND_VAL
|
||||
self.comfort_band_lower = COMFORT_BAND_VAL
|
||||
accel = CS.out.aEgo
|
||||
accel_vals = [0.0, 0.3, 0.6, 0.9, 1.2, 1.5]
|
||||
decel_vals = [-3.0, -2.0, -1.5, -1.0, -0.5, -0.05]
|
||||
comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
|
||||
|
||||
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]))
|
||||
|
||||
def get_tuning_state(self) -> None:
|
||||
"""Update the tuning state object with current control values.
|
||||
@@ -289,7 +292,7 @@ class LongitudinalController:
|
||||
self.get_stopping_state(actuators)
|
||||
self.calculate_jerk(CC, CS, long_control_state)
|
||||
self.calculate_accel(CC)
|
||||
self.calculate_comfort_band(CC)
|
||||
self.calculate_comfort_band(CC, CS)
|
||||
self.get_tuning_state()
|
||||
|
||||
self.long_control_state_last = long_control_state
|
||||
|
||||
@@ -5,145 +5,172 @@ 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 unittest
|
||||
import numpy as np
|
||||
from unittest.mock import Mock
|
||||
import pytest
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.controller import LongitudinalController, LongitudinalState
|
||||
from opendbc.car import structs
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.controller import LongitudinalController
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
from opendbc.car import DT_CTRL, structs
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
class TestLongitudinalTuningController(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.mock_CP = Mock(carFingerprint="KIA_NIRO_EV", flags=0)
|
||||
self.mock_CP.radarUnavailable = False # ensure tuning branch
|
||||
self.mock_CP_SP = Mock(flags=0)
|
||||
self.controller = LongitudinalController(self.mock_CP, self.mock_CP_SP)
|
||||
|
||||
def test_init(self):
|
||||
"""Test controller initialization"""
|
||||
self.assertIsInstance(self.controller.tuning, LongitudinalState)
|
||||
self.assertEqual(self.controller.desired_accel, 0.0)
|
||||
self.assertEqual(self.controller.actual_accel, 0.0)
|
||||
self.assertEqual(self.controller.jerk_upper, 0.0)
|
||||
self.assertEqual(self.controller.jerk_lower, 0.0)
|
||||
self.assertEqual(self.controller.comfort_band_upper, 0.0)
|
||||
self.assertEqual(self.controller.comfort_band_lower, 0.0)
|
||||
|
||||
def test_make_jerk_flag_off(self):
|
||||
"""Test when LONG_TUNING_DYNAMIC flag is off"""
|
||||
mock_CC, mock_CS = Mock(spec=structs.CarControl), Mock(spec=CarStateBase)
|
||||
mock_CS.out = Mock()
|
||||
mock_CS.out.vEgo = 0.0
|
||||
mock_CS.out.aEgo = 0.0
|
||||
mock_CS.aBasis = 0.0
|
||||
|
||||
# Test with PID state
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.pid)
|
||||
print(f"[PID state] jerk_upper={self.controller.jerk_upper:.2f}, jerk_lower={self.controller.jerk_lower:.2f}")
|
||||
self.assertEqual(self.controller.jerk_upper, 3.0)
|
||||
self.assertEqual(self.controller.jerk_lower, 5.0)
|
||||
|
||||
# Test with non-PID state
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.stopping)
|
||||
print(f"[Non-PID state] jerk_upper={self.controller.jerk_upper:.2f}, jerk_lower={self.controller.jerk_lower:.2f}")
|
||||
self.assertEqual(self.controller.jerk_upper, 1.0)
|
||||
self.assertEqual(self.controller.jerk_lower, 5.0)
|
||||
|
||||
def test_make_jerk_flag_on(self):
|
||||
"""Only verify that limits update when flags are on."""
|
||||
self.controller.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
self.controller.CP.flags = HyundaiFlags.CANFD
|
||||
mock_CC = Mock()
|
||||
mock_CC.actuators = Mock(accel=1.0)
|
||||
mock_CC.longActive = True
|
||||
self.controller.stopping = False
|
||||
mock_CS = Mock()
|
||||
mock_CS.out = Mock(aEgo=0.8, vEgo=3.0)
|
||||
mock_CS.aBasis = 0.8
|
||||
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.pid)
|
||||
print(f"[FlagOn] jerk_upper={self.controller.jerk_upper:.3f}, jerk_lower={self.controller.jerk_lower:.3f}")
|
||||
self.assertGreater(self.controller.jerk_upper, 0.0)
|
||||
self.assertGreater(self.controller.jerk_lower, 0.0)
|
||||
|
||||
def test_a_value_jerk_scaling(self):
|
||||
"""Test a_value jerk scaling under tuning branch."""
|
||||
self.controller.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
self.controller.CP.radarUnavailable = False
|
||||
mock_CC = Mock()
|
||||
mock_CC.actuators = Mock(accel=1.0)
|
||||
mock_CC.longActive = True
|
||||
print("[a_value] starting accel_last:", self.controller.tuning.accel_last)
|
||||
# first pass: limit to jerk_upper * DT_CTRL * 2 = 0.1
|
||||
self.controller.jerk_upper = 0.1 / (DT_CTRL * 2)
|
||||
self.controller.accel_cmd = 1.0 # ensure accel_cmd is set
|
||||
self.controller.calculate_accel(mock_CC)
|
||||
print(f"[a_value] pass1 actual_accel={self.controller.actual_accel:.5f}")
|
||||
self.assertAlmostEqual(self.controller.actual_accel, 0.1, places=5)
|
||||
|
||||
# second pass: limit increment by new jerk_upper
|
||||
mock_CC.actuators.accel = 0.7
|
||||
self.controller.jerk_upper = 0.2 / (DT_CTRL * 2)
|
||||
self.controller.accel_cmd = 0.7 # update accel_cmd
|
||||
self.controller.calculate_accel(mock_CC)
|
||||
print(f"[a_value] pass2 actual_accel={self.controller.actual_accel:.5f}")
|
||||
self.assertAlmostEqual(self.controller.actual_accel, 0.3, places=5)
|
||||
|
||||
def test_make_jerk_realistic_profile(self):
|
||||
"""Test make_jerk with realistic velocity and acceleration profile"""
|
||||
np.random.seed(42)
|
||||
num_points = 30
|
||||
segments = [
|
||||
np.random.uniform(0.3, 0.8, num_points//4),
|
||||
np.random.uniform(0.8, 1.6, num_points//4),
|
||||
np.random.uniform(-0.2, 0.2, num_points//4),
|
||||
np.random.uniform(-1.2, -0.5, num_points//8),
|
||||
np.random.uniform(-2.2, -1.2, num_points//8)
|
||||
]
|
||||
accels = np.concatenate(segments)[:num_points]
|
||||
vels = np.zeros_like(accels)
|
||||
vels[0] = 5.0
|
||||
for i in range(1, len(accels)):
|
||||
vels[i] = max(0.0, min(30.0, vels[i-1] + accels[i-1] * (DT_CTRL*2)))
|
||||
mock_CC, mock_CS = Mock(), Mock()
|
||||
mock_CC.actuators, mock_CS.out = Mock(), Mock()
|
||||
mock_CC.longActive = True
|
||||
self.controller.stopping = False
|
||||
|
||||
# Test with LONG_TUNING_DYNAMIC only
|
||||
self.controller.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
for v, a in zip(vels, accels, strict=True):
|
||||
mock_CS.out.vEgo = float(v)
|
||||
mock_CS.out.aEgo = float(a)
|
||||
mock_CS.aBasis = float(a)
|
||||
mock_CC.actuators.accel = float(a)
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.pid)
|
||||
print(f"[realistic][LONG_TUNING_DYNAMIC] v={v:.2f}, a={a:.2f}, jerk_upper={self.controller.jerk_upper:.2f}, jerk_lower={self.controller.jerk_lower:.2f}")
|
||||
self.assertGreater(self.controller.jerk_upper, 0.0)
|
||||
|
||||
# Reset controller before next test
|
||||
self.controller.tuning = LongitudinalState()
|
||||
self.controller.jerk_upper = 0.5
|
||||
self.controller.jerk_lower = 0.5
|
||||
|
||||
# Test with LONG_TUNING_DYNAMIC and LONG_TUNING_PREDICTIVE
|
||||
self.controller.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC | HyundaiFlagsSP.LONG_TUNING_PREDICTIVE
|
||||
for v, a in zip(vels, accels, strict=True):
|
||||
mock_CS.out.vEgo = float(v)
|
||||
mock_CS.out.aEgo = float(a)
|
||||
mock_CS.aBasis = float(a)
|
||||
mock_CC.actuators.accel = float(a)
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.pid)
|
||||
print(f"[realistic][LONG_TUNING_PREDICTIVE] v={v:.2f}, a={a:.2f}, " +
|
||||
f"jerk_upper={self.controller.jerk_upper:.2f}, jerk_lower={self.controller.jerk_lower:.2f}")
|
||||
self.assertGreater(self.controller.jerk_upper, 0.0)
|
||||
@dataclass
|
||||
class CP:
|
||||
carFingerprint: str = "KIA_NIRO_EV"
|
||||
flags: int = 0
|
||||
radarUnavailable: bool = False
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@dataclass
|
||||
class Actuators:
|
||||
accel: float = 0.0
|
||||
longControlState = LongCtrlState
|
||||
|
||||
|
||||
@dataclass
|
||||
class CC:
|
||||
actuators: Actuators = field(default_factory=lambda: Actuators())
|
||||
longActive: bool = True
|
||||
|
||||
|
||||
@dataclass
|
||||
class Out:
|
||||
vEgo: float = 0.0
|
||||
aEgo: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class CS:
|
||||
out: Out = field(default_factory=lambda: Out())
|
||||
aBasis: float = 0.0
|
||||
|
||||
|
||||
class TestLongitudinalTuningController:
|
||||
def setup_method(self):
|
||||
self.CP = CP(flags=0)
|
||||
self.CP_SP = CP(flags=0)
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
self.CS = CS()
|
||||
self.CC = CC()
|
||||
self.controller = LongitudinalController(self.CP, self.CP_SP)
|
||||
|
||||
def test_enabled_and_disabled(self):
|
||||
assert self.controller.enabled
|
||||
self.CP_SP.flags = 0
|
||||
assert not self.controller.enabled
|
||||
|
||||
def test_stopping_state(self):
|
||||
self.CC.actuators.longControlState = LongCtrlState.stopping
|
||||
self.controller.get_stopping_state(self.CC.actuators)
|
||||
assert self.controller.stopping
|
||||
assert self.controller.stopping_count == 0
|
||||
self.CC.actuators.longControlState = LongCtrlState.pid
|
||||
self.controller.get_stopping_state(self.CC.actuators)
|
||||
assert not self.controller.stopping
|
||||
|
||||
def test_calc_speed_based_jerk(self):
|
||||
assert (0.5, 5.0) == self.controller._calculate_speed_based_jerk_limits(0.0, LongCtrlState.stopping)
|
||||
velocities: list = [0.0, 2.0, 5.0, 7.0, 10.0, 15.0, 20.0, 25.0, 30.0]
|
||||
|
||||
for velocity in velocities:
|
||||
upper_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0]))
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
|
||||
expected: tuple = (upper_limit, lower_limit)
|
||||
actual: tuple = self.controller._calculate_speed_based_jerk_limits(velocity, LongCtrlState.pid)
|
||||
assert expected == actual
|
||||
|
||||
def test_calc_lookahead_jerk(self):
|
||||
assert pytest.approx((-1.0, -3.3), abs=0.1) == self.controller._calculate_lookahead_jerk(-0.5, 4.9)
|
||||
assert pytest.approx((1.0, 3.3), abs=0.1) == self.controller._calculate_lookahead_jerk(0.5, 4.9)
|
||||
|
||||
def test_calc_dynamic_low_jerk(self):
|
||||
self.controller.car_config.jerk_limits = 3.3
|
||||
assert 0.5 == self.controller._calculate_dynamic_lower_jerk(0.0, 10.0)
|
||||
assert 3.3 == self.controller._calculate_dynamic_lower_jerk(-2.0, 10.0)
|
||||
|
||||
def test_calc_jerk(self):
|
||||
self.CP_SP.flags = 0
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
|
||||
assert self.controller.jerk_upper == 3.0
|
||||
assert self.controller.jerk_lower == 5.0
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.off)
|
||||
assert self.controller.jerk_upper == 1.0
|
||||
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_PREDICTIVE
|
||||
self.controller.__init__(self.CP, self.CP_SP)
|
||||
self.CS.out.vEgo = 10.0
|
||||
self.controller.accel_cmd = -3.5
|
||||
self.controller.accel_last = -1.0
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
|
||||
assert self.controller.jerk_upper == 0.1 # ramp update first pass
|
||||
assert self.controller.jerk_lower == pytest.approx(3.33, abs=0.01)
|
||||
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
self.controller.__init__(self.CP, self.CP_SP)
|
||||
self.controller.car_config.jerk_limits = 3.3
|
||||
self.CS.out.vEgo = 10.0
|
||||
self.CS.aBasis = -3.3
|
||||
self.CS.out.aEgo = -3.5
|
||||
self.controller.accel_cmd = -3.5
|
||||
self.controller.accel_last = -1.0
|
||||
for _ in range(50):
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
|
||||
assert self.controller.jerk_upper == 0.5
|
||||
assert self.controller.jerk_lower == 3.3
|
||||
|
||||
def test_calc_accel(self):
|
||||
self.CP_SP.flags = 0
|
||||
self.controller.accel_cmd = 1.5
|
||||
self.controller.calculate_accel(self.CC)
|
||||
assert self.controller.desired_accel == self.controller.accel_cmd
|
||||
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
self.CC.longActive = False
|
||||
self.controller.calculate_accel(self.CC)
|
||||
assert self.controller.desired_accel == 0.0
|
||||
|
||||
self.CC.longActive = True
|
||||
self.controller.stopping = True
|
||||
self.controller.calculate_accel(self.CC)
|
||||
assert self.controller.desired_accel == 0.0
|
||||
|
||||
def test_calc_comfort_band(self):
|
||||
stock_decels_list: list = [-3.0, -2.0, -1.5, -1.0, -0.5, -0.05]
|
||||
stock_accels_list: list = [0.0, 0.3, 0.6, 0.9, 1.2, 1.5]
|
||||
stock_comfort_band_vals: list = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
|
||||
|
||||
decels_list: list = [-3.5, -3.1, -2.245, -1.853, -1.234, -0.64352, -0.06432, -0.00005]
|
||||
accels_list: list = [0.0, 0.23345, 0.456, 0.5677, 0.6788, 0.834, 1.0, 1.3456, 1.8]
|
||||
|
||||
for decel in decels_list:
|
||||
self.CS.out.aEgo = decel
|
||||
self.controller.calculate_comfort_band(self.CC, self.CS)
|
||||
actual = self.controller.comfort_band_lower
|
||||
expected = float(np.interp(decel, stock_decels_list, [0.1, 0.08, 0.06, 0.04, 0.02, 0.0]))
|
||||
assert actual == expected
|
||||
assert self.controller.comfort_band_upper == 0.0
|
||||
|
||||
for accel in accels_list:
|
||||
self.CS.out.aEgo = accel
|
||||
self.controller.calculate_comfort_band(self.CC, self.CS)
|
||||
actual = self.controller.comfort_band_upper
|
||||
expected = float(np.interp(accel, stock_accels_list, stock_comfort_band_vals))
|
||||
assert actual == expected
|
||||
assert self.controller.comfort_band_lower == 0.0
|
||||
|
||||
def test_update(self):
|
||||
self.CC.actuators.accel = 2.0
|
||||
self.CC.actuators.longControlState = LongCtrlState.pid
|
||||
self.CS.aBasis = 1.75
|
||||
self.CS.out.aEgo = 2.0
|
||||
self.CS.out.vEgo = 5.0
|
||||
for _ in range(50):
|
||||
self.controller.update(self.CC, self.CS)
|
||||
|
||||
assert self.controller.jerk_lower == 0.5
|
||||
assert self.controller.jerk_upper == pytest.approx(1.01, abs=0.01)
|
||||
assert self.controller.comfort_band_lower == 0.0
|
||||
assert self.controller.comfort_band_upper == 0.10
|
||||
assert self.controller.desired_accel == 2.0
|
||||
|
||||
@@ -13,6 +13,7 @@ from opendbc.car import structs
|
||||
from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.car.subaru.values import SubaruFlags
|
||||
from opendbc.car.toyota.values import ToyotaSafetyFlags
|
||||
from opendbc.sunnypilot.car.hyundai.enable_radar_tracks import enable_radar_tracks as hyundai_enable_radar_tracks
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import LongitudinalTuningType
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
@@ -138,3 +139,6 @@ def _initialize_toyota(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params
|
||||
|
||||
if toyota_stock_long:
|
||||
CP_SP.flags |= ToyotaFlagsSP.STOCK_LONGITUDINAL.value
|
||||
CP.alphaLongitudinalAvailable = False
|
||||
CP.openpilotLongitudinalControl = False
|
||||
CP.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
|
||||
|
||||
12
opendbc/sunnypilot/car/tesla/fingerprints_ext.py
Normal file
12
opendbc/sunnypilot/car/tesla/fingerprints_ext.py
Normal file
@@ -0,0 +1,12 @@
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.tesla.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
FW_VERSIONS_EXT = {
|
||||
CAR.TESLA_MODEL_3: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'TeMYG4_Main_0.0.0 (67),E4HP015.02.1',
|
||||
],
|
||||
},
|
||||
}
|
||||
Reference in New Issue
Block a user