mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-31 19:13:52 +08:00
Compare commits
68 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 0defb30c6a | |||
| bcafa532a2 | |||
| 667fb666b9 | |||
| 32f49f8527 | |||
| 8e521e6992 | |||
| f8214fe6c6 | |||
| 258cbb76ec | |||
| c7c07799d3 | |||
| 7f7dbc6a65 | |||
| 2e68e2d926 | |||
| 0623c1a0f2 | |||
| 69999fa6cc | |||
| 1f60231a84 | |||
| a651454e7d | |||
| 8928743749 | |||
| 148cba7260 | |||
| d9127cf936 | |||
| 6d51c06692 | |||
| 3a5d376b54 | |||
| c7fb5d545d | |||
| 9d36f4cd0d | |||
| 8693affef0 | |||
| df68b82a5e | |||
| 3980401632 | |||
| 7f48d7c53b | |||
| 6e83349fe2 | |||
| 89c189e528 | |||
| 698496dd60 | |||
| 4d4ed2b3da | |||
| a49d8772f3 | |||
| 639450281e | |||
| dc8d448b35 | |||
| 61fd19556f | |||
| 956aceaf0a | |||
| 51d09a6274 | |||
| 15bbbf3cc4 | |||
| ebd62915a2 | |||
| 934dadbcdb | |||
| e75470f2e8 | |||
| 6de77049b3 | |||
| 34ecbf9204 | |||
| d9bd49fb8a | |||
| 5e8501002d | |||
| c582bd3f2d | |||
| 15c7110647 | |||
| c2109496a7 | |||
| cede5ddc9d | |||
| 385428f6ea | |||
| f973d151c6 | |||
| e60595ccc3 | |||
| 7b06ec2ae6 | |||
| 12947fd616 | |||
| 189f2aafbb | |||
| 2efe6017bf | |||
| 3293a34219 | |||
| d2cd3fd0ed | |||
| 4ac874f6b4 | |||
| 1aefe9dc82 | |||
| 0857acfc50 | |||
| 5c3ddd58e1 | |||
| 7848a0090a | |||
| 86599a27bc | |||
| d9f58acbb7 | |||
| 42d4a5b207 | |||
| b22e2d678a | |||
| 26433e078e | |||
| bf1b916d50 | |||
| 2a12dbd0ad |
@@ -237,6 +237,8 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
|
||||
cscLearnedLatAccel @40 :Float32; # learned comfort at the current curvature, before margin
|
||||
cscBindingDistance @41 :Float32; # distance to the horizon point setting the target, m
|
||||
approachStopLength @42 :Float32; # pre-commit distance to a detected stop, m; 0 when off
|
||||
slcPresentedSpeedLimitSource @43 :Text; # source of the shown accepted or pending posted limit
|
||||
slcIsLimitingMaxSet @44 :Bool; # SLC target is below the configured Max Set
|
||||
}
|
||||
|
||||
struct StarPilotRadarState @0xb86e6369214c01c8 {
|
||||
|
||||
Binary file not shown.
@@ -389,6 +389,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ForceStopDistanceOffset", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"FordLKASButtonControlMigrated", {PERSISTENT, BOOL, "0", "0"}},
|
||||
{"SonataHybridLKASButtonControlMigrated", {PERSISTENT, BOOL, "0", "0"}},
|
||||
{"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
// These Ford curvature tuning concepts descend from BluePilot bp-7.0. StarPilot's key names and
|
||||
// settings integration are local; see /CREDITS.md and /THIRD_PARTY_NOTICES.md for provenance.
|
||||
@@ -728,6 +729,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SteerRatio", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
{"SteerRatioStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
{"EnableTorqueBarWidget", {PERSISTENT, BOOL, "1", "0", 0}},
|
||||
{"StingerObjectShadow", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"StockConfidenceBallWidget", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"StockDongleId", {PERSISTENT, STRING, "", ""}},
|
||||
{"StopAccel", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
@@ -745,6 +747,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SubaruRedneckCruise", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"TeslaAOLDisengageOnBrake", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"TeslaAOLScreenTap", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"TetheringEnabled", {PERSISTENT, INT, "0", "0", 0}},
|
||||
|
||||
Binary file not shown.
+2
-1
@@ -4,7 +4,7 @@
|
||||
|
||||
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
|
||||
|
||||
# 453 Supported Cars
|
||||
# 454 Supported Cars
|
||||
|
||||
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br> |Video|Setup Video|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
|
||||
@@ -265,6 +265,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2022-23">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (with HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K4 (without HDA II) 2026|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K4 (without HDA II) 2026">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 2021-24">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai M connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 (without HDA II) 2025">Buy Here</a></sub></details>|||
|
||||
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 Hybrid 2020-22">Buy Here</a></sub></details>|||
|
||||
|
||||
@@ -133,6 +133,10 @@ function launch {
|
||||
# start manager
|
||||
cd system/manager
|
||||
|
||||
if [ -f /AGNOS ]; then
|
||||
timeout 15s python3 "$DIR/tools/galaxy_bluetooth/settings_backup.py" || echo "Galaxy settings backup unavailable"
|
||||
fi
|
||||
|
||||
sp_launch_timing "launch_param_migrations_start"
|
||||
if ! python3 ./launch_param_migrations.py; then
|
||||
echo "Launch param migrations failed; continuing boot."
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<!--- AUTOGENERATED FROM selfdrive/car/CARS_template.md, DO NOT EDIT. --->
|
||||
|
||||
# Support Information for 518 Known Cars
|
||||
# Support Information for 519 Known Cars
|
||||
|
||||
|Make|Model|Package|Support Level|
|
||||
|---|---|---|:---:|
|
||||
@@ -287,6 +287,7 @@
|
||||
|Kia|Forte Non-SCC 2021|No Smart Cruise Control (Non-SCC)|[Community](community)|
|
||||
|Kia|K4 (with HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K4 (without HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K4 (without HDA II) 2026|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K5 2021-24|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K5 (without HDA II) 2025|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|
||||
@@ -4,7 +4,7 @@ from opendbc.can import CANPacker
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, structs
|
||||
from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
|
||||
from opendbc.car.ford import fordcan
|
||||
from opendbc.car.ford.values import CarControllerParams, FordFlags
|
||||
from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags
|
||||
from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
|
||||
# This Ford extension boundary substantially adapts BluePilot bp-7.0 work. See the root CREDITS.md
|
||||
# (including Alan Polk's d0aac605f and db2bdff05) and THIRD_PARTY_NOTICES.md.
|
||||
@@ -64,13 +64,29 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c
|
||||
return apply_curvature
|
||||
|
||||
|
||||
def apply_creep_compensation(accel: float, v_ego: float) -> float:
|
||||
def apply_creep_compensation(accel: float, v_ego: float, car_fingerprint: str, *, standstill: bool, stopping: bool) -> float:
|
||||
if car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and not (standstill and stopping):
|
||||
return accel
|
||||
creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.])
|
||||
creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.])
|
||||
accel -= creep_accel
|
||||
return float(accel)
|
||||
|
||||
|
||||
def apply_brake_rate_limit(accel: float, gas: float, accel_last: float, v_ego: float, car_fingerprint: str,
|
||||
*, stopping: bool, standstill: bool, fcw: bool) -> tuple[float, float]:
|
||||
mild_mach_e_braking = (
|
||||
car_fingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and
|
||||
-1.0 < gas < 0.0 and v_ego >= 5.0 and not (stopping or standstill or fcw)
|
||||
)
|
||||
if mild_mach_e_braking:
|
||||
accel = max(accel, min(accel_last, 0.0) - 2.0 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL)
|
||||
gas = accel
|
||||
else:
|
||||
accel = max(accel, accel_last - 3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL)
|
||||
return float(accel), float(gas)
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP):
|
||||
super().__init__(dbc_names, CP)
|
||||
@@ -181,16 +197,17 @@ class CarController(CarControllerBase):
|
||||
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
|
||||
accel = actuators.accel
|
||||
gas = accel
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
|
||||
if CC.longActive:
|
||||
# Compensate for engine creep at low speed.
|
||||
# Either the ABS does not account for engine creep, or the correction is very slow
|
||||
# TODO: verify this applies to EV/hybrid
|
||||
accel = apply_creep_compensation(accel, CS.out.vEgo)
|
||||
accel = apply_creep_compensation(accel, CS.out.vEgo, self.CP.carFingerprint,
|
||||
standstill=CS.out.standstill, stopping=stopping)
|
||||
|
||||
# The stock system has been seen rate limiting the brake accel to 5 m/s^3,
|
||||
# however even 3.5 m/s^3 causes some overshoot with a step response.
|
||||
accel = max(accel, self.accel - (3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL))
|
||||
accel, gas = apply_brake_rate_limit(
|
||||
accel, gas, self.accel, CS.out.vEgo, self.CP.carFingerprint,
|
||||
stopping=stopping, standstill=CS.out.standstill,
|
||||
fcw=fcw_alert or CS.out.stockFcw or CS.out.stockAeb,
|
||||
)
|
||||
|
||||
accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
gas = float(np.clip(gas, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
@@ -210,7 +227,6 @@ class CarController(CarControllerBase):
|
||||
elif accel_pitch_compensated < 0.0:
|
||||
self.brake_request = True
|
||||
|
||||
stopping = CC.actuators.longControlState == LongCtrlState.stopping
|
||||
# TODO: look into using the actuators packet to send the desired speed
|
||||
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
|
||||
|
||||
|
||||
@@ -29,6 +29,9 @@ class CarState(CarStateBase):
|
||||
|
||||
self.distance_button = 0
|
||||
self.lc_button = 0
|
||||
self.cancel_button = False
|
||||
self.cancel_resume_pressed = False
|
||||
self.cancel_resume_is_cancel = False
|
||||
self.lkas_available = False
|
||||
self.lateral_motion_control = None
|
||||
self.lateral_control_status = None
|
||||
@@ -156,6 +159,14 @@ class CarState(CarStateBase):
|
||||
prev_lc_button = self.lc_button
|
||||
self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]
|
||||
self.lc_button = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
||||
prev_cancel_button = self.cancel_button
|
||||
cancel_resume_pressed = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnCnclResPress"])
|
||||
if cancel_resume_pressed and not self.cancel_resume_pressed:
|
||||
self.cancel_resume_is_cancel = ret.cruiseState.available and ret.cruiseState.enabled
|
||||
self.cancel_resume_pressed = cancel_resume_pressed
|
||||
self.cancel_button = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnCnclPress"]) or (
|
||||
cancel_resume_pressed and self.cancel_resume_is_cancel
|
||||
)
|
||||
|
||||
# lock info
|
||||
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
|
||||
@@ -183,10 +194,13 @@ class CarState(CarStateBase):
|
||||
except KeyError:
|
||||
self.lateral_motion_control = None
|
||||
|
||||
ret.buttonEvents = [
|
||||
button_events = [
|
||||
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.lc_button, prev_lc_button, {1: ButtonType.lkas}),
|
||||
]
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
button_events += create_button_events(self.cancel_button, prev_cancel_button, {1: ButtonType.cancel})
|
||||
ret.buttonEvents = button_events
|
||||
|
||||
fp_ret = custom.StarPilotCarState.new_message()
|
||||
fp_ret.brakeLights = ret.brakePressed
|
||||
|
||||
@@ -40,6 +40,8 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.5]
|
||||
ret.longitudinalTuning.kpV = [0.]
|
||||
if candidate == CAR.FORD_MUSTANG_MACH_E_MK1:
|
||||
ret.longitudinalTuning.kiV = [0.3]
|
||||
|
||||
if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR:
|
||||
# average of 33.3 Hz radar timestep / 4 scan modes = 60 ms
|
||||
|
||||
@@ -6,15 +6,16 @@ from hypothesis import settings, given, strategies as st
|
||||
from parameterized import parameterized
|
||||
import pytest
|
||||
|
||||
from opendbc.car import Bus, gen_empty_fingerprint
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, gen_empty_fingerprint, structs
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car.ford import fordcan
|
||||
from opendbc.car.ford.carcontroller import FordStockCruiseButton
|
||||
from opendbc.car.ford.carcontroller import CarController, FordStockCruiseButton, apply_brake_rate_limit, apply_creep_compensation
|
||||
from opendbc.car.ford.carstate import CarState
|
||||
from opendbc.car.gps import FORD_MACH_E_GPS_MESSAGES, get_car_gps_config, parse_ford_can_gps
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.structs import CarParams, CarState as CarStateStruct
|
||||
from opendbc.car.fw_versions import build_fw_dict
|
||||
from opendbc.car.ford.interface import CarInterface
|
||||
from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, FordSafetyFlags, get_platform_codes, match_vin_to_car
|
||||
from opendbc.car.ford.values import CAR, DBC, FW_QUERY_CONFIG, FW_PATTERN, FordSafetyFlags, get_platform_codes, match_vin_to_car
|
||||
from opendbc.car.ford.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
@@ -38,6 +39,135 @@ def test_stock_cruise_button_ignores_press_with_cruise_master_off():
|
||||
assert button.update(True, cruise_available=False, cruise_enabled=False) == (False, False)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("op_long", (False, True))
|
||||
@pytest.mark.parametrize("standstill", (False, True))
|
||||
@pytest.mark.parametrize("cruise_status", (4, 5))
|
||||
@pytest.mark.parametrize("switch", ("CcAslButtnCnclResPress", "CcAslButtnCnclPress"))
|
||||
def test_ford_cancel_event_does_not_require_pcm_disengagement(op_long, standstill, cruise_status, switch):
|
||||
CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], op_long, False, False, None)
|
||||
state = CarState(CP, None)
|
||||
parsers = state.get_can_parsers(CP)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
for frame, (pressed, status) in enumerate(((True, cruise_status), (True, 3), (False, 3)), start=1):
|
||||
messages = [
|
||||
packer.make_can_msg("Steering_Data_FD1", fordcan.CanBus(CP).main, {switch: int(pressed)}),
|
||||
packer.make_can_msg("EngBrakeData", fordcan.CanBus(CP).main, {"CcStat_D_Actl": status}),
|
||||
packer.make_can_msg("DesiredTorqBrk", fordcan.CanBus(CP).main, {"VehStop_D_Stat": int(standstill)}),
|
||||
]
|
||||
parsers[Bus.pt].update([(frame * 100_000_000, messages)])
|
||||
ret, _ = state.update(parsers, None)
|
||||
assert ret.standstill == standstill
|
||||
expected = [(CarStateStruct.ButtonEvent.Type.cancel, pressed)] if op_long and frame != 2 else []
|
||||
assert [(event.type, event.pressed) for event in ret.buttonEvents] == expected
|
||||
|
||||
|
||||
@pytest.mark.parametrize("initial_status", (0, 3))
|
||||
def test_ford_resume_does_not_become_cancel_when_pcm_engages(initial_status):
|
||||
CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], True, False, False, None)
|
||||
state = CarState(CP, None)
|
||||
parsers = state.get_can_parsers(CP)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
for frame, (pressed, status) in enumerate(((True, initial_status), (True, 4), (False, 4)), start=1):
|
||||
messages = [
|
||||
packer.make_can_msg("Steering_Data_FD1", fordcan.CanBus(CP).main, {"CcAslButtnCnclResPress": int(pressed)}),
|
||||
packer.make_can_msg("EngBrakeData", fordcan.CanBus(CP).main, {"CcStat_D_Actl": status}),
|
||||
]
|
||||
parsers[Bus.pt].update([(frame * 100_000_000, messages)])
|
||||
ret, _ = state.update(parsers, None)
|
||||
assert not ret.buttonEvents
|
||||
|
||||
|
||||
def test_mach_e_does_not_apply_engine_creep_compensation():
|
||||
for accel in (-1.0, -0.1, 0.0, 0.1):
|
||||
assert apply_creep_compensation(accel, 0.5, CAR.FORD_MUSTANG_MACH_E_MK1,
|
||||
standstill=False, stopping=False) == accel
|
||||
|
||||
assert apply_creep_compensation(0.0, 0.0, CAR.FORD_MUSTANG_MACH_E_MK1,
|
||||
standstill=True, stopping=True) == -0.6
|
||||
assert apply_creep_compensation(0.0, 0.5, CAR.FORD_F_150_MK14,
|
||||
standstill=False, stopping=False) == -0.6
|
||||
|
||||
|
||||
@pytest.mark.parametrize("last", (0.0, 0.35, 0.8))
|
||||
def test_mach_e_mild_brake_transition_shapes_regen_and_brake_together(last):
|
||||
accel, gas = apply_brake_rate_limit(-0.5, -0.5, last, 11.0, CAR.FORD_MUSTANG_MACH_E_MK1,
|
||||
stopping=False, standstill=False, fcw=False)
|
||||
assert accel == pytest.approx(-0.04)
|
||||
assert gas == accel
|
||||
|
||||
|
||||
@pytest.mark.parametrize("accel,last", ((-0.2, -0.5), (-0.5, -0.48), (-0.8, -0.4)))
|
||||
def test_mach_e_mild_brake_ramp_does_not_delay_release(accel, last):
|
||||
brake, gas = apply_brake_rate_limit(accel, accel, last, 11.0, CAR.FORD_MUSTANG_MACH_E_MK1,
|
||||
stopping=False, standstill=False, fcw=False)
|
||||
assert brake == pytest.approx(max(accel, last - 0.04))
|
||||
assert gas == brake
|
||||
|
||||
|
||||
@pytest.mark.parametrize("accel,speed,stopping,standstill,fcw", (
|
||||
(-3.5, 11.0, False, False, False),
|
||||
(-1.0, 11.0, False, False, False),
|
||||
(-0.5, 4.9, False, False, False),
|
||||
(-0.5, 11.0, True, False, False),
|
||||
(-0.5, 11.0, False, True, False),
|
||||
(-0.5, 11.0, False, False, True),
|
||||
(0.2, 11.0, False, False, False),
|
||||
))
|
||||
def test_mach_e_strong_braking_stops_and_warnings_keep_original_ramp(accel, speed, stopping, standstill, fcw):
|
||||
brake, gas = apply_brake_rate_limit(accel, accel, 0.0, speed, CAR.FORD_MUSTANG_MACH_E_MK1,
|
||||
stopping=stopping, standstill=standstill, fcw=fcw)
|
||||
assert brake == pytest.approx(max(accel, -0.07))
|
||||
assert gas == accel
|
||||
|
||||
|
||||
@pytest.mark.parametrize("fingerprint", [car for car in CAR if car != CAR.FORD_MUSTANG_MACH_E_MK1])
|
||||
def test_other_fords_keep_original_brake_requests_and_integral_gain(fingerprint):
|
||||
brake, gas = apply_brake_rate_limit(-0.5, -0.5, 0.35, 11.0, fingerprint,
|
||||
stopping=False, standstill=False, fcw=False)
|
||||
assert brake == pytest.approx(0.28)
|
||||
assert gas == -0.5
|
||||
CP = CarInterface.get_params(fingerprint, gen_empty_fingerprint(), [], True, False, False, None)
|
||||
assert list(CP.longitudinalTuning.kiV) == [0.5]
|
||||
|
||||
|
||||
def test_mach_e_integral_gain_preserves_delay_and_braking_authority():
|
||||
CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], True, False, False, None)
|
||||
assert list(CP.longitudinalTuning.kiV) == pytest.approx([0.3])
|
||||
assert CP.longitudinalActuatorDelay == pytest.approx(0.15)
|
||||
assert CarInterface.get_pid_accel_limits(CP, 11.0, 20.0)[0] == -3.5
|
||||
|
||||
|
||||
def test_mach_e_can_brake_handoff_and_driver_override(monkeypatch):
|
||||
monkeypatch.setattr("opendbc.car.ford.carcontroller.FordLateralController",
|
||||
lambda _: SimpleNamespace(update_inputs=lambda: None))
|
||||
monkeypatch.setattr(fordcan, "create_acc_ui_msg", lambda *args: (0, b"", 0))
|
||||
monkeypatch.setattr(fordcan, "create_lkas_ui_msg", lambda *args: (0, b"", 0))
|
||||
CP = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], True, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ACCDATA", 50)], 0)
|
||||
CC = structs.CarControl.new_message()
|
||||
CC.enabled = True
|
||||
CC.actuators.longControlState = structs.CarControl.Actuators.LongControlState.pid
|
||||
CS = SimpleNamespace(out=structs.CarState.new_message(), acc_tja_status_stock_values={"Tja_D_Stat": 0})
|
||||
CS.out.vEgo = 11.0
|
||||
|
||||
for frame, active, request, expected in ((2, True, -0.5, -0.04), (4, False, 0.0, 0.0),
|
||||
(8, True, -0.5, -0.04), (14, True, -3.5, -0.11)):
|
||||
controller.frame = frame
|
||||
CC.longActive = active
|
||||
CC.actuators.accel = request
|
||||
output, messages = controller.update(CC.as_reader(), CS, frame * 10_000_000, None)
|
||||
parser.update([(frame * 10_000_000, [msg for msg in messages if msg[0] == 0x186])])
|
||||
values = parser.vl["ACCDATA"]
|
||||
assert output.accel == pytest.approx(expected)
|
||||
assert values["AccBrkTot_A_Rq"] == pytest.approx(expected, abs=0.01)
|
||||
assert values["AccPrpl_A_Rq"] == pytest.approx(expected if active and request > -1.0 else -5.0, abs=0.01)
|
||||
assert values["Cmbb_B_Enbl"] == int(active)
|
||||
assert values["AccResumEnbl_B_Rq"] == int(active)
|
||||
|
||||
|
||||
ECU_ADDRESSES = {
|
||||
Ecu.eps: 0x730, # Power Steering Control Module (PSCM)
|
||||
Ecu.abs: 0x760, # Anti-Lock Brake System (ABS)
|
||||
|
||||
@@ -8,6 +8,7 @@ from typing import Any
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.ford.values import CAR as FORD_CAR
|
||||
from opendbc.car.gm.values import CAR as GM_CAR
|
||||
from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN_CAR
|
||||
|
||||
|
||||
CarGpsSample = dict[str, Any]
|
||||
@@ -124,12 +125,66 @@ def parse_chevrolet_bolt_can_gps(position: Mapping[str, float]) -> CarGpsSample
|
||||
}
|
||||
|
||||
|
||||
def parse_volkswagen_taos_can_gps(position: Mapping[str, float], motion: Mapping[str, float],
|
||||
altitude: Mapping[str, float], status: Mapping[str, float]) -> CarGpsSample | None:
|
||||
packet_ids = (position["GNSS_Nachrichtenpaket_ID1"], motion["GNSS_Nachrichtenpaket_ID2"],
|
||||
altitude["GNSS_Nachrichtenpaket_ID4"], status["GNSS_Nachrichtenpaket_ID5"])
|
||||
if len(set(packet_ids)) != 1:
|
||||
return None
|
||||
|
||||
timestamp_ms = int(status["GNSS_UTC_Zeit"]) * 1000
|
||||
if timestamp_ms == 0:
|
||||
return None
|
||||
|
||||
latitude = float(position["GNSS_LatitudeMagnitude"])
|
||||
longitude = float(position["GNSS_LongitudeMagnitude"])
|
||||
coordinates_valid = (math.isfinite(latitude) and math.isfinite(longitude) and
|
||||
0.0 <= latitude <= 90.0 and 0.0 <= longitude <= 180.0 and (latitude != 0.0 or longitude != 0.0))
|
||||
if coordinates_valid:
|
||||
latitude *= -1.0 if position["GNSS_LatitudeSouth"] else 1.0
|
||||
longitude *= -1.0 if position["GNSS_LongitudeWest"] else 1.0
|
||||
else:
|
||||
latitude = longitude = 0.0
|
||||
|
||||
satellite_count = int(status["GNSS_Genutzte_Satelliten"])
|
||||
hemisphere_validated = position["GNSS_LatitudeSouth"] == 0 and position["GNSS_LongitudeWest"] == 1
|
||||
has_fix = (coordinates_valid and hemisphere_validated and position["GNSS_PositionStatus"] == 3 and status["GNSS_Empfaenger_Status"] == 1 and
|
||||
bool(status["GNSS_GPS_in_Nutzung"] or status["GNSS_GLONASS_in_Nutzung"]) and 4 <= satellite_count <= 31)
|
||||
|
||||
speed = float(motion["GNSS_Speed"])
|
||||
bearing = float(motion["GNSS_Bearing"])
|
||||
speed_valid = math.isfinite(speed) and 0.0 <= speed <= 127.5
|
||||
bearing_valid = math.isfinite(bearing) and 0.0 <= bearing < 360.0
|
||||
speed = speed if has_fix and speed_valid else 0.0
|
||||
bearing = bearing if has_fix and bearing_valid else 0.0
|
||||
height = float(altitude["GNSS_Ortung_Hoehe"])
|
||||
height_valid = math.isfinite(height) and -500.0 <= height <= 7686.0
|
||||
heading_rad = math.radians(bearing)
|
||||
|
||||
return {
|
||||
"latitude": latitude,
|
||||
"longitude": longitude,
|
||||
"altitude": height if has_fix and height_valid else 0.0,
|
||||
"speed": speed,
|
||||
"bearingDeg": bearing,
|
||||
"horizontalAccuracy": 20.0,
|
||||
"unixTimestampMillis": timestamp_ms,
|
||||
"verticalAccuracy": 50.0 if height_valid else 500.0,
|
||||
"bearingAccuracyDeg": 10.0 if has_fix and bearing_valid and speed > 1.0 else 180.0,
|
||||
"speedAccuracy": 1.5 if speed_valid and bearing_valid else 100.0,
|
||||
"hasFix": has_fix,
|
||||
"satelliteCount": satellite_count if 0 <= satellite_count <= 31 else 0,
|
||||
"vNED": [speed * math.cos(heading_rad), speed * math.sin(heading_rad), 0.0] if bearing_valid else [0.0, 0.0, 0.0],
|
||||
}
|
||||
|
||||
|
||||
FORD_MACH_E_GPS_MESSAGES = (
|
||||
"APIMGPS_Data_Nav_1_FD1",
|
||||
"APIMGPS_Data_Nav_2_FD1",
|
||||
"APIMGPS_Data_Nav_3_FD1",
|
||||
)
|
||||
CHEVROLET_BOLT_GPS_MESSAGES = ("TCICOnStarGPSPosition",)
|
||||
VOLKSWAGEN_TAOS_GPS_MESSAGES = ("GNSS_01", "GNSS_02", "GNSS_04", "GNSS_05")
|
||||
|
||||
CHEVROLET_BOLT_GPS_CARS = (
|
||||
GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,
|
||||
@@ -141,6 +196,11 @@ CHEVROLET_BOLT_GPS_CARS = (
|
||||
|
||||
|
||||
CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = {
|
||||
VOLKSWAGEN_CAR.VOLKSWAGEN_TAOS_MK1: CarGpsConfig(
|
||||
brand="volkswagen",
|
||||
messages=VOLKSWAGEN_TAOS_GPS_MESSAGES,
|
||||
decoder=parse_volkswagen_taos_can_gps,
|
||||
),
|
||||
FORD_CAR.FORD_MUSTANG_MACH_E_MK1: CarGpsConfig(
|
||||
brand="ford",
|
||||
messages=FORD_MACH_E_GPS_MESSAGES,
|
||||
|
||||
@@ -1065,21 +1065,27 @@ FW_VERSIONS = {
|
||||
CAR.HONDA_CITY_7G: {
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-T14-B030\x00\x00',
|
||||
b'39990-T14-B510\x00\x00',
|
||||
],
|
||||
(Ecu.gateway, 0x18daeff1, None): [
|
||||
b'38897-T14-M110\x00\x00',
|
||||
b'38897-T14-M210\x00\x00',
|
||||
],
|
||||
(Ecu.srs, 0x18da53f1, None): [
|
||||
b'77959-T00-B830\x00\x00',
|
||||
b'77959-T14-B810\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'36161-T14-P050\x00\x00',
|
||||
b'8S102-T14-P020\x00\x00',
|
||||
],
|
||||
(Ecu.vsa, 0x18da28f1, None): [
|
||||
b'57114-T14-B030\x00\x00',
|
||||
b'57114-T14-M510\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x18da1ef1, None): [
|
||||
b'28101-63B-M420\x00\x00',
|
||||
b'28101-63B-M510\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.HONDA_PASSPORT_4G: {
|
||||
|
||||
@@ -277,7 +277,7 @@ class CAR(Platforms):
|
||||
flags=HondaFlags.BOSCH_RADARLESS,
|
||||
)
|
||||
HONDA_CITY_7G = HondaBoschPlatformConfig(
|
||||
[HondaCarDocs("Honda City (Brazil only) 2023", "All")],
|
||||
[HondaCarDocs("Honda City (Brazil only) 2023-25", "All")],
|
||||
CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.6, steerRatio=19.0, centerToFrontRatio=0.41, minSteerSpeed=23. * CV.KPH_TO_MS),
|
||||
{Bus.pt: 'honda_bosch_radarless_generated'},
|
||||
flags=HondaFlags.BOSCH_RADARLESS,
|
||||
|
||||
@@ -770,7 +770,8 @@ class CarController(CarControllerBase):
|
||||
lead_data = self._can_lead_data.update(lead_distance, lead_rel_speed, lead_visible)
|
||||
|
||||
# HUD messages
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
|
||||
stinger_hud_enabled = CC.enabled or (self.CP.carFingerprint == CAR.KIA_STINGER_2022 and CC.latActive)
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(stinger_hud_enabled, self.car_fingerprint,
|
||||
hud_control)
|
||||
|
||||
if blended_hda2:
|
||||
@@ -1042,9 +1043,12 @@ class CarController(CarControllerBase):
|
||||
left_sound_active=left_warning.sound_active, right_sound_active=right_warning.sound_active,
|
||||
)
|
||||
else:
|
||||
host_speed = getattr(CS.out, "vEgoRaw", None) \
|
||||
if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN else None
|
||||
adrv_messages = hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame,
|
||||
car_fingerprint=self.CP.carFingerprint,
|
||||
drive_gear=drive_gear)
|
||||
drive_gear=drive_gear,
|
||||
v_ego=host_speed)
|
||||
can_sends.extend(adrv_messages)
|
||||
# The front radar treats ADAS_DRV's 0x100 broadcast as its host heartbeat
|
||||
# and stops publishing object tracks when it disappears.
|
||||
|
||||
@@ -26,6 +26,33 @@ ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL)
|
||||
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise,
|
||||
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel}
|
||||
|
||||
|
||||
class Ev6AolArmingState:
|
||||
def __init__(self, lkas_on_engage: bool):
|
||||
self.lkas_on_engage = lkas_on_engage
|
||||
self.main_on = False
|
||||
self.lkas_on = False
|
||||
self.prev_main_button = False
|
||||
self.prev_lkas_button = False
|
||||
self.prev_cruise_button = Buttons.NONE
|
||||
|
||||
def update(self, main_button: bool, lkas_button: bool, cruise_button: int):
|
||||
if main_button and not self.prev_main_button:
|
||||
self.main_on = not self.main_on
|
||||
if lkas_button and not self.prev_lkas_button:
|
||||
self.lkas_on = not self.lkas_on
|
||||
if (self.lkas_on_engage and cruise_button != self.prev_cruise_button and
|
||||
self.prev_cruise_button in (Buttons.SET_DECEL, Buttons.RES_ACCEL)):
|
||||
self.lkas_on = True
|
||||
self.prev_main_button = main_button
|
||||
self.prev_lkas_button = lkas_button
|
||||
self.prev_cruise_button = cruise_button
|
||||
|
||||
@property
|
||||
def authorized(self) -> bool:
|
||||
return self.main_on or self.lkas_on
|
||||
|
||||
|
||||
IONIQ_6_BLINDSPOT_RIGHT_MASK = 0x08
|
||||
IONIQ_6_BLINDSPOT_LEFT_MASK = 0x10
|
||||
CANFD_CAMERA_LEAD_MIN_DISTANCE = 0.1
|
||||
@@ -138,6 +165,13 @@ class CarState(CarStateBase):
|
||||
self.buttons_counter = 0
|
||||
self.main_cruise_on = False
|
||||
self.main_cruise_tracking = bool(getattr(FPCP, "flags", 0) & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING)
|
||||
self.ev6_aol_arming = None
|
||||
if CP.carFingerprint == CAR.KIA_EV6 and CP.openpilotLongitudinalControl and not CP.pcmCruise:
|
||||
lkas_on_engage = any(
|
||||
config.safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE
|
||||
for config in (*CP.safetyConfigs, *FPCP.safetyConfigs)
|
||||
)
|
||||
self.ev6_aol_arming = Ev6AolArmingState(lkas_on_engage)
|
||||
if CP.carFingerprint == CAR.KIA_RAY_EV:
|
||||
self.ray_pedal_state = 5
|
||||
self.ray_pedal_valid = False
|
||||
@@ -181,6 +215,10 @@ class CarState(CarStateBase):
|
||||
# Main button also can trigger an engagement on these cars
|
||||
return any(btn in ENABLE_BUTTONS for btn in self.cruise_buttons) or any(self.main_buttons)
|
||||
|
||||
@property
|
||||
def ev6_aol_authorized(self) -> bool:
|
||||
return self.ev6_aol_arming is not None and self.ev6_aol_arming.authorized
|
||||
|
||||
def update_main_cruise(self, ret: structs.CarState,
|
||||
button_events: list[structs.CarState.ButtonEvent] | None = None) -> bool:
|
||||
button_events = ret.buttonEvents if button_events is None else button_events
|
||||
@@ -640,6 +678,13 @@ class CarState(CarStateBase):
|
||||
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}),
|
||||
*create_button_events(self.lda_button, prev_lda_button, {1: ButtonType.lkas}),
|
||||
*create_button_events(self.left_paddle, prev_left_paddle, {1: ButtonType.altButton2})]
|
||||
if self.ev6_aol_arming is not None:
|
||||
buttons = cp.vl_all[self.cruise_btns_msg_canfd]
|
||||
for main, lkas, cruise in zip(buttons["ADAPTIVE_CRUISE_MAIN_BTN"], buttons["LDA_BTN"], buttons["CRUISE_BUTTONS"], strict=True):
|
||||
self.ev6_aol_arming.update(bool(main), bool(lkas), int(cruise))
|
||||
if not ret.cruiseState.available:
|
||||
self.ev6_aol_arming.main_on = False
|
||||
|
||||
if self.CP.openpilotLongitudinalControl and (self.CP.carFingerprint == CAR.KIA_EV9 or self.main_cruise_tracking):
|
||||
ret.cruiseState.available = self.update_main_cruise(ret)
|
||||
|
||||
|
||||
@@ -1335,6 +1335,7 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00CL4 MFC AT CAN LHD 1.00 1.02 99210-GG000 240708',
|
||||
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.02 99210-GG000 240708',
|
||||
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.04 99210-GG100 251205',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG000 ',
|
||||
|
||||
@@ -12,7 +12,7 @@ _adrv_0x51_templates: dict[CAR, bytes] = {}
|
||||
|
||||
|
||||
def cache_adrv_0x51_template(car_fingerprint: CAR, dat: bytes | None) -> None:
|
||||
if car_fingerprint != CAR.KIA_EV6:
|
||||
if car_fingerprint not in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN):
|
||||
return
|
||||
|
||||
if dat is None:
|
||||
@@ -21,15 +21,18 @@ def cache_adrv_0x51_template(car_fingerprint: CAR, dat: bytes | None) -> None:
|
||||
_adrv_0x51_templates[car_fingerprint] = bytes(dat)
|
||||
|
||||
|
||||
def create_adrv_0x51(packer, CAN, frame: int, car_fingerprint: CAR | None = None, drive_gear: bool = False):
|
||||
def create_adrv_0x51(packer, CAN, frame: int, car_fingerprint: CAR | None = None, drive_gear: bool = False,
|
||||
v_ego: float | None = None):
|
||||
template = _adrv_0x51_templates.get(car_fingerprint)
|
||||
if template is None:
|
||||
return packer.make_can_msg("ADRV_0x51", CAN.ACAN, {})
|
||||
|
||||
# EV6 MRR30 tracks stop when the ADAS takeover replaces this platform payload with zeros.
|
||||
dat = bytearray(template)
|
||||
dat[2] = (template[2] + frame + 1) & 0xFF
|
||||
dat[3] = (dat[3] & ~0x1) | int(drive_gear)
|
||||
if car_fingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN and v_ego is not None and np.isfinite(v_ego):
|
||||
speed_raw = int(np.clip(round(v_ego * 100.0), 0, 65534))
|
||||
dat[8:10] = speed_raw.to_bytes(2, "little")
|
||||
crc = hkg_can_fd_checksum(0x51, None, dat)
|
||||
dat[0] = crc & 0xFF
|
||||
dat[1] = (crc >> 8) & 0xFF
|
||||
@@ -816,13 +819,14 @@ def create_fca_warning_light(packer, CAN, frame):
|
||||
return ret
|
||||
|
||||
|
||||
def create_adrv_messages(packer, CAN, frame, blended_hda2=False, car_fingerprint=None, drive_gear=False):
|
||||
def create_adrv_messages(packer, CAN, frame, blended_hda2=False, car_fingerprint=None, drive_gear=False,
|
||||
v_ego=None):
|
||||
# messages needed to car happy after disabling
|
||||
# the ADAS Driving ECU to do longitudinal control
|
||||
|
||||
ret = []
|
||||
|
||||
ret.append(create_adrv_0x51(packer, CAN, frame, car_fingerprint, drive_gear))
|
||||
ret.append(create_adrv_0x51(packer, CAN, frame, car_fingerprint, drive_gear, v_ego))
|
||||
|
||||
if blended_hda2:
|
||||
return ret
|
||||
|
||||
@@ -395,7 +395,7 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
if not skip_disable_ecu:
|
||||
disable_can_recv = can_recv
|
||||
if CP.carFingerprint == CAR.KIA_EV6 and can_recv is not None:
|
||||
if CP.carFingerprint in (CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) and can_recv is not None:
|
||||
hyundaicanfd.cache_adrv_0x51_template(CP.carFingerprint, None)
|
||||
base_can_recv = can_recv
|
||||
adrv_bus = CanBus(CP).ACAN
|
||||
|
||||
@@ -21,7 +21,7 @@ from opendbc.car.hyundai.carcontroller import CarController, CANCEL_BUTTON_DELAY
|
||||
preserve_stock_canfd_lfa_status, \
|
||||
preserve_stock_canfd_lkas_status, \
|
||||
suppress_redundant_gv70_brake_cancel
|
||||
from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \
|
||||
from opendbc.car.hyundai.carstate import CarState, Ev6AolArmingState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \
|
||||
get_canfd_cruise_available
|
||||
from opendbc.car.hyundai.interface import CarInterface, KIA_EV9_ACCEL_MAX, get_communication_control_request
|
||||
from opendbc.car.hyundai import hyundaican, hyundaicanfd
|
||||
@@ -130,6 +130,73 @@ def get_test_toggles() -> SimpleNamespace:
|
||||
|
||||
|
||||
class TestHyundaiFingerprint:
|
||||
@pytest.mark.parametrize("candidate, alpha_long, needs_arming", (
|
||||
(CAR.KIA_EV6, True, True), (CAR.KIA_EV6, False, False),
|
||||
(CAR.KIA_EV6_2025, True, False), (CAR.KIA_EV9, True, False),
|
||||
(CAR.HYUNDAI_IONIQ_5, True, False), (CAR.HYUNDAI_IONIQ_6, True, False),
|
||||
))
|
||||
def test_ev6_aol_arming_is_vehicle_specific(self, candidate, alpha_long, needs_arming):
|
||||
toggles = get_test_toggles()
|
||||
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], alpha_long, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(candidate, gen_empty_fingerprint(), [], CP, toggles)
|
||||
CS = CarState(CP, FPCP)
|
||||
assert (CS.ev6_aol_arming is not None) == needs_arming
|
||||
assert not CS.ev6_aol_authorized
|
||||
|
||||
@pytest.mark.parametrize("alt_buttons", (False, True))
|
||||
@pytest.mark.parametrize("lkas_on_engage", (False, True))
|
||||
def test_ev6_aol_uses_all_physical_button_samples(self, alt_buttons, lkas_on_engage):
|
||||
toggles = get_test_toggles()
|
||||
toggles.always_on_lateral_lkas = lkas_on_engage
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
if not alt_buttons:
|
||||
fingerprint[0][0x1CF] = 8
|
||||
CP = CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], True, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_EV6, fingerprint, [], CP, toggles)
|
||||
CS = CarState(CP, FPCP)
|
||||
parsers = CS.get_can_parsers(CP)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
buttons_msg = CS.cruise_btns_msg_canfd
|
||||
frame = 0
|
||||
|
||||
def update(samples, acc_available=True):
|
||||
nonlocal frame
|
||||
frame += 1
|
||||
msgs = [packer.make_can_msg(buttons_msg, CanBus(CP).ECAN, {
|
||||
"ADAPTIVE_CRUISE_MAIN_BTN": main, "LDA_BTN": lkas, "CRUISE_BUTTONS": cruise,
|
||||
}) for main, lkas, cruise in samples]
|
||||
msgs.append(packer.make_can_msg("TCS", CanBus(CP).ECAN, {"ACCEnable": 0 if acc_available else 1}))
|
||||
parsers[Bus.pt].update([(frame * 10_000_000, msgs)])
|
||||
CS.update(parsers, toggles)
|
||||
return CS.ev6_aol_authorized
|
||||
|
||||
assert not update([(0, 0, Buttons.NONE)])
|
||||
assert update([(0, 1, Buttons.NONE), (0, 0, Buttons.NONE)])
|
||||
assert update([])
|
||||
assert not update([(0, 1, Buttons.NONE), (0, 0, Buttons.NONE)])
|
||||
assert update([(1, 0, Buttons.NONE), (1, 0, Buttons.NONE)])
|
||||
assert update([(0, 0, Buttons.NONE)])
|
||||
assert not update([(1, 0, Buttons.NONE), (0, 0, Buttons.NONE)])
|
||||
assert update([(1, 0, Buttons.NONE), (0, 0, Buttons.NONE)])
|
||||
assert not update([], acc_available=False)
|
||||
assert not update([])
|
||||
assert not update([(0, 0, Buttons.SET_DECEL)])
|
||||
assert update([(0, 0, Buttons.NONE)]) == lkas_on_engage
|
||||
assert update([]) == lkas_on_engage
|
||||
|
||||
@pytest.mark.parametrize("button", (Buttons.SET_DECEL, Buttons.RES_ACCEL))
|
||||
@pytest.mark.parametrize("lkas_on_engage", (False, True))
|
||||
def test_ev6_aol_engagement_latch_matches_safety_flag(self, button, lkas_on_engage):
|
||||
state = Ev6AolArmingState(lkas_on_engage)
|
||||
state.update(False, False, button)
|
||||
assert not state.authorized
|
||||
state.update(False, False, Buttons.NONE)
|
||||
assert state.authorized == lkas_on_engage
|
||||
state.update(False, False, Buttons.CANCEL)
|
||||
assert state.authorized == lkas_on_engage
|
||||
state.update(False, True, Buttons.NONE)
|
||||
assert state.authorized != lkas_on_engage
|
||||
|
||||
def test_egmp_communication_control_paths(self):
|
||||
stock_request = bytes([0x28, 0x83, 0x01])
|
||||
radar_keepalive_request = bytes([0x28, 0x01, 0x01])
|
||||
@@ -1316,6 +1383,103 @@ class TestHyundaiFingerprint:
|
||||
assert canfd_alt_buttons_cp.flags & HyundaiFlags.CANFD_ALT_BUTTONS
|
||||
assert not canfd_alt_buttons_fpcp.redneckCruiseAvailable
|
||||
|
||||
def test_sportage_hev_hda2_redneck_uses_stock_scc(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def __init__(self, *args, **kwargs):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def get_bool(key):
|
||||
return key == "RedneckCruise"
|
||||
|
||||
monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
|
||||
toggles = get_test_toggles()
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
can_bus = CanBus(None, fingerprint, True)
|
||||
fingerprint[can_bus.CAM][0x110] = 32
|
||||
fingerprint[can_bus.ECAN][0x1CF] = 8
|
||||
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
|
||||
assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
||||
assert FPCP.redneckCruiseAvailable
|
||||
assert not FPCP.pcmCruiseSpeed
|
||||
assert CP.pcmCruise
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
assert not CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
|
||||
controller = CarInterface(CP, FPCP).CC
|
||||
assert not controller.long_active_ecu
|
||||
|
||||
controller.frame = 30
|
||||
CS = SimpleNamespace(redneck_send_button=1, buttons_counter=5)
|
||||
msgs = controller._create_canfd_redneck_button_messages(CS)
|
||||
assert len(msgs) == 20
|
||||
assert all(msg[0] == 0x1CF and msg[2] == can_bus.ECAN for msg in msgs)
|
||||
assert all(msg[1][2] & 0x7 == Buttons.RES_ACCEL for msg in msgs)
|
||||
|
||||
controller.frame = 60
|
||||
CS.redneck_send_button = 2
|
||||
msgs = controller._create_canfd_redneck_button_messages(CS)
|
||||
assert len(msgs) == 20
|
||||
assert all(msg[1][2] & 0x7 == Buttons.SET_DECEL for msg in msgs)
|
||||
|
||||
monkeypatch.setattr(FakeParams, "get_bool", staticmethod(lambda key: False))
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
assert FPCP.redneckCruiseAvailable
|
||||
assert FPCP.pcmCruiseSpeed
|
||||
assert CP.pcmCruise
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
|
||||
def test_sportage_redneck_rejects_unverified_button_layouts(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def __init__(self, *args, **kwargs):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def get_bool(key):
|
||||
return key == "RedneckCruise"
|
||||
|
||||
monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
|
||||
toggles = get_test_toggles()
|
||||
for button_address, button_bus, button_length, lka_steering in (
|
||||
(0x1AA, 1, 16, True),
|
||||
(0x1CF, 0, 8, True),
|
||||
(0x1CF, 0, 8, False),
|
||||
(0x1CF, 1, 16, True),
|
||||
):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
can_bus = CanBus(None, fingerprint, lka_steering)
|
||||
if lka_steering:
|
||||
fingerprint[can_bus.CAM][0x110] = 32
|
||||
fingerprint[button_bus][button_address] = button_length
|
||||
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
assert not FPCP.redneckCruiseAvailable
|
||||
assert FPCP.pcmCruiseSpeed
|
||||
assert CP.pcmCruise
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
can_bus = CanBus(None, fingerprint, True)
|
||||
fingerprint[can_bus.CAM][0x110] = 32
|
||||
fingerprint[can_bus.ECAN][0x1CF] = 8
|
||||
fingerprint[can_bus.ECAN][0x1AA] = 16
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
assert not FPCP.redneckCruiseAvailable
|
||||
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
can_bus = CanBus(None, fingerprint, True)
|
||||
fingerprint[can_bus.CAM][0x110] = 32
|
||||
fingerprint[can_bus.ECAN][0x1CF] = 8
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, toggles)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], CP, toggles)
|
||||
assert not FPCP.redneckCruiseAvailable
|
||||
|
||||
def test_hyundai_non_scc_without_redneck_keeps_stock_longitudinal_mode(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def __init__(self, *args, **kwargs):
|
||||
@@ -1699,6 +1863,55 @@ class TestHyundaiFingerprint:
|
||||
assert exact
|
||||
assert matches == {candidate}
|
||||
|
||||
@pytest.mark.parametrize("alpha_long", (False, True))
|
||||
@pytest.mark.parametrize("alt_buttons", (False, True))
|
||||
@pytest.mark.parametrize("signal, value, button_type", (
|
||||
("LDA_BTN", 1, ButtonType.lkas),
|
||||
("ADAPTIVE_CRUISE_MAIN_BTN", 1, ButtonType.mainCruise),
|
||||
("CRUISE_BUTTONS", Buttons.RES_ACCEL, ButtonType.accelCruise),
|
||||
("CRUISE_BUTTONS", Buttons.SET_DECEL, ButtonType.decelCruise),
|
||||
("CRUISE_BUTTONS", Buttons.CANCEL, ButtonType.cancel),
|
||||
))
|
||||
def test_k4_2025_2026_physical_button_events(self, alpha_long, alt_buttons, signal, value, button_type):
|
||||
toggles = get_test_toggles()
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
if not alt_buttons:
|
||||
fingerprint[0][0x1CF] = 8
|
||||
CP = CarInterface.get_params(CAR.KIA_K4_2025, fingerprint, [], alpha_long, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_K4_2025, fingerprint, [], CP, toggles)
|
||||
car_state = CarState(CP, FPCP)
|
||||
parsers = car_state.get_can_parsers(CP)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
def update(button_value, frame):
|
||||
msg = packer.make_can_msg(car_state.cruise_btns_msg_canfd, CanBus(CP).ECAN, {signal: button_value})
|
||||
parsers[Bus.pt].update([(frame * 10_000_000, [msg])])
|
||||
return car_state.update(parsers, toggles)[0].buttonEvents
|
||||
|
||||
assert not update(0, 1)
|
||||
pressed = update(value, 2)
|
||||
assert [(event.type, event.pressed) for event in pressed] == [(button_type, True)]
|
||||
released = update(0, 3)
|
||||
assert [(event.type, event.pressed) for event in released] == [(button_type, False)]
|
||||
|
||||
@pytest.mark.parametrize("camera_fw", [
|
||||
b'\xf1\x00CL4 MFC AT CAN LHD 1.00 1.02 99210-GG000 240708',
|
||||
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.02 99210-GG000 240708',
|
||||
b'\xf1\x00CL4 MFC AT USA LHD 1.00 1.04 99210-GG100 251205',
|
||||
])
|
||||
@pytest.mark.parametrize("radar_fw", [
|
||||
b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG000 ',
|
||||
b'\xf1\x00CL4_ RDR ----- 1.00 1.01 99110-GG100 ',
|
||||
])
|
||||
def test_k4_2025_2026_fw_exact_matches(self, camera_fw, radar_fw):
|
||||
car_fw = [
|
||||
CarParams.CarFw(ecu=Ecu.fwdCamera, fwVersion=camera_fw, address=0x7c4, brand="hyundai"),
|
||||
CarParams.CarFw(ecu=Ecu.fwdRadar, fwVersion=radar_fw, address=0x7d0, brand="hyundai"),
|
||||
]
|
||||
exact, matches = match_fw_to_car(car_fw, "", allow_exact=True, allow_fuzzy=False, log=False)
|
||||
assert exact
|
||||
assert matches == {CAR.KIA_K4_2025}
|
||||
|
||||
def test_staria_2023_australian_route_fw_exact_matches(self):
|
||||
route_fw = {
|
||||
(Ecu.fwdCamera, 0x7c4): b'\xf1\x00US4 MFC AT AUS RHD 1.00 1.04 99211-CG000 210819',
|
||||
|
||||
@@ -606,6 +606,7 @@ class CAR(Platforms):
|
||||
KIA_K4_2025 = HyundaiCanFDPlatformConfig(
|
||||
[
|
||||
HyundaiCarDocs("Kia K4 (without HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_a])),
|
||||
HyundaiCarDocs("Kia K4 (without HDA II) 2026", car_parts=CarParts.common([CarHarness.hyundai_a])),
|
||||
HyundaiCarDocs("Kia K4 (with HDA II) 2025", car_parts=CarParts.common([CarHarness.hyundai_r])),
|
||||
],
|
||||
CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=13.4),
|
||||
|
||||
@@ -248,12 +248,24 @@ class CarInterfaceBase(ABC):
|
||||
(candidate != HYUNDAI.KIA_STINGER_2022 or fingerprint[2][0x53E] == 6):
|
||||
fp_ret.flags |= HyundaiStarPilotFlags.HAS_LKAS12.value
|
||||
|
||||
fp_ret.redneckCruiseAvailable = (bool(CP.flags & HyundaiFlags.NON_SCC) and
|
||||
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
|
||||
not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED))
|
||||
sportage_stock_scc_buttons = (
|
||||
candidate == HYUNDAI.KIA_SPORTAGE_HEV_2026 and
|
||||
not CP.openpilotLongitudinalControl and
|
||||
bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) and
|
||||
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
|
||||
fingerprint[CAN.ECAN].get(0x1CF) == 8 and
|
||||
0x1AA not in fingerprint[CAN.ECAN]
|
||||
)
|
||||
fp_ret.redneckCruiseAvailable = (
|
||||
(bool(CP.flags & HyundaiFlags.NON_SCC) and
|
||||
not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and
|
||||
not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED)) or
|
||||
sportage_stock_scc_buttons
|
||||
)
|
||||
if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise"):
|
||||
fp_ret.pcmCruiseSpeed = False
|
||||
CP.openpilotLongitudinalControl = True
|
||||
if CP.flags & HyundaiFlags.NON_SCC:
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
if candidate == HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and CP.openpilotLongitudinalControl:
|
||||
fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value
|
||||
|
||||
@@ -48,6 +48,7 @@ class CarController(CarControllerBase):
|
||||
self.angle_handoff_active = False
|
||||
self.ascent_angle_initialized = False
|
||||
self.ascent_aol_arm_frames = 0
|
||||
self.ascent_es_distance_counter_last = None
|
||||
|
||||
self.cruise_button_prev = 0
|
||||
self.steer_rate_counter = 0
|
||||
@@ -411,7 +412,12 @@ class CarController(CarControllerBase):
|
||||
can_sends.append(subarucan.create_es_distance(self.packer, self.frame // 5, CS.es_distance_msg, 0, pcm_cancel_cmd,
|
||||
self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle))
|
||||
else:
|
||||
if pcm_cancel_cmd:
|
||||
cancel_frame_ready = True
|
||||
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
|
||||
stock_counter = CS.es_distance_msg["COUNTER"]
|
||||
cancel_frame_ready = stock_counter != self.ascent_es_distance_counter_last
|
||||
self.ascent_es_distance_counter_last = stock_counter
|
||||
if pcm_cancel_cmd and cancel_frame_ready:
|
||||
if not (self.CP.flags & SubaruFlags.HYBRID):
|
||||
bus = CanBus.alt_for_cp(self.CP) if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else self.main_bus
|
||||
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd))
|
||||
|
||||
@@ -879,6 +879,69 @@ def test_ascent_hud_waits_for_angle_request():
|
||||
assert controller._lkas_status_active(CC)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("platform", [CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023, CAR.SUBARU_LEGACY_2025,
|
||||
CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_ASCENT])
|
||||
def test_stock_cruise_cancel_fresh_frame_gate_is_ascent_angle_only(platform):
|
||||
CP = CarInterface.get_non_essential_params(platform)
|
||||
controller = CarController({}, CP)
|
||||
CC = structs.CarControl()
|
||||
CC.cruiseControl.cancel = True
|
||||
CS = SimpleNamespace(
|
||||
out=structs.CarState(),
|
||||
es_distance_msg=defaultdict(int, COUNTER=11),
|
||||
es_dashstatus_msg=defaultdict(int),
|
||||
es_lkas_state_msg=defaultdict(int),
|
||||
es_infotainment_msg=defaultdict(int),
|
||||
)
|
||||
toggles = SimpleNamespace(subaru_sng=False)
|
||||
cancel_messages = []
|
||||
for frame in range(5):
|
||||
_, sends = controller.update(CC.as_reader(), CS, frame * 10_000_000, toggles)
|
||||
cancel_messages.extend(msg for msg in sends if msg[0] == 0x221)
|
||||
|
||||
assert len(cancel_messages) == (1 if platform == CAR.SUBARU_ASCENT_2023 else 5)
|
||||
assert all(msg[2] == (CanBus.alt if CP.flags & SubaruFlags.GLOBAL_GEN2 else CanBus.main) for msg in cancel_messages)
|
||||
parser = CANParser(DBC[platform][Bus.pt], [("ES_Distance", 0)], cancel_messages[0][2])
|
||||
parser.update([(1, [cancel_messages[0]])])
|
||||
assert parser.vl["ES_Distance"]["COUNTER"] == 12
|
||||
assert parser.vl["ES_Distance"]["Cruise_Cancel"] == 1
|
||||
assert parser.vl["ES_Distance"]["Cruise_Throttle"] == 1818
|
||||
|
||||
|
||||
def test_ascent_cancel_uses_fresh_stock_frames_and_handles_counter_rollover():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = structs.CarControl()
|
||||
CC.latActive = True
|
||||
CS = SimpleNamespace(
|
||||
out=structs.CarState(vEgoRaw=24.04, steeringAngleDeg=2.93, gearShifter="drive"),
|
||||
es_distance_msg=defaultdict(int, COUNTER=14),
|
||||
es_dashstatus_msg=defaultdict(int),
|
||||
es_lkas_state_msg=defaultdict(int),
|
||||
es_infotainment_msg=defaultdict(int),
|
||||
)
|
||||
CS.out.cruiseState.available = True
|
||||
toggles = SimpleNamespace(subaru_sng=False)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_Distance", 0)], CanBus.alt)
|
||||
cancel_counters = []
|
||||
for frame, (stock_counter, cancel) in enumerate([
|
||||
(14, False), (14, True), (15, True), (15, True), (0, True), (0, True),
|
||||
(1, False), (1, True), (2, True),
|
||||
]):
|
||||
CS.es_distance_msg["COUNTER"] = stock_counter
|
||||
CC.cruiseControl.cancel = cancel
|
||||
_, sends = controller.update(CC.as_reader(), CS, frame * 10_000_000, toggles)
|
||||
cancel_messages = [msg for msg in sends if msg[0] == 0x221]
|
||||
assert len(cancel_messages) <= 1
|
||||
if cancel_messages:
|
||||
parser.update([(frame + 1, cancel_messages)])
|
||||
cancel_counters.append(parser.vl["ES_Distance"]["COUNTER"])
|
||||
assert parser.vl["ES_Distance"]["Cruise_Cancel"] == 1
|
||||
assert parser.vl["ES_Distance"]["Cruise_Throttle"] == 1818
|
||||
|
||||
assert cancel_counters == [0, 1, 3]
|
||||
|
||||
|
||||
def test_other_angle_cars_keep_lateral_status_behavior():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
|
||||
controller = CarController({}, CP)
|
||||
|
||||
@@ -1,12 +1,12 @@
|
||||
import copy
|
||||
from cereal import custom
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.tesla.values import (
|
||||
DBC, CANBUS, GEAR_MAP, STEER_DISENGAGE_THRESHOLD, STEER_THRESHOLD, TeslaSafetyFlags,
|
||||
CAR, LEGACY_CARS,
|
||||
CAR, LEGACY_CARS, TeslaFlags,
|
||||
)
|
||||
from opendbc.car.tesla.preap.carstate import get_preap_can_parsers, update_preap
|
||||
from opendbc.car.tesla.preap.engagement import PreAPEngagement
|
||||
@@ -19,6 +19,20 @@ TESLA_GAS_PRESS_ON = 0.8
|
||||
TESLA_GAS_PRESS_OFF = 0.4
|
||||
|
||||
|
||||
class TeslaScreenCANParser(CANParser):
|
||||
def __init__(self):
|
||||
super().__init__("tesla_model3_vehicle", [("UI_status2", 0), ("SCCM_rightStalk", 0)], CANBUS.vehicle)
|
||||
|
||||
def update(self, strings, sendcan=False):
|
||||
if strings and not isinstance(strings[0], list | tuple):
|
||||
strings = [strings]
|
||||
# Match Panda's exact-length check before producing engagement button events.
|
||||
return super().update([
|
||||
(timestamp, [frame for frame in frames if (frame[0], len(frame[1])) in ((0x3DF, 8), (0x229, 3))])
|
||||
for timestamp, frames in strings
|
||||
], sendcan)
|
||||
|
||||
|
||||
def update_tesla_gas_pressed(previous: bool, pedal_position: float) -> bool:
|
||||
threshold = TESLA_GAS_PRESS_OFF if previous else TESLA_GAS_PRESS_ON
|
||||
return float(pedal_position) > threshold
|
||||
@@ -51,6 +65,8 @@ class CarState(CarStateBase):
|
||||
self.cruise_buttons = 0
|
||||
self.prev_cruise_buttons = 0
|
||||
self.gas_pressed = False
|
||||
self.active_touch_points = None
|
||||
self.screen_cancel_pressed = False
|
||||
self.msg_stw_actn_req = None
|
||||
self.speed_units = "MPH"
|
||||
self.cooperative_steering = any(
|
||||
@@ -86,6 +102,20 @@ class CarState(CarStateBase):
|
||||
return False
|
||||
return super().update_button_enable(buttonEvents)
|
||||
|
||||
def update_screen_button(self, cp_vehicle):
|
||||
events = []
|
||||
for stalk_status in cp_vehicle.vl_all["SCCM_rightStalk"]["SCCM_rightStalkStatus"]:
|
||||
cancel_pressed = int(stalk_status) in (1, 2) # UP_1, UP_2
|
||||
events.extend(create_button_events(cancel_pressed, self.screen_cancel_pressed, {True: ButtonType.cancel}))
|
||||
self.screen_cancel_pressed = cancel_pressed
|
||||
for touch_points in cp_vehicle.vl_all["UI_status2"]["UI_activeTouchPoints"]:
|
||||
touch_points = int(touch_points)
|
||||
# Establish a baseline first; a touch already held during boot is not an engagement request.
|
||||
if self.active_touch_points is not None:
|
||||
events.extend(create_button_events(touch_points, self.active_touch_points, {3: ButtonType.lkas}))
|
||||
self.active_touch_points = touch_points
|
||||
return events
|
||||
|
||||
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
|
||||
if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
|
||||
return update_preap(self, can_parsers)
|
||||
@@ -181,11 +211,15 @@ class CarState(CarStateBase):
|
||||
else:
|
||||
pass
|
||||
# Buttons # ToDo: add Gap adjust button
|
||||
if self.CP.flags & TeslaFlags.AOL_SCREEN_BUTTON:
|
||||
ret.buttonEvents = list(ret.buttonEvents) + self.update_screen_button(can_parsers[Bus.adas])
|
||||
|
||||
# Messages needed by carcontroller
|
||||
self.das_control = copy.copy(cp_ap_party.vl["DAS_control"])
|
||||
|
||||
fp_ret = custom.StarPilotCarState.new_message()
|
||||
if self.CP.flags & TeslaFlags.AOL_SCREEN_BUTTON:
|
||||
fp_ret.cancelPressed = self.screen_cancel_pressed
|
||||
|
||||
return ret, fp_ret
|
||||
|
||||
@@ -279,5 +313,7 @@ class CarState(CarStateBase):
|
||||
}
|
||||
return {
|
||||
Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
|
||||
Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party)
|
||||
Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party),
|
||||
**({Bus.adas: TeslaScreenCANParser()}
|
||||
if CP.flags & TeslaFlags.AOL_SCREEN_BUTTON else {}),
|
||||
}
|
||||
|
||||
@@ -3,7 +3,7 @@ from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.tesla.carcontroller import CarController
|
||||
from opendbc.car.tesla.carstate import CarState
|
||||
from opendbc.car.tesla.radar_interface import RadarInterface
|
||||
from opendbc.car.tesla.values import TeslaSafetyFlags, CAR, DBC, LEGACY_CARS
|
||||
from opendbc.car.tesla.values import TeslaFlags, TeslaSafetyFlags, CANBUS, CAR, DBC, LEGACY_CARS
|
||||
from opendbc.car.tesla.preap.interface import get_preap_accel_limits, get_preap_params
|
||||
|
||||
|
||||
@@ -23,6 +23,12 @@ class CarInterface(CarInterfaceBase):
|
||||
ret = super().get_params(candidate, fingerprint, car_fw, alpha_long, is_release, docs, starpilot_toggles)
|
||||
if candidate == CAR.TESLA_MODEL_3 and getattr(starpilot_toggles, "tesla_cooperative_steering", False):
|
||||
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.COOP_STEERING.value
|
||||
if (ret.flags & TeslaFlags.HAS_VEHICLE_BUS and
|
||||
getattr(starpilot_toggles, "tesla_aol_screen_tap_requested", False)):
|
||||
ret.flags |= TeslaFlags.AOL_SCREEN_BUTTON.value
|
||||
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.AOL_SCREEN_BUTTON.value
|
||||
if getattr(starpilot_toggles, "tesla_aol_screen_brake_disengage_requested", False):
|
||||
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE.value
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
@@ -49,6 +55,9 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)]
|
||||
|
||||
if candidate in (CAR.TESLA_MODEL_3, CAR.TESLA_MODEL_Y) and fingerprint[CANBUS.vehicle].get(0x3DF) == 8:
|
||||
ret.flags |= TeslaFlags.HAS_VEHICLE_BUS.value
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerAtStandstill = True
|
||||
|
||||
@@ -0,0 +1,155 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from cereal import custom
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, gen_empty_fingerprint
|
||||
from opendbc.car.tesla.carstate import ButtonType, CarState
|
||||
from opendbc.car.tesla.interface import CarInterface
|
||||
from opendbc.car.tesla.values import CANBUS, CAR, TeslaFlags, TeslaSafetyFlags
|
||||
|
||||
|
||||
def screen_params(candidate=CAR.TESLA_MODEL_3, enabled=True, bus=CANBUS.vehicle, length=8, alpha_long=False):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
if bus is not None:
|
||||
fingerprint[bus][0x3DF] = length
|
||||
toggles = SimpleNamespace(tesla_aol_screen_tap_requested=enabled, trailer_load_kg=0.0)
|
||||
return CarInterface.get_params(candidate, fingerprint, [], alpha_long, False, False, toggles)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("candidate", tuple(CAR))
|
||||
@pytest.mark.parametrize("enabled", (False, True))
|
||||
def test_detection_and_safety_flags_are_model_3_y_only(candidate, enabled):
|
||||
cp = screen_params(candidate, enabled)
|
||||
detected = candidate in (CAR.TESLA_MODEL_3, CAR.TESLA_MODEL_Y)
|
||||
assert bool(cp.flags & TeslaFlags.HAS_VEHICLE_BUS) == detected
|
||||
assert bool(cp.flags & TeslaFlags.AOL_SCREEN_BUTTON) == (detected and enabled)
|
||||
assert any(config.safetyParam & TeslaSafetyFlags.AOL_SCREEN_BUTTON for config in cp.safetyConfigs) == (detected and enabled)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("bus", "length"), ((None, 8), (0, 8), (2, 8), (1, 7), (1, 4)))
|
||||
def test_harness_requires_correct_bus_and_length(bus, length):
|
||||
cp = screen_params(bus=bus, length=length)
|
||||
assert not cp.flags & (TeslaFlags.HAS_VEHICLE_BUS | TeslaFlags.AOL_SCREEN_BUTTON)
|
||||
assert not cp.safetyConfigs[0].safetyParam & TeslaSafetyFlags.AOL_SCREEN_BUTTON
|
||||
assert Bus.adas not in CarState.get_can_parsers(cp)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("alpha_long", (False, True))
|
||||
def test_gesture_does_not_select_longitudinal(alpha_long):
|
||||
baseline = screen_params(enabled=False, alpha_long=alpha_long)
|
||||
enabled = screen_params(alpha_long=alpha_long)
|
||||
assert enabled.openpilotLongitudinalControl == baseline.openpilotLongitudinalControl
|
||||
assert enabled.pcmCruise == baseline.pcmCruise
|
||||
assert enabled.safetyConfigs[0].safetyParam == baseline.safetyConfigs[0].safetyParam | TeslaSafetyFlags.AOL_SCREEN_BUTTON
|
||||
|
||||
|
||||
@pytest.mark.parametrize("enabled", (False, True))
|
||||
def test_screen_brake_safety_flag_requires_the_gesture_feature(enabled):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[1][0x3DF] = 8
|
||||
toggles = SimpleNamespace(tesla_aol_screen_tap_requested=enabled, tesla_aol_screen_brake_disengage_requested=True,
|
||||
trailer_load_kg=0.0)
|
||||
cp = CarInterface.get_params(CAR.TESLA_MODEL_3, fingerprint, [], False, False, False, toggles)
|
||||
assert bool(cp.safetyConfigs[0].safetyParam & TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE) == enabled
|
||||
|
||||
|
||||
def test_parser_only_added_with_setting_and_detected_addon():
|
||||
assert Bus.adas not in CarState.get_can_parsers(screen_params(enabled=False))
|
||||
parser = CarState.get_can_parsers(screen_params())[Bus.adas]
|
||||
assert parser.bus == CANBUS.vehicle
|
||||
assert parser.message_states[0x3DF].ignore_alive
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("counts", "expected"), (
|
||||
([0, 3, 3, 3, 0, 0, 3, 0], [True, False, True, False]),
|
||||
([3, 3, 0, 3, 0], [False, True, False]),
|
||||
([0, 1, 2, 4, 5, 0], []),
|
||||
))
|
||||
def test_touch_edges_and_startup_baseline(counts, expected):
|
||||
cp = screen_params()
|
||||
fp_cp = custom.StarPilotCarParams.new_message()
|
||||
cs = CarState(cp, fp_cp)
|
||||
parser = CarState.get_can_parsers(cp)[Bus.adas]
|
||||
packer = CANPacker("tesla_model3_vehicle")
|
||||
events = []
|
||||
for i, count in enumerate(counts):
|
||||
parser.update([[int((i + 1) * 0.5e9), [packer.make_can_msg("UI_status2", CANBUS.vehicle, {"UI_activeTouchPoints": count})]]])
|
||||
events.extend(cs.update_screen_button(parser))
|
||||
assert [event.pressed for event in events if event.type == ButtonType.lkas] == expected
|
||||
parser.update([[60_000_000_000, []]])
|
||||
assert cs.update_screen_button(parser) == []
|
||||
assert parser.can_valid
|
||||
assert not parser.bus_timeout
|
||||
|
||||
|
||||
def test_multiple_touch_edges_in_one_update_are_not_lost():
|
||||
cp = screen_params()
|
||||
fp_cp = custom.StarPilotCarParams.new_message()
|
||||
cs = CarState(cp, fp_cp)
|
||||
parser = CarState.get_can_parsers(cp)[Bus.adas]
|
||||
packer = CANPacker("tesla_model3_vehicle")
|
||||
parser.update([[i * 10_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": count})]]
|
||||
for i, count in enumerate((0, 3, 0, 3, 0), 1)])
|
||||
assert [event.pressed for event in cs.update_screen_button(parser) if event.type == ButtonType.lkas] == [True, False, True, False]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("length", (0, 3, 4, 5, 6, 7, 9, 12))
|
||||
def test_malformed_touch_frames_do_not_produce_button_events(length):
|
||||
cp = screen_params()
|
||||
cs = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
parser = CarState.get_can_parsers(cp)[Bus.adas]
|
||||
packer = CANPacker("tesla_model3_vehicle")
|
||||
parser.update([[1_000_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": 0})]]])
|
||||
cs.update_screen_button(parser)
|
||||
address, data, bus = packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": 3})
|
||||
parser.update([[2_000_000_000, [(address, data[:length].ljust(length, b"\x00"), bus)]]])
|
||||
assert cs.update_screen_button(parser) == []
|
||||
assert cs.active_touch_points == 0
|
||||
parser.update([[3_000_000_000, [(address, data, bus)]]])
|
||||
assert any(event.type == ButtonType.lkas and event.pressed for event in cs.update_screen_button(parser))
|
||||
|
||||
|
||||
def test_button_is_appended_without_changing_cruise_state():
|
||||
cp = screen_params()
|
||||
fp_cp = custom.StarPilotCarParams.new_message()
|
||||
cs = CarState(cp, fp_cp)
|
||||
parsers = CarState.get_can_parsers(cp)
|
||||
parsers[Bus.party].vl["DI_state"]["DI_cruiseState"] = 0
|
||||
packer = CANPacker("tesla_model3_vehicle")
|
||||
for count in (0, 3):
|
||||
parsers[Bus.adas].update([[1_000_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": count})]]])
|
||||
ret, _ = cs.update(parsers, SimpleNamespace())
|
||||
assert any(event.type == ButtonType.lkas and event.pressed for event in ret.buttonEvents)
|
||||
assert not ret.cruiseState.enabled
|
||||
assert not ret.cruiseState.available
|
||||
|
||||
|
||||
def test_addon_stalk_cancel_events_and_held_state():
|
||||
cp = screen_params()
|
||||
cs = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
parsers = CarState.get_can_parsers(cp)
|
||||
packer = CANPacker("tesla_model3_vehicle")
|
||||
events = []
|
||||
for i, status in enumerate((0, 1, 1, 2, 0, 3, 4, 0, 2, 0), 1):
|
||||
parsers[Bus.adas].update([[i * 10_000_000, [packer.make_can_msg("SCCM_rightStalk", 1, {"SCCM_rightStalkStatus": status})]]])
|
||||
ret, fp_ret = cs.update(parsers, SimpleNamespace())
|
||||
assert fp_ret.cancelPressed == (status in (1, 2))
|
||||
events.extend(ret.buttonEvents)
|
||||
assert [event.pressed for event in events if event.type == ButtonType.cancel] == [True, False, True, False]
|
||||
parsers[Bus.adas].update([[60_000_000_000, []]])
|
||||
assert parsers[Bus.adas].can_valid
|
||||
assert not parsers[Bus.adas].bus_timeout
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("bus", "length"), ((0, 3), (2, 3), (1, 2), (1, 4), (1, 8)))
|
||||
def test_wrong_bus_or_length_stalk_frames_do_not_cancel(bus, length):
|
||||
cp = screen_params()
|
||||
cs = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
parser = CarState.get_can_parsers(cp)[Bus.adas]
|
||||
packer = CANPacker("tesla_model3_vehicle")
|
||||
address, data, _ = packer.make_can_msg("SCCM_rightStalk", bus, {"SCCM_rightStalkStatus": 1})
|
||||
parser.update([[1_000_000_000, [(address, data[:length].ljust(length, b"\x00"), bus)]]])
|
||||
assert cs.update_screen_button(parser) == []
|
||||
assert not cs.screen_cancel_pressed
|
||||
@@ -144,10 +144,14 @@ class TeslaSafetyFlags(IntFlag):
|
||||
FLAG_EXTERNAL_PANDA = 4
|
||||
FLAG_HW1 = 8
|
||||
COOP_STEERING = 256
|
||||
AOL_SCREEN_BUTTON = 512
|
||||
AOL_SCREEN_DISENGAGE_ON_BRAKE = 1024
|
||||
|
||||
|
||||
class TeslaFlags(IntFlag):
|
||||
LONG_CONTROL = 1
|
||||
HAS_VEHICLE_BUS = 2
|
||||
AOL_SCREEN_BUTTON = 4
|
||||
|
||||
|
||||
class CruiseButtons:
|
||||
|
||||
@@ -10,8 +10,8 @@ from opendbc.car.secoc import add_mac, build_sync_mac
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.toyota import toyotacan
|
||||
from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \
|
||||
CarControllerParams, ToyotaFlags, \
|
||||
UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS
|
||||
CarControllerParams, ToyotaFlags, ToyotaSafetyFlags, \
|
||||
UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, uses_toyota_auto_hold_aeb
|
||||
from opendbc.can import CANPacker
|
||||
|
||||
Ecu = structs.CarParams.Ecu
|
||||
@@ -68,6 +68,11 @@ def is_ths_hybrid(CP) -> bool:
|
||||
return CP.carFingerprint in LEGACY_PRIUS_CAR or is_camry_hybrid(CP)
|
||||
|
||||
|
||||
def uses_rav4_hybrid_sdsu_longitudinal(CP) -> bool:
|
||||
return bool(CP.carFingerprint == CAR.TOYOTA_RAV4H and CP.openpilotLongitudinalControl and
|
||||
CP.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.LONG_FILTER.value)
|
||||
|
||||
|
||||
def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
|
||||
highlander_sdsu = (
|
||||
CP.carFingerprint == CAR.TOYOTA_HIGHLANDER and
|
||||
@@ -107,7 +112,7 @@ def get_long_tune(CP, params):
|
||||
kiV = [0.5, 0.25]
|
||||
k_f = 1.0
|
||||
|
||||
if is_ths_hybrid(CP):
|
||||
if is_ths_hybrid(CP) or uses_rav4_hybrid_sdsu_longitudinal(CP):
|
||||
k_f = 0.8 if CP.carFingerprint in LEGACY_PRIUS_CAR else 1.0
|
||||
elif CP.carFingerprint not in TSS2_CAR:
|
||||
kiBP = [0., 5., 35.]
|
||||
@@ -278,7 +283,7 @@ class CarController(CarControllerBase):
|
||||
self._brake_hold_counter = 0
|
||||
|
||||
def _compute_interceptor_gas_cmd(self, CC, CS):
|
||||
if not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive):
|
||||
if self.brake_hold_active or not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive):
|
||||
return 0.0
|
||||
|
||||
if CS.out.standstill:
|
||||
@@ -327,12 +332,14 @@ class CarController(CarControllerBase):
|
||||
not CS.out.gasPressed and not CS.out.cruiseState.enabled and
|
||||
CS.out.gearShifter not in (PARK, REVERSE))
|
||||
|
||||
if brake_hold_allowed and not self.brake_hold_active and CS.out.brakePressed:
|
||||
self._brake_hold_counter += 1
|
||||
self.brake_hold_active = self._brake_hold_counter > activation_frames
|
||||
elif not brake_hold_allowed:
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
if not brake_hold_allowed:
|
||||
self.reset_auto_hold_state()
|
||||
elif not self.brake_hold_active:
|
||||
if CS.out.brakePressed:
|
||||
self._brake_hold_counter += 1
|
||||
self.brake_hold_active = self._brake_hold_counter > activation_frames
|
||||
else:
|
||||
self._brake_hold_counter = 0
|
||||
|
||||
return self.brake_hold_active
|
||||
|
||||
@@ -353,6 +360,8 @@ class CarController(CarControllerBase):
|
||||
return []
|
||||
|
||||
def reset_auto_hold_state(self):
|
||||
if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP):
|
||||
self.standstill_req = False
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
|
||||
@@ -452,7 +461,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
self._update_standstill_request(CC, CS, actuators, starpilot_toggles)
|
||||
if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)):
|
||||
if self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS:
|
||||
if uses_toyota_auto_hold_aeb(self.CP):
|
||||
can_sends.extend(self.create_auto_brake_hold_messages(CS))
|
||||
else:
|
||||
self.update_auto_hold_state(CS, pcm_cancel_cmd)
|
||||
@@ -565,7 +574,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
|
||||
|
||||
if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS:
|
||||
if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP):
|
||||
pcm_accel_cmd = TOYOTA_AUTO_HOLD_ACCEL
|
||||
self.permit_braking = True
|
||||
self.standstill_req = True
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
from opendbc.car import Bus, structs, get_safety_config, uds
|
||||
from opendbc.car.toyota.carstate import CarState
|
||||
from opendbc.car.toyota.carcontroller import CarController
|
||||
from opendbc.car.toyota.carcontroller import CarController, uses_rav4_hybrid_sdsu_longitudinal
|
||||
from opendbc.car.toyota.radar_interface import RadarInterface
|
||||
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
|
||||
MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
|
||||
ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, \
|
||||
TOYOTA_AUTO_HOLD_AEB_CARS
|
||||
uses_toyota_auto_hold_aeb
|
||||
from opendbc.car.disable_ecu import disable_ecu
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
@@ -102,7 +102,7 @@ class CarInterface(CarInterfaceBase):
|
||||
# https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
|
||||
stop_and_go = candidate != CAR.TOYOTA_AVALON
|
||||
|
||||
elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023, CAR.TOYOTA_RAV4_PRIME):
|
||||
elif candidate in ANGLE_CONTROL_CAR:
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kiBP = [0.0]
|
||||
ret.lateralTuning.pid.kpBP = [0.0]
|
||||
@@ -167,7 +167,7 @@ class CarInterface(CarInterfaceBase):
|
||||
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
|
||||
if toyota_auto_hold and ret.openpilotLongitudinalControl and candidate in TOYOTA_AUTO_HOLD_CARS:
|
||||
ret.alternativeExperience |= (ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
if candidate in TOYOTA_AUTO_HOLD_AEB_CARS
|
||||
if uses_toyota_auto_hold_aeb(ret)
|
||||
else ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD)
|
||||
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
|
||||
@@ -176,7 +176,8 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter.
|
||||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED) else MIN_ACC_SPEED
|
||||
rav4_hybrid_sdsu_long_defaults = uses_rav4_hybrid_sdsu_longitudinal(ret)
|
||||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED or rav4_hybrid_sdsu_long_defaults) else MIN_ACC_SPEED
|
||||
|
||||
prius_long_defaults = candidate in LEGACY_PRIUS_CAR and ret.openpilotLongitudinalControl
|
||||
camry_hybrid_long_defaults = (candidate == CAR.TOYOTA_CAMRY and ret.openpilotLongitudinalControl and
|
||||
@@ -193,7 +194,7 @@ class CarInterface(CarInterfaceBase):
|
||||
if ret.flags & ToyotaFlags.HYBRID.value:
|
||||
ret.longitudinalActuatorDelay = 0.05
|
||||
|
||||
if camry_hybrid_long_defaults:
|
||||
if camry_hybrid_long_defaults or rav4_hybrid_sdsu_long_defaults:
|
||||
# The THS eCVT responds much faster than the legacy non-TSS2 ICE tune.
|
||||
ret.longitudinalActuatorDelay = 0.05
|
||||
ret.vEgoStopping = 0.25
|
||||
|
||||
@@ -25,7 +25,7 @@ from opendbc.car.toyota.radar_interface import RadarInterface, TSSP_RADAR_EGO_SP
|
||||
from opendbc.car.toyota.values import CAR, DBC, MIN_ACC_SPEED, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
|
||||
FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \
|
||||
ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, TOYOTA_AUTO_HOLD_CARS, \
|
||||
TOYOTA_AUTO_HOLD_AEB_CARS, \
|
||||
uses_toyota_auto_hold_aeb, \
|
||||
get_platform_codes
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from openpilot.common.params import Params
|
||||
@@ -85,11 +85,48 @@ class TestToyotaInterfaces:
|
||||
SimpleNamespace(force_torque_controller=True, nnff=False, nnff_lite=False),
|
||||
)
|
||||
|
||||
assert default_params.lateralTuning.which() == "pid"
|
||||
assert default_params.lateralTuning.which() == "torque"
|
||||
assert forced_params.lateralTuning.which() == "torque"
|
||||
assert default_params.lateralTuning.to_dict() == forced_params.lateralTuning.to_dict()
|
||||
assert forced_params.lateralTuning.torque.latAccelFactor == pytest.approx(1.7)
|
||||
assert forced_params.lateralTuning.torque.friction == pytest.approx(0.14)
|
||||
|
||||
@pytest.mark.parametrize("candidate", list(CAR))
|
||||
def test_toyota_default_controller_preserves_native_angle_path(self, candidate):
|
||||
fingerprint = {bus: {} for bus in range(8)}
|
||||
default_params = CarInterface.get_params(
|
||||
candidate, fingerprint, [], False, False, False,
|
||||
SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False),
|
||||
)
|
||||
forced_params = CarInterface.get_params(
|
||||
candidate, fingerprint, [], False, False, False,
|
||||
SimpleNamespace(force_torque_controller=True, nnff=False, nnff_lite=False),
|
||||
)
|
||||
|
||||
if candidate in ANGLE_CONTROL_CAR:
|
||||
assert default_params.steerControlType == CarParams.SteerControlType.angle
|
||||
assert default_params.lateralTuning.which() == "pid"
|
||||
assert default_params.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.LTA.value
|
||||
else:
|
||||
assert default_params.steerControlType == CarParams.SteerControlType.torque
|
||||
assert default_params.lateralTuning.which() == "torque"
|
||||
assert not default_params.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.LTA.value
|
||||
|
||||
assert default_params.to_dict() == forced_params.to_dict()
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_PRIME])
|
||||
@pytest.mark.parametrize("eps_version", [b'\x028965B0R01400', b'8965B42181\x00\x00\x00\x00\x00\x00', b'8965B0R01200'])
|
||||
def test_rav4_rack_variants_default_to_torque(self, candidate, eps_version):
|
||||
params = CarInterface.get_params(
|
||||
candidate, {bus: {} for bus in range(8)}, [CarParams.CarFw(ecu=Ecu.eps, fwVersion=eps_version)],
|
||||
False, False, False,
|
||||
SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False),
|
||||
)
|
||||
|
||||
assert params.lateralTuning.which() == "torque"
|
||||
assert params.steerActuatorDelay == pytest.approx(0.12)
|
||||
assert params.steerControlType == CarParams.SteerControlType.torque
|
||||
|
||||
def test_prius_force_torque_controller_preserves_vehicle_tune(self):
|
||||
fingerprint = {bus: {} for bus in range(8)}
|
||||
car_fw = [CarParams.CarFw(ecu=Ecu.eps, fwVersion=b'8965B47050\x00\x00\x00\x00\x00\x00')]
|
||||
@@ -193,8 +230,15 @@ class TestToyotaInterfaces:
|
||||
if car_model in TSS2_CAR and car_model not in SECOC_CAR:
|
||||
assert dbc[Bus.pt] == "toyota_nodsu_pt_generated"
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||
def test_auto_hold_sets_flag_on_supported_toyota(self, candidate):
|
||||
@pytest.mark.parametrize("candidate,hybrid", [
|
||||
(CAR.TOYOTA_CAMRY_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4, False),
|
||||
(CAR.TOYOTA_RAV4H, True),
|
||||
(CAR.TOYOTA_RAV4_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4_TSS2, True),
|
||||
(CAR.TOYOTA_COROLLA_TSS2, True),
|
||||
])
|
||||
def test_auto_hold_sets_flag_on_supported_toyota(self, candidate, hybrid):
|
||||
params = Params()
|
||||
try:
|
||||
params.put_bool("ToyotaAutoHold", True)
|
||||
@@ -202,7 +246,7 @@ class TestToyotaInterfaces:
|
||||
candidate,
|
||||
{bus: ({0x2FF: 8} if candidate in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H) and bus == 0 else {})
|
||||
for bus in range(8)},
|
||||
[],
|
||||
[CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] if hybrid else [],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
@@ -212,7 +256,9 @@ class TestToyotaInterfaces:
|
||||
params.remove("ToyotaAutoHold")
|
||||
|
||||
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
if candidate in TOYOTA_AUTO_HOLD_AEB_CARS:
|
||||
legacy_hold = candidate == CAR.TOYOTA_CAMRY_TSS2 or (candidate == CAR.TOYOTA_RAV4_TSS2 and hybrid)
|
||||
assert uses_toyota_auto_hold_aeb(car_params) == legacy_hold
|
||||
if legacy_hold:
|
||||
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
else:
|
||||
@@ -222,16 +268,24 @@ class TestToyotaInterfaces:
|
||||
can_parsers = CarState.get_can_parsers(car_params)
|
||||
car_state = CarState(car_params, SimpleNamespace(flags=0))
|
||||
car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0))
|
||||
assert (0x344 in can_parsers[Bus.cam].vl) == (candidate in TOYOTA_AUTO_HOLD_AEB_CARS)
|
||||
assert (0x344 in can_parsers[Bus.cam].vl) == legacy_hold
|
||||
assert car_state.auto_brake_hold == legacy_hold
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||
def test_auto_hold_is_disabled_by_default(self, candidate):
|
||||
@pytest.mark.parametrize("candidate,hybrid", [
|
||||
(CAR.TOYOTA_CAMRY_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4, False),
|
||||
(CAR.TOYOTA_RAV4H, True),
|
||||
(CAR.TOYOTA_RAV4_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4_TSS2, True),
|
||||
(CAR.TOYOTA_RAV4_TSS2_2022, True),
|
||||
])
|
||||
def test_auto_hold_is_disabled_by_default(self, candidate, hybrid):
|
||||
params = Params()
|
||||
params.remove("ToyotaAutoHold")
|
||||
car_params = CarInterface.get_params(
|
||||
candidate,
|
||||
{bus: {} for bus in range(8)},
|
||||
[],
|
||||
[CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] if hybrid else [],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
@@ -240,6 +294,8 @@ class TestToyotaInterfaces:
|
||||
|
||||
assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
assert 0x344 not in CarState.get_can_parsers(car_params)[Bus.cam].vl
|
||||
|
||||
def test_prius_openpilot_long_uses_hybrid_long_defaults(self):
|
||||
car_params = CarInterface.get_params(
|
||||
@@ -1093,6 +1149,17 @@ class TestToyotaCarController:
|
||||
|
||||
assert gas_cmd == 0.12
|
||||
|
||||
def test_interceptor_does_not_apply_gas_during_auto_hold(self):
|
||||
controller = self._make_controller()
|
||||
controller.CP.enableGasInterceptorDEPRECATED = True
|
||||
controller.accel = 1.5
|
||||
controller.brake_hold_active = True
|
||||
|
||||
assert controller._compute_interceptor_gas_cmd(
|
||||
SimpleNamespace(longActive=True),
|
||||
SimpleNamespace(out=SimpleNamespace(standstill=True, vEgo=0.0)),
|
||||
) == 0.0
|
||||
|
||||
def test_interceptor_non_stop_and_go_scales_with_accel_request(self):
|
||||
controller = self._make_controller()
|
||||
controller.CP.enableGasInterceptorDEPRECATED = True
|
||||
@@ -1225,6 +1292,271 @@ class TestToyotaCarController:
|
||||
assert CP.stopAccel == -1.5
|
||||
|
||||
|
||||
class TestToyotaAutoHoldCruise:
|
||||
@staticmethod
|
||||
def _make_car(*, candidate=CAR.TOYOTA_RAV4_TSS2, hybrid=False, enabled=True, capability=True):
|
||||
cp = CarInterface.get_non_essential_params(candidate)
|
||||
cp.openpilotLongitudinalControl = True
|
||||
cp.flags &= ~(ToyotaFlags.AUTO_BRAKE_HOLD.value | ToyotaFlags.HYBRID.value)
|
||||
if capability:
|
||||
cp.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
if hybrid:
|
||||
cp.flags |= ToyotaFlags.HYBRID.value
|
||||
controller = CarController(DBC[candidate], cp)
|
||||
cc = structs.CarControl(enabled=True, longActive=True)
|
||||
cc.actuators.accel = -0.7
|
||||
cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.stopping
|
||||
cc.hudControl.leadDistanceBars = 3
|
||||
cs = SimpleNamespace(
|
||||
out=structs.CarState(
|
||||
standstill=True,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
cruiseState=structs.CarState.CruiseState(available=True, enabled=True, standstill=True),
|
||||
),
|
||||
gvc=0.0,
|
||||
acc_type=1,
|
||||
pcm_acc_status=7,
|
||||
pcm_follow_distance=1,
|
||||
lkas_hud={},
|
||||
pre_collision_2={},
|
||||
)
|
||||
toggles = SimpleNamespace(toyota_auto_hold=enabled, sng_hack=False, lock_doors=False, unlock_doors=False)
|
||||
parser = CANParser(DBC[candidate][Bus.pt], [("ACC_CONTROL", 0), ("PRE_COLLISION_2", 0)], 0)
|
||||
return SimpleNamespace(controller=controller, cc=cc, cs=cs, toggles=toggles, parser=parser)
|
||||
|
||||
@staticmethod
|
||||
def _tick(car):
|
||||
# Run through a complete ACC_CONTROL send interval and decode the actual
|
||||
# controller output, including the ordinary standstill and PID paths.
|
||||
car.can_sends = []
|
||||
for _ in range(3):
|
||||
now_nanos = car.controller.frame * 10_000_000
|
||||
_, messages = car.controller.update(car.cc.as_reader(), car.cs, now_nanos, car.toggles)
|
||||
car.can_sends.extend(messages)
|
||||
car.parser.update([(now_nanos, messages)])
|
||||
return car.parser.vl["ACC_CONTROL"]
|
||||
|
||||
@pytest.mark.parametrize("candidate,hybrid", [
|
||||
(CAR.TOYOTA_RAV4, False),
|
||||
(CAR.TOYOTA_RAV4H, True),
|
||||
(CAR.TOYOTA_RAV4_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4_TSS2, True),
|
||||
(CAR.TOYOTA_CAMRY_TSS2, False),
|
||||
])
|
||||
@pytest.mark.parametrize("sng_hack", [False, True])
|
||||
def test_auto_hold_does_not_change_cruise_stop_and_go(self, candidate, hybrid, sng_hack):
|
||||
car = self._make_car(candidate=candidate, hybrid=hybrid)
|
||||
baseline = self._make_car(candidate=candidate, hybrid=hybrid, enabled=False)
|
||||
car.toggles.sng_hack = baseline.toggles.sng_hack = sng_hack
|
||||
|
||||
for _ in range(50):
|
||||
command = self._tick(car)
|
||||
expected = self._tick(baseline)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert dict(command) == dict(expected)
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 0
|
||||
|
||||
for vehicle in (car, baseline):
|
||||
vehicle.cc.actuators.accel = 1.5
|
||||
vehicle.cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.starting
|
||||
vehicle.cc.cruiseControl.resume = True
|
||||
for _ in range(30):
|
||||
command = self._tick(car)
|
||||
expected = self._tick(baseline)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert dict(command) == dict(expected)
|
||||
assert not car.cs.out.gasPressed
|
||||
assert command["ACCEL_CMD"] > 0
|
||||
assert command["RELEASE_STANDSTILL"] == 1
|
||||
|
||||
def test_manual_stop_still_holds_with_only_aol_active(self):
|
||||
car = self._make_car()
|
||||
car.cc.enabled = False
|
||||
car.cc.longActive = False
|
||||
car.cc.latActive = True
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.brakePressed = True
|
||||
# Include the first ACC_CONTROL transmission after the one-second latch.
|
||||
for _ in range(35):
|
||||
command = self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
assert command["ACCEL_CMD"] == -1.0
|
||||
assert command["RELEASE_STANDSTILL"] == 0
|
||||
|
||||
car.cs.out.brakePressed = False
|
||||
command = self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
assert command["ACCEL_CMD"] == -1.0
|
||||
|
||||
car.cs.out.gasPressed = True
|
||||
command = self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert command["ACCEL_CMD"] == 0.0
|
||||
assert command["RELEASE_STANDSTILL"] == 1
|
||||
|
||||
car.cs.out.gasPressed = False
|
||||
for _ in range(50):
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
|
||||
car.cs.out.brakePressed = True
|
||||
for _ in range(35):
|
||||
command = self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
assert command["ACCEL_CMD"] == -1.0
|
||||
|
||||
@pytest.mark.parametrize("long_state", ["off", "pid", "starting", "stopping"])
|
||||
@pytest.mark.parametrize("brake_pressed", [False, True])
|
||||
def test_engaged_cruise_never_activates_auto_hold(self, long_state, brake_pressed):
|
||||
car = self._make_car()
|
||||
car.cc.actuators.longControlState = getattr(structs.CarControl.Actuators.LongControlState, long_state)
|
||||
car.cs.out.brakePressed = brake_pressed
|
||||
for _ in range(50):
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
|
||||
@pytest.mark.parametrize("enabled,capability,candidate", [
|
||||
(False, True, CAR.TOYOTA_RAV4_TSS2),
|
||||
(True, False, CAR.TOYOTA_RAV4_TSS2),
|
||||
])
|
||||
def test_manual_hold_requires_toggle_and_capability(self, enabled, capability, candidate):
|
||||
car = self._make_car(candidate=candidate, enabled=enabled, capability=capability)
|
||||
car.cc.longActive = False
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.brakePressed = True
|
||||
for _ in range(50):
|
||||
command = self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert command["ACCEL_CMD"] == 0.0
|
||||
|
||||
@pytest.mark.parametrize("release", ["toggle", "cancel", "main", "park", "reverse", "moving", "cruise"])
|
||||
def test_manual_hold_releases_when_conditions_change(self, release):
|
||||
car = self._make_car()
|
||||
car.cc.longActive = False
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.brakePressed = True
|
||||
for _ in range(35):
|
||||
self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
|
||||
if release == "toggle":
|
||||
car.toggles.toyota_auto_hold = False
|
||||
elif release == "cancel":
|
||||
car.cc.cruiseControl.cancel = True
|
||||
elif release == "main":
|
||||
car.cs.out.cruiseState.available = False
|
||||
elif release in ("park", "reverse"):
|
||||
car.cs.out.gearShifter = getattr(structs.CarState.GearShifter, release)
|
||||
elif release == "moving":
|
||||
car.cs.out.standstill = False
|
||||
else:
|
||||
car.cs.out.cruiseState.enabled = True
|
||||
car.cc.longActive = True
|
||||
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
|
||||
if release == "cruise":
|
||||
car.cs.out.brakePressed = False
|
||||
car.cc.actuators.accel = 1.5
|
||||
car.cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.starting
|
||||
car.cc.cruiseControl.resume = True
|
||||
for _ in range(20):
|
||||
command = self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert command["ACCEL_CMD"] > 0
|
||||
assert command["RELEASE_STANDSTILL"] == 1
|
||||
|
||||
|
||||
class TestToyotaAutoHoldAeb:
|
||||
_make_car = staticmethod(TestToyotaAutoHoldCruise._make_car)
|
||||
_tick = staticmethod(TestToyotaAutoHoldCruise._tick)
|
||||
|
||||
@pytest.mark.parametrize("candidate", list(CAR))
|
||||
@pytest.mark.parametrize("hybrid", [False, True])
|
||||
def test_legacy_path_is_scoped_to_camry_and_early_rav4_hybrid(self, candidate, hybrid):
|
||||
cp = SimpleNamespace(carFingerprint=candidate, flags=ToyotaFlags.HYBRID.value if hybrid else 0)
|
||||
expected = candidate == CAR.TOYOTA_CAMRY_TSS2 or (candidate == CAR.TOYOTA_RAV4_TSS2 and hybrid)
|
||||
assert uses_toyota_auto_hold_aeb(cp) == expected
|
||||
|
||||
@pytest.mark.parametrize("candidate,hybrid", [(CAR.TOYOTA_CAMRY_TSS2, False), (CAR.TOYOTA_RAV4_TSS2, True)])
|
||||
def test_manual_stop_uses_legacy_hold_and_releases_on_gas(self, candidate, hybrid):
|
||||
car = self._make_car(candidate=candidate, hybrid=hybrid)
|
||||
car.cc.enabled = False
|
||||
car.cc.longActive = False
|
||||
car.cc.latActive = True
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.cruiseState.standstill = False
|
||||
car.cs.out.brakePressed = True
|
||||
car.cs.pre_collision_2 = {"DSS1GDRV": -0.1, "PBRTRGR": 0}
|
||||
|
||||
# Pass through the camera message until the pedal has been held at a stop.
|
||||
command = self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -0.1
|
||||
for _ in range(34):
|
||||
command = self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
assert any(msg[0] == 0x344 for msg in car.can_sends)
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1
|
||||
# Do not simultaneously inject an ACC_CONTROL hold on legacy-path cars.
|
||||
assert command["ACCEL_CMD"] == 0.0
|
||||
assert command["RELEASE_STANDSTILL"] == 1
|
||||
|
||||
car.cs.out.brakePressed = False
|
||||
for _ in range(50):
|
||||
self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
|
||||
|
||||
car.cs.out.gasPressed = True
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -0.1
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 0
|
||||
|
||||
@pytest.mark.parametrize("enabled,capability", [(False, True), (True, False)])
|
||||
def test_rav4_hybrid_does_not_inject_legacy_messages_without_opt_in(self, enabled, capability):
|
||||
car = self._make_car(hybrid=True, enabled=enabled, capability=capability)
|
||||
car.cc.longActive = False
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.brakePressed = True
|
||||
for _ in range(50):
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert all(msg[0] != 0x344 for msg in car.can_sends)
|
||||
|
||||
@pytest.mark.parametrize("release", ["toggle", "main", "park", "reverse", "moving", "cruise"])
|
||||
def test_rav4_hybrid_legacy_hold_releases_when_conditions_change(self, release):
|
||||
car = self._make_car(hybrid=True)
|
||||
car.cc.longActive = False
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.brakePressed = True
|
||||
for _ in range(35):
|
||||
self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
|
||||
if release == "toggle":
|
||||
car.toggles.toyota_auto_hold = False
|
||||
elif release == "main":
|
||||
car.cs.out.cruiseState.available = False
|
||||
elif release in ("park", "reverse"):
|
||||
car.cs.out.gearShifter = getattr(structs.CarState.GearShifter, release)
|
||||
elif release == "moving":
|
||||
car.cs.out.standstill = False
|
||||
else:
|
||||
car.cs.out.cruiseState.enabled = True
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
|
||||
|
||||
class TestToyotaCarState:
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT])
|
||||
def test_legacy_prius_distance_button_generates_events(self, candidate):
|
||||
|
||||
@@ -629,10 +629,16 @@ TOYOTA_AUTO_HOLD_CARS = (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR) | {
|
||||
CAR.TOYOTA_RAV4H,
|
||||
}
|
||||
|
||||
# The Camry uses the legacy camera AEB replacement for Auto Hold. Other
|
||||
# supported Toyota models use the ACC_CONTROL hold request.
|
||||
# The Camry uses the legacy camera AEB replacement for Auto Hold. The
|
||||
# 2019-2021 RAV4 uses it only with the detected hybrid powertrain.
|
||||
TOYOTA_AUTO_HOLD_AEB_CARS = {CAR.TOYOTA_CAMRY_TSS2}
|
||||
|
||||
|
||||
def uses_toyota_auto_hold_aeb(CP: CarParams) -> bool:
|
||||
return (CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS or
|
||||
(CP.carFingerprint == CAR.TOYOTA_RAV4_TSS2 and bool(CP.flags & ToyotaFlags.HYBRID.value)))
|
||||
|
||||
|
||||
# no resume button press required
|
||||
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
|
||||
|
||||
|
||||
@@ -3,6 +3,7 @@ from opendbc.can import CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gps import get_car_gps_config
|
||||
from opendbc.car.volkswagen.values import DBC, CanBus, NetworkLocation, TransmissionType, GearShifter, \
|
||||
CarControllerParams, VolkswagenFlags
|
||||
|
||||
@@ -24,6 +25,32 @@ class CarState(CarStateBase):
|
||||
self.travel_assist_available = False
|
||||
self.curvature_meas = 0.
|
||||
self.klr_stock_values = {}
|
||||
self.car_gps_config = get_car_gps_config(CP)
|
||||
self.car_gps_supported = self.car_gps_config is not None
|
||||
self.car_gps = None
|
||||
self._car_gps_timestamp_nanos = 0
|
||||
|
||||
def _update_car_gps(self, cp) -> None:
|
||||
if self.car_gps_config is None:
|
||||
return
|
||||
|
||||
timestamps = [max(cp.ts_nanos[name].values(), default=0) for name in self.car_gps_config.messages]
|
||||
if min(timestamps) > self._car_gps_timestamp_nanos and max(timestamps) - min(timestamps) <= 2_000_000_000:
|
||||
gps = self.car_gps_config.decoder(*(cp.vl[name] for name in self.car_gps_config.messages))
|
||||
if gps is not None and (self.car_gps is None or not gps["hasFix"] or
|
||||
gps["unixTimestampMillis"] > self.car_gps["unixTimestampMillis"]):
|
||||
timestamp_nanos = max(timestamps)
|
||||
gps["timestamp_nanos"] = timestamp_nanos
|
||||
self.car_gps = gps
|
||||
self._car_gps_timestamp_nanos = timestamp_nanos
|
||||
|
||||
if self.car_gps is not None and cp._last_update_nanos - self._car_gps_timestamp_nanos > 2_500_000_000:
|
||||
if self.car_gps["hasFix"]:
|
||||
self.car_gps = {**self.car_gps, "hasFix": False, "speed": 0.0, "vNED": [0.0, 0.0, 0.0],
|
||||
"bearingAccuracyDeg": 180.0, "timestamp_nanos": cp._last_update_nanos}
|
||||
|
||||
def get_car_gps(self):
|
||||
return self.car_gps
|
||||
|
||||
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
|
||||
if not self.CP.pcmCruise:
|
||||
@@ -51,6 +78,7 @@ class CarState(CarStateBase):
|
||||
pt_cp = can_parsers[Bus.pt]
|
||||
cam_cp = can_parsers[Bus.cam]
|
||||
ext_cp = pt_cp if self.CP.networkLocation == NetworkLocation.fwdCamera else cam_cp
|
||||
self._update_car_gps(pt_cp)
|
||||
|
||||
if self.CP.flags & VolkswagenFlags.PQ:
|
||||
return self.update_pq(pt_cp, cam_cp, ext_cp)
|
||||
@@ -427,6 +455,9 @@ class CarState(CarStateBase):
|
||||
|
||||
# manually configure some optional and variable-rate/edge-triggered messages
|
||||
pt_messages, cam_messages = [], []
|
||||
gps_config = get_car_gps_config(CP)
|
||||
if gps_config is not None:
|
||||
pt_messages += [(name, 0) for name in gps_config.messages]
|
||||
|
||||
if not CP.flags & VolkswagenFlags.MLB:
|
||||
pt_messages += [
|
||||
|
||||
@@ -0,0 +1,196 @@
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.gps import VOLKSWAGEN_TAOS_GPS_MESSAGES, get_car_gps_config, parse_volkswagen_taos_can_gps
|
||||
from opendbc.car.volkswagen.carstate import CarState
|
||||
from opendbc.car.volkswagen.interface import CarInterface
|
||||
from opendbc.car.volkswagen.values import CAR, DBC
|
||||
|
||||
|
||||
# Synthetic frames, not route data: 40 N / 110 W, 12.5 m/s westbound,
|
||||
# altitude 300 m, UTC 1,800,000,000, ten satellites, packet ID 2.
|
||||
GPS_FRAMES = (
|
||||
(0x36F, bytes.fromhex("02688909e09da31d"), 0),
|
||||
(0x374, bytes.fromhex("02c0a83200000000"), 0),
|
||||
(0x378, bytes.fromhex("0221415290010000"), 0),
|
||||
(0x37B, bytes.fromhex("00d2496b7f4c0900"), 0),
|
||||
)
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def gps_state():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_TAOS_MK1)
|
||||
state = CarState(cp, None)
|
||||
parsers = state.get_can_parsers(cp)
|
||||
return state, parsers
|
||||
|
||||
|
||||
def decode_frames(frames=GPS_FRAMES):
|
||||
parser = CANParser("vw_mqb", [(name, 0) for name in VOLKSWAGEN_TAOS_GPS_MESSAGES], 0)
|
||||
parser.update([(1_000_000_000, list(frames))])
|
||||
return parser, [parser.vl[name] for name in VOLKSWAGEN_TAOS_GPS_MESSAGES]
|
||||
|
||||
|
||||
def test_synthetic_gps_frame_decode():
|
||||
parser, values = decode_frames()
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert parser.can_valid
|
||||
assert gps["hasFix"]
|
||||
assert gps["latitude"] == pytest.approx(40.0)
|
||||
assert gps["longitude"] == pytest.approx(-110.0)
|
||||
assert gps["speed"] == 12.5
|
||||
assert gps["bearingDeg"] == 270.0
|
||||
assert gps["altitude"] == 300.0
|
||||
assert gps["unixTimestampMillis"] == 1_800_000_000_000
|
||||
assert gps["satelliteCount"] == 10
|
||||
assert gps["vNED"] == pytest.approx([0.0, -12.5, 0.0])
|
||||
|
||||
|
||||
def test_unvalidated_hemisphere_is_not_a_fix():
|
||||
frames = ((0x36F, bytes.fromhex("02688929e09da319"), 0), *GPS_FRAMES[1:])
|
||||
_, values = decode_frames(frames)
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert not gps["hasFix"]
|
||||
assert gps["vNED"] == [0.0, 0.0, 0.0]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("message,signal,value", (
|
||||
(0, "GNSS_LatitudeMagnitude", 91.0),
|
||||
(0, "GNSS_LongitudeMagnitude", 181.0),
|
||||
(0, "GNSS_LatitudeMagnitude", float("nan")),
|
||||
(0, "GNSS_PositionStatus", 0),
|
||||
(0, "GNSS_PositionStatus", 1),
|
||||
(0, "GNSS_PositionStatus", 2),
|
||||
(3, "GNSS_Empfaenger_Status", 0),
|
||||
(3, "GNSS_Genutzte_Satelliten", 0),
|
||||
(3, "GNSS_Genutzte_Satelliten", 3),
|
||||
))
|
||||
def test_invalid_solution_is_not_a_fix(message, signal, value):
|
||||
_, values = decode_frames()
|
||||
values[message][signal] = value
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert not gps["hasFix"]
|
||||
assert gps["speed"] == 0.0
|
||||
assert gps["vNED"] == [0.0, 0.0, 0.0]
|
||||
|
||||
|
||||
def test_no_constellation_or_null_position_is_not_a_fix():
|
||||
_, values = decode_frames()
|
||||
values[3]["GNSS_GPS_in_Nutzung"] = values[3]["GNSS_GLONASS_in_Nutzung"] = 0
|
||||
assert not parse_volkswagen_taos_can_gps(*values)["hasFix"]
|
||||
values[3]["GNSS_GPS_in_Nutzung"] = 1
|
||||
values[0]["GNSS_LatitudeMagnitude"] = values[0]["GNSS_LongitudeMagnitude"] = 0
|
||||
assert not parse_volkswagen_taos_can_gps(*values)["hasFix"]
|
||||
|
||||
|
||||
def test_initial_clock_and_mixed_epochs_are_rejected():
|
||||
_, values = decode_frames()
|
||||
values[3]["GNSS_UTC_Zeit"] = 0
|
||||
assert parse_volkswagen_taos_can_gps(*values) is None
|
||||
values[3]["GNSS_UTC_Zeit"] = 1_800_000_000
|
||||
values[1]["GNSS_Nachrichtenpaket_ID2"] = 1
|
||||
assert parse_volkswagen_taos_can_gps(*values) is None
|
||||
|
||||
|
||||
def test_invalid_motion_and_altitude_have_unknown_accuracy():
|
||||
_, values = decode_frames()
|
||||
values[1]["GNSS_Speed"] = 127.75
|
||||
values[1]["GNSS_Bearing"] = 409.5
|
||||
values[2]["GNSS_Ortung_Hoehe"] = 7690.0
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert gps["hasFix"]
|
||||
assert gps["speed"] == gps["bearingDeg"] == gps["altitude"] == 0.0
|
||||
assert gps["bearingAccuracyDeg"] == 180.0
|
||||
assert gps["speedAccuracy"] == 100.0
|
||||
assert gps["verticalAccuracy"] == 500.0
|
||||
assert gps["vNED"] == [0.0, 0.0, 0.0]
|
||||
|
||||
|
||||
def test_unknown_course_does_not_report_accurate_zero_velocity():
|
||||
_, values = decode_frames()
|
||||
values[1]["GNSS_Bearing"] = 409.5
|
||||
gps = parse_volkswagen_taos_can_gps(*values)
|
||||
assert gps["hasFix"]
|
||||
assert gps["speed"] == 12.5
|
||||
assert gps["speedAccuracy"] == 100.0
|
||||
assert gps["vNED"] == [0.0, 0.0, 0.0]
|
||||
|
||||
|
||||
def test_gps_is_only_enabled_for_the_taos():
|
||||
for model in (CAR.VOLKSWAGEN_TAOS_MK1, CAR.VOLKSWAGEN_GOLF_MK7, CAR.VOLKSWAGEN_ID4_MK1):
|
||||
cp = CarInterface.get_non_essential_params(model)
|
||||
state = CarState(cp, None)
|
||||
config = get_car_gps_config(cp)
|
||||
expected = model == CAR.VOLKSWAGEN_TAOS_MK1
|
||||
assert state.car_gps_supported is expected
|
||||
assert (config is not None) is expected
|
||||
if expected:
|
||||
assert config.messages == VOLKSWAGEN_TAOS_GPS_MESSAGES
|
||||
cp.brand = "mock"
|
||||
assert get_car_gps_config(cp) is None
|
||||
|
||||
|
||||
def test_carstate_update_reads_gps(gps_state):
|
||||
state, parsers = gps_state
|
||||
parsers[Bus.pt].update([(1_000_000_000, list(GPS_FRAMES))])
|
||||
state.update(parsers, None)
|
||||
gps = state.get_car_gps()
|
||||
assert gps["hasFix"]
|
||||
assert gps["timestamp_nanos"] == 1_000_000_000
|
||||
assert gps["longitude"] == pytest.approx(-110.0)
|
||||
|
||||
|
||||
def test_partial_startup_and_missing_gps_are_optional(gps_state):
|
||||
state, parsers = gps_state
|
||||
parser = parsers[Bus.pt]
|
||||
packer = CANPacker(DBC[state.CP.carFingerprint][Bus.pt])
|
||||
blink = packer.make_can_msg("Blinkmodi_02", parser.bus, {})
|
||||
for time in (1_000_000_000, 2_000_000_000):
|
||||
parser.update([(time, [blink])])
|
||||
state._update_car_gps(parser)
|
||||
assert state.get_car_gps() is None
|
||||
assert parser.can_valid
|
||||
parser.update([(3_000_000_000, list(GPS_FRAMES[:3]))])
|
||||
state._update_car_gps(parser)
|
||||
assert state.get_car_gps() is None
|
||||
assert all(parser.message_states[address].ignore_alive for address, _, _ in GPS_FRAMES)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("dropout", ("missing_position", "frozen_clock", "missing_all"))
|
||||
def test_gps_dropout_and_recovery(gps_state, dropout):
|
||||
state, parsers = gps_state
|
||||
parser = parsers[Bus.pt]
|
||||
parser.update([(1_000_000_000, list(GPS_FRAMES))])
|
||||
state._update_car_gps(parser)
|
||||
assert state.get_car_gps()["hasFix"]
|
||||
|
||||
frames = list(GPS_FRAMES)
|
||||
if dropout == "missing_position":
|
||||
frames = frames[1:]
|
||||
elif dropout == "missing_all":
|
||||
frames = []
|
||||
parser.update([(4_000_000_000, frames)])
|
||||
state._update_car_gps(parser)
|
||||
assert not state.get_car_gps()["hasFix"]
|
||||
assert state.get_car_gps()["timestamp_nanos"] == 4_000_000_000
|
||||
assert state.get_car_gps()["speed"] == 0.0
|
||||
|
||||
packer = CANPacker("vw_mqb")
|
||||
status = {**parser.vl["GNSS_05"], "GNSS_UTC_Zeit": 1_800_000_004}
|
||||
parser.update([(5_000_000_000, [*GPS_FRAMES[:3], packer.make_can_msg("GNSS_05", parser.bus, status)])])
|
||||
state._update_car_gps(parser)
|
||||
assert state.get_car_gps()["hasFix"]
|
||||
assert state.get_car_gps()["unixTimestampMillis"] == 1_800_000_004_000
|
||||
|
||||
|
||||
def test_invalid_receiver_status_clears_existing_fix(gps_state):
|
||||
state, parsers = gps_state
|
||||
parser = parsers[Bus.pt]
|
||||
parser.update([(1_000_000_000, list(GPS_FRAMES))])
|
||||
state._update_car_gps(parser)
|
||||
status = {**parser.vl["GNSS_05"], "GNSS_Empfaenger_Status": 0}
|
||||
packer = CANPacker("vw_mqb")
|
||||
parser.update([(2_000_000_000, [*GPS_FRAMES[:3], packer.make_can_msg("GNSS_05", parser.bus, status)])])
|
||||
state._update_car_gps(parser)
|
||||
assert not state.get_car_gps()["hasFix"]
|
||||
@@ -248,12 +248,12 @@ class CarController(CarControllerBase):
|
||||
# LCA_5 (formerly SPEED_1) - 0x67 - 50 Hz
|
||||
# Contains wheel speeds + LCA signals (LCA_TURN_BITS, LCA_5_STEER)
|
||||
if self.frame % 2 == 0: # 50 Hz
|
||||
# Initialize counter from CarState on first run
|
||||
if self.lca_5_counter is None:
|
||||
if not lat_active or self.lca_5_counter is None:
|
||||
self.lca_5_counter = CS.msg_lca_5['COUNTER']
|
||||
|
||||
# Increment counter by +4, wrap at 15 (0xF never used)
|
||||
self.lca_5_counter = (self.lca_5_counter + 4) % 15
|
||||
if lat_active:
|
||||
self.lca_5_counter = (self.lca_5_counter + 4) % 15
|
||||
|
||||
can_sends.append(create_lca_5_message(self.packer, lat_active, apply_angle,
|
||||
CS.msg_lca_5, self.lca_5_counter))
|
||||
|
||||
@@ -3,6 +3,7 @@ from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car.volvo.carcontroller import CarController
|
||||
from opendbc.car.volvo.helpers import checksum_lca_5_message
|
||||
from opendbc.car.volvo.interface import CarInterface
|
||||
@@ -56,21 +57,36 @@ def test_controller_emits_valid_eight_byte_messages_and_lca5_checksum():
|
||||
assert data[2] == checksum_lca_5_message(data[0], data[1], data[3], data[4], data[5])
|
||||
|
||||
|
||||
def test_controller_relays_stock_lca5_angle_when_inactive():
|
||||
cp = CarInterface.get_non_essential_params("VOLVO_XC40_RECHARGE")
|
||||
@pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE])
|
||||
def test_controller_relays_complete_stock_lca5_when_inactive(fingerprint):
|
||||
cp = CarInterface.get_non_essential_params(fingerprint)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
parser = CANParser("volvo_mid_1", [("LCA_5", 50)], 0)
|
||||
stock_data = bytes.fromhex("88c04cef1190ba00")
|
||||
parser.update([0, [(0x67, stock_data, 0)]])
|
||||
cs = _state()
|
||||
cs.msg_lca_5["LCA_5_STEER"] = 12.0
|
||||
cs.msg_lca_5 = parser.vl["LCA_5"]
|
||||
cc = SimpleNamespace(latActive=False, actuators=_Actuators())
|
||||
safety = libsafety_py.libsafety
|
||||
config = cp.safetyConfigs[0]
|
||||
assert safety.set_safety_hooks(config.safetyModel.raw, config.safetyParam) == 0
|
||||
safety.init_tests()
|
||||
safety.set_controls_allowed(False)
|
||||
safety.safety_rx_hook(libsafety_py.make_CANPacket(0x67, 0, stock_data))
|
||||
|
||||
_, can_sends = controller.update(cc, cs, 0, None)
|
||||
lca5 = next(msg for msg in can_sends if msg[0] == 0x67)
|
||||
assert lca5[1] == stock_data
|
||||
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(lca5[0], lca5[2], lca5[1]))
|
||||
|
||||
# The inactive path must not manufacture a new angle command.
|
||||
raw = ((lca5[1][6] & 0x7F) << 8) | lca5[1][7]
|
||||
if raw & (1 << 14):
|
||||
raw -= 1 << 15
|
||||
assert abs(raw * 0.05596 - 12.0) < 0.1
|
||||
cs.msg_lca_5["COUNTER"] = 7
|
||||
controller.update(cc, cs, 0, None)
|
||||
controller.update(cc, cs, 0, None)
|
||||
controller.update(cc, cs, 0, None)
|
||||
cc.latActive = True
|
||||
_, can_sends = controller.update(cc, cs, 0, None)
|
||||
active_lca5 = next(msg for msg in can_sends if msg[0] == 0x67)
|
||||
assert active_lca5[1][3] >> 4 == 11
|
||||
|
||||
|
||||
@pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE])
|
||||
|
||||
@@ -250,6 +250,9 @@ def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_
|
||||
CAN message for LCA_5 on bus 2
|
||||
"""
|
||||
|
||||
if not lat_active:
|
||||
return packer.make_can_msg('LCA_5', 2, msg_lca_5)
|
||||
|
||||
# DBC defines LCA_5_STEER as 15-bit signed with scale 0.05596 deg/count
|
||||
# Packer handles the encoding automatically - just pass the angle in degrees
|
||||
|
||||
@@ -261,7 +264,7 @@ def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_
|
||||
'WHEEL_SPEED_2': msg_lca_5['WHEEL_SPEED_2'],
|
||||
'NEW_SIGNAL_5': msg_lca_5['NEW_SIGNAL_5'],
|
||||
'NEW_SIGNAL_2': msg_lca_5['NEW_SIGNAL_2'],
|
||||
'LCA_5_STEER': target_angle_deg if lat_active else msg_lca_5['LCA_5_STEER'],
|
||||
'LCA_5_STEER': target_angle_deg,
|
||||
'COUNTER': counter,
|
||||
}
|
||||
|
||||
|
||||
@@ -236,6 +236,9 @@ BO_ 1013 ID3F5VCFRONT_lighting: 8 VEH
|
||||
SG_ VCFRONT_indicatorRightRequest : 2|2@1+ (1,0) [0|2] "" Receiver
|
||||
SG_ VCFRONT_indicatorLeftRequest : 0|2@1+ (1,0) [0|2] "" Receiver
|
||||
|
||||
BO_ 991 UI_status2: 8 VehicleBus
|
||||
SG_ UI_activeTouchPoints : 24|8@1+ (1,0) [0|255] "" VehicleBus
|
||||
|
||||
VAL_ 568 SpdCtrlLvr_Stat 32 "DN_1ST" 16 "UP_1ST" 8 "DN_2ND" 4 "UP_2ND" 2 "RWD" 1 "FWD" 0 "IDLE" ;
|
||||
VAL_ 568 DTR_Dist_Rq 255 "SNA" 200 "ACC_DIST_7" 166 "ACC_DIST_6" 133 "ACC_DIST_5" 100 "ACC_DIST_4" 66 "ACC_DIST_3" 33 "ACC_DIST_2" 0 "ACC_DIST_1" ;
|
||||
VAL_ 568 TurnIndLvr_Stat 3 "SNA" 2 "RIGHT" 1 "LEFT" 0 "IDLE" ;
|
||||
|
||||
@@ -1610,6 +1610,36 @@ BO_ 391 Motor_EV_01: 8 Motor_MQB_BEV
|
||||
SG_ MO_HVEM_Eskalation : 54|1@1+ (1.0,0.0) [0.0|1] "" XXX
|
||||
SG_ MO_HVEM_MaxLeistung : 55|9@1+ (50,0) [0|25450] "Unit_Watt" XXX
|
||||
|
||||
BO_ 879 GNSS_01: 8 XXX
|
||||
SG_ GNSS_Nachrichtenpaket_ID1 : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ GNSS_LatitudeMagnitude : 2|27@1+ (0.000001,0) [0|90] "deg" XXX
|
||||
SG_ GNSS_LatitudeSouth : 29|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_LongitudeMagnitude : 30|28@1+ (0.000001,0) [0|180] "deg" XXX
|
||||
SG_ GNSS_LongitudeWest : 58|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_PositionStatus : 59|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 884 GNSS_02: 8 XXX
|
||||
SG_ GNSS_Nachrichtenpaket_ID2 : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ GNSS_Bearing : 12|12@1+ (0.1,0) [0|359.9] "deg" XXX
|
||||
SG_ GNSS_Speed : 24|9@1+ (0.25,0) [0|127.5] "m/s" XXX
|
||||
|
||||
BO_ 888 GNSS_04: 8 XXX
|
||||
SG_ GNSS_Nachrichtenpaket_ID4 : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ GNSS_Ortung_Zeit_in_GPSWoche : 2|30@1+ (1,0) [0|604800001] "ms" XXX
|
||||
SG_ GNSS_Ortung_Hoehe : 32|12@1+ (2,-500) [-500|7686] "m" XXX
|
||||
|
||||
BO_ 891 GNSS_05: 8 XXX
|
||||
SG_ GNSS_UTC_Zeit : 0|32@1+ (1,0) [1|4294967295] "s" XXX
|
||||
SG_ GNSS_Empfaenger_Status : 32|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_GPS_in_Nutzung : 33|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_GLONASS_in_Nutzung : 34|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ GNSS_Empfangbare_Satelliten : 35|5@1+ (1,0) [0|31] "" XXX
|
||||
SG_ GNSS_Sichtbare_Satelliten : 40|5@1+ (1,0) [0|31] "" XXX
|
||||
SG_ GNSS_Genutzte_Satelliten : 45|5@1+ (1,0) [0|31] "" XXX
|
||||
SG_ GNSS_Nachrichtenpaket_ID5 : 50|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
CM_ BO_ 879 "Taos coordinate layout decoded from CAN/GPS comparison. Only north/west coordinates and position status 3 have been validated.";
|
||||
CM_ BO_ 884 "Taos course and ground speed decoded from CAN/GPS comparison. Remaining fields are not decoded.";
|
||||
CM_ SG_ 134 LWI_Lenkradwinkel "Steering angle WITH variable ratio effect included";
|
||||
CM_ SG_ 159 EPS_HCA_Status "Status of Heading Control Assist feature";
|
||||
CM_ SG_ 159 EPS_Lenkmoment "Steering input by driver, torque";
|
||||
|
||||
@@ -4,6 +4,10 @@
|
||||
|
||||
static bool tesla_longitudinal = false;
|
||||
static bool tesla_coop_steering = false;
|
||||
static bool tesla_aol_screen_button = false;
|
||||
static bool tesla_screen_disengage_on_brake = false;
|
||||
static bool tesla_touch_initialized = false;
|
||||
static bool tesla_screen_cancel_pressed = false;
|
||||
static bool tesla_stock_aeb = false;
|
||||
|
||||
#define TESLA_STEERING_DISENGAGE_TORQUE 500 // cNm
|
||||
@@ -94,6 +98,27 @@ static bool tesla_get_quality_flag_valid(const CANPacket_t *msg) {
|
||||
return valid;
|
||||
}
|
||||
|
||||
static void tesla_rx_all_hook(const CANPacket_t *msg) {
|
||||
if (tesla_aol_screen_button && (msg->bus == 1U) && (msg->addr == 0x229U) && (GET_LEN(msg) == 3)) {
|
||||
const int stalk_status = (msg->data[1] >> 4) & 0x7U; // SCCM_rightStalkStatus
|
||||
tesla_screen_cancel_pressed = (stalk_status == 1) || (stalk_status == 2); // UP_1, UP_2
|
||||
if (tesla_screen_cancel_pressed) {
|
||||
lkas_on = false;
|
||||
}
|
||||
}
|
||||
// The add-on's UI message is asynchronous, not a required periodic safety input.
|
||||
if (tesla_aol_screen_button && (msg->bus == 1U) && (msg->addr == 0x3DFU) && (GET_LEN(msg) == 8)) {
|
||||
const bool screen_button = msg->data[3] == 3U; // UI_activeTouchPoints
|
||||
if (tesla_touch_initialized && screen_button && !lkas_button_prev && !steering_disengage && !tesla_screen_cancel_pressed &&
|
||||
!(tesla_screen_disengage_on_brake && brake_pressed)) {
|
||||
lkas_on = !lkas_on;
|
||||
}
|
||||
// A touch held during startup is not an engagement request.
|
||||
tesla_touch_initialized = true;
|
||||
lkas_button_prev = screen_button;
|
||||
}
|
||||
}
|
||||
|
||||
static void tesla_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
if (msg->bus == 0U) {
|
||||
@@ -112,6 +137,9 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
|
||||
steering_disengage = (hands_on_level >= 3) ||
|
||||
(tesla_coop_steering && (SAFETY_ABS(torsion_bar_torque) > TESLA_STEERING_DISENGAGE_TORQUE)) ||
|
||||
((eac_status == 0) && (eac_error_code == 9));
|
||||
if (tesla_aol_screen_button && steering_disengage) {
|
||||
lkas_on = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Vehicle speed (DI_speed)
|
||||
@@ -136,6 +164,9 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
|
||||
// Brake pressed
|
||||
if (msg->addr == 0x39dU) {
|
||||
brake_pressed = (msg->data[2] & 0x03U) == 2U;
|
||||
if (tesla_screen_disengage_on_brake && brake_pressed) {
|
||||
lkas_on = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Cruise and Autopark/Summon state
|
||||
@@ -164,9 +195,22 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
|
||||
(cruise_state == 7); // PRE_CANCEL
|
||||
cruise_engaged = cruise_engaged && !tesla_autopark;
|
||||
|
||||
const bool cruise_engagement_started = cruise_engaged && !cruise_engaged_prev;
|
||||
const bool cruise_cancelled = cruise_engaged_prev && !cruise_engaged && !brake_pressed;
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
|
||||
acc_main_on = ((cruise_state == 1) || cruise_engaged) && !tesla_autopark;
|
||||
const bool acc_main_on_now = ((cruise_state == 1) || cruise_engaged) && !tesla_autopark;
|
||||
if (tesla_aol_screen_button) {
|
||||
if (cruise_engagement_started && !tesla_screen_cancel_pressed && !steering_disengage &&
|
||||
!(tesla_screen_disengage_on_brake && brake_pressed)) {
|
||||
lkas_on = true;
|
||||
}
|
||||
if (cruise_cancelled) {
|
||||
lkas_on = false;
|
||||
}
|
||||
}
|
||||
// Availability alone is not an engagement request on the screen-toggle path.
|
||||
acc_main_on = !tesla_aol_screen_button && acc_main_on_now;
|
||||
}
|
||||
|
||||
if (msg->addr == 0x155U) {
|
||||
@@ -187,7 +231,8 @@ static void tesla_rx_hook(const CANPacket_t *msg) {
|
||||
bool tesla_stock_lkas_now = steering_control_type == 2; // "LANE_KEEP_ASSIST"
|
||||
|
||||
// Only consider rising edges while controls are not allowed
|
||||
if (tesla_stock_lkas_now && !tesla_stock_lkas_prev && !controls_allowed) {
|
||||
if (tesla_stock_lkas_now && !tesla_stock_lkas_prev &&
|
||||
!(controls_allowed || (tesla_aol_screen_button && aol_allowed))) {
|
||||
tesla_stock_lkas = true;
|
||||
}
|
||||
if (!tesla_stock_lkas_now) {
|
||||
@@ -337,6 +382,12 @@ static safety_config tesla_init(uint16_t param) {
|
||||
};
|
||||
|
||||
SAFETY_UNUSED(param);
|
||||
const uint16_t TESLA_FLAG_AOL_SCREEN_BUTTON = 512;
|
||||
const uint16_t TESLA_FLAG_AOL_SCREEN_DISENGAGE_ON_BRAKE = 1024;
|
||||
tesla_aol_screen_button = GET_FLAG(param, TESLA_FLAG_AOL_SCREEN_BUTTON);
|
||||
tesla_screen_disengage_on_brake = tesla_aol_screen_button && GET_FLAG(param, TESLA_FLAG_AOL_SCREEN_DISENGAGE_ON_BRAKE);
|
||||
tesla_touch_initialized = false;
|
||||
tesla_screen_cancel_pressed = false;
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint16_t TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
|
||||
const uint16_t TESLA_FLAG_COOP_STEERING = 256;
|
||||
@@ -377,6 +428,7 @@ static safety_config tesla_init(uint16_t param) {
|
||||
|
||||
const safety_hooks tesla_hooks = {
|
||||
.init = tesla_init,
|
||||
.rx_all = tesla_rx_all_hook,
|
||||
.rx = tesla_rx_hook,
|
||||
.tx = tesla_tx_hook,
|
||||
.fwd = tesla_fwd_hook,
|
||||
|
||||
@@ -71,6 +71,27 @@ static uint16_t volvo_ecm_1_addr;
|
||||
static uint16_t volvo_bus1_cruise_control_addr;
|
||||
static bool volvo_c1;
|
||||
|
||||
#define VOLVO_STOCK_LCA5_FRAMES 4U
|
||||
#define VOLVO_STOCK_LCA5_MAX_AGE_US 100000U
|
||||
static uint8_t volvo_stock_lca5_data[VOLVO_STOCK_LCA5_FRAMES][8];
|
||||
static uint32_t volvo_stock_lca5_ts[VOLVO_STOCK_LCA5_FRAMES];
|
||||
static bool volvo_stock_lca5_valid[VOLVO_STOCK_LCA5_FRAMES];
|
||||
static uint8_t volvo_stock_lca5_index;
|
||||
|
||||
static bool volvo_lca5_stock_relay(const CANPacket_t *msg) {
|
||||
const uint32_t now = microsecond_timer_get();
|
||||
bool matches_stock = false;
|
||||
for (uint8_t i = 0U; i < VOLVO_STOCK_LCA5_FRAMES; i++) {
|
||||
bool matches = volvo_stock_lca5_valid[i] &&
|
||||
(safety_get_ts_elapsed(now, volvo_stock_lca5_ts[i]) <= VOLVO_STOCK_LCA5_MAX_AGE_US);
|
||||
for (uint8_t byte = 0U; byte < 8U; byte++) {
|
||||
matches &= msg->data[byte] == volvo_stock_lca5_data[i][byte];
|
||||
}
|
||||
matches_stock |= matches;
|
||||
}
|
||||
return matches_stock;
|
||||
}
|
||||
|
||||
static int volvo_be_15(const CANPacket_t *msg, uint8_t byte) {
|
||||
return (int)(((uint16_t)(msg->data[byte] & 0x7FU) << 8U) | msg->data[byte + 1U]);
|
||||
}
|
||||
@@ -159,6 +180,15 @@ static void volvo_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
// Main bus (bus 0) messages
|
||||
if (msg->bus == VOLVO_MAIN_BUS) {
|
||||
if (msg->addr == VOLVO_LCA_5) {
|
||||
for (uint8_t byte = 0U; byte < 8U; byte++) {
|
||||
volvo_stock_lca5_data[volvo_stock_lca5_index][byte] = msg->data[byte];
|
||||
}
|
||||
volvo_stock_lca5_ts[volvo_stock_lca5_index] = microsecond_timer_get();
|
||||
volvo_stock_lca5_valid[volvo_stock_lca5_index] = true;
|
||||
volvo_stock_lca5_index = (volvo_stock_lca5_index + 1U) % VOLVO_STOCK_LCA5_FRAMES;
|
||||
}
|
||||
|
||||
// Update brake pedal and cruise state from BCM2
|
||||
if (msg->addr == VOLVO_LCA_2) {
|
||||
// DBC: SG_ BRAKE_PEDAL_PRESSED_A : 47|1@0+ (-1,1) - inverted in DBC, so we invert raw bit
|
||||
@@ -263,9 +293,13 @@ static bool volvo_tx_hook(const CANPacket_t *msg) {
|
||||
// LCA frame also contains an angle-shaped field, but the imported controller
|
||||
// deliberately leaves that field at the observed vehicle value.
|
||||
if (msg->addr == VOLVO_LCA_5) {
|
||||
const int desired_angle = volvo_lca_5_angle(msg);
|
||||
tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN;
|
||||
tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS);
|
||||
if (!controls_allowed && volvo_lca5_stock_relay(msg)) {
|
||||
desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -VOLVO_MAX_ANGLE_CAN, VOLVO_MAX_ANGLE_CAN);
|
||||
} else {
|
||||
const int desired_angle = volvo_lca_5_angle(msg);
|
||||
tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN;
|
||||
tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS);
|
||||
}
|
||||
}
|
||||
|
||||
// Keep the two torque-authority arms and the companion LCA angle bounded even
|
||||
@@ -357,6 +391,10 @@ static bool volvo_tx_hook(const CANPacket_t *msg) {
|
||||
static safety_config volvo_init(uint16_t param) {
|
||||
bool spa = GET_FLAG(param, VOLVO_FLAG_SPA);
|
||||
volvo_c1 = GET_FLAG(param, VOLVO_FLAG_C1);
|
||||
volvo_stock_lca5_index = 0U;
|
||||
for (uint8_t i = 0U; i < VOLVO_STOCK_LCA5_FRAMES; i++) {
|
||||
volvo_stock_lca5_valid[i] = false;
|
||||
}
|
||||
|
||||
if (volvo_c1) {
|
||||
static const CanMsg VOLVO_C1_TX_MSGS[] = {
|
||||
|
||||
@@ -502,6 +502,45 @@ class TestFordMachEExtendedCurvatureSafety(TestFordCANFDStockSafety):
|
||||
self._reset_curvature_measurement(0.02, 9.0)
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.055, 0.02, 0.0)))
|
||||
|
||||
def test_mach_e_aol_keeps_curvature_without_path_angle_assist(self):
|
||||
self.safety.set_alternative_experience(32)
|
||||
self._rx(self._toggle_aol(True))
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self._tx(self._extended_lka_msg()))
|
||||
for sign in (-1, 1):
|
||||
self._reset_curvature_measurement(sign * 0.02, 7.5)
|
||||
self._set_prev_desired_angle(sign * 0.02)
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0)))
|
||||
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, sign * 0.02, 0.0)))
|
||||
|
||||
def test_mach_e_accelerator_assist_requires_retained_engagement(self):
|
||||
for sign in (-1, 1):
|
||||
self.setUp()
|
||||
self.safety.set_alternative_experience(32)
|
||||
self._rx(self._toggle_aol(True))
|
||||
self._reset_curvature_measurement(sign * 0.02, 7.5)
|
||||
self._set_prev_desired_angle(sign * 0.02)
|
||||
self._rx(self._pcm_status_msg(False))
|
||||
self._rx(self._pcm_status_msg(True))
|
||||
self.assertTrue(self._tx(self._extended_lka_msg()))
|
||||
for gas in (0.0, 25.0, 0.0):
|
||||
self._rx(self._user_gas_msg(gas))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0)))
|
||||
|
||||
self._rx(self._toggle_aol(True))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0)))
|
||||
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, sign * 0.02, 0.0)))
|
||||
|
||||
self._rx(self._pcm_status_msg(True))
|
||||
self._rx(self._user_brake_msg(True))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0)))
|
||||
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, sign * 0.02, 0.0)))
|
||||
|
||||
def test_other_canfd_fords_keep_original_error(self):
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD)
|
||||
self.safety.init_tests()
|
||||
|
||||
@@ -992,6 +992,22 @@ class TestSportageNoStockLka(unittest.TestCase):
|
||||
"Damping_Gain": 100,
|
||||
})
|
||||
|
||||
def test_stock_scc_buttons_require_engagement(self):
|
||||
resume = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.RESUME})
|
||||
set_button = self.packer.make_can_msg_safety("CRUISE_BUTTONS", 1, {"CRUISE_BUTTONS": Buttons.SET})
|
||||
self.assertFalse(self.safety.safety_tx_hook(resume))
|
||||
self.assertFalse(self.safety.safety_tx_hook(set_button))
|
||||
|
||||
self.safety.safety_rx_hook(set_button)
|
||||
self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 1}))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self.safety.safety_tx_hook(resume))
|
||||
self.assertTrue(self.safety.safety_tx_hook(set_button))
|
||||
|
||||
self.safety.safety_rx_hook(self.packer.make_can_msg_safety("SCC_CONTROL", 1, {"ACCMode": 0}))
|
||||
self.assertFalse(self.safety.safety_tx_hook(resume))
|
||||
self.assertFalse(self.safety.safety_tx_hook(set_button))
|
||||
|
||||
def test_aol_toggle_keeps_stock_blocked_and_inactive_status_allowed(self):
|
||||
self._speed(30)
|
||||
for expected_aol in (False, True, False, True, False):
|
||||
|
||||
@@ -0,0 +1,257 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.tesla.values import TeslaSafetyFlags
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from opendbc.safety.tests.common import CANPackerSafety, make_msg
|
||||
from opendbc.safety.tests import test_tesla
|
||||
|
||||
|
||||
class TestTeslaScreenButton(unittest.TestCase):
|
||||
TX_MSGS = None
|
||||
|
||||
def setUp(self):
|
||||
self.helper = test_tesla.TestTeslaStockSafety()
|
||||
self.helper.setUp()
|
||||
self.safety = self.helper.safety
|
||||
self.vehicle_packer = CANPackerSafety("tesla_model3_vehicle")
|
||||
self.init_safety()
|
||||
|
||||
def init_safety(self, flag=True, aol=True, long_control=False, disengage_on_brake=False, cooperative=False):
|
||||
param = (TeslaSafetyFlags.AOL_SCREEN_BUTTON if flag else 0) | (TeslaSafetyFlags.LONG_CONTROL if long_control else 0)
|
||||
if cooperative:
|
||||
param |= TeslaSafetyFlags.COOP_STEERING
|
||||
if disengage_on_brake:
|
||||
param |= TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE
|
||||
self.assertEqual(self.safety.set_safety_hooks(CarParams.SafetyModel.tesla, param), 0)
|
||||
self.safety.init_tests()
|
||||
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL if aol else 0)
|
||||
self.helper._rx(self.helper._pcm_status_msg(False))
|
||||
self.safety.set_angle_meas(0, 0)
|
||||
self.safety.set_desired_angle_last(0)
|
||||
|
||||
def touch(self, fingers, bus=1):
|
||||
self.helper._rx(self.vehicle_packer.make_can_msg_safety("UI_status2", bus, {"UI_activeTouchPoints": fingers}))
|
||||
|
||||
def gesture(self):
|
||||
self.touch(0)
|
||||
self.touch(3)
|
||||
|
||||
def stalk(self, status, bus=1):
|
||||
self.helper._rx(self.vehicle_packer.make_can_msg_safety("SCCM_rightStalk", bus, {"SCCM_rightStalkStatus": status}))
|
||||
|
||||
def test_low_speed_acc_availability_does_not_clear_gesture(self):
|
||||
for cooperative in (False, True):
|
||||
for initial_available in (False, True):
|
||||
with self.subTest(cooperative=cooperative, initial_available=initial_available):
|
||||
self.init_safety(cooperative=cooperative)
|
||||
msg = self.helper.packer.make_can_msg_safety("DI_state", 0, {"DI_cruiseState": 1 if initial_available else 0})
|
||||
self.helper._rx(msg)
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.gesture()
|
||||
for available in (True, False) * 20:
|
||||
self.helper._rx(self.helper._speed_msg(3.0))
|
||||
self.helper._rx(self.helper._speed_msg_2(3.0))
|
||||
msg = self.helper.packer.make_can_msg_safety("DI_state", 0, {"DI_cruiseState": 1 if available else 0})
|
||||
self.helper._rx(msg)
|
||||
self.assertTrue(self.safety.get_lkas_on())
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
self.gesture()
|
||||
for available in (True, False) * 20:
|
||||
msg = self.helper.packer.make_can_msg_safety("DI_state", 0, {"DI_cruiseState": 1 if available else 0})
|
||||
self.helper._rx(msg)
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
|
||||
def test_cruise_engagement_arms_lateral_and_brake_respects_option(self):
|
||||
for disengage_on_brake in (False, True):
|
||||
with self.subTest(disengage_on_brake=disengage_on_brake):
|
||||
self.init_safety(disengage_on_brake=disengage_on_brake)
|
||||
self.helper._rx(self.helper._pcm_status_msg(True))
|
||||
self.assertTrue(self.safety.get_lkas_on())
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
self.helper._rx(self.helper._user_brake_msg(True))
|
||||
self.helper._rx(self.helper._pcm_status_msg(False))
|
||||
self.helper._rx(self.helper._user_brake_msg(False))
|
||||
self.assertEqual(self.safety.get_lkas_on(), not disengage_on_brake)
|
||||
self.assertEqual(self.safety.get_aol_allowed(), not disengage_on_brake)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_cruise_engagement_does_not_override_screen_disengagement_inputs(self):
|
||||
for blocked_by in ("brake", "steering", "cancel"):
|
||||
with self.subTest(blocked_by=blocked_by):
|
||||
self.init_safety(disengage_on_brake=True)
|
||||
if blocked_by == "brake":
|
||||
self.helper._rx(self.helper._user_brake_msg(True))
|
||||
elif blocked_by == "steering":
|
||||
self.helper._rx(self.helper._angle_meas_msg(0, hands_on_level=3))
|
||||
else:
|
||||
self.stalk(1)
|
||||
self.helper._rx(self.helper._pcm_status_msg(True))
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
|
||||
def test_stalk_cancel_clears_gesture_and_blocks_tap_while_held(self):
|
||||
for status in (1, 2):
|
||||
with self.subTest(status=status):
|
||||
self.init_safety()
|
||||
self.gesture()
|
||||
self.stalk(status)
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
self.gesture()
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
self.stalk(0)
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
self.gesture()
|
||||
self.assertTrue(self.safety.get_lkas_on())
|
||||
|
||||
def test_non_cancel_stalk_positions_do_not_clear_gesture(self):
|
||||
self.gesture()
|
||||
for status in (0, 3, 4, 5, 6, 7):
|
||||
self.stalk(status)
|
||||
self.assertTrue(self.safety.get_lkas_on())
|
||||
|
||||
def test_stalk_cancel_is_addon_only(self):
|
||||
self.gesture()
|
||||
for bus in (0, 2, 3):
|
||||
self.stalk(1, bus)
|
||||
self.assertTrue(self.safety.get_lkas_on())
|
||||
for length in (0, 1, 2, 4, 5, 8):
|
||||
msg = make_msg(1, 0x229, length)
|
||||
if length > 1:
|
||||
msg[0].data[1] = 0x10
|
||||
self.helper._rx(msg)
|
||||
self.assertTrue(self.safety.get_lkas_on())
|
||||
self.init_safety(flag=False)
|
||||
self.helper._rx(self.helper._pcm_status_msg(True))
|
||||
self.stalk(1)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_gesture_enables_only_lateral(self):
|
||||
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
self.gesture()
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
self.assertTrue(self.safety.get_lkas_on())
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
self.assertFalse(self.helper._tx(self.helper._long_control_msg(10, acc_state=2, accel_limits=(0, 1))))
|
||||
self.gesture()
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
|
||||
def test_does_not_enable_alpha_long_actuation(self):
|
||||
self.init_safety(long_control=True)
|
||||
self.gesture()
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertFalse(self.helper._tx(self.helper._long_control_msg(10, acc_state=2, accel_limits=(0, 1))))
|
||||
|
||||
def test_flag_and_aol_are_required(self):
|
||||
for flag, aol in ((False, False), (False, True), (True, False)):
|
||||
with self.subTest(flag=flag, aol=aol):
|
||||
self.init_safety(flag=flag, aol=aol)
|
||||
self.gesture()
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
|
||||
def test_wrong_bus_and_length_do_not_toggle(self):
|
||||
for bus in (0, 2, 3):
|
||||
self.gesture_on_wrong_bus(bus)
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
self.touch(0)
|
||||
for length in (0, 3, 4, 5, 6, 7):
|
||||
msg = make_msg(1, 0x3DF, length)
|
||||
if length > 3:
|
||||
msg[0].data[3] = 3
|
||||
self.helper._rx(msg)
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
|
||||
def gesture_on_wrong_bus(self, bus):
|
||||
self.touch(0, bus)
|
||||
self.touch(3, bus)
|
||||
|
||||
def test_held_touch_and_other_finger_counts(self):
|
||||
for fingers in (3, 3, 0, 1, 2, 4, 5, 255, 0):
|
||||
self.touch(fingers)
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
self.touch(3)
|
||||
for _ in range(20):
|
||||
self.touch(3)
|
||||
self.assertTrue(self.safety.get_lkas_on())
|
||||
self.touch(0)
|
||||
self.touch(3)
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
|
||||
def test_brake_disables_long_but_not_gesture_lateral(self):
|
||||
self.gesture()
|
||||
self.helper._rx(self.helper._user_brake_msg(True))
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
|
||||
def test_opt_in_brake_disengage_needs_brake_release_and_new_gesture(self):
|
||||
self.init_safety(disengage_on_brake=True)
|
||||
self.gesture()
|
||||
self.helper._rx(self.helper._user_brake_msg(True))
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.gesture()
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.helper._rx(self.helper._user_brake_msg(False))
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.gesture()
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
|
||||
def test_steering_override_requires_a_new_gesture(self):
|
||||
self.gesture()
|
||||
self.helper._rx(self.helper._angle_meas_msg(0, hands_on_level=3))
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.gesture()
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.helper._rx(self.helper._angle_meas_msg(0))
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.gesture()
|
||||
self.assertTrue(self.safety.get_aol_allowed())
|
||||
|
||||
def test_cancelled_cruise_clears_gesture_latch(self):
|
||||
self.gesture()
|
||||
self.helper._rx(self.helper._pcm_status_msg(True))
|
||||
self.helper._rx(self.helper._pcm_status_msg(False))
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self.safety.get_lkas_on())
|
||||
|
||||
def test_safety_reinitialization_clears_touch_latch(self):
|
||||
self.gesture()
|
||||
self.init_safety()
|
||||
self.touch(3)
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
|
||||
def test_stock_lkas_does_not_block_authorized_gesture_steering(self):
|
||||
self.gesture()
|
||||
self.helper._rx(self.helper._angle_cmd_msg(0, state=2, bus=2))
|
||||
self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
|
||||
def test_stock_lkas_still_blocks_when_not_authorized(self):
|
||||
self.helper._rx(self.helper._angle_cmd_msg(0, state=2, bus=2))
|
||||
self.gesture()
|
||||
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
|
||||
def test_autopark_still_blocks_steering(self):
|
||||
self.helper._rx(self.helper._pcm_status_msg(False, autopark_state=3))
|
||||
self.gesture()
|
||||
self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0)))
|
||||
|
||||
def test_aeb_still_blocks_longitudinal_and_is_forwarded(self):
|
||||
self.init_safety(long_control=True)
|
||||
self.gesture()
|
||||
self.helper._rx(self.helper._long_control_msg(0, aeb_event=1, bus=2))
|
||||
self.assertFalse(self.helper._tx(self.helper._long_control_msg(0, acc_state=13)))
|
||||
self.assertEqual(self.safety.safety_fwd_hook(2, 0x2B9), 0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -177,6 +177,68 @@ class TestVolvoSafetyBase(common.CarSafetyTest):
|
||||
self.assertTrue(self._tx(self._angle_cmd_msg(10)))
|
||||
self.assertFalse(self._tx(self._angle_cmd_msg(20)))
|
||||
|
||||
STOCK_LCA5 = bytes.fromhex("88c04cef1190ba00")
|
||||
|
||||
def _stock_lca5(self, data=None, bus=VOLVO_PARTY_BUS):
|
||||
return libsafety_py.make_CANPacket(VOLVO_LCA_5, bus, self.STOCK_LCA5 if data is None else data)
|
||||
|
||||
def test_inactive_lca5_stock_relay_requires_unchanged_received_frame(self):
|
||||
self._reset_angle_measurement(10)
|
||||
self.safety.set_timer(100000)
|
||||
self.assertFalse(self._tx(self._stock_lca5()))
|
||||
self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
|
||||
self.assertTrue(self._tx(self._stock_lca5()))
|
||||
self.assertEqual(self.safety.get_desired_angle_last(), round(10 / 0.05596))
|
||||
|
||||
for byte in range(8):
|
||||
altered = bytearray(self.STOCK_LCA5)
|
||||
altered[byte] ^= 1
|
||||
self.assertFalse(self._tx(self._stock_lca5(bytes(altered))), f"altered {byte=}")
|
||||
self.assertFalse(self._tx(self._stock_lca5(bus=VOLVO_MAIN_BUS)))
|
||||
self.assertFalse(self._tx(self._stock_lca5(bus=VOLVO_PT_BUS)))
|
||||
|
||||
def test_inactive_lca5_stock_relay_wrong_rx_bus_and_length(self):
|
||||
self._rx(self._stock_lca5(bus=VOLVO_PT_BUS))
|
||||
self._rx(self._stock_lca5(self.STOCK_LCA5[:7], bus=VOLVO_MAIN_BUS))
|
||||
self.assertFalse(self._tx(self._stock_lca5()))
|
||||
|
||||
def test_inactive_lca5_stock_relay_expires_across_timer_wrap(self):
|
||||
start = 0xFFFF0000
|
||||
self.safety.set_timer(start)
|
||||
self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
|
||||
self.safety.set_timer((start + 100000) & 0xFFFFFFFF)
|
||||
self.assertTrue(self._tx(self._stock_lca5()))
|
||||
self.safety.set_timer((start + 100001) & 0xFFFFFFFF)
|
||||
self.assertFalse(self._tx(self._stock_lca5()))
|
||||
|
||||
def test_inactive_lca5_stock_relay_history_is_bounded(self):
|
||||
frames = []
|
||||
for i in range(5):
|
||||
data = bytearray(self.STOCK_LCA5)
|
||||
data[3] = (data[3] + i) & 0xFF
|
||||
frames.append(bytes(data))
|
||||
self.safety.set_timer(i * 20000)
|
||||
self._rx(self._stock_lca5(frames[-1], bus=VOLVO_MAIN_BUS))
|
||||
self.assertFalse(self._tx(self._stock_lca5(frames[0])))
|
||||
for data in frames[1:]:
|
||||
self.assertTrue(self._tx(self._stock_lca5(data)))
|
||||
|
||||
def test_inactive_lca5_stock_relay_cleared_on_safety_init(self):
|
||||
self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
|
||||
self.assertTrue(self._tx(self._stock_lca5()))
|
||||
self.safety.set_safety_hooks(SAFETY_VOLVO, self.SAFETY_PARAM)
|
||||
self.assertFalse(self._tx(self._stock_lca5()))
|
||||
|
||||
def test_stock_lca5_placeholder_does_not_bypass_active_angle_limits(self):
|
||||
self._reset_angle_measurement(10)
|
||||
self._reset_speed_measurement(50)
|
||||
self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
|
||||
self.assertTrue(self._tx(self._stock_lca5()))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.assertFalse(self._tx(self._stock_lca5()))
|
||||
self.assertTrue(self._tx(self._angle_cmd_msg(10.4)))
|
||||
self.assertFalse(self._tx(self._angle_cmd_msg(11)))
|
||||
|
||||
def test_angle_tx_rate_matches_controller_cadence(self):
|
||||
"""LCA_5 is 50 Hz, so each frame may contain two 100 Hz controller steps."""
|
||||
self._reset_speed_measurement(50)
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-96ef704d-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-956aceaf-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user