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
Whitespace-only changes.
@@ -0,0 +1,64 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.sunnypilot.car.hyundai.escc import EnhancedSmartCruiseControl, ESCC_MSG
|
||||
from opendbc.car.hyundai.carstate import CarState
|
||||
from opendbc.car import structs
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
|
||||
class TestEscc(unittest.TestCase):
|
||||
def _make_car_params(self):
|
||||
params = structs.CarParams()
|
||||
params.carFingerprint = "HYUNDAI_SONATA"
|
||||
return params
|
||||
|
||||
def _make_car_params_sp(self):
|
||||
params = structs.CarParamsSP()
|
||||
params.flags = HyundaiFlagsSP.ENHANCED_SCC
|
||||
return params
|
||||
|
||||
def _make_escc(self):
|
||||
return EnhancedSmartCruiseControl(self._make_car_params(), self._make_car_params_sp())
|
||||
|
||||
def test_escc_msg_id(self):
|
||||
escc = self._make_escc()
|
||||
self.assertEqual(escc.trigger_msg, ESCC_MSG)
|
||||
|
||||
def test_enabled_flag(self):
|
||||
car_params = self._make_car_params()
|
||||
for value in range(256):
|
||||
with self.subTest(flags=value):
|
||||
car_params_sp = structs.CarParamsSP()
|
||||
car_params_sp.flags = value
|
||||
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
|
||||
self.assertEqual(escc.enabled, bool(value & HyundaiFlagsSP.ENHANCED_SCC))
|
||||
|
||||
def test_update_car_state(self):
|
||||
car_params = self._make_car_params()
|
||||
car_params_sp = self._make_car_params_sp()
|
||||
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
|
||||
car_state = CarState(car_params, car_params_sp)
|
||||
car_state.escc_cmd_act = 1
|
||||
car_state.escc_aeb_warning = 1
|
||||
car_state.escc_aeb_dec_cmd_act = 1
|
||||
car_state.escc_aeb_dec_cmd = 1
|
||||
escc.update_car_state(car_state)
|
||||
self.assertEqual(escc.car_state, car_state)
|
||||
|
||||
def test_update_scc12(self):
|
||||
car_params = self._make_car_params()
|
||||
car_params_sp = self._make_car_params_sp()
|
||||
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
|
||||
car_state = CarState(car_params, car_params_sp)
|
||||
car_state.escc_cmd_act = 1
|
||||
car_state.escc_aeb_warning = 1
|
||||
car_state.escc_aeb_dec_cmd_act = 1
|
||||
car_state.escc_aeb_dec_cmd = 1
|
||||
escc.update_car_state(car_state)
|
||||
scc12_message = {}
|
||||
escc.update_scc12(scc12_message)
|
||||
self.assertEqual(scc12_message["AEB_CmdAct"], 1)
|
||||
self.assertEqual(scc12_message["CF_VSM_Warn"], 1)
|
||||
self.assertEqual(scc12_message["CF_VSM_DecCmdAct"], 1)
|
||||
self.assertEqual(scc12_message["CR_VSM_DecCmd"], 1)
|
||||
self.assertEqual(scc12_message["AEB_Status"], 2)
|
||||
@@ -0,0 +1,78 @@
|
||||
import unittest
|
||||
from enum import IntFlag
|
||||
|
||||
from opendbc.sunnypilot.car.hyundai.lead_data_ext import LeadDataCarController, CanLeadData, CanFdLeadData
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
|
||||
|
||||
def make_carparams(flags: IntFlag = HyundaiFlags.LEGACY):
|
||||
cp = structs.CarParams()
|
||||
cp.carFingerprint = "HYUNDAI_SONATA"
|
||||
cp.flags = flags.value
|
||||
return cp
|
||||
|
||||
|
||||
def make_carcontrolsp(leadDistance=10.0, leadRelSpeed=0.0, leadVisible=True):
|
||||
c = structs.CarControlSP()
|
||||
c.leadOne.dRel = leadDistance
|
||||
c.leadOne.vRel = leadRelSpeed
|
||||
c.leadOne.status = leadVisible
|
||||
return c
|
||||
|
||||
|
||||
class TestLeadDataCarController(unittest.TestCase):
|
||||
def test_update_object_gap(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
# Initial value should be 0
|
||||
self.assertEqual(ctrl.object_gap, 0)
|
||||
|
||||
# Set to 15 (should become 2 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(15)
|
||||
self.assertEqual(ctrl.object_gap, 2)
|
||||
|
||||
# Set to 22 (should become 3 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(22)
|
||||
self.assertEqual(ctrl.object_gap, 3)
|
||||
|
||||
# Set to 0 (should become 0 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(0)
|
||||
self.assertEqual(ctrl.object_gap, 0)
|
||||
|
||||
def test_update_lead_visible_hysteresis(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
ctrl._update_lead_visible_hysteresis(True)
|
||||
self.assertIsInstance(ctrl.lead_visible, bool)
|
||||
ctrl._update_lead_visible_hysteresis(False)
|
||||
self.assertIsInstance(ctrl.lead_visible, bool)
|
||||
|
||||
def test_update(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
sp = make_carcontrolsp(leadDistance=25, leadRelSpeed=-0.5, leadVisible=True)
|
||||
ctrl.update(sp)
|
||||
self.assertEqual(ctrl.lead_distance, 25)
|
||||
self.assertEqual(ctrl.lead_rel_speed, -0.5)
|
||||
self.assertIsInstance(ctrl.lead_visible, bool)
|
||||
|
||||
def test_lead_data_can(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
ctrl.object_gap = 1
|
||||
ctrl.lead_distance = 10
|
||||
ctrl.lead_rel_speed = -0.3
|
||||
ctrl.lead_visible = True
|
||||
ld = ctrl.lead_data
|
||||
self.assertIsInstance(ld, CanLeadData)
|
||||
self.assertEqual(ld.object_rel_gap, 2)
|
||||
|
||||
def test_lead_data_canfd(self):
|
||||
ctrl = LeadDataCarController(make_carparams(HyundaiFlags.CANFD))
|
||||
ctrl.object_gap = 1
|
||||
ctrl.lead_distance = 10
|
||||
ctrl.lead_rel_speed = 1.0
|
||||
ctrl.lead_visible = True
|
||||
ld = ctrl.lead_data
|
||||
self.assertIsInstance(ld, CanFdLeadData)
|
||||
self.assertEqual(ld.object_rel_gap, 1)
|
||||
@@ -0,0 +1,121 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.testing import parameterized
|
||||
|
||||
from opendbc.car import CanData
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.hyundai.values import CAR, HyundaiFlags
|
||||
from opendbc.sunnypilot.car.hyundai.escc import ESCC_MSG
|
||||
|
||||
ESCC_CARS = [
|
||||
(CAR.HYUNDAI_ELANTRA_2021, ESCC_MSG),
|
||||
]
|
||||
|
||||
CAMERA_SCC_CARS = [
|
||||
(CAR.HYUNDAI_KONA_EV_2022, 0, 0x420, "SCC11"),
|
||||
(CAR.HYUNDAI_IONIQ_5, HyundaiFlags.CANFD_CAMERA_SCC.value, 0x1A0, "SCC_CONTROL"),
|
||||
]
|
||||
|
||||
STANDARD_RADAR_CARS = [
|
||||
(CAR.HYUNDAI_ELANTRA_2021, 0),
|
||||
(CAR.HYUNDAI_SANTA_FE, 0),
|
||||
]
|
||||
|
||||
|
||||
class TestRadarInterfaceExt(unittest.TestCase):
|
||||
|
||||
@staticmethod
|
||||
def _setup_platform(car_name, additional_flags=0, escc_msg=None):
|
||||
"""Set up the platform with specific parameters"""
|
||||
CarInterface = interfaces[car_name]
|
||||
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP.flags |= additional_flags
|
||||
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
|
||||
RD = CI.RadarInterface(CP, CP_SP)
|
||||
|
||||
if escc_msg is not None and hasattr(RD, 'use_escc'):
|
||||
try:
|
||||
RD.use_escc = True
|
||||
except AttributeError:
|
||||
object.__setattr__(RD, 'use_escc', True)
|
||||
|
||||
return RD, CP, CP_SP
|
||||
|
||||
@parameterized("car_name, escc_msg", ESCC_CARS)
|
||||
def test_escc_radar_interface(self, car_name, escc_msg):
|
||||
"""Test radar interface for ESCC-enabled cars"""
|
||||
RD, CP, CP_SP = self._setup_platform(car_name, escc_msg=escc_msg)
|
||||
|
||||
# Assert that ESCC features are present
|
||||
if hasattr(RD, 'use_escc'):
|
||||
self.assertTrue(RD.use_escc, "ESCC car should have use_escc=True")
|
||||
if hasattr(RD, 'use_radar_interface_ext'):
|
||||
self.assertTrue(RD.use_radar_interface_ext, "ESCC car should use radar interface ext")
|
||||
|
||||
# Run radar interface once
|
||||
RD.update([])
|
||||
|
||||
# Test radar fault
|
||||
if not CP.radarUnavailable and RD.rcp is not None:
|
||||
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
|
||||
rr = RD.update(cans)
|
||||
self.assertTrue(rr is None or len(rr.errors) > 0)
|
||||
|
||||
@parameterized("car_name, flags, expected_trigger, msg_src", CAMERA_SCC_CARS)
|
||||
def test_camera_scc_radar_interface(self, car_name, flags, expected_trigger, msg_src):
|
||||
"""Test radar interface for Camera SCC cars"""
|
||||
RD, CP, CP_SP = self._setup_platform(car_name, additional_flags=flags)
|
||||
|
||||
# Assert Camera SCC flag is set appropriately
|
||||
if flags & HyundaiFlags.CAMERA_SCC:
|
||||
self.assertTrue(CP.flags & HyundaiFlags.CAMERA_SCC, "Car should have CAMERA_SCC flag")
|
||||
if flags & HyundaiFlags.CANFD_CAMERA_SCC:
|
||||
self.assertTrue(CP.flags & HyundaiFlags.CANFD_CAMERA_SCC, "Car should have CANFD_CAMERA_SCC flag")
|
||||
|
||||
# Check if using radar interface ext
|
||||
if hasattr(RD, 'use_radar_interface_ext'):
|
||||
self.assertTrue(RD.use_radar_interface_ext, "Camera SCC car should use radar interface ext")
|
||||
|
||||
# Verify trigger message
|
||||
if hasattr(RD, 'trigger_msg'):
|
||||
self.assertEqual(RD.trigger_msg, expected_trigger, f"Expected trigger_msg {expected_trigger}, got {RD.trigger_msg}")
|
||||
|
||||
# Run radar interface once
|
||||
RD.update([])
|
||||
|
||||
# Test radar fault
|
||||
if not CP.radarUnavailable and RD.rcp is not None:
|
||||
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
|
||||
rr = RD.update(cans)
|
||||
self.assertTrue(rr is None or len(rr.errors) > 0)
|
||||
|
||||
@parameterized("car_name, flags", STANDARD_RADAR_CARS)
|
||||
def test_standard_radar_interface(self, car_name, flags):
|
||||
"""Test radar interface for standard radar cars"""
|
||||
RD, CP, CP_SP = self._setup_platform(car_name, additional_flags=flags)
|
||||
|
||||
# Standard cars should not use radar interface ext
|
||||
if hasattr(RD, 'use_radar_interface_ext'):
|
||||
self.assertFalse(RD.use_radar_interface_ext, "Standard car should not use radar interface ext")
|
||||
|
||||
# Run radar interface once
|
||||
RD.update([])
|
||||
|
||||
# For standard radar, test the _update method directly if available
|
||||
if not CP.radarUnavailable and RD.rcp is not None and \
|
||||
hasattr(RD, '_update') and hasattr(RD, 'trigger_msg'):
|
||||
# Setup for _update test if needed
|
||||
if hasattr(RD, 'updated_messages'):
|
||||
RD.updated_messages = {RD.trigger_msg}
|
||||
RD._update(RD.updated_messages)
|
||||
|
||||
# Test radar fault
|
||||
if not CP.radarUnavailable and RD.rcp is not None:
|
||||
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
|
||||
rr = RD.update(cans)
|
||||
self.assertTrue(rr is None or len(rr.errors) > 0)
|
||||
@@ -0,0 +1,173 @@
|
||||
"""
|
||||
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 unittest
|
||||
import numpy as np
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.controller import LongitudinalController
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
@dataclass
|
||||
class CP:
|
||||
carFingerprint: str = "KIA_NIRO_EV"
|
||||
flags: int = 0
|
||||
radarUnavailable: bool = False
|
||||
|
||||
|
||||
@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(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.CP = CP(flags=0)
|
||||
self.CP_SP = CP(flags=0)
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_PREDICTIVE
|
||||
self.CS = CS()
|
||||
self.CC = CC()
|
||||
self.controller = LongitudinalController(self.CP, self.CP_SP)
|
||||
|
||||
def test_enabled_and_disabled(self):
|
||||
self.assertTrue(self.controller.enabled)
|
||||
self.CP_SP.flags = 0
|
||||
self.assertFalse(self.controller.enabled)
|
||||
|
||||
def test_stopping_state(self):
|
||||
self.CC.actuators.longControlState = LongCtrlState.stopping
|
||||
self.controller.get_stopping_state(self.CC.actuators)
|
||||
self.assertTrue(self.controller.stopping)
|
||||
self.assertEqual(self.controller.stopping_count, 0)
|
||||
self.CC.actuators.longControlState = LongCtrlState.pid
|
||||
self.controller.get_stopping_state(self.CC.actuators)
|
||||
self.assertFalse(self.controller.stopping)
|
||||
|
||||
def test_calc_speed_based_jerk(self):
|
||||
self.assertEqual((0.5, 5.0), self.controller._calculate_speed_based_jerk_limits(0.0, LongCtrlState.stopping))
|
||||
velocities = [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 = (upper_limit, lower_limit)
|
||||
actual = self.controller._calculate_speed_based_jerk_limits(velocity, LongCtrlState.pid)
|
||||
self.assertEqual(expected, actual)
|
||||
|
||||
def test_calc_lookahead_jerk(self):
|
||||
upper, lower = self.controller._calculate_lookahead_jerk(-0.5, 4.9)
|
||||
self.assertAlmostEqual(upper, -1.1, delta=0.1)
|
||||
self.assertAlmostEqual(lower, -1.1, delta=0.1)
|
||||
upper, lower = self.controller._calculate_lookahead_jerk(0.5, 5.0)
|
||||
self.assertAlmostEqual(upper, 1.1, delta=0.1)
|
||||
self.assertAlmostEqual(lower, 1.1, delta=0.1)
|
||||
|
||||
def test_calc_jerk(self):
|
||||
self.CP_SP.flags = 0
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
|
||||
self.assertEqual(self.controller.jerk_upper, 3.0)
|
||||
self.assertEqual(self.controller.jerk_lower, 5.0)
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.off)
|
||||
self.assertEqual(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)
|
||||
self.assertEqual(self.controller.jerk_upper, 0.5)
|
||||
self.assertAlmostEqual(self.controller.jerk_lower, 3.3, delta=0.01)
|
||||
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
self.controller.__init__(self.CP, self.CP_SP)
|
||||
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)
|
||||
self.assertAlmostEqual(self.controller.jerk_upper, 2.67, delta=0.01)
|
||||
self.assertAlmostEqual(self.controller.jerk_lower, 3.33, delta=0.01)
|
||||
|
||||
def test_calc_accel(self):
|
||||
self.CP_SP.flags = 0
|
||||
self.controller.accel_cmd = 1.5
|
||||
self.controller.calculate_accel(self.CC)
|
||||
self.assertEqual(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)
|
||||
self.assertEqual(self.controller.desired_accel, 0.0)
|
||||
|
||||
self.CC.longActive = True
|
||||
self.controller.stopping = True
|
||||
self.controller.calculate_accel(self.CC)
|
||||
self.assertEqual(self.controller.desired_accel, 0.0)
|
||||
|
||||
def test_calc_comfort_band(self):
|
||||
stock_decels_list = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05]
|
||||
stock_accels_list = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0]
|
||||
stock_comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
|
||||
|
||||
decels_list = [-3.5, -3.1, -2.245, -1.853, -1.234, -0.64352, -0.06432, -0.00005]
|
||||
accels_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]))
|
||||
self.assertEqual(actual, expected)
|
||||
self.assertEqual(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))
|
||||
self.assertEqual(actual, expected)
|
||||
self.assertEqual(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
|
||||
|
||||
self.controller.update(self.CC, self.CS)
|
||||
self.assertEqual(self.controller.jerk_lower, 0.5)
|
||||
self.assertEqual(self.controller.jerk_upper, 3.0)
|
||||
self.assertEqual(self.controller.comfort_band_lower, 0.0)
|
||||
self.assertEqual(self.controller.comfort_band_upper, 0.10)
|
||||
self.assertEqual(self.controller.desired_accel, 2.0)
|
||||
Reference in new issue
Block a user