mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-01 20:03:51 +08:00
Compare commits
145 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 5a9deaca5f | |||
| 673ca37396 | |||
| 44beb5b778 | |||
| 00ac287223 | |||
| 09b53ccf9f | |||
| 0fee545400 | |||
| 14370fe9cf | |||
| 7316871e62 | |||
| 239121b0e1 | |||
| 26de11932a | |||
| 1f8b955a0f | |||
| b41b0ab95f | |||
| a8d1f2318e | |||
| dac7140410 | |||
| 0cf86c5c3b | |||
| d3ec77b0b4 | |||
| 814af739d0 | |||
| 68b75fc51e | |||
| 7ff3682aba | |||
| 91052ea0e1 | |||
| 20f38f3d8e | |||
| cdaf33529a | |||
| 6e37c0917c | |||
| 1db1ff9b91 | |||
| 3ba36ed4fc | |||
| cbe6f39030 | |||
| 6aa9abf046 | |||
| 9332886242 | |||
| c3e4ec630f | |||
| 65c8581db3 | |||
| 9136e13fdf | |||
| 9e5a3e288b | |||
| 2878d13c3d | |||
| 16ec6bc5c9 | |||
| 1d6d0cb5ba | |||
| 200ac08499 | |||
| 71649a2ac1 | |||
| fc852fed06 | |||
| 7222b29a88 | |||
| 2d3f483f3a | |||
| b4dbc18a14 | |||
| 8d73b7b679 | |||
| b640bbc20e | |||
| 9d0ab7a849 | |||
| 6f3d863ecd | |||
| ab6c97fef8 | |||
| a51205e302 | |||
| 64f8b75551 | |||
| 497b906121 | |||
| 04ba07e706 | |||
| ad1c970cdd | |||
| 7a7b391656 | |||
| 688631b6bd | |||
| 9677a3bd78 | |||
| 4d585dbcbc | |||
| f88ef758ca | |||
| c4f46c51b2 | |||
| 6b8bb279d4 | |||
| c51b96879a | |||
| 524cffa19c | |||
| 549c12cd1b | |||
| f6664f5466 | |||
| 151b07462c | |||
| 3506c2561b | |||
| 5bead81598 | |||
| 01ae511879 | |||
| 8ccaacb919 | |||
| 38c98cf9f7 | |||
| 737c8499eb | |||
| 12251b663a | |||
| ed008e2bf3 | |||
| 024465323b | |||
| 32381eb182 | |||
| 3f4d1e4fcc | |||
| 50a8d1abdb | |||
| 185c501809 | |||
| eb088bccfa | |||
| d0585de42d | |||
| e517a83541 | |||
| c7b9a77782 | |||
| 07dc300d5b | |||
| 59d3c4dd66 | |||
| d6712e2a10 | |||
| be263a6fa1 | |||
| f1cb143bd7 | |||
| ecbd6362f7 | |||
| fbfadc65da | |||
| 334f32f5d8 | |||
| 52c61da75d | |||
| 08a11c445c | |||
| 14b15022eb | |||
| 340d225039 | |||
| a3d8c8948e | |||
| 1b1989f794 | |||
| d8a4be7e98 | |||
| b942e08f58 | |||
| 8eb46987ff | |||
| 46596218ba | |||
| 202ea33690 | |||
| 23821bad24 | |||
| 49940935a9 | |||
| 14da2ebbd9 | |||
| 29dbf8d084 | |||
| b67bc26763 | |||
| 943c739521 | |||
| 85703712df | |||
| 88b3b3eeea | |||
| ab9011c825 | |||
| 91535cc086 | |||
| b135a43d97 | |||
| 0b8a9a503b | |||
| bf00f88be4 | |||
| 58d2b6838d | |||
| 9832c3de4f | |||
| 24b8789c79 | |||
| 01dd14bfae | |||
| bb04e93527 | |||
| 244aa67371 | |||
| dd607aa07d | |||
| 79d73d6ed4 | |||
| c48247ddb0 | |||
| 22707891bd | |||
| 962d8b6719 | |||
| 0f3bdb34b3 | |||
| c2921c1a8f | |||
| a19beda327 | |||
| b7775991bf | |||
| eedd73e522 | |||
| 50e2c21dbd | |||
| 54c3fb13f3 | |||
| 7f3bd61292 | |||
| dec4a0884a | |||
| 4edc8ab86a | |||
| b3a14cb48d | |||
| f47322cbee | |||
| a08065f282 | |||
| a33bec1ca4 | |||
| ca3d8a3816 | |||
| 2360ff9b0f | |||
| 0976fd804d | |||
| 2504441a4e | |||
| 0b5ccb31e1 | |||
| b91ea3e1da | |||
| 1588f7041a | |||
| bcf152e6f7 |
@@ -27,6 +27,10 @@ add_panda_targets() {
|
||||
panda_h7_remote_can_ignition_only
|
||||
panda_hkg_remote_can_ignition_only
|
||||
panda_h7_hkg_remote_can_ignition_only
|
||||
panda_tesla_wake
|
||||
panda_h7_tesla_wake
|
||||
panda_tesla_wake_can_ignition_only
|
||||
panda_h7_tesla_wake_can_ignition_only
|
||||
panda_jungle_h7
|
||||
body_h7
|
||||
)
|
||||
|
||||
@@ -14,6 +14,7 @@ using Car = import "car.capnp";
|
||||
|
||||
struct StarPilotCarControl @0x81c2f05a394cf4af {
|
||||
hudControl @0 :HUDControl;
|
||||
steeringLimitInfo @1 :SteeringLimitInfo;
|
||||
|
||||
struct HUDControl {
|
||||
audibleAlert @0 :AudibleAlert;
|
||||
@@ -49,6 +50,16 @@ struct StarPilotCarControl @0x81c2f05a394cf4af {
|
||||
uwu @22;
|
||||
}
|
||||
}
|
||||
|
||||
struct SteeringLimitInfo {
|
||||
valid @0 :Bool;
|
||||
modelLimitErrorDeg @1 :Float32;
|
||||
resumeLimitErrorDeg @2 :Float32;
|
||||
cooperativeLimitErrorDeg @3 :Float32;
|
||||
cooperativeOffsetDeg @4 :Float32;
|
||||
monoTime @5 :UInt64;
|
||||
combinedLimitErrorDeg @6 :Float32;
|
||||
}
|
||||
}
|
||||
|
||||
struct StarPilotCarParams @0xaedffd8f31e7b55d {
|
||||
|
||||
Binary file not shown.
@@ -152,7 +152,8 @@ class FrequencyTracker:
|
||||
class SubMaster:
|
||||
def __init__(self, services: List[str], poll: Optional[str] = None,
|
||||
ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None,
|
||||
ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", frequency: Optional[float] = None):
|
||||
ignore_valid: Optional[list[str]] = None, addr: str = "127.0.0.1", frequency: Optional[float] = None,
|
||||
drain_services: list[str] | None = None):
|
||||
self.frame = -1
|
||||
self.services = services
|
||||
self.seen = {s: False for s in services}
|
||||
@@ -160,6 +161,9 @@ class SubMaster:
|
||||
self.recv_time = {s: 0. for s in services}
|
||||
self.recv_frame = {s: 0 for s in services}
|
||||
self.sock = {}
|
||||
self.drained = {s: [] for s in (drain_services or [])}
|
||||
if not self.drained.keys() <= set(services):
|
||||
raise ValueError("Drained services must be subscribed")
|
||||
self.data = {}
|
||||
self.logMonoTime = {s: 0 for s in services}
|
||||
|
||||
@@ -187,7 +191,7 @@ class SubMaster:
|
||||
|
||||
for s in services:
|
||||
p = self.poller if s not in self.non_polled_services else None
|
||||
self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=True)
|
||||
self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=s not in self.drained)
|
||||
|
||||
try:
|
||||
data = new_message(s)
|
||||
@@ -207,14 +211,28 @@ class SubMaster:
|
||||
def _check_avg_freq(self, s: str) -> bool:
|
||||
return SERVICE_LIST[s].frequency > 0.99 and (s not in self.ignore_average_freq) and (s not in self.ignore_alive)
|
||||
|
||||
def _recv_socket(self, sock):
|
||||
message = recv_one_or_none(sock)
|
||||
if not self.drained or message is None:
|
||||
return message
|
||||
# Native Poller returns fresh socket wrappers; identify the service by data.
|
||||
service = message.which()
|
||||
if service not in self.drained:
|
||||
return message
|
||||
# Preserve event edges for observers, but update state/frequency only once.
|
||||
self.drained[service] = [message, *drain_sock(sock)]
|
||||
return self.drained[service][-1]
|
||||
|
||||
def update(self, timeout: int = 100) -> None:
|
||||
for service in self.drained:
|
||||
self.drained[service] = []
|
||||
msgs = []
|
||||
for sock in self.poller.poll(timeout):
|
||||
msgs.append(recv_one_or_none(sock))
|
||||
msgs.append(self._recv_socket(sock))
|
||||
|
||||
# non-blocking receive for non-polled sockets
|
||||
for s in self.non_polled_services:
|
||||
msgs.append(recv_one_or_none(self.sock[s]))
|
||||
msgs.append(self._recv_socket(self.sock[s]))
|
||||
self.update_msgs(time.monotonic(), msgs)
|
||||
|
||||
def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStructReader]) -> None:
|
||||
@@ -262,6 +280,7 @@ class SubMaster:
|
||||
ignore_valid=self.ignore_valid,
|
||||
addr=self.addr,
|
||||
frequency=None if self.poll is not None else self.update_freq,
|
||||
drain_services=list(self.drained),
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
import random
|
||||
import time
|
||||
import pytest
|
||||
from typing import Sized, cast
|
||||
|
||||
import cereal.messaging as messaging
|
||||
@@ -16,6 +17,29 @@ class TestSubMaster:
|
||||
# sleep to prevent multiple publishers error between tests
|
||||
zmq_sleep(3)
|
||||
|
||||
@pytest.mark.parametrize("poll", [None, "deviceState"])
|
||||
def test_drain_preserves_short_events_with_native_socket_wrappers(self, poll):
|
||||
pub = messaging.PubMaster(["carState", "deviceState"])
|
||||
sm = messaging.SubMaster(["carState", "deviceState"], poll=poll, drain_services=["carState"])
|
||||
zmq_sleep()
|
||||
pressed = messaging.new_message("carState", valid=True)
|
||||
button = pressed.carState.init("buttonEvents", 1)[0]
|
||||
button.type, button.pressed = "accelCruise", True
|
||||
pub.send("carState", pressed)
|
||||
latest = messaging.new_message("carState", valid=True)
|
||||
latest.carState.vEgo = 12.0
|
||||
pub.send("carState", latest)
|
||||
pub.send("deviceState", messaging.new_message("deviceState", valid=True))
|
||||
sm.update(1000)
|
||||
assert len(sm.drained["carState"]) == 2
|
||||
assert sm.drained["carState"][0].carState.buttonEvents[0].pressed
|
||||
assert sm["carState"].vEgo == 12.0 and not sm["carState"].buttonEvents
|
||||
assert sm.logMonoTime["carState"] == latest.logMonoTime
|
||||
assert sm.frame == 0 and all(sm.updated.values())
|
||||
sm.update(0)
|
||||
assert sm.drained["carState"] == []
|
||||
assert sm.frame == 1 and not any(sm.updated.values())
|
||||
|
||||
def test_init(self):
|
||||
sm = messaging.SubMaster(events)
|
||||
for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive,
|
||||
|
||||
+2
-19
@@ -1,25 +1,8 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
def gm_car_params_present(params: Params, CP: car.CarParams | None = None) -> bool:
|
||||
if CP is not None and getattr(CP, "brand", None):
|
||||
return CP.brand == "gm"
|
||||
try:
|
||||
raw_car_params = params.get("CarParams")
|
||||
if raw_car_params is None:
|
||||
return False
|
||||
with car.CarParams.from_bytes(raw_car_params) as parsed_cp:
|
||||
return parsed_cp.brand == "gm"
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
|
||||
def get_gps_location_service(params: Params, CP: car.CarParams | None = None) -> str:
|
||||
# GM arbitrates device/PPS/OnStar through gpsLocationExternal.
|
||||
if gm_car_params_present(params, CP) or params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
|
||||
def get_gps_location_service(params: Params) -> str:
|
||||
if params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
|
||||
return "gpsLocationExternal"
|
||||
else:
|
||||
return "gpsLocation"
|
||||
|
||||
Binary file not shown.
+23
-2
@@ -18,6 +18,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"BootCount", {PERSISTENT, INT}},
|
||||
{"BluetoothAudioAddress", {PERSISTENT, STRING}},
|
||||
{"BluetoothAudioTestActive", {CLEAR_ON_MANAGER_START | DONT_LOG, BOOL}},
|
||||
{"BluetoothDisconnectControllersOffroad", {PERSISTENT, BOOL, "0"}},
|
||||
{"BluetoothEnabled", {PERSISTENT, BOOL, "0"}},
|
||||
{"CalibrationParams", {PERSISTENT, BYTES}},
|
||||
{"CameraDebugExpGain", {CLEAR_ON_MANAGER_START, STRING}},
|
||||
@@ -109,6 +110,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"LocationFilterInitialState", {PERSISTENT, BYTES}},
|
||||
{"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
|
||||
{"LongitudinalPersonality", {PERSISTENT, INT, std::to_string(static_cast<int>(cereal::LongitudinalPersonality::STANDARD))}},
|
||||
{"LongitudinalPersonalityProfiles", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
|
||||
{"NetworkMetered", {PERSISTENT, BOOL}},
|
||||
{"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
||||
{"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
||||
@@ -316,7 +318,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DeveloperSidebarMetric7", {PERSISTENT, INT, "7", "0", 3}},
|
||||
{"DeveloperUI", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"GalaxyDeveloperMode", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"GalaxyMobileDefault", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_ADVANCED}},
|
||||
{"GalaxyDeviceName", {PERSISTENT | DONT_LOG, STRING, "", "", 0}},
|
||||
{"GalaxyMobileDefault", {PERSISTENT | DONT_LOG, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},
|
||||
{"DeveloperWidgets", {PERSISTENT, BOOL, "1", "0", 3}},
|
||||
{"DeviceManagement", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"DeviceShutdown", {PERSISTENT, INT, "6", "6", 1, SETTINGS_SIMPLE}},
|
||||
@@ -350,6 +353,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DrivingModelVersion", {PERSISTENT, STRING, "v15", "v15", 1}},
|
||||
{"DynamicPathWidth", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"DynamicPedalsOnUI", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"GpuModelReadySound", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"EngageVolume", {PERSISTENT, INT, "101", "101", 2, SETTINGS_SIMPLE}},
|
||||
{"EVTuning", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"Fahrenheit", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
@@ -360,6 +364,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"IgnoreIgnitionLine", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"LongPitch", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"RemoteStartBootsComma", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"TeslaWakeOnCAN", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"RemapCancelToDistance", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"NAPAdaptiveAccel", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},
|
||||
{"NAPFollowDistance", {PERSISTENT, INT, "4", "4"}},
|
||||
@@ -440,6 +445,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"IncreaseFollowingRain", {PERSISTENT, FLOAT, "0.0", "0.0", 2, SETTINGS_SIMPLE}},
|
||||
{"IncreaseFollowingRainStorm", {PERSISTENT, FLOAT, "0.0", "0.0", 2, SETTINGS_SIMPLE}},
|
||||
{"IncreaseFollowingSnow", {PERSISTENT, FLOAT, "0.0", "0.0", 2, SETTINGS_SIMPLE}},
|
||||
{"AggressiveCoolingEnabled", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"IncreaseThermalLimits", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"IssueReported", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}},
|
||||
{"KonikDongleId", {PERSISTENT, STRING, "", "", 0}},
|
||||
@@ -463,7 +469,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"LeadDepartingAlert", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"LeadDetectionThreshold", {PERSISTENT, INT, "35", "50", 3}},
|
||||
{"LeadIndicator", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"LeadInfo", {PERSISTENT, BOOL, "1", "0", 3}},
|
||||
{"LeadInfo", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"LeadInfoMode", {PERSISTENT, INT, "2", "2", 3}},
|
||||
{"LKASButtonControl", {PERSISTENT, INT, "5", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"LockDoors", {PERSISTENT, BOOL, "1", "0", 0}},
|
||||
{"LockDoorsTimer", {PERSISTENT, INT, "0", "0", 0}},
|
||||
@@ -608,6 +615,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"RelaxedJerkDeceleration", {PERSISTENT, FLOAT, "100.0", "100.0", 3}},
|
||||
{"RelaxedJerkSpeed", {PERSISTENT, FLOAT, "100.0", "100.0", 3}},
|
||||
{"RelaxedJerkSpeedDecrease", {PERSISTENT, FLOAT, "100.0", "100.0", 3}},
|
||||
{"ReverseCruise", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"RivianAngleControl", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"RivianAngleSaturated", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL, "0", "0"}},
|
||||
{"RivianToiRecoveryFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL, "0", "0"}},
|
||||
@@ -617,6 +625,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"RotatingWheel", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"ScreenBrightness", {PERSISTENT, INT, "101", "101", 2, SETTINGS_SIMPLE}},
|
||||
{"ScreenBrightnessOnroad", {PERSISTENT, INT, "101", "101", 2, SETTINGS_SIMPLE}},
|
||||
{"ScreenBrightnessManual", {PERSISTENT, INT, "100", "100", 2, SETTINGS_SIMPLE}},
|
||||
{"ScreenBrightnessOffset", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"ScreenBrightnessOnroadManual", {PERSISTENT, INT, "100", "100", 2, SETTINGS_SIMPLE}},
|
||||
{"ScreenBrightnessOnroadOffset", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"ScreenManagement", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"ScreenRecorder", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"ScreenTimeout", {PERSISTENT, INT, "30", "30", 2, SETTINGS_SIMPLE}},
|
||||
@@ -626,6 +638,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SafeModeBackup", {PERSISTENT, JSON, "{}", "{}"}},
|
||||
{"SetSpeedLimit", {PERSISTENT, BOOL, "0", "0", 1}},
|
||||
{"SetSpeedOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 2, SETTINGS_SIMPLE}},
|
||||
{"ShowBrakeStatus", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"ShowCEMStatus", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"ShowCCMStatus", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"ShowCPU", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
@@ -687,6 +700,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"StandardJerkSpeed", {PERSISTENT, FLOAT, "100.0", "100.0", 3}},
|
||||
{"StandardJerkSpeedDecrease", {PERSISTENT, FLOAT, "100.0", "100.0", 3}},
|
||||
{"StandbyMode", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeButton", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyButtonPressTime", {CLEAR_ON_MANAGER_START | DONT_LOG, INT, "0", "0"}},
|
||||
{"StandbyWakeEngage", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeDisengage", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeInfoAlert", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeWarningAlert", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeCriticalAlert", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}},
|
||||
{"StandbyWakeTurnSignal", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"StartAccel", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
{"StartAccelStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
{"StartupMessageBottom", {PERSISTENT, STRING, "Always keep hands on wheel and eyes on road", "Always keep hands on wheel and eyes on road", 0}},
|
||||
|
||||
Binary file not shown.
@@ -5,7 +5,7 @@ import threading
|
||||
import time
|
||||
import uuid
|
||||
|
||||
from openpilot.common.params import Params, ParamKeyFlag, UnknownKeyName
|
||||
from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType, UnknownKeyName
|
||||
|
||||
class TestParams:
|
||||
def setup_method(self):
|
||||
@@ -128,6 +128,31 @@ class TestParams:
|
||||
assert self.params.get("LiveParameters") is None
|
||||
assert self.params.get("LiveParameters", return_default=True) is None
|
||||
|
||||
def test_longitudinal_personality_profiles_json_round_trip(self):
|
||||
key = "LongitudinalPersonalityProfiles"
|
||||
value = {
|
||||
"schemaVersion": 1,
|
||||
"enabled": False,
|
||||
"axes": {
|
||||
"acceleration": {
|
||||
"speed": {"unit": "mph", "values": [0.0, 11.184681, 22.369363, 33.554044, 44.738726, 55.923407, 89.477452]},
|
||||
"value": {"unit": "m/s^2", "meaning": "maximum_requested_acceleration"},
|
||||
},
|
||||
"braking": {
|
||||
"speed": {"unit": "mph", "values": [0.0, 11.184681, 22.369363, 33.554044, 44.738726, 55.923407, 89.477452]},
|
||||
"value": {"unit": "m/s^2", "meaning": "cruise_slc_deceleration_magnitude"},
|
||||
},
|
||||
"following": {"speed": {"unit": "mph", "values": [0, 10, 20, 30, 40, 50, 60, 70, 80, 90]}, "value": {"unit": "s", "meaning": "base_time_headway"}},
|
||||
},
|
||||
"profiles": {},
|
||||
}
|
||||
self.params.remove(key)
|
||||
|
||||
assert self.params.get_type(key) == ParamKeyType.JSON
|
||||
assert self.params.get(key) is None
|
||||
self.params.put(key, value)
|
||||
assert self.params.get(key) == value
|
||||
|
||||
def test_params_get_type(self):
|
||||
# json
|
||||
self.params.put("ApiCache_DriveStats", {"a": 0})
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
# Custom personality graphs
|
||||
|
||||
Each personality keeps its own Custom acceleration, braking and following curve.
|
||||
Selecting a named preset changes the active selection without deleting Custom
|
||||
points. Selecting Custom again restores those points, including after a reload
|
||||
or restart. If a category has never had Custom points, it is initialized from
|
||||
the current selection, as before.
|
||||
|
||||
The existing **Reset to default** button, below each Custom graph's numeric
|
||||
points in New Galaxy's Advanced section, replaces only that category's Custom
|
||||
curve. It leaves the category set to Custom. The server resolves the reset
|
||||
values; the dashed **Dom default** line uses the same resolver.
|
||||
|
||||
Defaults are Dom's configured base curves sampled at the editor's 10 mph
|
||||
points. They include Traffic's dedicated acceleration and braking, following
|
||||
settings, global tuning switches and powertrain overrides. Where gear mapping
|
||||
is enabled, the reference uses normal gear. Live Eco/Sport gear, weather,
|
||||
lead/stop and overspeed adjustments remain on the existing controller paths.
|
||||
Sampling cannot reproduce every native breakpoint or between-point value;
|
||||
resetting a Custom graph is not the same as delegating to the Dom-default
|
||||
runtime path.
|
||||
|
||||
Dom-default points outside the ordinary editor range (such as Traffic braking
|
||||
at 0.35 m/s², configured Traffic following at 0.5 seconds or truck acceleration
|
||||
at 6 m/s²) remain visible and are preserved when another point is edited.
|
||||
New point edits still use the existing authoring bounds. This does not expand
|
||||
braking authority or change acceleration/braking preset definitions.
|
||||
|
||||
## Following presets
|
||||
|
||||
Named following presets now match Dom's factory following settings with custom
|
||||
personalities enabled. Close follows Aggressive, Medium follows Standard and
|
||||
Far follows Relaxed. The presets are available in every personality.
|
||||
|
||||
| Preset | Previous curve | Revised curve |
|
||||
| --- | --- | --- |
|
||||
| Close | 1.25 s at every speed | 1.25 s through 45 mph, falling to 1.0 s at 70 mph |
|
||||
| Medium | 1.45 s at every speed | 1.45 s through 45 mph, falling to 1.2 s at 70 mph |
|
||||
| Far | 1.75 s at every speed | 1.6 s through 45 mph, falling to 1.4 s at 70 mph |
|
||||
| Traffic | No named preset | 0.75 s at rest, rising to 1.6 s at 25 m/s (55.92 mph) |
|
||||
|
||||
Interpolation is linear between the stated breakpoints and constant outside
|
||||
them. Named presets use the exact native speed axes at runtime. First-use
|
||||
Custom conversion samples them onto the existing 10 mph editor grid.
|
||||
|
||||
Existing v1/v2 Close, Medium and Far selections keep their old fixed headways
|
||||
as `legacy_close`, `legacy_medium` and `legacy_far`. Both Galaxy pickers show
|
||||
the selected compatibility entry as **Previous Close**, **Previous Medium**
|
||||
or **Previous Far**. Explicitly selecting a current preset adopts its new curve.
|
||||
The previous entry disappears when it is no longer selected.
|
||||
|
||||
Existing `dom_default` selections continue to inherit configured settings;
|
||||
they are not silently converted to fixed named presets. Fresh profiles also
|
||||
retain this inheritance. The named curves match untouched factory settings;
|
||||
users' changed global following values can still differ from them.
|
||||
|
||||
Acceleration and braking presets are unchanged. Standard acceleration and Eco
|
||||
braking match the normal factory defaults for Aggressive, Standard and Relaxed
|
||||
when named-preset and global powertrain tuning agree. Named presets use detected
|
||||
EV/truck tuning; the Dom-default resolver respects the global tuning switches,
|
||||
so these can differ. Traffic retains its dedicated acceleration/braking defaults;
|
||||
this change adds only its named following preset.
|
||||
|
||||
## Storage compatibility
|
||||
|
||||
Profile document version 3 retains `curve` and optional `legacyCurve` while
|
||||
`preset` is a named preset or `dom_default`. These retained values are dormant;
|
||||
only Custom uses them. An actual graph edit or reset retires preserved v1
|
||||
interpolation for that category; a preset switch or unchanged submission does
|
||||
not.
|
||||
|
||||
Valid v2 documents retain their runtime meaning and are upgraded on the next
|
||||
normal write, including the fixed following compatibility names above.
|
||||
Version 1 keeps its existing explicit, verified migration flow. Reads never
|
||||
rewrite Params. Category conflict detection, off-road checks and atomic profile
|
||||
document writes still apply to edits and resets.
|
||||
|
||||
Older builds do not understand v3 documents. Retain a compatible settings
|
||||
backup before rolling back to one of those builds. Curves discarded before
|
||||
this change cannot be recovered automatically.
|
||||
@@ -1,3 +1,4 @@
|
||||
include opendbc/car/car.capnp
|
||||
include opendbc/car/include/c++.capnp
|
||||
include opendbc/dbc/hyundai_kia_ray_pedal.dbc
|
||||
recursive-include opendbc/safety *.h
|
||||
|
||||
@@ -13,7 +13,7 @@ from opendbc.car.subaru.subarucan import subaru_checksum
|
||||
from opendbc.car.chrysler.chryslercan import chrysler_checksum, fca_giorgio_checksum
|
||||
from opendbc.car.hyundai.hyundaicanfd import hkg_can_fd_checksum
|
||||
from opendbc.car.volkswagen.mlbcan import volkswagen_mlb_checksum
|
||||
from opendbc.car.volkswagen.mqbcan import volkswagen_mqb_meb_checksum, xor_checksum
|
||||
from opendbc.car.volkswagen.mqbcan import volkswagen_meb_alt_crc_checksum, volkswagen_mqb_meb_checksum, xor_checksum
|
||||
from opendbc.car.tesla.teslacan import tesla_checksum
|
||||
from opendbc.car.body.bodycan import body_checksum
|
||||
from opendbc.car.psa.psacan import psa_checksum
|
||||
@@ -194,8 +194,10 @@ def get_checksum_state(dbc_name: str) -> ChecksumState | None:
|
||||
return ChecksumState(4, 2, 3, 5, False, SignalType.HONDA_CHECKSUM, honda_checksum)
|
||||
elif dbc_name.startswith(("toyota_", "lexus_")):
|
||||
return ChecksumState(8, -1, 7, -1, False, SignalType.TOYOTA_CHECKSUM, toyota_checksum)
|
||||
elif dbc_name.startswith("hyundai_canfd_generated"):
|
||||
elif dbc_name.startswith(("hyundai_canfd_generated", "hyundai_radar_210_21f_generated")):
|
||||
return ChecksumState(16, -1, 0, -1, True, SignalType.HKG_CAN_FD_CHECKSUM, hkg_can_fd_checksum)
|
||||
elif dbc_name.startswith("vw_meb_2024"):
|
||||
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_meb_alt_crc_checksum)
|
||||
elif dbc_name.startswith(("vw_mqb", "vw_mqbevo", "vw_meb")):
|
||||
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_mqb_meb_checksum)
|
||||
elif dbc_name.startswith("vw_mlb"):
|
||||
|
||||
@@ -90,6 +90,7 @@ class Bus(StrEnum):
|
||||
main = auto()
|
||||
party = auto()
|
||||
ap_party = auto()
|
||||
ap_pt = auto()
|
||||
|
||||
|
||||
def rate_limit(new_value, last_value, dw_step, up_step):
|
||||
|
||||
@@ -56,6 +56,20 @@ GM_CANDIDATE_PREFIXES = ("CHEVROLET_", "GMC_", "CADILLAC_", "BUICK_", "HOLDEN_")
|
||||
GM_CORE_FINGERPRINT_MSGS = frozenset((190, 201, 209, 211, 241))
|
||||
GM_CAMERA_BUS = 2
|
||||
GM_VOLT_CAMERA_MSG = 0x320
|
||||
GM_SUBURBAN_CAMERA_VIN_PREFIX = "1GNSKJKJ"
|
||||
GM_SUBURBAN_CAMERA_PT_SIGNATURE = {
|
||||
190: 6,
|
||||
201: 8,
|
||||
209: 7,
|
||||
211: 2,
|
||||
241: 6,
|
||||
304: 1,
|
||||
320: 3,
|
||||
}
|
||||
GM_CAMERA_DIAGNOSTIC_MESSAGES = {
|
||||
0x24b: 8,
|
||||
0x64b: 8,
|
||||
}
|
||||
|
||||
|
||||
def _normalize_forced_candidate(candidate: str | None) -> str | None:
|
||||
@@ -152,6 +166,24 @@ def _normalize_gm_volt_candidate(candidate: str | None, fingerprints: dict[int,
|
||||
return candidate
|
||||
|
||||
|
||||
def _normalize_gm_suburban_camera_candidate(candidate: str | None, fingerprints: dict[int, dict], vin: str | None) -> str | None:
|
||||
"""Resolve the 2019 Suburban camera-harness variant when CAN is shared with Yukon."""
|
||||
if candidate not in (None, "GMC_YUKON", "GMC_YUKON_CC"):
|
||||
return candidate
|
||||
|
||||
if not isinstance(vin, str) or not vin.startswith(GM_SUBURBAN_CAMERA_VIN_PREFIX):
|
||||
return candidate
|
||||
|
||||
powertrain = fingerprints.get(0, {})
|
||||
camera = fingerprints.get(GM_CAMERA_BUS, {})
|
||||
if not all(powertrain.get(address) == length for address, length in GM_SUBURBAN_CAMERA_PT_SIGNATURE.items()):
|
||||
return candidate
|
||||
if not all(camera.get(address) == length for address, length in GM_CAMERA_DIAGNOSTIC_MESSAGES.items()):
|
||||
return candidate
|
||||
|
||||
return "CHEVROLET_SUBURBAN_CAMERA"
|
||||
|
||||
|
||||
def _is_gm_candidate(candidate: str | None) -> bool:
|
||||
return isinstance(candidate, str) and candidate.startswith(GM_CANDIDATE_PREFIXES)
|
||||
|
||||
@@ -307,6 +339,10 @@ def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multip
|
||||
stored_candidate = _normalize_forced_candidate(params.get("CarModel"))
|
||||
cached_candidate = _normalize_forced_candidate(getattr(cached_params, "carFingerprint", None))
|
||||
|
||||
if candidate is None and stored_candidate is None and cached_candidate is None:
|
||||
candidate = _normalize_gm_suburban_camera_candidate(candidate, fingerprints, vin)
|
||||
fingerprinted_candidate = candidate
|
||||
|
||||
if candidate is None:
|
||||
gm_fallback_candidate = _get_gm_stored_candidate_fallback(fingerprints, stored_candidate, cached_candidate)
|
||||
if gm_fallback_candidate is not None:
|
||||
|
||||
@@ -6,7 +6,7 @@ from opendbc.car.lateral import apply_driver_steer_torque_limits
|
||||
from opendbc.car.gm import gmcan
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gm.values import (
|
||||
ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams,
|
||||
ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, GM_AUTO_HOLD_CARS, SDGM_CAR, AccState, CanBus, CarControllerParams,
|
||||
CruiseButtons, GMFlags, GMSafetyFlags,
|
||||
)
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
@@ -309,7 +309,7 @@ def supports_volt_auto_hold(CP, auto_hold_enabled: bool):
|
||||
auto_hold_enabled and
|
||||
getattr(CP, "openpilotLongitudinalControl", False) and
|
||||
stock_hold_safety_ready and
|
||||
CP.carFingerprint in AUTO_HOLD_VOLT_CARS
|
||||
CP.carFingerprint in GM_AUTO_HOLD_CARS
|
||||
)
|
||||
|
||||
|
||||
@@ -852,7 +852,6 @@ class CarController(CarControllerBase):
|
||||
CAR.CHEVROLET_VOLT_CC,
|
||||
CAR.CHEVROLET_MALIBU_CC,
|
||||
CAR.CHEVROLET_MALIBU_HYBRID_CC,
|
||||
CAR.BUICK_LACROSSE,
|
||||
}
|
||||
|
||||
if (self.CP.enableGasInterceptorDEPRECATED and self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
||||
@@ -1160,7 +1159,13 @@ class CarController(CarControllerBase):
|
||||
if should_send_cc_button_spam(self.CP, CC, CS):
|
||||
if self.CP.carFingerprint != CAR.CADILLAC_XT4_CC:
|
||||
# Using extend instead of append since the message is only sent intermittently
|
||||
can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles))
|
||||
longitudinal_adjustment_active = bool(getattr(
|
||||
CS, "openpilot_longitudinal_adjustment_active", CC.hudControl.leadVisible,
|
||||
))
|
||||
can_sends.extend(gmcan.create_gm_cc_spam_command(
|
||||
self.packer_pt, self, CS, actuators, starpilot_toggles,
|
||||
longitudinal_adjustment_active=longitudinal_adjustment_active,
|
||||
))
|
||||
else:
|
||||
if (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and
|
||||
CS.cruise_buttons == CruiseButtons.UNPRESS and CS.out.gasPressed and CS.out.cruiseState.speed < CS.out.vEgo < hud_v_cruise):
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
import copy
|
||||
import math
|
||||
from datetime import UTC, datetime, timedelta
|
||||
from collections.abc import Mapping
|
||||
from cereal import custom
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
@@ -10,14 +8,17 @@ from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gps import get_car_gps_config
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.gm.values import (
|
||||
ALT_ACCS,
|
||||
ASCM_INT,
|
||||
CAMERA_ACC_CAR,
|
||||
CAR,
|
||||
CC_ONLY_CAR,
|
||||
CC_REGEN_PADDLE_CAR,
|
||||
DBC,
|
||||
AccState,
|
||||
CanBus,
|
||||
CruiseButtons,
|
||||
GM_AUTO_HOLD_CARS,
|
||||
GMFlags,
|
||||
SDGM_CAR,
|
||||
STEER_THRESHOLD,
|
||||
@@ -32,118 +33,13 @@ STANDSTILL_THRESHOLD = 10 * 0.0311
|
||||
VOLT_EBCM_BRAKE_PRESSED_THRESHOLD = 6 / 0xd0
|
||||
AUTO_HOLD_MIN_DRIVE_TIME_S = 3.0
|
||||
AUTO_HOLD_REGEN_RELEASE_COOLDOWN_S = 1.0
|
||||
ACC_STARTUP_FAULT_GRACE_PERIOD_S = 5.0
|
||||
|
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||
CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
HARD_BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise}
|
||||
NORMAL_CRUISE_BUTTONS = (CruiseButtons.RES_ACCEL, CruiseButtons.DECEL_SET)
|
||||
|
||||
# Optional ~10 Hz CT6 PPS GPS messages on the powertrain bus.
|
||||
PPS_GPS_MESSAGES = (
|
||||
"PPS_ElevHdSpd_FO",
|
||||
"PPS_PosLat_FO",
|
||||
"PPS_PosLong_FO",
|
||||
"PPS_Time_FO",
|
||||
"PPS_QualMetrics_FO",
|
||||
)
|
||||
# PPS_SigAcqTime_FO is omitted: its validity bit stays 1 even during valid fixes.
|
||||
|
||||
|
||||
def pps_checksum_ok(data: bytes) -> bool:
|
||||
"""Validate the 11-bit checksum used by the observed PPS frames."""
|
||||
if len(data) < 2:
|
||||
return False
|
||||
received = ((data[-2] & 0x07) << 8) | data[-1]
|
||||
expected = sum(data[:-2]) + (data[-2] >> 3) + 0x4C
|
||||
return (expected & 0x7FF) == received
|
||||
|
||||
|
||||
def decode_gm_pps_gps(values: Mapping[str, Mapping[str, float]], raw: Mapping[str, bytes],
|
||||
timestamp_nanos: int) -> dict | None:
|
||||
"""Decode one coherent PPS bundle into the existing car-GPS sample shape."""
|
||||
if any(not pps_checksum_ok(raw.get(name, b"")) for name in PPS_GPS_MESSAGES):
|
||||
return None
|
||||
|
||||
try:
|
||||
pos_lat = values["PPS_PosLat_FO"]
|
||||
pos_long = values["PPS_PosLong_FO"]
|
||||
timestamp_values = values["PPS_Time_FO"]
|
||||
quality = values["PPS_QualMetrics_FO"]
|
||||
if (int(pos_lat.get("PPSLatV", 1)) != 0 or
|
||||
int(pos_long.get("PPSLongV", 1)) != 0 or
|
||||
int(quality.get("PPS2DAbsPosErrEstmtV", 1)) != 0 or
|
||||
int(quality.get("PPSMdV", 1)) != 0 or
|
||||
int(quality.get("PPSPstnDilPrcsV", 1)) != 0 or
|
||||
int(timestamp_values.get("PPSTmdayV", 1)) != 0 or
|
||||
int(timestamp_values.get("PPSCldrDayV", 1)) != 0 or
|
||||
int(timestamp_values.get("PPSCldrYrV", 1)) != 0):
|
||||
return None
|
||||
# Reject mode 6 (dead reckoning only without GNSS).
|
||||
if int(quality["PPSMd"]) == 6:
|
||||
return None
|
||||
|
||||
latitude = float(pos_lat["PPSLat"]) / 3_600_000.0
|
||||
longitude = float(pos_long["PPSLong"]) / 3_600_000.0
|
||||
if not (math.isfinite(latitude) and math.isfinite(longitude) and
|
||||
-90.0 <= latitude <= 90.0 and -180.0 <= longitude <= 180.0 and
|
||||
(latitude != 0.0 or longitude != 0.0)):
|
||||
return None
|
||||
|
||||
year = int(timestamp_values["PPSCldrYr"])
|
||||
day_of_year = int(timestamp_values["PPSCldrDay"])
|
||||
millis_of_day = int(timestamp_values["PPSTmday"])
|
||||
if not 2014 <= year <= 2141 or day_of_year < 1 or not 0 <= millis_of_day < 86_400_000:
|
||||
return None
|
||||
timestamp = datetime(year, 1, 1, tzinfo=UTC) + timedelta(days=day_of_year - 1, milliseconds=millis_of_day)
|
||||
if timestamp.year != year:
|
||||
return None
|
||||
|
||||
elev = values["PPS_ElevHdSpd_FO"]
|
||||
speed = float(elev["PPSVel"]) * CV.KPH_TO_MS
|
||||
if int(elev.get("PPSVelV", 1)) != 0 or not math.isfinite(speed) or not 0.0 <= speed <= 200.0:
|
||||
speed = 0.0
|
||||
heading = float(elev["PPSHedng"])
|
||||
if (int(elev.get("PPSHedngV", 1)) != 0 or
|
||||
not math.isfinite(heading) or not 0.0 <= heading < 360.0):
|
||||
heading = 0.0
|
||||
|
||||
altitude = float(elev["PPSElvtn"]) / 100.0
|
||||
if int(elev.get("PPSElvtnV", 1)) != 0 or not math.isfinite(altitude):
|
||||
altitude = 0.0
|
||||
|
||||
horizontal_accuracy = float(quality["PPS2DAbsPosErrEstmt"])
|
||||
if not math.isfinite(horizontal_accuracy) or horizontal_accuracy < 0.0:
|
||||
horizontal_accuracy = 0.0
|
||||
|
||||
vertical_accuracy = float(quality["PPS3DAbsPosErrEstmt"])
|
||||
if int(quality.get("PPS3DAbsPosErrEstmtV", 1)) != 0 or not math.isfinite(vertical_accuracy) or vertical_accuracy < 0.0:
|
||||
vertical_accuracy = 0.0
|
||||
|
||||
bearing_accuracy = float(quality["PPSAbsHdngErrEstmt"])
|
||||
if int(quality.get("PPSAbsHdngErrEstmtV", 1)) != 0 or not math.isfinite(bearing_accuracy) or bearing_accuracy < 0.0:
|
||||
bearing_accuracy = 180.0
|
||||
except (KeyError, TypeError, ValueError, OverflowError, AttributeError):
|
||||
return None
|
||||
|
||||
heading_rad = math.radians(heading)
|
||||
return {
|
||||
"timestamp_nanos": timestamp_nanos,
|
||||
"latitude": latitude,
|
||||
"longitude": longitude,
|
||||
"altitude": altitude,
|
||||
"speed": speed,
|
||||
"bearingDeg": heading,
|
||||
"horizontalAccuracy": horizontal_accuracy,
|
||||
"unixTimestampMillis": round(timestamp.timestamp() * 1000),
|
||||
"verticalAccuracy": vertical_accuracy,
|
||||
"bearingAccuracyDeg": bearing_accuracy,
|
||||
# Velocity error units are undocumented in DBC; omit conversion.
|
||||
"speedAccuracy": 0.0,
|
||||
"hasFix": True,
|
||||
"satelliteCount": 0,
|
||||
"vNED": [speed * math.cos(heading_rad), speed * math.sin(heading_rad), 0.0],
|
||||
}
|
||||
|
||||
|
||||
def get_hard_cruise_buttons(steering_button_msg: dict) -> int:
|
||||
return steering_button_msg.get("ACCButtonsHard", CruiseButtons.INIT)
|
||||
@@ -173,6 +69,36 @@ def update_auto_hold_drive_timers(in_drive_for_hold: bool, moving_for_hold: bool
|
||||
return auto_hold_drive_time, one_pedal_drive_time
|
||||
|
||||
|
||||
def is_gm_auto_hold_active(car_fingerprint: str, auto_hold_engaged: bool, in_drive_for_hold: bool,
|
||||
cruise_available: bool, standstill: bool, gas_pressed: bool) -> bool:
|
||||
return (
|
||||
auto_hold_engaged and
|
||||
car_fingerprint in GM_AUTO_HOLD_CARS and
|
||||
in_drive_for_hold and
|
||||
cruise_available and
|
||||
standstill and
|
||||
not gas_pressed
|
||||
)
|
||||
|
||||
|
||||
def update_startup_acc_fault_suppression(car_fingerprint: str, system_power_mode: int,
|
||||
previous_system_power_mode: int, timer: float,
|
||||
acc_state: int, friction_brake_unavailable: bool) -> tuple[float, bool]:
|
||||
if car_fingerprint != CAR.BUICK_LACROSSE:
|
||||
return 0.0, False
|
||||
|
||||
if system_power_mode == 2 and previous_system_power_mode != 2:
|
||||
timer = ACC_STARTUP_FAULT_GRACE_PERIOD_S
|
||||
elif system_power_mode != 2:
|
||||
timer = 0.0
|
||||
|
||||
if timer <= 0.0 or acc_state != AccState.FAULTED:
|
||||
return 0.0, False
|
||||
|
||||
timer = max(timer - DT_CTRL, 0.0)
|
||||
return timer, timer > 0.0 and not friction_brake_unavailable
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, FPCP):
|
||||
super().__init__(CP, FPCP)
|
||||
@@ -211,19 +137,17 @@ class CarState(CarStateBase):
|
||||
self.lkas_previously_enabled = 0
|
||||
self.lkas_enabled = 0
|
||||
self.pcm_acc_status = AccState.OFF
|
||||
self.system_power_mode = 0
|
||||
self.startup_acc_fault_suppression_timer = 0.0
|
||||
self.stock_fcw_alert = 0
|
||||
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.onstar_gps = None
|
||||
self._car_gps_timestamp_nanos = 0
|
||||
self._prev_gps_lat = None
|
||||
self._prev_gps_lon = None
|
||||
self._last_gps_bearing = None
|
||||
|
||||
self.pps_gps = None
|
||||
self._pps_gps_timestamp_nanos = 0
|
||||
|
||||
def _update_car_gps(self, cp, v_ego: float = 0.0) -> None:
|
||||
if self.car_gps_config is None:
|
||||
return
|
||||
@@ -258,47 +182,12 @@ class CarState(CarStateBase):
|
||||
else:
|
||||
self._prev_gps_lat = self._prev_gps_lon = None
|
||||
|
||||
self.onstar_gps = gps
|
||||
self.car_gps = gps
|
||||
self._car_gps_timestamp_nanos = timestamp_nanos
|
||||
|
||||
def _update_pps_gps(self, cp) -> None:
|
||||
"""Decode a complete, checksum-valid PPS burst when one is available."""
|
||||
timestamps = [max(cp.ts_nanos[name].values(), default=0) for name in PPS_GPS_MESSAGES]
|
||||
if not all(timestamps):
|
||||
return
|
||||
|
||||
timestamp_nanos = max(timestamps)
|
||||
if timestamp_nanos <= self._pps_gps_timestamp_nanos:
|
||||
return
|
||||
if timestamp_nanos - min(timestamps) > 100_000_000:
|
||||
return
|
||||
|
||||
vl = cp.vl
|
||||
try:
|
||||
first_id = int(vl["PPS_ElevHdSpd_FO"]["PPSElvHedngSpdBrstID"])
|
||||
if not (first_id == int(vl["PPS_PosLat_FO"]["PPSLatBrstID"]) ==
|
||||
int(vl["PPS_PosLong_FO"]["PPSLongBrstID"]) ==
|
||||
int(vl["PPS_Time_FO"]["PPSTmBrstID"]) ==
|
||||
int(vl["PPS_QualMetrics_FO"]["PPSPosQltyMtcBrstID"])):
|
||||
return
|
||||
except (KeyError, ValueError, TypeError, OverflowError):
|
||||
return
|
||||
|
||||
values = {name: cp.vl[name] for name in PPS_GPS_MESSAGES}
|
||||
raw = {name: cp.vl_raw[name] for name in PPS_GPS_MESSAGES}
|
||||
self.pps_gps = decode_gm_pps_gps(values, raw, timestamp_nanos)
|
||||
self._pps_gps_timestamp_nanos = timestamp_nanos
|
||||
|
||||
def get_car_gps(self) -> dict | None:
|
||||
def get_car_gps(self):
|
||||
return self.car_gps
|
||||
|
||||
def get_car_gps_sources(self) -> dict[str, dict | None]:
|
||||
return {
|
||||
"pps": self.pps_gps,
|
||||
"onstar": self.onstar_gps,
|
||||
}
|
||||
|
||||
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
|
||||
if not self.CP.pcmCruise:
|
||||
for b in buttonEvents:
|
||||
@@ -384,9 +273,6 @@ class CarState(CarStateBase):
|
||||
abs(pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"]) <= STANDSTILL_THRESHOLD
|
||||
|
||||
self._update_car_gps(pt_cp, ret.vEgo)
|
||||
pps_cp = can_parsers.get(Bus.adas)
|
||||
if pps_cp is not None:
|
||||
self._update_pps_gps(pps_cp)
|
||||
|
||||
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
|
||||
ret.gearShifter = self.parse_gear_shifter("T")
|
||||
@@ -498,8 +384,18 @@ class CarState(CarStateBase):
|
||||
|
||||
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
||||
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
|
||||
pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1)
|
||||
acc_state = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
|
||||
friction_brake_unavailable = pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1
|
||||
self.startup_acc_fault_suppression_timer, suppress_startup_acc_fault = update_startup_acc_fault_suppression(
|
||||
self.CP.carFingerprint,
|
||||
int(pt_cp.vl["BCMGeneralPlatformStatus"]["SystemPowerMode"]),
|
||||
self.system_power_mode,
|
||||
self.startup_acc_fault_suppression_timer,
|
||||
acc_state,
|
||||
friction_brake_unavailable,
|
||||
)
|
||||
self.system_power_mode = int(pt_cp.vl["BCMGeneralPlatformStatus"]["SystemPowerMode"])
|
||||
ret.accFaulted = (acc_state == AccState.FAULTED and not suppress_startup_acc_fault) or friction_brake_unavailable
|
||||
|
||||
ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
|
||||
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
|
||||
@@ -548,6 +444,11 @@ class CarState(CarStateBase):
|
||||
self.auto_hold_fault_suppression_timer = max(self.auto_hold_fault_suppression_timer - DT_CTRL, 0.0)
|
||||
ret.accFaulted = False
|
||||
|
||||
ret.brakeHoldActive = is_gm_auto_hold_active(
|
||||
self.CP.carFingerprint, self.auto_hold_engaged, in_drive_for_hold,
|
||||
ret.cruiseState.available, ret.standstill, ret.gasPressed,
|
||||
)
|
||||
|
||||
if self.CP.enableBsm and not sdgm_non_volt:
|
||||
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
|
||||
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
|
||||
@@ -736,16 +637,8 @@ class CarState(CarStateBase):
|
||||
("ASCMLKASteeringCmd", 0),
|
||||
]
|
||||
|
||||
parsers = {
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus.POWERTRAIN),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus.CAMERA),
|
||||
Bus.loopback: CANParser(DBC[CP.carFingerprint][Bus.pt], loopback_messages, CanBus.LOOPBACK),
|
||||
}
|
||||
if getattr(CP, "brand", None) == "gm":
|
||||
# Optional CT6 PPS parser on Bus.adas; non-PPS vehicles remain CAN-valid.
|
||||
parsers[Bus.adas] = CANParser(
|
||||
"cadillac_ct6_object",
|
||||
[(name, 0) for name in PPS_GPS_MESSAGES],
|
||||
CanBus.POWERTRAIN,
|
||||
)
|
||||
return parsers
|
||||
|
||||
@@ -212,6 +212,10 @@ FINGERPRINTS.update({
|
||||
CAR.CADILLAC_ESCALADE_ASCM: FINGERPRINTS[CAR.CADILLAC_ESCALADE],
|
||||
CAR.CADILLAC_ESCALADE_ESV_2019_ASCM: [{**fp, SASCM_ADDRESS: 8} for fp in FINGERPRINTS[CAR.CADILLAC_ESCALADE_ESV_2019]],
|
||||
CAR.CHEVROLET_SUBURBAN: FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_CC],
|
||||
CAR.CHEVROLET_SUBURBAN_ASCM: FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_CC],
|
||||
# The camera-harness Suburban shares the observed CAN map with the 2019 Yukon;
|
||||
# VIN and camera-bus diagnostics disambiguate it during live fingerprinting.
|
||||
CAR.CHEVROLET_SUBURBAN_CAMERA: FINGERPRINTS[CAR.GMC_YUKON],
|
||||
CAR.GMC_YUKON_CC: FINGERPRINTS[CAR.GMC_YUKON],
|
||||
CAR.CADILLAC_XT6: FINGERPRINTS[CAR.CHEVROLET_TRAVERSE],
|
||||
CAR.CADILLAC_XT5: FINGERPRINTS[CAR.CHEVROLET_TRAVERSE],
|
||||
|
||||
@@ -31,6 +31,8 @@ BOLT_CC_DIRECTION_MEMORY_S = 1.5
|
||||
VOLT_CC_CARS = {
|
||||
CAR.CHEVROLET_VOLT_CC,
|
||||
}
|
||||
VOLT_CC_FREE_REQUEST_DEADBAND_MPH = 5.0
|
||||
VOLT_CC_ACTIVE_REQUEST_DEADBAND_MPH = 2.0
|
||||
|
||||
|
||||
def malibu_phase_map_for_button(button):
|
||||
@@ -339,10 +341,35 @@ def stabilize_bolt_cc_button(controller, CP, requested_button):
|
||||
return requested_button
|
||||
|
||||
|
||||
def _create_volt_cc_spam_command(CS, actuators, ms_convert):
|
||||
def _create_volt_cc_spam_command(CS, actuators, ms_convert, longitudinal_adjustment_active):
|
||||
accel = float(actuators.accel)
|
||||
speed_setpoint = int(round(CS.out.cruiseState.speed * ms_convert))
|
||||
ego_speed = CS.out.vEgo * ms_convert
|
||||
requested_setpoint = (CS.out.vEgo * 1.01 + 3 * accel) * ms_convert
|
||||
deadband_mph = (
|
||||
VOLT_CC_ACTIVE_REQUEST_DEADBAND_MPH if longitudinal_adjustment_active
|
||||
else VOLT_CC_FREE_REQUEST_DEADBAND_MPH
|
||||
)
|
||||
request_deadband = deadband_mph * (CV.MPH_TO_KPH if ms_convert == CV.MS_TO_KPH else 1.0)
|
||||
|
||||
target_setpoint = None
|
||||
v_cruise_kph = float(getattr(CS.out, "vCruise", 0.0))
|
||||
if 0.0 < v_cruise_kph < 255.0:
|
||||
is_metric = ms_convert == CV.MS_TO_KPH
|
||||
target_setpoint = int(round(v_cruise_kph if is_metric else v_cruise_kph * CV.KPH_TO_MPH))
|
||||
|
||||
moving_toward_target = target_setpoint is not None and (
|
||||
(accel > 0.0 and speed_setpoint < target_setpoint) or
|
||||
(accel < 0.0 and speed_setpoint > target_setpoint)
|
||||
)
|
||||
if target_setpoint is not None and accel > 0.0 and speed_setpoint >= target_setpoint:
|
||||
return CruiseButtons.INIT, float("inf")
|
||||
if (target_setpoint is not None and accel < 0.0 and speed_setpoint <= target_setpoint and
|
||||
not longitudinal_adjustment_active):
|
||||
return CruiseButtons.INIT, float("inf")
|
||||
|
||||
if not moving_toward_target and abs(requested_setpoint - speed_setpoint) <= request_deadband:
|
||||
return CruiseButtons.INIT, float("inf")
|
||||
|
||||
if accel == 0.0:
|
||||
return CruiseButtons.INIT, float("inf")
|
||||
@@ -361,7 +388,7 @@ def _create_volt_cc_spam_command(CS, actuators, ms_convert):
|
||||
return CruiseButtons.RES_ACCEL, rate
|
||||
|
||||
|
||||
def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggles):
|
||||
def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggles, longitudinal_adjustment_active=False):
|
||||
accel = actuators.accel
|
||||
v_ego = CS.out.vEgo
|
||||
cruise_btn = CruiseButtons.INIT
|
||||
@@ -376,7 +403,7 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl
|
||||
comparison_setpoint = projected_setpoint if bolt_cc else desired_setpoint
|
||||
|
||||
if CS.CP.carFingerprint in VOLT_CC_CARS:
|
||||
cruise_btn, rate = _create_volt_cc_spam_command(CS, actuators, ms_convert)
|
||||
cruise_btn, rate = _create_volt_cc_spam_command(CS, actuators, ms_convert, longitudinal_adjustment_active)
|
||||
else:
|
||||
if CS.CP.minEnableSpeed - (desired_setpoint / ms_convert) > 3.25:
|
||||
cruise_btn = CruiseButtons.CANCEL
|
||||
|
||||
@@ -15,6 +15,7 @@ from opendbc.car.gm.values import (
|
||||
CC_ONLY_CAR,
|
||||
CC_REGEN_PADDLE_CAR,
|
||||
EV_CAR,
|
||||
GM_AUTO_HOLD_CARS,
|
||||
SDGM_CAR,
|
||||
CarControllerParams,
|
||||
CanBus,
|
||||
@@ -305,7 +306,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM_LONG.value
|
||||
|
||||
elif is_camera_acc:
|
||||
ret.alphaLongitudinalAvailable = (candidate not in CC_ONLY_CAR) and not ret.enableGasInterceptorDEPRECATED
|
||||
ret.alphaLongitudinalAvailable = candidate not in (CC_ONLY_CAR | ALT_ACCS) and not ret.enableGasInterceptorDEPRECATED
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
ret.radarUnavailable = True
|
||||
ret.pcmCruise = not ret.enableGasInterceptorDEPRECATED
|
||||
@@ -408,7 +409,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.radarTimeStepDEPRECATED = 0.0667 # GM radar runs at 15Hz instead of the standard 20Hz
|
||||
ret.radarTimeStepDEPRECATED = 0.15 if candidate == CAR.BUICK_LACROSSE else 0.0667
|
||||
ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking
|
||||
|
||||
if candidate in (
|
||||
@@ -440,7 +441,7 @@ class CarInterface(CarInterfaceBase):
|
||||
elif candidate in (CAR.BUICK_LACROSSE, CAR.BUICK_LACROSSE_ASCM, CAR.BUICK_LACROSSE_ASCM_19US):
|
||||
CarInterfaceBase.configure_torque_tune(CAR.BUICK_LACROSSE, ret.lateralTuning)
|
||||
if candidate == CAR.BUICK_LACROSSE_ASCM_19US:
|
||||
ret.minSteerSpeed = 27 * CV.MPH_TO_MS
|
||||
ret.minSteerSpeed = 28 * CV.MPH_TO_MS
|
||||
|
||||
elif candidate == CAR.CADILLAC_ESCALADE:
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||
@@ -521,7 +522,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate in (CAR.CHEVROLET_SUBURBAN, CAR.CHEVROLET_SUBURBAN_CC):
|
||||
elif candidate in (CAR.CHEVROLET_SUBURBAN, CAR.CHEVROLET_SUBURBAN_ASCM, CAR.CHEVROLET_SUBURBAN_CAMERA, CAR.CHEVROLET_SUBURBAN_CC):
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
@@ -710,18 +711,19 @@ class CarInterface(CarInterfaceBase):
|
||||
if remote_start_boots_comma:
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_REMOTE_START_BOOTS_COMMA.value
|
||||
|
||||
volt_stock_friction_brake_safety = (
|
||||
gm_stock_friction_brake_safety = (
|
||||
ret.openpilotLongitudinalControl and
|
||||
(gm_auto_hold or volt_one_pedal_mode) and
|
||||
candidate in {
|
||||
CAR.CHEVROLET_VOLT,
|
||||
CAR.CHEVROLET_VOLT_2019,
|
||||
CAR.CHEVROLET_VOLT_ASCM,
|
||||
CAR.CHEVROLET_VOLT_CAMERA,
|
||||
}
|
||||
(
|
||||
(gm_auto_hold and candidate in GM_AUTO_HOLD_CARS) or
|
||||
(volt_one_pedal_mode and candidate in {
|
||||
CAR.CHEVROLET_VOLT,
|
||||
CAR.CHEVROLET_VOLT_2019,
|
||||
CAR.CHEVROLET_VOLT_ASCM,
|
||||
CAR.CHEVROLET_VOLT_CAMERA,
|
||||
})
|
||||
)
|
||||
)
|
||||
if volt_stock_friction_brake_safety:
|
||||
# Reuse the paddle-scheduler safety bit as a Volt stock friction-brake
|
||||
if gm_stock_friction_brake_safety:
|
||||
# marker on non-pedal paths. Auto hold and one-pedal can run while OP
|
||||
# longitudinal is configured but not currently active, so the bit must
|
||||
# be present regardless of the current long-control mode. Do not expose
|
||||
|
||||
@@ -431,6 +431,15 @@ def test_volt_auto_hold_requires_toggle_supported_non_cc_only_volt_and_stock_saf
|
||||
),
|
||||
True,
|
||||
)
|
||||
assert supports_volt_auto_hold(
|
||||
SimpleNamespace(
|
||||
carFingerprint=CAR.BUICK_LACROSSE,
|
||||
openpilotLongitudinalControl=True,
|
||||
networkLocation=CarParams.NetworkLocation.gateway,
|
||||
safetyConfigs=stock_safety,
|
||||
),
|
||||
True,
|
||||
)
|
||||
assert not supports_volt_auto_hold(
|
||||
SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT,
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
import pytest
|
||||
import numpy as np
|
||||
from datetime import UTC, datetime
|
||||
from types import SimpleNamespace
|
||||
from parameterized import parameterized
|
||||
|
||||
@@ -11,11 +10,10 @@ from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.gm import gmcan
|
||||
from opendbc.car.gm.carstate import (
|
||||
CarState as GMCarState,
|
||||
PPS_GPS_MESSAGES,
|
||||
decode_gm_pps_gps,
|
||||
get_hard_cruise_buttons,
|
||||
pps_checksum_ok,
|
||||
is_gm_auto_hold_active,
|
||||
update_auto_hold_drive_timers,
|
||||
update_startup_acc_fault_suppression,
|
||||
)
|
||||
from opendbc.car.gm.carcontroller import (
|
||||
VisualAlert,
|
||||
@@ -30,7 +28,7 @@ import opendbc.car.gm.interface as gm_interface
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.gps import CHEVROLET_BOLT_GPS_CARS, CHEVROLET_BOLT_GPS_MESSAGES, get_car_gps_config, parse_chevrolet_bolt_can_gps
|
||||
from opendbc.car.gm.fingerprints import FINGERPRINTS
|
||||
from opendbc.car.gm.values import ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, DBC, GM_RX_OFFSET, CarControllerParams, CruiseButtons, GMFlags, GMSafetyFlags
|
||||
from opendbc.car.gm.values import ALT_ACCS, ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, DBC, GM_RX_OFFSET, CarControllerParams, CruiseButtons, GMFlags, GMSafetyFlags
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from openpilot.common.params import Params
|
||||
|
||||
@@ -103,146 +101,6 @@ class TestBoltGps:
|
||||
assert gps["verticalAccuracy"] == 10.0
|
||||
assert gps["speedAccuracy"] == 0.5
|
||||
|
||||
|
||||
class TestPpsGps:
|
||||
_frames = [
|
||||
(0x260, bytes.fromhex("10ddac000d831277"), 0),
|
||||
(0x261, bytes.fromhex("08386fce09ca"), 0),
|
||||
(0x262, bytes.fromhex("6d98820341de"), 0),
|
||||
(0x264, bytes.fromhex("0018f90578b5eaac"), 0),
|
||||
(0x265, bytes.fromhex("1a0000800a0258fd"), 0),
|
||||
]
|
||||
|
||||
def test_observed_bundle_checksum_and_conversion(self):
|
||||
parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
|
||||
parser.update([(1_000_000_000, self._frames)])
|
||||
|
||||
assert all(pps_checksum_ok(parser.vl_raw[name]) for name in PPS_GPS_MESSAGES)
|
||||
gps = decode_gm_pps_gps(
|
||||
{name: parser.vl[name] for name in PPS_GPS_MESSAGES},
|
||||
{name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES},
|
||||
1_000_000_000,
|
||||
)
|
||||
assert gps is not None
|
||||
assert gps["hasFix"]
|
||||
assert gps["latitude"] == pytest.approx(38.3101, abs=1e-4)
|
||||
assert gps["longitude"] == pytest.approx(-85.7701, abs=1e-4)
|
||||
assert gps["altitude"] == pytest.approx(106.9)
|
||||
assert gps["bearingDeg"] == pytest.approx(56.748)
|
||||
assert gps["horizontalAccuracy"] == pytest.approx(1.0)
|
||||
assert gps["unixTimestampMillis"] == 1788655668655
|
||||
|
||||
@pytest.fixture
|
||||
def bundle(self):
|
||||
parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
|
||||
parser.update([(1_000_000_000, self._frames)])
|
||||
return ({name: dict(parser.vl[name]) for name in PPS_GPS_MESSAGES},
|
||||
{name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES})
|
||||
|
||||
@pytest.mark.parametrize("year,day,date", [
|
||||
(2025, 1, "2025-01-01"), (2025, 365, "2025-12-31"), (2025, 366, None),
|
||||
(2024, 366, "2024-12-31"), (2024, 367, None), (2025, 0, None),
|
||||
])
|
||||
def test_one_based_day_of_year(self, bundle, year, day, date):
|
||||
values, raw = bundle
|
||||
values["PPS_Time_FO"].update(PPSCldrYr=year, PPSCldrDay=day, PPSTmday=1234)
|
||||
gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
|
||||
if date is None:
|
||||
assert gps is None
|
||||
else:
|
||||
expected = int(datetime.fromisoformat(date).replace(tzinfo=UTC).timestamp() * 1000) + 1234
|
||||
assert gps["unixTimestampMillis"] == expected
|
||||
|
||||
@pytest.mark.parametrize("bad_data", [b"", b"\x01"])
|
||||
def test_checksum_short_input(self, bad_data):
|
||||
assert not pps_checksum_ok(bad_data)
|
||||
|
||||
@pytest.mark.parametrize("lat,lon,valid", [(0.0, 10.0 * 3_600_000, True), (10.0 * 3_600_000, 0.0, True), (0.0, 0.0, False)])
|
||||
def test_coordinate_axes(self, bundle, lat, lon, valid):
|
||||
values, raw = bundle
|
||||
values["PPS_PosLat_FO"]["PPSLat"] = lat
|
||||
values["PPS_PosLong_FO"]["PPSLong"] = lon
|
||||
gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
|
||||
if valid:
|
||||
assert gps is not None
|
||||
assert gps["hasFix"]
|
||||
else:
|
||||
assert gps is None
|
||||
|
||||
def test_get_car_gps_sources_shape(self):
|
||||
cs = GMCarState.__new__(GMCarState)
|
||||
cs.pps_gps = {"hasFix": True}
|
||||
cs.onstar_gps = None
|
||||
sources = cs.get_car_gps_sources()
|
||||
assert sources == {"pps": {"hasFix": True}, "onstar": None}
|
||||
|
||||
@pytest.mark.parametrize("message,signal,value", [
|
||||
("PPS_PosLat_FO", "PPSLatV", 1),
|
||||
("PPS_PosLong_FO", "PPSLongV", 1),
|
||||
("PPS_QualMetrics_FO", "PPS2DAbsPosErrEstmtV", 1),
|
||||
("PPS_PosLat_FO", "PPSLat", float("nan")),
|
||||
("PPS_PosLong_FO", "PPSLong", 181 * 3_600_000),
|
||||
("PPS_QualMetrics_FO", "PPSMd", 6),
|
||||
("PPS_Time_FO", "PPSTmdayV", 1),
|
||||
])
|
||||
def test_unusable_position_rejected(self, bundle, message, signal, value):
|
||||
values, raw = bundle
|
||||
values[message][signal] = value
|
||||
assert decode_gm_pps_gps(values, raw, 1_000_000_000) is None
|
||||
|
||||
@pytest.mark.parametrize("invalidity", ["checksum", "position-validity"])
|
||||
def test_burst_cache_and_explicit_invalidation(self, invalidity):
|
||||
parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
|
||||
cs = GMCarState.__new__(GMCarState)
|
||||
cs.pps_gps = None
|
||||
cs._pps_gps_timestamp_nanos = 0
|
||||
parser.update([(1_000_000_000, self._frames[:-1])])
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is None # Incomplete startup burst.
|
||||
parser.update([(1_000_000_000, self._frames[-1:])])
|
||||
cs._update_pps_gps(parser)
|
||||
good = cs.pps_gps
|
||||
assert good is not None
|
||||
# No complete new burst: keep its original timestamp for freshness.
|
||||
parser.update([(2_000_000_000, self._frames[:1])])
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is good
|
||||
parser.update([(2_100_000_000, self._frames)])
|
||||
parser.vl["PPS_PosLat_FO"]["PPSLatBrstID"] = int(parser.vl["PPS_PosLat_FO"]["PPSLatBrstID"]) ^ 1
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is good # A mismatched burst must not refresh the fix.
|
||||
bad_frames = ([(addr, data[:-1] + bytes([data[-1] ^ 1]), bus) for addr, data, bus in self._frames]
|
||||
if invalidity == "checksum" else self._frames)
|
||||
parser.update([(3_000_000_000, bad_frames)])
|
||||
if invalidity == "position-validity":
|
||||
parser.vl["PPS_PosLat_FO"]["PPSLatV"] = 1
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is None
|
||||
parser.update([(4_000_000_000, self._frames)])
|
||||
cs._update_pps_gps(parser)
|
||||
assert cs.pps_gps is not None
|
||||
|
||||
@pytest.mark.parametrize("speed_bad,heading_bad,elevation_bad,vertical_bad,bearing_bad", [
|
||||
(0, 0, 0, 0, 0), (1, 0, 0, 0, 0), (0, 1, 0, 0, 0), (0, 0, 1, 0, 0),
|
||||
(0, 0, 0, 1, 0), (0, 0, 0, 0, 1), (1, 1, 1, 1, 1),
|
||||
], ids=["valid", "speed", "heading", "elevation", "vertical-accuracy", "bearing-accuracy", "all-invalid"])
|
||||
def test_optional_field_fallbacks(self, bundle, speed_bad, heading_bad, elevation_bad, vertical_bad, bearing_bad):
|
||||
values, raw = bundle
|
||||
values["PPS_ElevHdSpd_FO"].update(PPSVel=36, PPSVelV=speed_bad, PPSHedng=90, PPSHedngV=heading_bad,
|
||||
PPSElvtn=12345, PPSElvtnV=elevation_bad)
|
||||
values["PPS_QualMetrics_FO"].update(PPS2DAbsPosErrEstmt=3.2, PPS3DAbsPosErrEstmt=4.5, PPS3DAbsPosErrEstmtV=vertical_bad,
|
||||
PPSAbsHdngErrEstmt=6.0, PPSAbsHdngErrEstmtV=bearing_bad)
|
||||
gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
|
||||
assert gps is not None
|
||||
assert gps["hasFix"]
|
||||
assert gps["speed"] == pytest.approx(0.0 if speed_bad else 10.0)
|
||||
assert gps["bearingDeg"] == (0 if heading_bad else 90)
|
||||
assert gps["altitude"] == pytest.approx(0.0 if elevation_bad else 123.45)
|
||||
assert gps["vNED"] == pytest.approx([gps["speed"], 0, 0] if heading_bad else [0, gps["speed"], 0])
|
||||
assert gps["horizontalAccuracy"] == pytest.approx(3.2)
|
||||
assert gps["verticalAccuracy"] == pytest.approx(0.0 if vertical_bad else 4.5)
|
||||
assert gps["bearingAccuracyDeg"] == pytest.approx(180.0 if bearing_bad else 6.0)
|
||||
|
||||
def test_bolt_gps_heading_and_speed_derivation(self):
|
||||
cp = SimpleNamespace(
|
||||
brand="gm",
|
||||
@@ -352,7 +210,179 @@ class TestPpsGps:
|
||||
assert all(message in parsers[Bus.pt].vl for message in CHEVROLET_BOLT_GPS_MESSAGES)
|
||||
|
||||
|
||||
class TestGMCarState:
|
||||
@parameterized.expand([
|
||||
(CAR.BUICK_LACROSSE, True, True, True, True, False, True),
|
||||
(CAR.CHEVROLET_VOLT, True, True, True, True, False, True),
|
||||
(CAR.CHEVROLET_BOLT_CC_2017, True, True, True, True, False, False),
|
||||
(CAR.BUICK_LACROSSE, True, True, True, False, False, False),
|
||||
(CAR.BUICK_LACROSSE, True, True, True, True, True, False),
|
||||
])
|
||||
def test_auto_hold_alert_state_requires_supported_complete_stop(self, car_fingerprint, engaged, in_drive,
|
||||
cruise_available, standstill, gas_pressed, expected):
|
||||
assert is_gm_auto_hold_active(
|
||||
car_fingerprint, engaged, in_drive, cruise_available, standstill, gas_pressed,
|
||||
) is expected
|
||||
|
||||
def test_lacrosse_startup_acc_fault_is_suppressed(self):
|
||||
timer, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 0, 0.0, 3, False,
|
||||
)
|
||||
|
||||
assert suppressed
|
||||
assert timer == pytest.approx(5.0 - DT_CTRL)
|
||||
|
||||
timer, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 2, timer, 0, False,
|
||||
)
|
||||
|
||||
assert timer == 0.0
|
||||
assert not suppressed
|
||||
|
||||
def test_lacrosse_persistent_acc_fault_is_reported_after_startup(self):
|
||||
timer, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 0, 0.0, 3, False,
|
||||
)
|
||||
|
||||
for _ in range(int(5.0 / DT_CTRL)):
|
||||
timer, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 2, timer, 3, False,
|
||||
)
|
||||
|
||||
assert timer == 0.0
|
||||
assert not suppressed
|
||||
|
||||
def test_lacrosse_brake_unavailable_fault_is_never_suppressed(self):
|
||||
_, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_LACROSSE, 2, 0, 0.0, 3, True,
|
||||
)
|
||||
|
||||
assert not suppressed
|
||||
|
||||
def test_startup_acc_fault_suppression_is_scoped_to_lacrosse(self):
|
||||
_, suppressed = update_startup_acc_fault_suppression(
|
||||
CAR.BUICK_REGAL, 2, 0, 0.0, 3, False,
|
||||
)
|
||||
|
||||
assert not suppressed
|
||||
|
||||
|
||||
class TestGMInterface:
|
||||
def test_suburban_obd_and_ascm_integrations_remain_separate(self):
|
||||
fingerprint = _empty_fingerprint()
|
||||
fingerprint[0] = FINGERPRINTS[CAR.CHEVROLET_SUBURBAN][0].copy()
|
||||
|
||||
assert CAR.CHEVROLET_SUBURBAN_ASCM in ASCM_INT
|
||||
assert FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_ASCM] == FINGERPRINTS[CAR.CHEVROLET_SUBURBAN]
|
||||
|
||||
obd_params = interfaces[CAR.CHEVROLET_SUBURBAN].get_params(
|
||||
CAR.CHEVROLET_SUBURBAN,
|
||||
fingerprint,
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
ascm_params = interfaces[CAR.CHEVROLET_SUBURBAN_ASCM].get_params(
|
||||
CAR.CHEVROLET_SUBURBAN_ASCM,
|
||||
fingerprint,
|
||||
[],
|
||||
alpha_long=True,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
|
||||
assert obd_params.networkLocation == structs.CarParams.NetworkLocation.gateway
|
||||
assert obd_params.openpilotLongitudinalControl
|
||||
assert not obd_params.pcmCruise
|
||||
assert obd_params.safetyConfigs[0].safetyParam == 0
|
||||
|
||||
assert ascm_params.networkLocation == structs.CarParams.NetworkLocation.fwdCamera
|
||||
assert not ascm_params.flags & GMFlags.SASCM.value
|
||||
assert not ascm_params.alphaLongitudinalAvailable
|
||||
assert not ascm_params.openpilotLongitudinalControl
|
||||
assert ascm_params.pcmCruise
|
||||
assert ascm_params.safetyConfigs[0].safetyParam == GMSafetyFlags.HW_CAM.value | GMSafetyFlags.HW_ASCM_INT.value
|
||||
assert ascm_params.lateralTuning.torque.latAccelFactor == pytest.approx(obd_params.lateralTuning.torque.latAccelFactor)
|
||||
assert ascm_params.lateralTuning.torque.friction == pytest.approx(obd_params.lateralTuning.torque.friction)
|
||||
|
||||
def test_suburban_camera_harness_preserves_stock_acc(self):
|
||||
fingerprint = _empty_fingerprint()
|
||||
fingerprint[0] = FINGERPRINTS[CAR.CHEVROLET_SUBURBAN][0].copy()
|
||||
fingerprint[2] = fingerprint[0].copy()
|
||||
|
||||
assert CAR.CHEVROLET_SUBURBAN_CAMERA in CAMERA_ACC_CAR
|
||||
assert CAR.CHEVROLET_SUBURBAN_CAMERA in ALT_ACCS
|
||||
assert CAR.CHEVROLET_SUBURBAN_CAMERA not in CC_ONLY_CAR
|
||||
assert CAR.CHEVROLET_SUBURBAN_CAMERA not in ASCM_INT
|
||||
assert all(fp[CAMERA_DIAGNOSTIC_ADDRESS] == 8 for fp in FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_CAMERA])
|
||||
assert all(fp[CAMERA_DIAGNOSTIC_ADDRESS + GM_RX_OFFSET] == 8 for fp in FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_CAMERA])
|
||||
|
||||
camera_params = interfaces[CAR.CHEVROLET_SUBURBAN_CAMERA].get_params(
|
||||
CAR.CHEVROLET_SUBURBAN_CAMERA,
|
||||
fingerprint,
|
||||
[],
|
||||
alpha_long=True,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
|
||||
assert camera_params.networkLocation == structs.CarParams.NetworkLocation.fwdCamera
|
||||
assert camera_params.pcmCruise
|
||||
assert not camera_params.alphaLongitudinalAvailable
|
||||
assert not camera_params.openpilotLongitudinalControl
|
||||
assert camera_params.safetyConfigs[0].safetyParam == GMSafetyFlags.HW_CAM.value
|
||||
|
||||
def test_suburban_cc_remains_no_acc_gateway_profile(self):
|
||||
fingerprint = _empty_fingerprint()
|
||||
fingerprint[0] = FINGERPRINTS[CAR.CHEVROLET_SUBURBAN_CC][0].copy()
|
||||
|
||||
cc_params = interfaces[CAR.CHEVROLET_SUBURBAN_CC].get_params(
|
||||
CAR.CHEVROLET_SUBURBAN_CC,
|
||||
fingerprint,
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
|
||||
assert cc_params.networkLocation == structs.CarParams.NetworkLocation.gateway
|
||||
assert cc_params.openpilotLongitudinalControl
|
||||
assert not cc_params.pcmCruise
|
||||
assert cc_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_CC_LONG.value
|
||||
assert cc_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_NO_ACC.value
|
||||
|
||||
def test_lacrosse_obd_and_ascm_integrations_remain_separate(self):
|
||||
obd_params = interfaces[CAR.BUICK_LACROSSE].get_params(
|
||||
CAR.BUICK_LACROSSE,
|
||||
_empty_fingerprint(),
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
ascm_params = interfaces[CAR.BUICK_LACROSSE_ASCM].get_params(
|
||||
CAR.BUICK_LACROSSE_ASCM,
|
||||
_empty_fingerprint(),
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
|
||||
assert obd_params.networkLocation == structs.CarParams.NetworkLocation.gateway
|
||||
assert obd_params.openpilotLongitudinalControl
|
||||
assert obd_params.radarTimeStepDEPRECATED == pytest.approx(0.15)
|
||||
assert ascm_params.networkLocation == structs.CarParams.NetworkLocation.fwdCamera
|
||||
assert not ascm_params.openpilotLongitudinalControl
|
||||
assert ascm_params.radarTimeStepDEPRECATED == pytest.approx(0.0667)
|
||||
|
||||
@parameterized.expand([
|
||||
CAR.CHEVROLET_BOLT_CC_2017,
|
||||
CAR.CHEVROLET_BOLT_CC_2018_2021,
|
||||
@@ -439,6 +469,14 @@ class TestGMInterface:
|
||||
|
||||
assert car_params.minSteerSpeed == pytest.approx(7 * CV.MPH_TO_MS)
|
||||
|
||||
def test_lacrosse_2019_ascm_min_steer_speed_is_28_mph(self):
|
||||
car_model = CAR.BUICK_LACROSSE_ASCM_19US
|
||||
CarInterface = interfaces[car_model]
|
||||
car_params = CarInterface.get_params(car_model, _empty_fingerprint(), [], alpha_long=False, is_release=False, docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles())
|
||||
|
||||
assert car_params.minSteerSpeed == pytest.approx(28 * CV.MPH_TO_MS)
|
||||
|
||||
@parameterized.expand([
|
||||
("interceptor", True),
|
||||
("ascm_int", False),
|
||||
@@ -634,6 +672,43 @@ class TestGMInterface:
|
||||
assert car_params.openpilotLongitudinalControl
|
||||
assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
|
||||
|
||||
def test_buick_lacrosse_auto_hold_sets_stock_hold_safety_bit_with_op_long_enabled(self):
|
||||
params = Params()
|
||||
try:
|
||||
params.put_bool("GMAutoHold", True)
|
||||
car_params = interfaces[CAR.BUICK_LACROSSE].get_params(
|
||||
CAR.BUICK_LACROSSE,
|
||||
_empty_fingerprint(),
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
finally:
|
||||
params.remove("GMAutoHold")
|
||||
|
||||
assert car_params.openpilotLongitudinalControl
|
||||
assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
|
||||
|
||||
def test_buick_lacrosse_auto_hold_is_off_when_toggle_is_disabled(self):
|
||||
params = Params()
|
||||
try:
|
||||
params.put_bool("GMAutoHold", False)
|
||||
car_params = interfaces[CAR.BUICK_LACROSSE].get_params(
|
||||
CAR.BUICK_LACROSSE,
|
||||
_empty_fingerprint(),
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=_test_starpilot_toggles(),
|
||||
)
|
||||
finally:
|
||||
params.remove("GMAutoHold")
|
||||
|
||||
assert not car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
|
||||
|
||||
def test_volt_auto_hold_does_not_set_stock_hold_safety_bit_with_op_long_disabled(self):
|
||||
CarInterface = interfaces[CAR.CHEVROLET_VOLT_ASCM]
|
||||
fingerprint = _empty_fingerprint()
|
||||
@@ -932,7 +1007,7 @@ class TestGMCarController:
|
||||
|
||||
def test_volt_cc_redneck_rate_limits_setpoint_changes_by_planner_acceleration(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.5 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
controller = SimpleNamespace(frame=int(0.2 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
@@ -948,17 +1023,224 @@ class TestGMCarController:
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||
packer, controller, cs, SimpleNamespace(accel=1.0), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
|
||||
controller.frame = int(0.7 / DT_CTRL)
|
||||
controller.frame = int(0.3 / DT_CTRL)
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=1.0), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert len(msgs) == 1
|
||||
|
||||
def test_volt_cc_redneck_does_not_raise_stock_setpoint_above_max(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(2.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=100.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=100.0 * CV.KPH_TO_MS),
|
||||
vCruise=100.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
assert controller.apply_speed == 100
|
||||
|
||||
def test_volt_cc_redneck_tracks_max_inside_request_deadband(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(3.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=52.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=52.0 * CV.KPH_TO_MS),
|
||||
vCruise=60.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.1), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert len(msgs) == 1
|
||||
assert controller.apply_speed == 53
|
||||
|
||||
def test_volt_cc_redneck_tracks_max_down_inside_request_deadband(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(3.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=68.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=68.0 * CV.KPH_TO_MS),
|
||||
vCruise=60.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=-0.1), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert len(msgs) == 1
|
||||
assert controller.apply_speed == 67
|
||||
|
||||
def test_volt_cc_redneck_holds_small_decel_request_at_max(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(3.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=100.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=100.0 * CV.KPH_TO_MS),
|
||||
vCruise=100.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=-0.1), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
assert controller.apply_speed == 100
|
||||
|
||||
def test_volt_cc_redneck_holds_strong_decel_request_at_max_during_free_cruise(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(3.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=100.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=100.0 * CV.KPH_TO_MS),
|
||||
vCruise=100.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=-1.2), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
assert controller.apply_speed == 100
|
||||
|
||||
def test_volt_cc_redneck_brakes_for_active_lead_inside_free_road_deadband(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(3.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=100.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=100.0 * CV.KPH_TO_MS),
|
||||
vCruise=100.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=-0.5), SimpleNamespace(is_metric=True), longitudinal_adjustment_active=True,
|
||||
)
|
||||
|
||||
assert len(msgs) == 1
|
||||
assert controller.apply_speed == 99
|
||||
|
||||
def test_volt_cc_redneck_accelerates_when_pseudo_speed_request_exceeds_deadband(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.1 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=44.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=44.0 * CV.KPH_TO_MS),
|
||||
vCruise=50.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=2.0), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
|
||||
controller.frame = int(0.3 / DT_CTRL)
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=2.0), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert len(msgs) == 1
|
||||
assert controller.apply_speed == 45
|
||||
|
||||
def test_volt_cc_redneck_brakes_when_pseudo_speed_request_exceeds_deadband(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=50.7 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=49.0 * CV.KPH_TO_MS),
|
||||
vCruise=49.0,
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=-1.36), SimpleNamespace(is_metric=True), longitudinal_adjustment_active=True,
|
||||
)
|
||||
|
||||
assert len(msgs) == 1
|
||||
assert controller.apply_speed == 48
|
||||
|
||||
def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
|
||||
@@ -357,6 +357,14 @@ class CAR(Platforms):
|
||||
[GMCarDocs("Chevrolet Suburban Premier 2016-20")],
|
||||
CarSpecs(mass=2731, wheelbase=3.302, steerRatio=17.3, centerToFrontRatio=0.49),
|
||||
)
|
||||
CHEVROLET_SUBURBAN_ASCM = GMPlatformConfig(
|
||||
[GMCarDocs("Chevrolet Suburban Premier ASCM Harness 2016-20", "Adaptive Cruise Control (ACC) & LKAS")],
|
||||
CHEVROLET_SUBURBAN.specs,
|
||||
)
|
||||
CHEVROLET_SUBURBAN_CAMERA = GMPlatformConfig(
|
||||
[GMCarDocs("Chevrolet Suburban Premier Camera Harness 2016-20", "Adaptive Cruise Control (ACC) & LKAS")],
|
||||
CHEVROLET_SUBURBAN.specs,
|
||||
)
|
||||
GMC_YUKON_CC = GMPlatformConfig(
|
||||
[GMCarDocs("GMC Yukon No-ACC 2019-20")],
|
||||
CarSpecs(mass=2541, wheelbase=2.95, steerRatio=16.3, centerToFrontRatio=0.4),
|
||||
@@ -533,12 +541,21 @@ EV_CAR = {
|
||||
CAR.CHEVROLET_MALIBU_HYBRID_CC,
|
||||
}
|
||||
|
||||
GM_AUTO_HOLD_CARS = {
|
||||
CAR.CHEVROLET_VOLT,
|
||||
CAR.CHEVROLET_VOLT_2019,
|
||||
CAR.CHEVROLET_VOLT_ASCM,
|
||||
CAR.CHEVROLET_VOLT_CAMERA,
|
||||
CAR.BUICK_LACROSSE,
|
||||
}
|
||||
|
||||
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
|
||||
CAMERA_ACC_CAR = {
|
||||
CAR.CHEVROLET_BOLT_ACC_2022_2023,
|
||||
CAR.CHEVROLET_SILVERADO,
|
||||
CAR.CHEVROLET_EQUINOX,
|
||||
CAR.CHEVROLET_TRAILBLAZER,
|
||||
CAR.CHEVROLET_SUBURBAN_CAMERA,
|
||||
CAR.CHEVROLET_VOLT_CAMERA,
|
||||
CAR.CHEVROLET_BLAZER,
|
||||
CAR.CHEVROLET_TRAX,
|
||||
@@ -546,7 +563,7 @@ CAMERA_ACC_CAR = {
|
||||
}
|
||||
|
||||
# Alt ASCMActiveCruiseControlStatus
|
||||
ALT_ACCS = {CAR.GMC_YUKON, CAR.GMC_YUKON_CC}
|
||||
ALT_ACCS = {CAR.CHEVROLET_SUBURBAN_CAMERA, CAR.GMC_YUKON, CAR.GMC_YUKON_CC}
|
||||
|
||||
# We're integrated at the Safety Data Gateway Module on these cars
|
||||
SDGM_CAR = {
|
||||
@@ -585,9 +602,10 @@ CC_REGEN_PADDLE_CAR = {
|
||||
}
|
||||
CAMERA_ACC_CAR.update(CC_ONLY_CAR)
|
||||
|
||||
# ASCM-INT paths are only enabled when SASCM (0x2FF) is detected at runtime
|
||||
# ASCM-intercept variants preserve stock ACC. SASCM (0x2FF) enables alpha-long where supported.
|
||||
ASCM_INT = {
|
||||
CAR.CHEVROLET_VOLT_ASCM,
|
||||
CAR.CHEVROLET_SUBURBAN_ASCM,
|
||||
CAR.GMC_ACADIA_ASCM,
|
||||
CAR.CHEVROLET_MALIBU_ASCM,
|
||||
CAR.CADILLAC_ESCALADE_ASCM,
|
||||
|
||||
@@ -6,10 +6,8 @@ from collections.abc import Callable, Mapping
|
||||
from typing import Any
|
||||
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.can.dbc import DBC as DBC_FILE
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.ford.values import CAR as FORD_CAR
|
||||
from opendbc.car.gm.values import CAR as GM_CAR, DBC as GM_DBC
|
||||
from opendbc.car.gm.values import CAR as GM_CAR
|
||||
|
||||
|
||||
CarGpsSample = dict[str, Any]
|
||||
@@ -160,25 +158,8 @@ CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = {
|
||||
|
||||
|
||||
def get_car_gps_config(CP) -> CarGpsConfig | None:
|
||||
cp_brand = getattr(CP, "brand", None)
|
||||
config = CAR_GPS_CONFIGS.get(CP.carFingerprint)
|
||||
if config is not None and config.brand == cp_brand:
|
||||
return config
|
||||
|
||||
# Enable OnStar GPS for GM cars whose powertrain DBC defines it.
|
||||
if cp_brand == "gm":
|
||||
try:
|
||||
dbc_name = GM_DBC[CP.carFingerprint][Bus.pt]
|
||||
if "TCICOnStarGPSPosition" in DBC_FILE(dbc_name).name_to_msg:
|
||||
return CarGpsConfig(
|
||||
brand="gm",
|
||||
messages=CHEVROLET_BOLT_GPS_MESSAGES,
|
||||
decoder=parse_chevrolet_bolt_can_gps,
|
||||
)
|
||||
except (KeyError, OSError, TypeError, RuntimeError):
|
||||
pass
|
||||
|
||||
return None
|
||||
return config if config is not None and config.brand == CP.brand else None
|
||||
|
||||
|
||||
def car_gps_available(CP) -> bool:
|
||||
|
||||
@@ -23,6 +23,20 @@ from openpilot.common.params import Params
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
BOSCH_BRAKE_FORCE_ON = -0.12
|
||||
BOSCH_BRAKE_FORCE_RELEASE = -0.02
|
||||
|
||||
|
||||
def update_honda_bosch_braking(braking: bool, gas_pedal_force: float, stopping: bool, long_active: bool) -> bool:
|
||||
"""Select Bosch brake mode from the same road-load-adjusted force used for gas."""
|
||||
if not long_active:
|
||||
return False
|
||||
if stopping:
|
||||
return True
|
||||
if braking:
|
||||
return gas_pedal_force <= BOSCH_BRAKE_FORCE_RELEASE
|
||||
return gas_pedal_force < BOSCH_BRAKE_FORCE_ON
|
||||
|
||||
|
||||
def get_civic_bosch_modified_torque_lpf_tau(torque_cmd: float, prev_torque_cmd: float, v_ego: float) -> float:
|
||||
torque_delta = abs(float(torque_cmd) - float(prev_torque_cmd))
|
||||
@@ -238,6 +252,7 @@ class CarController(CarControllerBase):
|
||||
self.steering_pressed_filter_s = 0.0
|
||||
self.steering_pressed_robust_prev = False
|
||||
self.bosch_last_gas = 0.0
|
||||
self.bosch_braking = False
|
||||
self.bosch_gas_factor = self.param_store.get_float("HondaGasFactorParams", default=1.0)
|
||||
self.bosch_wind_factor = self.param_store.get_float("HondaWindFactorParams", default=1.0)
|
||||
self.bosch_wind_factor_before_brake = self.bosch_wind_factor
|
||||
@@ -472,12 +487,16 @@ class CarController(CarControllerBase):
|
||||
self.bosch_last_gas = self.gas
|
||||
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
bosch_braking = None
|
||||
if not self.mvl_accord_mode:
|
||||
self.bosch_braking = update_honda_bosch_braking(self.bosch_braking, gas_pedal_force, stopping, CC.longActive)
|
||||
bosch_braking = self.bosch_braking
|
||||
self.stopping_counter = self.stopping_counter + 1 if stopping else 0
|
||||
if not self.mvl_accord_mode or mvl_radar_owned:
|
||||
can_sends.extend(
|
||||
hondacan.create_acc_commands(
|
||||
self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas, self.stopping_counter, self.CP,
|
||||
gas_force=gas_pedal_force if self.mvl_accord_mode else None,
|
||||
gas_force=gas_pedal_force, braking=bosch_braking,
|
||||
)
|
||||
)
|
||||
else:
|
||||
|
||||
@@ -71,16 +71,18 @@ def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_ca
|
||||
return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values)
|
||||
|
||||
|
||||
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, CP, gas_force=None):
|
||||
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, CP, gas_force=None, braking=None):
|
||||
commands = []
|
||||
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
|
||||
|
||||
control_on = 5 if enabled else 0
|
||||
if gas_force is None:
|
||||
gas_force = accel
|
||||
gas_command = gas if active and gas_force > min_gas_accel else -30000
|
||||
if braking is None:
|
||||
braking = gas_force < min_gas_accel
|
||||
braking = int(active and braking)
|
||||
gas_command = gas if active and gas_force > min_gas_accel and not braking else -30000
|
||||
accel_command = accel if active else 0
|
||||
braking = 1 if active and gas_force < min_gas_accel else 0
|
||||
standstill = 1 if active and stopping_counter > 0 else 0
|
||||
standstill_release = 1 if active and stopping_counter == 0 else 0
|
||||
|
||||
|
||||
@@ -7,13 +7,16 @@ from opendbc.car.structs import CarParams
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
from opendbc.car.honda.carcontroller import (
|
||||
BOSCH_BRAKE_FORCE_ON,
|
||||
BOSCH_BRAKE_FORCE_RELEASE,
|
||||
CarController,
|
||||
get_civic_bosch_modified_steering_pressed,
|
||||
get_civic_bosch_modified_torque_lpf_tau,
|
||||
get_honda_bosch_wind_brake_mps2,
|
||||
update_honda_bosch_braking,
|
||||
update_honda_bosch_live_learning,
|
||||
)
|
||||
from opendbc.car.honda.hondacan import create_lkas_hud
|
||||
from opendbc.car.honda.hondacan import create_acc_commands, create_lkas_hud
|
||||
from opendbc.car.honda.fingerprints import FW_VERSIONS
|
||||
from opendbc.car.honda.values import CAR, DBC, HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL, CarControllerParams, HondaFlags, HondaSafetyFlags, \
|
||||
HondaStarPilotFlags
|
||||
@@ -26,6 +29,67 @@ def get_test_toggles() -> SimpleNamespace:
|
||||
|
||||
|
||||
class TestHondaFingerprint:
|
||||
@staticmethod
|
||||
def _acc_control_values(active, accel, gas=500, gas_force=0.5, braking=False):
|
||||
class FakePacker:
|
||||
@staticmethod
|
||||
def make_can_msg(name, bus, values):
|
||||
return name, bus, values
|
||||
|
||||
can = SimpleNamespace(pt=1)
|
||||
cp = SimpleNamespace(carFingerprint=CAR.HONDA_CRV_5G)
|
||||
commands = create_acc_commands(FakePacker(), can, True, active, accel, gas, 0, cp, gas_force, braking)
|
||||
assert commands[-1][0] == "ACC_CONTROL"
|
||||
return commands[-1][2]
|
||||
|
||||
def test_bosch_acc_commands_reject_fault_route_gas_brake_conflict(self):
|
||||
braking = update_honda_bosch_braking(False, 0.2, False, True)
|
||||
values = self._acc_control_values(True, -0.27, gas=160, gas_force=0.2, braking=braking)
|
||||
|
||||
assert values["GAS_COMMAND"] == 160
|
||||
assert values["ACCEL_COMMAND"] == pytest.approx(-0.27)
|
||||
assert values["BRAKE_REQUEST"] == 0
|
||||
assert values["BRAKE_LIGHTS"] == 0
|
||||
|
||||
@pytest.mark.parametrize("active", [False, True])
|
||||
@pytest.mark.parametrize("accel", [-3.5, -0.27, -0.2, -0.1, 0.0, 0.01, 2.0])
|
||||
@pytest.mark.parametrize("gas_force", [-0.5, 0.0, 0.5])
|
||||
@pytest.mark.parametrize("braking", [False, True])
|
||||
def test_bosch_acc_commands_never_request_gas_and_braking_together(self, active, accel, gas_force, braking):
|
||||
values = self._acc_control_values(active, accel, gas_force=gas_force, braking=braking)
|
||||
|
||||
assert not (values["GAS_COMMAND"] > 0 and values["BRAKE_REQUEST"] == 1)
|
||||
assert not (values["GAS_COMMAND"] > 0 and values["BRAKE_LIGHTS"] == 1)
|
||||
if values["GAS_COMMAND"] > 0:
|
||||
assert active
|
||||
|
||||
def test_bosch_acc_commands_preserve_road_load_gas_above_brake_threshold(self):
|
||||
values = self._acc_control_values(True, -0.27, gas=500, gas_force=0.3)
|
||||
|
||||
assert values["GAS_COMMAND"] == 500
|
||||
assert values["ACCEL_COMMAND"] == pytest.approx(-0.27)
|
||||
assert values["BRAKE_REQUEST"] == 0
|
||||
assert values["BRAKE_LIGHTS"] == 0
|
||||
|
||||
def test_bosch_acc_commands_do_not_send_gas_without_positive_force(self):
|
||||
values = self._acc_control_values(True, 0.2, gas=500, gas_force=-0.4)
|
||||
|
||||
assert values["GAS_COMMAND"] == -30000
|
||||
|
||||
def test_bosch_braking_uses_force_hysteresis(self):
|
||||
braking = update_honda_bosch_braking(False, BOSCH_BRAKE_FORCE_ON - 0.01, False, True)
|
||||
assert braking
|
||||
|
||||
braking = update_honda_bosch_braking(braking, -0.05, False, True)
|
||||
assert braking
|
||||
|
||||
braking = update_honda_bosch_braking(braking, BOSCH_BRAKE_FORCE_RELEASE + 0.01, False, True)
|
||||
assert not braking
|
||||
|
||||
def test_bosch_braking_preserves_stopping_and_resets_inactive(self):
|
||||
assert update_honda_bosch_braking(False, 0.5, True, True)
|
||||
assert not update_honda_bosch_braking(True, -1.0, False, False)
|
||||
|
||||
def test_honda_lkas_hud_shows_lane_lines_when_lateral_only_is_active(self):
|
||||
class FakePacker:
|
||||
@staticmethod
|
||||
|
||||
@@ -4,18 +4,20 @@ from dataclasses import dataclass
|
||||
# hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
import numpy as np
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs
|
||||
from opendbc.car import Bus, DT_CTRL, create_gas_interceptor_command, make_tester_present_msg, rate_limit, structs
|
||||
from opendbc.car.common.filter_simple import FirstOrderFilter
|
||||
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance, get_max_angle_delta_vm, get_max_angle_vm
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.hyundai import hyundaicanfd, hyundaican
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.lead_data import CanLeadDataState
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, HyundaiSafetyFlags, HyundaiStarPilotFlags, Buttons, CarControllerParams, CAR, CANFD_ANGLE_LONGITUDINAL_CAR, \
|
||||
CANFD_RADAR_LIVE_LONGITUDINAL_CAR, CANFD_ALT_BUTTONS_RESUME_CAR, kia_ev6_gt_line_longitudinal_tuning, \
|
||||
CANFD_RADAR_ECU_KEEPALIVE_CAR, CANFD_ALT_BUTTONS_RESUME_CAR, kia_ev6_gt_line_longitudinal_tuning, \
|
||||
KIA_EV6_GT_LINE_LONG_TUNING_TESTING_GROUND_ID
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_hyundai_canfd_scc_jerk_limits, shape_hyundai_canfd_scc_accel
|
||||
from openpilot.starpilot.common.testing_grounds import testing_ground
|
||||
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
@@ -40,6 +42,9 @@ IONIQ_6_RESPONSE_MULTIPLIER = 1.2
|
||||
IONIQ_6_CANFD_SCC_ACCEL_STEP = (6.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER
|
||||
IONIQ_6_CANFD_SCC_DECEL_STEP = (15.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER
|
||||
EV9_CANFD_SCC_DECEL_STEP = 10.0 / 50.0
|
||||
RAY_PEDAL_COMMAND_CAP = 0.35 # Ray firmware voltage scaling is route-derived; validate before raising.
|
||||
RAY_PEDAL_RATE_UP = 0.012 # per 25 Hz command (0.30 normalized pedal per second)
|
||||
RAY_PEDAL_RATE_DOWN = 0.06
|
||||
GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0]
|
||||
GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.10, -0.10, -0.12, -0.18, -0.30, -0.50, -0.75, -1.00, -1.40, -1.80]
|
||||
GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0]
|
||||
@@ -473,6 +478,7 @@ class CarController(CarControllerBase):
|
||||
self._ioniq_6_lane_change_ui_frames = 0
|
||||
self._ioniq_6_long_tuning = Ioniq6LongitudinalTuningState()
|
||||
self._genesis_g90_long_tuning = GenesisG90LongitudinalTuningState()
|
||||
self._can_lead_data = CanLeadDataState()
|
||||
self._dash_lat_disengage_blink_frame = 0
|
||||
self._dash_lat_disengage_init = False
|
||||
self._dash_prev_lat_active = False
|
||||
@@ -482,6 +488,9 @@ class CarController(CarControllerBase):
|
||||
CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS
|
||||
)
|
||||
self._ray_lfa_packer = CANPacker("hyundai_kia_ray_lfa") if self._ray_lfa_8byte else None
|
||||
self._ray_pedal = CP.carFingerprint == CAR.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED
|
||||
self._ray_pedal_packer = CANPacker("hyundai_kia_ray_pedal") if self._ray_pedal else None
|
||||
self._ray_pedal_gas_last = 0.0
|
||||
|
||||
def _update_dash_icon_state(self, CC):
|
||||
if CC.latActive:
|
||||
@@ -502,7 +511,9 @@ class CarController(CarControllerBase):
|
||||
return lka_icon, lfa_icon
|
||||
|
||||
def _get_canfd_scc_lead_state(self, CC, CS, now_nanos):
|
||||
openpilot_lead_visible = bool(getattr(CS, "openpilot_lead_visible", False) or CC.hudControl.leadVisible)
|
||||
openpilot_lead_visible = bool(
|
||||
getattr(CS, "openpilot_lead_visible", False) or getattr(CC.hudControl, "leadVisible", False)
|
||||
)
|
||||
openpilot_lead_distance = float(np.clip(getattr(CS, "openpilot_lead_distance", 0.0), 0.0, 204.7))
|
||||
openpilot_lead_rel_speed = float(np.clip(getattr(CS, "openpilot_lead_rel_speed", 0.0), -16.4, 34.7))
|
||||
stock_camera_lead_fresh = now_nanos - getattr(CS, "stock_camera_lead_ts", 0) <= CANFD_CAMERA_LEAD_STALE_NS
|
||||
@@ -756,6 +767,13 @@ class CarController(CarControllerBase):
|
||||
can_canfd_blended = bool(self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED)
|
||||
blended_hda2 = can_canfd_blended and bool(self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING)
|
||||
longitudinal_active = bool(self.long_active_ecu and getattr(CC, "longActive", False))
|
||||
lead_visible = bool(getattr(CS, "openpilot_lead_visible", False) or getattr(hud_control, "leadVisible", False))
|
||||
lead_distance = float(np.clip(getattr(CS, "openpilot_lead_distance", 0.0), 0.0, 204.7))
|
||||
lead_rel_speed = float(np.clip(getattr(CS, "openpilot_lead_rel_speed", 0.0), -170.0, 239.5))
|
||||
if lead_visible and lead_distance <= CANFD_LEAD_MIN_DISTANCE:
|
||||
lead_distance = CANFD_FALLBACK_LEAD_DISTANCE
|
||||
lead_rel_speed = 0.0
|
||||
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,
|
||||
@@ -764,6 +782,7 @@ class CarController(CarControllerBase):
|
||||
if blended_hda2:
|
||||
can_sends.extend(hyundaicanfd.create_steering_messages(
|
||||
self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, 0.0,
|
||||
lka_icon=lka_icon,
|
||||
longitudinal_active=longitudinal_active,
|
||||
))
|
||||
if self.long_active_ecu:
|
||||
@@ -774,6 +793,7 @@ class CarController(CarControllerBase):
|
||||
left_lane_warning, right_lane_warning, CS.msg_364,
|
||||
include_alerts=False,
|
||||
counter_mod=0xF,
|
||||
fcw_opt_usm=2 if apply_steer_req or lka_icon == 3 else 1,
|
||||
))
|
||||
if self.frame % 5 == 0:
|
||||
can_sends.append(hyundaicanfd.create_suppress_lfa(
|
||||
@@ -799,7 +819,11 @@ class CarController(CarControllerBase):
|
||||
if not self.long_active_ecu:
|
||||
if self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES:
|
||||
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
|
||||
elif CC.cruiseControl.resume:
|
||||
elif self._ray_pedal and CC.longActive and CS.out.cruiseState.enabled:
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
|
||||
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
|
||||
self.last_button_frame = self.frame
|
||||
elif CC.cruiseControl.resume and not self._ray_pedal:
|
||||
# send resume at a max freq of 10Hz
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
|
||||
# send 25 messages at a time to increases the likelihood of resume being accepted
|
||||
@@ -807,7 +831,24 @@ class CarController(CarControllerBase):
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
|
||||
self.last_button_frame = self.frame
|
||||
else:
|
||||
can_sends.extend(self._create_can_redneck_button_messages(CS))
|
||||
if not self._ray_pedal:
|
||||
can_sends.extend(self._create_can_redneck_button_messages(CS))
|
||||
|
||||
if self._ray_pedal and self.frame % 4 == 0:
|
||||
pedal_ready = CS.ray_pedal_valid and CS.ray_pedal_state == 0
|
||||
pedal_active = (CC.longActive and pedal_ready and not CC.cruiseControl.override and
|
||||
not CS.out.gasPressed and not CS.out.brakePressed and
|
||||
not CS.out.cruiseState.enabled and CS.out.vEgo >= self.CP.minEnableSpeed)
|
||||
if pedal_active:
|
||||
target = float(np.clip(accel / CarControllerParams.ACCEL_MAX * RAY_PEDAL_COMMAND_CAP,
|
||||
0.0, RAY_PEDAL_COMMAND_CAP))
|
||||
self._ray_pedal_gas_last = rate_limit(
|
||||
target, self._ray_pedal_gas_last, -RAY_PEDAL_RATE_DOWN, RAY_PEDAL_RATE_UP,
|
||||
)
|
||||
else:
|
||||
self._ray_pedal_gas_last = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(
|
||||
self._ray_pedal_packer, self._ray_pedal_gas_last, (self.frame // 4) & 0xF))
|
||||
|
||||
if self.long_active_ecu and can_canfd_blended:
|
||||
if blended_hda2:
|
||||
@@ -835,7 +876,7 @@ class CarController(CarControllerBase):
|
||||
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
|
||||
hud_control, set_speed_in_units, stopping,
|
||||
CC.cruiseControl.override, use_fca, self.CP,
|
||||
main_cruise_enabled))
|
||||
main_cruise_enabled, lead_data))
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
if self.frame % 5 == 0 and (self.CP.flags & HyundaiFlags.SEND_LFA.value or (self.long_active_ecu and blended_hda2)):
|
||||
@@ -859,8 +900,13 @@ class CarController(CarControllerBase):
|
||||
can_sends = []
|
||||
|
||||
lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
||||
longitudinal_active = bool(self.long_active_ecu and getattr(CC, "longActive", False))
|
||||
lfa_longitudinal_active = longitudinal_active if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN else self.CP.openpilotLongitudinalControl
|
||||
persistent_lfa_status_cars = (
|
||||
CAR.HYUNDAI_IONIQ_6,
|
||||
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
|
||||
CAR.KIA_EV6,
|
||||
)
|
||||
lfa_longitudinal_active = self.CP.openpilotLongitudinalControl \
|
||||
if self.CP.carFingerprint in persistent_lfa_status_cars else self.long_active_ecu
|
||||
lka_steering_long = lka_steering and lfa_longitudinal_active
|
||||
ccnc_non_hda2 = self.CP.flags & HyundaiFlags.CCNC and not lka_steering
|
||||
use_egmp_dynamic_long_tuning = egmp_dynamic_longitudinal_tuning(self.CP) and self.long_active_ecu and \
|
||||
@@ -887,7 +933,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
gear = getattr(getattr(CS, "out", None), "gearShifter", None)
|
||||
drive_gear = gear == structs.CarState.GearShifter.drive
|
||||
if angle_lkas_alt:
|
||||
if angle_lkas_alt and self.CP.carFingerprint != CAR.KIA_SPORTAGE_HEV_2026:
|
||||
steering_msg_active = bool(steering_msg_active and drive_gear)
|
||||
angle_lkas_alt_standstill_handoff = bool(getattr(CS.out, "standstill", False) and not CC.latActive)
|
||||
forward_stock_lkas = self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and angle_lkas_alt and (
|
||||
@@ -917,7 +963,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
# prevent LFA from activating on LKA steering cars by sending "no lane lines detected" to ADAS ECU
|
||||
suppress_lfa = bool(lka_steering)
|
||||
if angle_lkas_alt:
|
||||
if angle_lkas_alt and self.CP.carFingerprint != CAR.KIA_SPORTAGE_HEV_2026:
|
||||
suppress_lfa = bool(lka_steering and drive_gear and (CC.latActive or (ccnc_angle_long and CC.enabled)))
|
||||
if self.frame % 5 == 0 and suppress_lfa:
|
||||
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.lfa_block_msg,
|
||||
@@ -987,12 +1033,14 @@ class CarController(CarControllerBase):
|
||||
left_sound_active=left_warning.sound_active, right_sound_active=right_warning.sound_active,
|
||||
)
|
||||
else:
|
||||
adrv_messages = hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)
|
||||
adrv_messages = hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame,
|
||||
car_fingerprint=self.CP.carFingerprint,
|
||||
drive_gear=drive_gear)
|
||||
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.
|
||||
radar_heartbeat_step = 1 if ccnc_angle_long else 4
|
||||
if self.CP.carFingerprint in CANFD_RADAR_LIVE_LONGITUDINAL_CAR and self.frame % radar_heartbeat_step == 0:
|
||||
if self.CP.carFingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR and self.frame % radar_heartbeat_step == 0:
|
||||
can_sends.append(hyundaicanfd.create_accelerator_brake_alt_spoof(0, self.frame // radar_heartbeat_step,
|
||||
CS.out.brakePressed, CS.out.gasPressed,
|
||||
self.CP.carFingerprint))
|
||||
@@ -1016,10 +1064,23 @@ class CarController(CarControllerBase):
|
||||
CC.leftBlinker,
|
||||
CC.rightBlinker))
|
||||
if self.frame % 2 == 0:
|
||||
lead_visible, lead_distance, lead_rel_speed = self._get_canfd_scc_lead_state(CC, CS, now_nanos)
|
||||
if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN:
|
||||
acc_kwargs = {}
|
||||
scc_jerk_limits = get_hyundai_canfd_scc_jerk_limits(self.CP, stopping, accel)
|
||||
raw_accel = accel
|
||||
accel = shape_hyundai_canfd_scc_accel(
|
||||
self.CP, CC.enabled, CC.cruiseControl.override, stopping, accel, self.accel_last,
|
||||
)
|
||||
acc_kwargs = {
|
||||
"direct_accel": True,
|
||||
"raw_accel": raw_accel,
|
||||
"jerk_upper": scc_jerk_limits[0],
|
||||
"jerk_lower": scc_jerk_limits[1],
|
||||
"lead_distance": lead_distance,
|
||||
"lead_rel_speed": lead_rel_speed,
|
||||
"lead_visible": lead_visible,
|
||||
}
|
||||
else:
|
||||
lead_visible, lead_distance, lead_rel_speed = self._get_canfd_scc_lead_state(CC, CS, now_nanos)
|
||||
acc_kwargs = {
|
||||
"main_mode_acc": int(CS.out.cruiseState.available),
|
||||
"direct_accel": True,
|
||||
|
||||
@@ -138,6 +138,9 @@ 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)
|
||||
if CP.carFingerprint == CAR.KIA_RAY_EV:
|
||||
self.ray_pedal_state = 5
|
||||
self.ray_pedal_valid = False
|
||||
|
||||
self.cruise_info = {}
|
||||
self.msg_161 = {}
|
||||
@@ -300,6 +303,7 @@ class CarState(CarStateBase):
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
cp_alt = can_parsers.get(Bus.alt)
|
||||
cp_pedal = can_parsers.get(Bus.party)
|
||||
|
||||
if self.CP.flags & HyundaiFlags.CANFD:
|
||||
return self.update_canfd(can_parsers)
|
||||
@@ -393,6 +397,11 @@ class CarState(CarStateBase):
|
||||
ret.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1
|
||||
ret.espActive = cp.vl["TCS11"]["ABS_ACT"] == 1
|
||||
ret.accFaulted = False if no_scc else cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
|
||||
if self.CP.carFingerprint == CAR.KIA_RAY_EV and self.CP.enableGasInterceptorDEPRECATED:
|
||||
self.ray_pedal_valid = bool(cp_pedal is not None and cp_pedal.can_valid and
|
||||
cp_pedal.ts_nanos["GAS_SENSOR"]["STATE"] > 0)
|
||||
self.ray_pedal_state = int(cp_pedal.vl["GAS_SENSOR"]["STATE"]) if cp_pedal is not None else 5
|
||||
ret.accFaulted = not self.ray_pedal_valid or self.ray_pedal_state != 0
|
||||
|
||||
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV | HyundaiFlags.FCEV):
|
||||
if self.CP.flags & HyundaiFlags.FCEV:
|
||||
@@ -404,6 +413,12 @@ class CarState(CarStateBase):
|
||||
else:
|
||||
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
|
||||
|
||||
if self.CP.carFingerprint == CAR.KIA_RAY_EV and self.CP.enableGasInterceptorDEPRECATED and self.ray_pedal_valid:
|
||||
driver_pedal = cp_pedal.vl_raw["GAS_SENSOR"]
|
||||
track1 = int.from_bytes(driver_pedal[:2], "big")
|
||||
track2 = int.from_bytes(driver_pedal[2:4], "big")
|
||||
ret.gasPressed = track1 > 272 or track2 > 513
|
||||
|
||||
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
|
||||
# as this seems to be standard over all cars, but is not the preferred method.
|
||||
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
|
||||
@@ -748,4 +763,6 @@ class CarState(CarStateBase):
|
||||
}
|
||||
if CP.carFingerprint in ALT_BUS_LDA_BUTTON_CARS:
|
||||
parsers[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.pt], [("CLU13", 0)], 1)
|
||||
if CP.carFingerprint == CAR.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED:
|
||||
parsers[Bus.party] = CANParser("hyundai_kia_ray_pedal", [("GAS_SENSOR", 50)], 0)
|
||||
return parsers
|
||||
|
||||
@@ -185,6 +185,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300',
|
||||
b'\xf1\x00DN ESC \x01 107 \x07\x03 58910-L1300',
|
||||
b'\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300',
|
||||
b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100',
|
||||
b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100',
|
||||
@@ -208,6 +209,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC102',
|
||||
b'\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103',
|
||||
b'\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1030 4DNDC103',
|
||||
b'\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1210 4DNDC103',
|
||||
b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100',
|
||||
b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP101',
|
||||
b'\xf1\x00DN8 MDPS R 1.00 1.02 57700-L1000 4DNDP105',
|
||||
@@ -215,6 +217,7 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422',
|
||||
b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.04 99211-L1000 191016',
|
||||
b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.06 99211-L1000 210325',
|
||||
b'\xf1\x00DN8 MFC AT RUS LHD 1.00 1.03 99211-L1000 190705',
|
||||
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.00 99211-L0000 190716',
|
||||
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L0000 191016',
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
import crcmod
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.lead_data import CanLeadData
|
||||
from opendbc.car.hyundai.values import CAR, HyundaiFlags
|
||||
|
||||
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
|
||||
@@ -51,7 +52,7 @@ def create_lkas11(packer, frame, CP, apply_torque, steer_req,
|
||||
# FcwOpt_USM 2 = Green car + lanes
|
||||
# FcwOpt_USM 1 = White car + lanes
|
||||
# FcwOpt_USM 0 = No car + lanes
|
||||
values["CF_Lkas_FcwOpt_USM"] = lka_icon if CP.carFingerprint == CAR.GENESIS_G70_2020 else 2 if enabled else 1
|
||||
values["CF_Lkas_FcwOpt_USM"] = lka_icon
|
||||
|
||||
# SysWarning 4 = keep hands on wheel
|
||||
# SysWarning 5 = keep hands on wheel (red)
|
||||
@@ -128,13 +129,13 @@ def create_lkas11_can_canfd_blended(packer, frame, CP, apply_steer, steer_req,
|
||||
torque_fault, lkas11, sys_warning, sys_state, enabled,
|
||||
left_lane, right_lane,
|
||||
left_lane_depart, right_lane_depart, msg_364,
|
||||
include_alerts=True, counter_mod=0x10):
|
||||
include_alerts=True, counter_mod=0x10, fcw_opt_usm=None):
|
||||
bus = CanBus(CP).ECAN
|
||||
values = {
|
||||
"CF_Lkas_LdwsActivemode": int(left_lane) + (int(right_lane) << 1),
|
||||
"CF_Lkas_LdwsLHWarning": left_lane_depart,
|
||||
"CF_Lkas_LdwsRHWarning": right_lane_depart,
|
||||
"CF_Lkas_FcwOpt_USM": 2 if enabled else 1,
|
||||
"CF_Lkas_FcwOpt_USM": (2 if enabled else 1) if fcw_opt_usm is None else fcw_opt_usm,
|
||||
"CR_Lkas_StrToqReq": apply_steer,
|
||||
"CF_Lkas_ActToi": steer_req,
|
||||
"CF_Lkas_ToiFlt": torque_fault,
|
||||
@@ -317,19 +318,20 @@ def create_acc_commands_can_canfd_blended_hda2(packer, enabled, accel, accel_las
|
||||
|
||||
|
||||
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP,
|
||||
main_cruise_enabled=True):
|
||||
main_cruise_enabled=True, lead_data: CanLeadData | None = None):
|
||||
commands = []
|
||||
lead_data = lead_data or CanLeadData()
|
||||
|
||||
scc11_values = {
|
||||
"MainMode_ACC": int(bool(main_cruise_enabled)),
|
||||
"TauGapSet": hud_control.leadDistanceBars,
|
||||
"VSetDis": set_speed if enabled else 0,
|
||||
"AliveCounterACC": idx % 0x10,
|
||||
"ObjValid": 1, # close lead makes controls tighter
|
||||
"ACC_ObjStatus": 1, # close lead makes controls tighter
|
||||
"ObjValid": int(lead_data.lead_visible),
|
||||
"ACC_ObjStatus": int(lead_data.lead_visible),
|
||||
"ACC_ObjLatPos": 0,
|
||||
"ACC_ObjRelSpd": 0,
|
||||
"ACC_ObjDist": 1, # close lead makes controls tighter
|
||||
"ACC_ObjRelSpd": lead_data.lead_rel_speed,
|
||||
"ACC_ObjDist": int(lead_data.lead_distance),
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
|
||||
|
||||
@@ -357,7 +359,8 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
|
||||
"JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
|
||||
"JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
|
||||
"ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
||||
"ObjGap": 2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
|
||||
"ObjGap": lead_data.object_gap, # 5: >30 m, 4: 25-30 m, 3: 20-25 m, 2: <20 m, 0: no lead
|
||||
"ObjDistStat": lead_data.object_rel_gap,
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
|
||||
|
||||
|
||||
@@ -8,6 +8,34 @@ from opendbc.car.crc import CRC16_XMODEM
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, CAR, CANFD_ALT_BUTTONS_RESUME_CAR
|
||||
|
||||
|
||||
_adrv_0x51_templates: dict[CAR, bytes] = {}
|
||||
|
||||
|
||||
def cache_adrv_0x51_template(car_fingerprint: CAR, dat: bytes | None) -> None:
|
||||
if car_fingerprint != CAR.KIA_EV6:
|
||||
return
|
||||
|
||||
if dat is None:
|
||||
_adrv_0x51_templates.pop(car_fingerprint, None)
|
||||
elif len(dat) == 32 and any(dat[3:]):
|
||||
_adrv_0x51_templates[car_fingerprint] = bytes(dat)
|
||||
|
||||
|
||||
def create_adrv_0x51(packer, CAN, frame: int, car_fingerprint: CAR | None = None, drive_gear: bool = False):
|
||||
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)
|
||||
crc = hkg_can_fd_checksum(0x51, None, dat)
|
||||
dat[0] = crc & 0xFF
|
||||
dat[1] = (crc >> 8) & 0xFF
|
||||
return CanData(0x51, bytes(dat), CAN.ACAN)
|
||||
|
||||
|
||||
def _set_value(msg: bytearray, sig, ival: int) -> None:
|
||||
i = sig.lsb // 8
|
||||
bits = sig.size
|
||||
@@ -123,7 +151,12 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
|
||||
else:
|
||||
lkas_values = copy.copy(control_values)
|
||||
lkas_values["LKA_AVAILABLE"] = 0
|
||||
if CP.carFingerprint in (CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_CARNIVAL_2025, CAR.KIA_CARNIVAL_HEV_4TH_GEN):
|
||||
if CP.carFingerprint in (
|
||||
CAR.KIA_CARNIVAL_4TH_GEN,
|
||||
CAR.KIA_CARNIVAL_2025,
|
||||
CAR.KIA_CARNIVAL_HEV_4TH_GEN,
|
||||
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
|
||||
):
|
||||
lkas_values["DAMP_FACTOR"] = 100
|
||||
|
||||
if lfa_base_values:
|
||||
@@ -142,7 +175,21 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
|
||||
lkas_values["LKAS_ANGLE_ACTIVE"] = 2 if lat_active else 1
|
||||
lkas_values["ADAS_ACIAnglTqRedcGainVal"] = apply_torque if lat_active else 0.0
|
||||
if angle_lkas_alt:
|
||||
if lat_active:
|
||||
if CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026:
|
||||
lkas_values = {
|
||||
"LKA_OptUsmSta": 0,
|
||||
"LKA_SysIndReq": 2 if enabled else 1,
|
||||
"StrTqReqVal": 0,
|
||||
"LKA_SysWrn": 0,
|
||||
"ActToiSta": 0,
|
||||
"LKA_UsmMod": 0,
|
||||
"LKA_RcgSta": 3 if lat_active else 0,
|
||||
"Damping_Gain": 100,
|
||||
"ADAS_StrAnglReqVal": apply_angle,
|
||||
"LKAS_ANGLE_ACTIVE": 2 if lat_active else 1,
|
||||
"ADAS_ACIAnglTqRedcGainVal": apply_torque if lat_active else 0.0,
|
||||
}
|
||||
elif lat_active:
|
||||
lkas_values = {
|
||||
"LKA_OptUsmSta": 0,
|
||||
"LKA_RcgSta": 3,
|
||||
@@ -685,13 +732,13 @@ def create_ioniq_6_cluster_lane_change_messages(CAN, frame, side=None):
|
||||
|
||||
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control,
|
||||
main_mode_acc=1, jerk_lower=None, jerk_upper=None, direct_accel=False,
|
||||
lead_distance=None, lead_rel_speed=None, lead_visible=None, cruise_info=None):
|
||||
lead_distance=None, lead_rel_speed=None, lead_visible=None, cruise_info=None, raw_accel=None):
|
||||
jerk = 5
|
||||
jn = jerk / 50
|
||||
if not enabled or gas_override:
|
||||
a_val, a_raw = 0, 0
|
||||
elif direct_accel:
|
||||
a_raw = accel
|
||||
a_raw = accel if raw_accel is None else raw_accel
|
||||
a_val = accel
|
||||
else:
|
||||
a_raw = accel
|
||||
@@ -769,15 +816,13 @@ def create_fca_warning_light(packer, CAN, frame):
|
||||
return ret
|
||||
|
||||
|
||||
def create_adrv_messages(packer, CAN, frame, blended_hda2=False):
|
||||
def create_adrv_messages(packer, CAN, frame, blended_hda2=False, car_fingerprint=None, drive_gear=False):
|
||||
# messages needed to car happy after disabling
|
||||
# the ADAS Driving ECU to do longitudinal control
|
||||
|
||||
ret = []
|
||||
|
||||
values = {
|
||||
}
|
||||
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
|
||||
ret.append(create_adrv_0x51(packer, CAN, frame, car_fingerprint, drive_gear))
|
||||
|
||||
if blended_hda2:
|
||||
return ret
|
||||
|
||||
@@ -2,12 +2,13 @@ import time
|
||||
# Provenance: portions of HKG angle integration are adapted from sunnypilot/opendbc's
|
||||
# hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
from opendbc.car import get_safety_config, structs, uds
|
||||
from opendbc.car.hyundai import hyundaicanfd
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, CAR, CarControllerParams, \
|
||||
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, \
|
||||
CANFD_SECURITYACCESS_CAR, \
|
||||
CANFD_ANGLE_LONGITUDINAL_CAR, \
|
||||
CANFD_RADAR_LIVE_LONGITUDINAL_CAR, \
|
||||
CANFD_RADAR_ECU_KEEPALIVE_CAR, \
|
||||
RADAR_LIVE_LONGITUDINAL_CAR, \
|
||||
UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, \
|
||||
LEGACY_LONGITUDINAL_CAR, \
|
||||
@@ -27,6 +28,15 @@ from openpilot.starpilot.common.testing_grounds import testing_ground
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
Ecu = structs.CarParams.Ecu
|
||||
|
||||
|
||||
def get_communication_control_request(car_fingerprint):
|
||||
if car_fingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR:
|
||||
return bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
|
||||
return bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.DISABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
|
||||
# Cancel button can sometimes be ACC pause/resume button, main button can also enable on some cars
|
||||
ENABLE_BUTTONS = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.cancel, ButtonType.mainCruise)
|
||||
|
||||
@@ -34,6 +44,7 @@ ENABLE_BUTTONS = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.can
|
||||
ECU_DISABLE_TIMESTAMP = 0.0
|
||||
KONA_NON_SCC_FCA_RADAR_ADDR = 0x602
|
||||
KIA_EV9_ACCEL_MAX = 2.2
|
||||
RAY_PEDAL_SENSOR_ADDR = 0x201
|
||||
|
||||
|
||||
def apply_platform_longitudinal_params(ret: structs.CarParams) -> None:
|
||||
@@ -293,6 +304,18 @@ class CarInterface(CarInterfaceBase):
|
||||
elif ret.flags & HyundaiFlags.FCEV:
|
||||
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.FCEV_GAS.value
|
||||
|
||||
if (candidate == CAR.KIA_RAY_EV and fingerprint[0].get(RAY_PEDAL_SENSOR_ADDR) == 6 and
|
||||
ret.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS and
|
||||
ret.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON):
|
||||
ret.enableGasInterceptorDEPRECATED = True
|
||||
ret.alphaLongitudinalAvailable = True
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.pcmCruise = False
|
||||
ret.radarUnavailable = True
|
||||
ret.autoResumeSng = False
|
||||
ret.minEnableSpeed = 5.0 # pedal-only: no commanded friction brake/standstill hold
|
||||
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.LONG.value
|
||||
|
||||
# Car specific configuration overrides
|
||||
|
||||
if candidate == CAR.GENESIS_G90:
|
||||
@@ -353,14 +376,7 @@ class CarInterface(CarInterfaceBase):
|
||||
params = Params()
|
||||
|
||||
if communication_control is None:
|
||||
if CP.carFingerprint in CANFD_RADAR_LIVE_LONGITUDINAL_CAR:
|
||||
# Don't use 0x80 suppress bit so we can read the ECU response.
|
||||
# Use ENABLE_RX_DISABLE_TX (0x01) so the ECU can still receive from rear radars for BSM
|
||||
# while blocking SCC TX.
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
|
||||
else:
|
||||
# 0x80 silences response for other cars (original behavior)
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.DISABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
|
||||
communication_control = get_communication_control_request(CP.carFingerprint)
|
||||
|
||||
ecu_log(f"=== init() called: opLong={CP.openpilotLongitudinalControl}, flags=0x{CP.flags:x}, safetyParam={CP.safetyConfigs[-1].safetyParam} ===")
|
||||
|
||||
@@ -376,11 +392,25 @@ class CarInterface(CarInterfaceBase):
|
||||
skip_disable_ecu = True
|
||||
|
||||
if not skip_disable_ecu:
|
||||
disable_can_recv = can_recv
|
||||
if CP.carFingerprint == CAR.KIA_EV6 and can_recv is not None:
|
||||
hyundaicanfd.cache_adrv_0x51_template(CP.carFingerprint, None)
|
||||
base_can_recv = can_recv
|
||||
adrv_bus = CanBus(CP).ACAN
|
||||
|
||||
def disable_can_recv(*args, **kwargs):
|
||||
packets = base_can_recv(*args, **kwargs)
|
||||
for packet in packets or []:
|
||||
for msg in packet:
|
||||
if msg.src == adrv_bus and msg.address == 0x51:
|
||||
hyundaicanfd.cache_adrv_0x51_template(CP.carFingerprint, msg.dat)
|
||||
return packets
|
||||
|
||||
# Try ECU disable. If it succeeds (IGN-ON mode), enable longitudinal.
|
||||
# If it fails (READY mode returns NRC 0x22, or timeout), strip LONG safety flag
|
||||
# so panda forwards stock SCC messages normally (lateral-only mode).
|
||||
ecu_log(f"=== ECU DISABLE attempt: addr=0x{addr:x}, bus={bus} ===")
|
||||
ecu_disabled = disable_ecu(can_recv, can_send, bus=bus, addr=addr, com_cont_req=communication_control,
|
||||
ecu_disabled = disable_ecu(disable_can_recv, can_send, bus=bus, addr=addr, com_cont_req=communication_control,
|
||||
reset=bool(CP.flags & HyundaiFlags.CAN_CANFD_BLENDED))
|
||||
|
||||
if CP.carFingerprint in (CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5_PE):
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CanLeadData:
|
||||
object_gap: int = 0
|
||||
lead_distance: float = 0.0
|
||||
lead_rel_speed: float = 0.0
|
||||
lead_visible: bool = False
|
||||
|
||||
@property
|
||||
def object_rel_gap(self) -> int:
|
||||
return 0 if self.lead_distance == 0 else 2 if self.lead_rel_speed < -0.2 else 1
|
||||
|
||||
|
||||
def _hysteresis_update(current, new_value, counter, threshold):
|
||||
if new_value == current:
|
||||
return current, 0
|
||||
|
||||
counter += 1
|
||||
return (new_value, 0) if counter >= threshold else (current, counter)
|
||||
|
||||
|
||||
class CanLeadDataState:
|
||||
LEAD_HYSTERESIS_FRAMES = 50
|
||||
|
||||
def __init__(self):
|
||||
self._lead_on_counter = 0
|
||||
self._lead_off_counter = 0
|
||||
self._gap_counter = 0
|
||||
self._lead_visible = False
|
||||
self._object_gap = 0
|
||||
|
||||
@staticmethod
|
||||
def _get_object_gap(lead_distance: float) -> int:
|
||||
if lead_distance == 0:
|
||||
return 0
|
||||
if lead_distance < 20:
|
||||
return 2
|
||||
if lead_distance < 25:
|
||||
return 3
|
||||
if lead_distance < 30:
|
||||
return 4
|
||||
return 5
|
||||
|
||||
def update(self, lead_distance: float, lead_rel_speed: float, lead_visible: bool) -> CanLeadData:
|
||||
counter = self._lead_on_counter if lead_visible else self._lead_off_counter
|
||||
self._lead_visible, counter = _hysteresis_update(
|
||||
self._lead_visible, lead_visible, counter, self.LEAD_HYSTERESIS_FRAMES,
|
||||
)
|
||||
if lead_visible:
|
||||
self._lead_on_counter = counter
|
||||
self._lead_off_counter = 0
|
||||
else:
|
||||
self._lead_off_counter = counter
|
||||
self._lead_on_counter = 0
|
||||
|
||||
object_gap = self._get_object_gap(lead_distance)
|
||||
self._object_gap, self._gap_counter = _hysteresis_update(
|
||||
self._object_gap, object_gap, self._gap_counter, self.LEAD_HYSTERESIS_FRAMES,
|
||||
)
|
||||
|
||||
return CanLeadData(self._object_gap, lead_distance, lead_rel_speed, self._lead_visible)
|
||||
@@ -19,6 +19,9 @@ MRR30_RADAR_START_ADDR = 0x210
|
||||
MRR30_RADAR_MSG_COUNT = 16
|
||||
MRR35_RADAR_START_ADDR = 0x3A5
|
||||
MRR35_RADAR_MSG_COUNT = 32
|
||||
GV70_RADAR_START_ADDR = 0x210
|
||||
GV70_RADAR_MSG_COUNT = 16
|
||||
GV70_RADAR_DBC = "hyundai_radar_210_21f_generated"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -30,6 +33,7 @@ class RadarTrackConfig:
|
||||
frequency: int = 50
|
||||
parser_msg_count: int | None = None
|
||||
expected_length: int | None = None
|
||||
dbc_name: str | None = None
|
||||
|
||||
@property
|
||||
def can_parser_msg_count(self) -> int:
|
||||
@@ -47,6 +51,10 @@ RADAR_TRACK_CONFIGS = {
|
||||
|
||||
|
||||
def get_radar_track_config(car_fingerprint, flags: int = 0) -> RadarTrackConfig | None:
|
||||
if car_fingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN:
|
||||
return RadarTrackConfig(GV70_RADAR_START_ADDR, GV70_RADAR_MSG_COUNT, "gv70_210", bus=0,
|
||||
frequency=20, expected_length=32, dbc_name=GV70_RADAR_DBC)
|
||||
|
||||
radar_dbc = DBC[car_fingerprint].get(Bus.radar)
|
||||
if car_fingerprint == CAR.GENESIS_G90 and radar_dbc == HYUNDAI_MANDO_FRONT_RADAR_DBC:
|
||||
return RadarTrackConfig(RADAR_START_ADDR, G90_RADAR_MSG_COUNT, "mando", parser_msg_count=RADAR_MSG_COUNT)
|
||||
@@ -65,6 +73,10 @@ def radar_tracks_available(radar_config: RadarTrackConfig | None, fingerprint) -
|
||||
if radar_config is None:
|
||||
return False
|
||||
|
||||
if radar_config.radar_type == "gv70_210":
|
||||
return all(fingerprint[radar_config.bus].get(addr) == radar_config.expected_length
|
||||
for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.msg_count))
|
||||
|
||||
msg_len = fingerprint[radar_config.bus].get(radar_config.start_addr)
|
||||
if msg_len is None:
|
||||
return False
|
||||
@@ -78,7 +90,8 @@ def get_radar_can_parser(CP, radar_config):
|
||||
|
||||
messages = [(f"RADAR_TRACK_{addr:x}", radar_config.frequency)
|
||||
for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.can_parser_msg_count)]
|
||||
return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, radar_config.bus)
|
||||
dbc_name = radar_config.dbc_name or DBC[CP.carFingerprint][Bus.radar]
|
||||
return CANParser(dbc_name, messages, radar_config.bus)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
@@ -223,6 +236,27 @@ class RadarInterface(RadarInterfaceBase):
|
||||
del self.pts[track_key]
|
||||
continue
|
||||
|
||||
if radar_type == "gv70_210":
|
||||
for i in ("1", "2"):
|
||||
track_key = addr * 2 + int(i) - 1
|
||||
valid = msg[f"{i}_STATE"] in (3, 4)
|
||||
if valid:
|
||||
pt = self.pts.get(track_key)
|
||||
if pt is None:
|
||||
pt = structs.RadarData.RadarPoint()
|
||||
pt.trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[track_key] = pt
|
||||
pt.measured = True
|
||||
pt.dRel = msg[f"{i}_LONG_DIST"]
|
||||
pt.yRel = msg[f"{i}_LAT_DIST"]
|
||||
pt.vRel = msg[f"{i}_REL_SPEED"]
|
||||
pt.aRel = msg[f"{i}_REL_ACCEL"]
|
||||
pt.yvRel = msg[f"{i}_REL_LAT_SPEED"]
|
||||
elif track_key in self.pts:
|
||||
del self.pts[track_key]
|
||||
continue
|
||||
|
||||
if radar_type == "mrrevo14f":
|
||||
for i in ("1", "2"):
|
||||
track_key = addr * 2 + int(i) - 1
|
||||
|
||||
@@ -24,16 +24,18 @@ from opendbc.car.hyundai.carcontroller import CarController, CANCEL_BUTTON_DELAY
|
||||
clear_ioniq_6_torque_when_request_inactive
|
||||
from opendbc.car.hyundai.carstate import CarState, 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
|
||||
from opendbc.car.hyundai.interface import CarInterface, KIA_EV9_ACCEL_MAX, get_communication_control_request
|
||||
from opendbc.car.hyundai import hyundaican, hyundaicanfd
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus, hkg_can_fd_checksum
|
||||
from opendbc.car.hyundai.lead_data import CanLeadData, CanLeadDataState
|
||||
from opendbc.car.hyundai.radar_interface import MRREVO14F_RADAR_START_ADDR, MRR30_RADAR_START_ADDR, MRR35_RADAR_START_ADDR, \
|
||||
RADAR_START_ADDR, get_radar_track_config
|
||||
RADAR_START_ADDR, RadarInterface, get_radar_track_config, radar_tracks_available
|
||||
from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, DATELESS_FUZZY_CARS, \
|
||||
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \
|
||||
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
|
||||
LEGACY_LONGITUDINAL_CAR, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \
|
||||
HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, kia_ev6_gt_line_longitudinal_tuning
|
||||
HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, \
|
||||
CANFD_RADAR_ECU_KEEPALIVE_CAR, CANFD_RADAR_LIVE_LONGITUDINAL_CAR, kia_ev6_gt_line_longitudinal_tuning
|
||||
|
||||
LongCtrlState = CarControl.Actuators.LongControlState
|
||||
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
|
||||
@@ -129,6 +131,78 @@ def get_test_toggles() -> SimpleNamespace:
|
||||
|
||||
|
||||
class TestHyundaiFingerprint:
|
||||
def test_egmp_communication_control_paths(self):
|
||||
stock_request = bytes([0x28, 0x83, 0x01])
|
||||
radar_keepalive_request = bytes([0x28, 0x01, 0x01])
|
||||
|
||||
assert CAR.KIA_EV6 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
|
||||
assert CAR.KIA_EV6 not in CANFD_RADAR_ECU_KEEPALIVE_CAR
|
||||
assert get_communication_control_request(CAR.KIA_EV6) == stock_request
|
||||
|
||||
assert CAR.HYUNDAI_IONIQ_5 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
|
||||
assert CAR.HYUNDAI_IONIQ_5 not in CANFD_RADAR_ECU_KEEPALIVE_CAR
|
||||
assert get_communication_control_request(CAR.HYUNDAI_IONIQ_5) == stock_request
|
||||
|
||||
assert CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
|
||||
assert CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN not in CANFD_RADAR_ECU_KEEPALIVE_CAR
|
||||
assert get_communication_control_request(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) == stock_request
|
||||
|
||||
assert get_communication_control_request(CAR.HYUNDAI_IONIQ_6) == radar_keepalive_request
|
||||
|
||||
def test_ev6_adrv_0x51_replays_factory_payload(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.KIA_EV6
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.EV)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
can_bus = CanBus(CP)
|
||||
factory = bytes.fromhex("88ed2e091700ffff5e0d0000012006ff021c2200000000000800000010000000")
|
||||
|
||||
hyundaicanfd.cache_adrv_0x51_template(CAR.KIA_EV6, factory)
|
||||
try:
|
||||
address, dat, bus = hyundaicanfd.create_adrv_0x51(packer, can_bus, 7, CAR.KIA_EV6, drive_gear=True)
|
||||
_, parked_dat, _ = hyundaicanfd.create_adrv_0x51(packer, can_bus, 8, CAR.KIA_EV6, drive_gear=False)
|
||||
_, other_dat, _ = hyundaicanfd.create_adrv_0x51(packer, can_bus, 7, CAR.HYUNDAI_IONIQ_6)
|
||||
finally:
|
||||
hyundaicanfd.cache_adrv_0x51_template(CAR.KIA_EV6, None)
|
||||
|
||||
assert address == 0x51
|
||||
assert bus == can_bus.ACAN
|
||||
assert dat[2] == (factory[2] + 8) & 0xFF
|
||||
assert dat[3:] == factory[3:]
|
||||
assert int.from_bytes(dat[:2], "little") == hkg_can_fd_checksum(address, None, bytearray(dat))
|
||||
assert parked_dat[3] == factory[3] & ~0x1
|
||||
assert parked_dat[4:] == factory[4:]
|
||||
assert int.from_bytes(parked_dat[:2], "little") == hkg_can_fd_checksum(address, None, bytearray(parked_dat))
|
||||
assert other_dat[3:] == bytes(29)
|
||||
|
||||
def test_ev6_init_captures_factory_adrv_0x51(self, monkeypatch):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[CanBus(None, fingerprint).CAM][0x50] = 16
|
||||
radar_config = get_radar_track_config(CAR.KIA_EV6)
|
||||
fingerprint[radar_config.bus][radar_config.start_addr] = radar_config.expected_length
|
||||
car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
|
||||
CP = CarInterface.get_params(CAR.KIA_EV6, fingerprint, car_fw, True, False, False, get_test_toggles())
|
||||
factory = bytes.fromhex("6b657d090900e1ff000000000020ffff00000000000000000800000010000000")
|
||||
|
||||
def can_recv(*, wait_for_one=True):
|
||||
msg = SimpleNamespace(address=0x51, src=CanBus(CP).ACAN, dat=factory)
|
||||
return [[msg]]
|
||||
|
||||
def fake_disable_ecu(capturing_can_recv, *_args, **_kwargs):
|
||||
capturing_can_recv(wait_for_one=True)
|
||||
return True
|
||||
|
||||
monkeypatch.setattr("opendbc.car.hyundai.interface.disable_ecu", fake_disable_ecu)
|
||||
CarInterface.init(CP, can_recv, None)
|
||||
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
try:
|
||||
_, dat, _ = hyundaicanfd.create_adrv_0x51(packer, CanBus(CP), 0, CAR.KIA_EV6, drive_gear=True)
|
||||
finally:
|
||||
hyundaicanfd.cache_adrv_0x51_template(CAR.KIA_EV6, None)
|
||||
|
||||
assert dat[3:] == factory[3:]
|
||||
|
||||
def test_carnival_hev_low_speed_torque_rate_limits(self):
|
||||
CP = CarInterface.get_params(CAR.KIA_CARNIVAL_HEV_4TH_GEN, gen_empty_fingerprint(), [],
|
||||
False, False, False, None)
|
||||
@@ -426,6 +500,42 @@ class TestHyundaiFingerprint:
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_LKA_STEERING
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.EV_GAS
|
||||
|
||||
gv70_radar_config = get_radar_track_config(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
|
||||
assert gv70_radar_config.radar_type == "gv70_210"
|
||||
for addr in range(gv70_radar_config.start_addr, gv70_radar_config.start_addr + gv70_radar_config.msg_count):
|
||||
gv70_fingerprint[gv70_radar_config.bus][addr] = gv70_radar_config.expected_length
|
||||
assert radar_tracks_available(gv70_radar_config, gv70_fingerprint)
|
||||
|
||||
CP = CarInterface.get_params(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, gv70_fingerprint, gv70_car_fw,
|
||||
True, False, False, None)
|
||||
assert not CP.radarUnavailable
|
||||
|
||||
radar = RadarInterface(CP)
|
||||
packer = CANPacker(gv70_radar_config.dbc_name)
|
||||
messages = []
|
||||
for addr in range(gv70_radar_config.start_addr, gv70_radar_config.start_addr + gv70_radar_config.msg_count):
|
||||
message = packer.make_can_msg(f"RADAR_TRACK_{addr:x}", 0, {
|
||||
"1_STATE": 3,
|
||||
"1_LONG_DIST": 25.0,
|
||||
"1_LAT_DIST": 0.5,
|
||||
"1_REL_SPEED": -2.0,
|
||||
"1_REL_LAT_SPEED": 0.1,
|
||||
"1_REL_ACCEL": -0.2,
|
||||
})
|
||||
data = bytearray(message[1])
|
||||
checksum = hkg_can_fd_checksum(addr, None, data)
|
||||
data[0] = checksum & 0xff
|
||||
data[1] = (checksum >> 8) & 0xff
|
||||
messages.append((message[0], bytes(data), message[2]))
|
||||
radar_data = radar.update([(1, messages)])
|
||||
assert radar_data is not None
|
||||
assert len(radar_data.points) == 16
|
||||
assert radar_data.points[0].dRel == pytest.approx(25.0)
|
||||
|
||||
other_config = get_radar_track_config(CAR.HYUNDAI_IONIQ_5)
|
||||
assert other_config.radar_type == "mrr30"
|
||||
assert other_config.dbc_name is None
|
||||
|
||||
for candidate in HYUNDAI_NON_SCC_CARS:
|
||||
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, None)
|
||||
assert bool(CP.flags & HyundaiFlags.NON_SCC)
|
||||
@@ -671,6 +781,45 @@ class TestHyundaiFingerprint:
|
||||
} <= msg_addrs_buses
|
||||
assert (0x364, 1) not in msg_addrs_buses
|
||||
|
||||
def test_palisade_telluride_hda2_aol_keeps_lkas_status_after_long_cancel(self):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[2][0x50] = 16
|
||||
car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, car_fw, True, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 1
|
||||
|
||||
hud_control = SimpleNamespace(
|
||||
visualAlert=CarControl.HUDControl.VisualAlert.none,
|
||||
leftLaneVisible=True,
|
||||
rightLaneVisible=True,
|
||||
leftLaneDepart=False,
|
||||
rightLaneDepart=False,
|
||||
leadDistanceBars=3,
|
||||
leadVisible=False,
|
||||
)
|
||||
lfa_block_msg = {f"BYTE{i}": 0 for i in range(3, 24) if i != 7}
|
||||
lfa_block_msg["COUNTER"] = 0
|
||||
CS = SimpleNamespace(lfa_block_msg=lfa_block_msg, redneck_send_button=Buttons.NONE, lkas11={}, msg_364={},
|
||||
out=SimpleNamespace(vEgoRaw=5.0))
|
||||
CC = SimpleNamespace(enabled=False, latActive=True, longActive=False,
|
||||
cruiseControl=SimpleNamespace(cancel=False, resume=False, override=False))
|
||||
actuators = SimpleNamespace(longControlState=LongCtrlState.off)
|
||||
adas_parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS", 0)], 0)
|
||||
ecan_parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0)], 1)
|
||||
|
||||
for steering_requested, icon, expected_status in ((True, 2, 2), (False, 1, 1)):
|
||||
msgs = controller.create_can_msgs(steering_requested, 16 if steering_requested else 0, False,
|
||||
0.0, 0.0, False, hud_control, actuators, CS, CC, icon, icon)
|
||||
adas_parser.update([(1, [msg for msg in msgs if msg[0] == 0x50])])
|
||||
ecan_parser.update([(1, [msg for msg in msgs if msg[0] == 0x340])])
|
||||
|
||||
assert adas_parser.vl["LKAS"]["LKA_ICON"] == icon
|
||||
assert adas_parser.vl["LKAS"]["STEER_REQ"] == int(steering_requested)
|
||||
assert ecan_parser.vl["LKAS11"]["CF_Lkas_FcwOpt_USM"] == expected_status
|
||||
assert ecan_parser.vl["LKAS11"]["CF_Lkas_ActToi"] == int(steering_requested)
|
||||
assert not any(msg[0] == 0x364 for msg in msgs)
|
||||
|
||||
def test_g70_aol_uses_active_lkas_icon(self):
|
||||
CP = CarInterface.get_params(CAR.GENESIS_G70_2020, gen_empty_fingerprint(), [], False, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
@@ -693,6 +842,48 @@ class TestHyundaiFingerprint:
|
||||
|
||||
assert parser.vl["LKAS11"]["CF_Lkas_FcwOpt_USM"] == 2
|
||||
|
||||
@pytest.mark.parametrize(("candidate", "expected_status"), (
|
||||
(CAR.KIA_NIRO_PHEV_2022, 2),
|
||||
(CAR.KIA_NIRO_HEV_2021, 2),
|
||||
))
|
||||
def test_classic_niro_aol_keeps_active_lkas_status(self, candidate, expected_status):
|
||||
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, get_test_toggles())
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 1
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0)], 0)
|
||||
|
||||
hud_control = SimpleNamespace(
|
||||
visualAlert=CarControl.HUDControl.VisualAlert.none,
|
||||
leftLaneVisible=True,
|
||||
rightLaneVisible=True,
|
||||
leftLaneDepart=False,
|
||||
rightLaneDepart=False,
|
||||
leadVisible=False,
|
||||
)
|
||||
CS = SimpleNamespace(lkas11=parser.vl["LKAS11"])
|
||||
CC = SimpleNamespace(
|
||||
enabled=False,
|
||||
latActive=True,
|
||||
longActive=False,
|
||||
cruiseControl=SimpleNamespace(cancel=False, resume=False, override=False),
|
||||
)
|
||||
actuators = SimpleNamespace(longControlState=LongCtrlState.off)
|
||||
|
||||
msgs = controller.create_can_msgs(True, 156, False, 0.0, 0.0, False, hud_control, actuators, CS, CC, 2, 0)
|
||||
lkas11 = next(msg for msg in msgs if msg[0] == 0x340)
|
||||
parser.update([(1, [lkas11])])
|
||||
|
||||
assert parser.vl["LKAS11"]["CF_Lkas_ActToi"] == 1
|
||||
assert parser.vl["LKAS11"]["CF_Lkas_FcwOpt_USM"] == expected_status
|
||||
|
||||
CC.latActive = False
|
||||
msgs = controller.create_can_msgs(False, 0, False, 0.0, 0.0, False, hud_control, actuators, CS, CC, 1, 0)
|
||||
lkas11 = next(msg for msg in msgs if msg[0] == 0x340)
|
||||
parser.update([(2, [lkas11])])
|
||||
|
||||
assert parser.vl["LKAS11"]["CF_Lkas_ActToi"] == 0
|
||||
assert parser.vl["LKAS11"]["CF_Lkas_FcwOpt_USM"] == 1
|
||||
|
||||
def test_kona_non_scc_uses_no_individual_lane_lkas_status(self):
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_KONA_NON_SCC, gen_empty_fingerprint(), [], False, False, False, None)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
@@ -2480,14 +2671,15 @@ class TestHyundaiFingerprint:
|
||||
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0)
|
||||
assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(8.5)
|
||||
|
||||
def test_gv70_electrified_uses_generic_lkas_status_payload(self):
|
||||
def test_gv70_electrified_uses_clean_damped_lkas_status_payload(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
|
||||
CP.openpilotLongitudinalControl = False
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 1
|
||||
controller.long_active_ecu = True
|
||||
can_bus = CanBus(CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS", 0)], can_bus.ACAN)
|
||||
stock_lkas = {
|
||||
@@ -2508,6 +2700,7 @@ class TestHyundaiFingerprint:
|
||||
"DAMP_FACTOR": 100,
|
||||
}
|
||||
cc = SimpleNamespace(enabled=True, latActive=True,
|
||||
longActive=False,
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||
leftBlinker=False, rightBlinker=False,
|
||||
hudControl=SimpleNamespace())
|
||||
@@ -2523,13 +2716,12 @@ class TestHyundaiFingerprint:
|
||||
parser.update([(1, lkas_msgs)])
|
||||
assert parser.can_valid
|
||||
assert parser.vl["LKAS"]["HAS_LANE_SAFETY"] == 0
|
||||
assert parser.vl["LKAS"]["DAMP_FACTOR"] == 0
|
||||
assert parser.vl["LKAS"]["DAMP_FACTOR"] == 100
|
||||
assert parser.vl["LKAS"]["TORQUE_REQUEST"] == 0
|
||||
assert parser.vl["LKAS"]["STEER_REQ"] == 1
|
||||
assert parser.vl["LKAS"]["STEER_MODE"] == 0
|
||||
assert parser.vl["LKAS"]["NEW_SIGNAL_2"] == 0
|
||||
assert parser.vl["LKAS"]["STEER_MODE"] == 2
|
||||
assert parser.vl["LKAS"]["NEW_SIGNAL_2"] == 3
|
||||
|
||||
CP.openpilotLongitudinalControl = True
|
||||
lfa_parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LFA", 0)], can_bus.ECAN)
|
||||
lfa_msgs = hyundaicanfd.create_steering_messages(controller.packer, CP, can_bus, True, True, 0, 0.0)
|
||||
assert [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in lfa_msgs] == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
||||
@@ -2537,13 +2729,12 @@ class TestHyundaiFingerprint:
|
||||
assert lfa_parser.can_valid
|
||||
assert lfa_parser.vl["LFA"]["DAMP_FACTOR"] == 100
|
||||
|
||||
controller.long_active_ecu = True
|
||||
cc.longActive = False
|
||||
inactive_msgs = controller.create_canfd_msgs(0, True, 0.44, 0.0, 0.0, 0.0, False,
|
||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2)
|
||||
steering_names = [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in inactive_msgs
|
||||
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
|
||||
assert steering_names == [("LKAS", can_bus.ACAN)]
|
||||
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
||||
|
||||
controller.frame = 1
|
||||
cc.longActive = True
|
||||
@@ -2553,9 +2744,67 @@ class TestHyundaiFingerprint:
|
||||
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
|
||||
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
||||
|
||||
def test_ioniq_6_keeps_lfa_status_when_longitudinal_is_inactive(self):
|
||||
def test_gv70_electrified_stock_long_uses_damped_lkas_request(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.HYUNDAI_IONIQ_6
|
||||
CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
|
||||
CP.openpilotLongitudinalControl = False
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
can_bus = CanBus(CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS", 0)], can_bus.ACAN)
|
||||
msgs = hyundaicanfd.create_steering_messages(controller.packer, CP, can_bus, True, True, 123, 0.0)
|
||||
|
||||
assert [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in msgs] == [("LKAS", can_bus.ACAN)]
|
||||
parser.update([(1, msgs)])
|
||||
assert parser.can_valid
|
||||
assert parser.vl["LKAS"]["TORQUE_REQUEST"] == 123
|
||||
assert parser.vl["LKAS"]["STEER_REQ"] == 1
|
||||
assert parser.vl["LKAS"]["HAS_LANE_SAFETY"] == 0
|
||||
assert parser.vl["LKAS"]["DAMP_FACTOR"] == 100
|
||||
assert parser.vl["LKAS"]["STEER_MODE"] == 2
|
||||
assert parser.vl["LKAS"]["NEW_SIGNAL_2"] == 3
|
||||
|
||||
@pytest.mark.parametrize(("car", "powertrain_flag"), [
|
||||
(CAR.HYUNDAI_IONIQ_5, HyundaiFlags.EV),
|
||||
(CAR.HYUNDAI_IONIQ_6, HyundaiFlags.EV),
|
||||
(CAR.KIA_EV6, HyundaiFlags.EV),
|
||||
(CAR.KIA_CARNIVAL_2025, 0),
|
||||
(CAR.KIA_CARNIVAL_HEV_4TH_GEN, HyundaiFlags.HYBRID),
|
||||
(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, HyundaiFlags.EV),
|
||||
])
|
||||
def test_hda2_keeps_lfa_status_when_longitudinal_is_inactive(self, car, powertrain_flag):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = car
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | powertrain_flag)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
cc = SimpleNamespace(
|
||||
enabled=False, latActive=False, longActive=False,
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace(),
|
||||
)
|
||||
cs = SimpleNamespace(
|
||||
stock_lfa_msg=None, stock_lkas_msg=None,
|
||||
left_blindspot_from_radar=False, right_blindspot_from_radar=False,
|
||||
out=SimpleNamespace(gearShifter=structs.CarState.GearShifter.park),
|
||||
)
|
||||
|
||||
controller.frame = 1
|
||||
controller.long_active_ecu = True
|
||||
msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False,
|
||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
|
||||
assert any(addr == 0x12A for addr, _, _ in msgs)
|
||||
|
||||
@pytest.mark.parametrize("car", [
|
||||
CAR.HYUNDAI_IONIQ_6,
|
||||
CAR.KIA_EV6,
|
||||
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
|
||||
])
|
||||
def test_egmp_persistent_lfa_status_survives_ecu_fallback_state(self, car):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = car
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
@@ -2569,6 +2818,7 @@ class TestHyundaiFingerprint:
|
||||
)
|
||||
cs = SimpleNamespace(
|
||||
stock_lfa_msg=None, stock_lkas_msg=None,
|
||||
left_blindspot_from_radar=False, right_blindspot_from_radar=False,
|
||||
out=SimpleNamespace(gearShifter=structs.CarState.GearShifter.park),
|
||||
)
|
||||
|
||||
@@ -2591,10 +2841,11 @@ class TestHyundaiFingerprint:
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.pid),
|
||||
cruiseControl=SimpleNamespace(override=False, cancel=False, resume=False),
|
||||
leftBlinker=False, rightBlinker=False,
|
||||
hudControl=SimpleNamespace(leadDistanceBars=3),
|
||||
hudControl=SimpleNamespace(leadDistanceBars=3, leadVisible=True),
|
||||
)
|
||||
cs = SimpleNamespace(
|
||||
stock_lfa_msg=None, stock_lkas_msg=None,
|
||||
openpilot_lead_visible=True, openpilot_lead_distance=37.5, openpilot_lead_rel_speed=-1.3,
|
||||
out=SimpleNamespace(steeringAngleDeg=0.0, gearShifter=structs.CarState.GearShifter.drive),
|
||||
)
|
||||
|
||||
@@ -2607,7 +2858,8 @@ class TestHyundaiFingerprint:
|
||||
parser.update([(1, scc_msgs)])
|
||||
assert parser.can_valid
|
||||
assert parser.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
|
||||
assert parser.vl["SCC_CONTROL"]["ACC_ObjDist"] == pytest.approx(1.0)
|
||||
assert parser.vl["SCC_CONTROL"]["ACC_ObjDist"] == pytest.approx(37.5)
|
||||
assert parser.vl["SCC_CONTROL"]["ACC_ObjRelSpd"] == pytest.approx(-1.3)
|
||||
assert parser.vl["SCC_CONTROL"]["ObjValid"] == 0
|
||||
assert parser.vl["SCC_CONTROL"]["aReqValue"] == pytest.approx(-0.1)
|
||||
assert parser.vl["SCC_CONTROL"]["aReqRaw"] == pytest.approx(-1.0)
|
||||
@@ -2708,7 +2960,7 @@ class TestHyundaiFingerprint:
|
||||
assert len([msg for msg in msgs if msg[0] == 0x110]) == expected_lkas_msgs
|
||||
|
||||
@pytest.mark.parametrize("standstill", [False, True])
|
||||
def test_sportage_angle_lkas_alt_keeps_inactive_status_in_drive(self, standstill):
|
||||
def test_sportage_angle_lkas_alt_keeps_status_and_suppression_alive(self, standstill):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.HYBRID | HyundaiFlags.CANFD_ANGLE_STEERING |
|
||||
@@ -2716,45 +2968,14 @@ class TestHyundaiFingerprint:
|
||||
CP.openpilotLongitudinalControl = False
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 5
|
||||
can_bus = CanBus(CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
|
||||
stock_lkas = {
|
||||
"CHECKSUM": 1234,
|
||||
"COUNTER": 42,
|
||||
"LKA_OptUsmSta": 2,
|
||||
"LKA_MODE": 2,
|
||||
"LKA_RcgSta": 3,
|
||||
"LKA_AVAILABLE": 3,
|
||||
"LKA_LHLnWrnSta": 3,
|
||||
"LKA_RHLnWrnSta": 3,
|
||||
"LKA_WARNING": 1,
|
||||
"LKA_HndsoffSnd": 1,
|
||||
"LKA_StrSnd": 1,
|
||||
"LKA_SysIndReq": 4,
|
||||
"LKA_ICON": 2,
|
||||
"FCA_SYSWARN": 1,
|
||||
"StrTqReqVal": 17,
|
||||
"TORQUE_REQUEST": 17,
|
||||
"ActToiSta": 3,
|
||||
"STEER_REQ": 1,
|
||||
"ToiFltSta": 3,
|
||||
"LFA_BUTTON": 1,
|
||||
"LKA_SysWrn": 15,
|
||||
"LKA_ASSIST": 1,
|
||||
"Damping_Gain": 0,
|
||||
"STEER_MODE": 5,
|
||||
"NEW_SIGNAL_2": 0,
|
||||
"LKAS_ANGLE_ACTIVE": 2,
|
||||
"LKA_UsmMod": 3,
|
||||
"HAS_LANE_SAFETY": 1,
|
||||
"ADAS_StrAnglReqVal": 12.3,
|
||||
"ADAS_ACIAnglTqRedcGainVal": 0.42,
|
||||
"DAMP_FACTOR": 0,
|
||||
}
|
||||
cc = SimpleNamespace(enabled=False, latActive=False,
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace())
|
||||
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas,
|
||||
lfa_block_msg = {f"BYTE{i}": 0 for i in range(3, 32) if i != 7}
|
||||
lfa_block_msg["COUNTER"] = 0
|
||||
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg={}, lfa_block_msg=lfa_block_msg,
|
||||
out=SimpleNamespace(standstill=standstill, steeringAngleDeg=0.0,
|
||||
gearShifter=structs.CarState.GearShifter.drive))
|
||||
|
||||
@@ -2762,14 +2983,52 @@ class TestHyundaiFingerprint:
|
||||
get_test_toggles(), lka_icon=1, lfa_icon=1)
|
||||
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
|
||||
assert len(lkas_msgs) == 1
|
||||
assert len([msg for msg in msgs if msg[0] == 0x362]) == 1
|
||||
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
|
||||
parser.update([(1, lkas_msgs)])
|
||||
assert parser.can_valid
|
||||
assert parser.vl["LKAS_ALT"]["LKA_StrSnd"] == 2
|
||||
assert parser.vl["LKAS_ALT"]["LKA_OptUsmSta"] == 0
|
||||
assert parser.vl["LKAS_ALT"]["LKA_SysIndReq"] == 1
|
||||
assert parser.vl["LKAS_ALT"]["LKA_RcgSta"] == 0
|
||||
assert parser.vl["LKAS_ALT"]["LKA_AVAILABLE"] == 0
|
||||
assert parser.vl["LKAS_ALT"]["Damping_Gain"] == 100
|
||||
assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 1
|
||||
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == 0.0
|
||||
|
||||
def test_sportage_angle_lkas_alt_active_status_matches_vehicle_contract(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.HYBRID | HyundaiFlags.CANFD_ANGLE_STEERING |
|
||||
HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
|
||||
CP.openpilotLongitudinalControl = False
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 5
|
||||
can_bus = CanBus(CP)
|
||||
cc = SimpleNamespace(enabled=True, latActive=True,
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace())
|
||||
lfa_block_msg = {f"BYTE{i}": 0 for i in range(3, 32) if i != 7}
|
||||
lfa_block_msg["COUNTER"] = 0
|
||||
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg={}, lfa_block_msg=lfa_block_msg,
|
||||
out=SimpleNamespace(standstill=False, steeringAngleDeg=10.0,
|
||||
gearShifter=structs.CarState.GearShifter.drive))
|
||||
|
||||
msgs = controller.create_canfd_msgs(0, True, 0.4, 12.0, 0.0, 0.0, False, cc.hudControl, cs, cc,
|
||||
get_test_toggles(), lka_icon=2, lfa_icon=2)
|
||||
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
|
||||
assert len(lkas_msgs) == 1
|
||||
assert len([msg for msg in msgs if msg[0] == 0x362]) == 1
|
||||
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
|
||||
parser.update([(1, lkas_msgs)])
|
||||
assert parser.can_valid
|
||||
assert parser.vl["LKAS_ALT"]["LKA_OptUsmSta"] == 0
|
||||
assert parser.vl["LKAS_ALT"]["LKA_SysIndReq"] == 2
|
||||
assert parser.vl["LKAS_ALT"]["LKA_RcgSta"] == 3
|
||||
assert parser.vl["LKAS_ALT"]["Damping_Gain"] == 100
|
||||
assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 2
|
||||
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.4)
|
||||
|
||||
def test_ev9_inactive_angle_steering_does_not_suppress_stock_lfa(self):
|
||||
CP = CarParams.new_message()
|
||||
@@ -2895,6 +3154,50 @@ class TestHyundaiFingerprint:
|
||||
assert parser.vl["SCC14"]["ComfortBandUpper"] == pytest.approx(0.0)
|
||||
assert parser.vl["SCC14"]["ComfortBandLower"] == pytest.approx(0.0)
|
||||
assert parser.vl["SCC14"]["JerkLowerLimit"] == pytest.approx(5.0)
|
||||
assert parser.vl["SCC11"]["ObjValid"] == 0
|
||||
assert parser.vl["SCC11"]["ACC_ObjDist"] == 0
|
||||
assert parser.vl["SCC14"]["ObjGap"] == 0
|
||||
assert parser.vl["SCC14"]["ObjDistStat"] == 0
|
||||
|
||||
def test_can_acc_commands_show_approaching_lead(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.HYUNDAI_ELANTRA_2021
|
||||
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("SCC11", 0), ("SCC14", 0)], 0)
|
||||
lead_data = CanLeadData(object_gap=4, lead_distance=27.5, lead_rel_speed=-1.3, lead_visible=True)
|
||||
|
||||
msgs = hyundaican.create_acc_commands(packer, enabled=True, accel=0.0, upper_jerk=1.0, idx=3,
|
||||
hud_control=SimpleNamespace(leadDistanceBars=3), set_speed=42,
|
||||
stopping=False, long_override=False, use_fca=False, CP=CP,
|
||||
lead_data=lead_data)
|
||||
parser.update([(1, msgs)])
|
||||
|
||||
assert parser.can_valid
|
||||
assert parser.vl["SCC11"]["ObjValid"] == 1
|
||||
assert parser.vl["SCC11"]["ACC_ObjStatus"] == 1
|
||||
assert parser.vl["SCC11"]["ACC_ObjDist"] == pytest.approx(27.0)
|
||||
assert parser.vl["SCC11"]["ACC_ObjRelSpd"] == pytest.approx(-1.3)
|
||||
assert parser.vl["SCC14"]["ObjGap"] == 4
|
||||
assert parser.vl["SCC14"]["ObjDistStat"] == 2
|
||||
|
||||
def test_can_lead_data_hysteresis_and_distance_bands(self):
|
||||
state = CanLeadDataState()
|
||||
|
||||
for _ in range(state.LEAD_HYSTERESIS_FRAMES - 1):
|
||||
lead_data = state.update(18.0, -0.5, True)
|
||||
assert not lead_data.lead_visible
|
||||
assert lead_data.object_gap == 0
|
||||
|
||||
lead_data = state.update(18.0, -0.5, True)
|
||||
assert lead_data.lead_visible
|
||||
assert lead_data.object_gap == 2
|
||||
assert lead_data.object_rel_gap == 2
|
||||
|
||||
for _ in range(state.LEAD_HYSTERESIS_FRAMES):
|
||||
lead_data = state.update(32.0, 0.5, True)
|
||||
assert lead_data.object_gap == 5
|
||||
assert lead_data.object_rel_gap == 1
|
||||
|
||||
def test_can_acc_commands_follow_sonata_main_cruise_state(self):
|
||||
CP = CarParams.new_message()
|
||||
|
||||
@@ -0,0 +1,170 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, gen_empty_fingerprint
|
||||
from opendbc.car.hyundai.carcontroller import CarController
|
||||
from opendbc.car.hyundai.carstate import CarState
|
||||
from opendbc.car.hyundai.interface import CarInterface
|
||||
from opendbc.car.hyundai.values import CAR, DBC, HyundaiSafetyFlags
|
||||
from opendbc.car.structs import CarControl
|
||||
|
||||
|
||||
def ray_fingerprint(sensor_length=6, lfa_length=8):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[0][0x201] = sensor_length
|
||||
fingerprint[0][0x391] = 8
|
||||
fingerprint[2][0x485] = lfa_length
|
||||
return fingerprint
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("candidate", "fingerprint", "has_pedal"), [
|
||||
(CAR.KIA_RAY_EV, ray_fingerprint(), True),
|
||||
(CAR.KIA_RAY_EV, ray_fingerprint(sensor_length=8), False),
|
||||
(CAR.KIA_RAY_EV, ray_fingerprint(lfa_length=4), False),
|
||||
(CAR.HYUNDAI_KONA_EV_NON_SCC, ray_fingerprint(), False),
|
||||
])
|
||||
def test_ray_pedal_fingerprint_isolation(candidate, fingerprint, has_pedal):
|
||||
CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False, None)
|
||||
assert CP.enableGasInterceptorDEPRECATED is has_pedal
|
||||
assert CP.openpilotLongitudinalControl is has_pedal
|
||||
if has_pedal:
|
||||
assert not CP.pcmCruise
|
||||
assert CP.safetyConfigs[-1].safetyParam == 0x9405
|
||||
assert CP.minEnableSpeed == 5.0
|
||||
assert not CP.autoResumeSng
|
||||
FPCP = CarInterface.get_starpilot_params(candidate, fingerprint, [], CP, SimpleNamespace())
|
||||
assert FPCP.canUsePedal
|
||||
assert not FPCP.pcmCruiseSpeed
|
||||
assert not FPCP.redneckCruiseAvailable
|
||||
else:
|
||||
assert CP.pcmCruise
|
||||
assert not (CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG)
|
||||
|
||||
|
||||
def test_ray_pedal_safety_signature_is_unique_across_hyundai_platforms():
|
||||
for candidate in CAR:
|
||||
for alpha_long in (False, True):
|
||||
CP = CarInterface.get_params(candidate, ray_fingerprint(), [],
|
||||
alpha_long, False, False, None)
|
||||
has_ray_signature = (CP.safetyConfigs[-1].safetyParam & ~(32 | 128 | 2048)) == 0x9405
|
||||
assert has_ray_signature is (candidate == CAR.KIA_RAY_EV)
|
||||
assert CP.enableGasInterceptorDEPRECATED is (candidate == CAR.KIA_RAY_EV)
|
||||
|
||||
|
||||
def test_ray_pedal_parser_validates_actual_route_frames():
|
||||
CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
|
||||
parser = CarState(CP, None).get_can_parsers(CP)[Bus.party]
|
||||
assert parser.dbc_name == "hyundai_kia_ray_pedal"
|
||||
# Consecutive bus-0 GAS_SENSOR frames from Sept. 15 Ray rlog segment 4.
|
||||
samples = [bytes.fromhex(s) for s in (
|
||||
"01f403d55de8", "01f603d55ef1", "01f403d55f51", "01f603d3503f",
|
||||
"01f903d551ab", "01f903d552a4", "01f703d55370",
|
||||
)]
|
||||
for idx, dat in enumerate(samples):
|
||||
parser.update([(1_000_000_000 + idx * 20_000_000, [(0x201, dat, 0)])])
|
||||
assert parser.can_valid
|
||||
assert parser.vl["GAS_SENSOR"]["STATE"] == 5 # FAULT_TIMEOUT: no 0x200 was sent
|
||||
|
||||
prior = parser.vl_raw["GAS_SENSOR"]
|
||||
bad = bytearray(samples[-1])
|
||||
bad[-1] ^= 1
|
||||
parser.update([(1_160_000_000, [(0x201, bytes(bad), 0)])])
|
||||
assert parser.vl_raw["GAS_SENSOR"] == prior
|
||||
|
||||
|
||||
def test_ray_pedal_fault_clears_only_with_healthy_sensor_state():
|
||||
CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
|
||||
state = CarState(CP, None)
|
||||
parsers = state.get_can_parsers(CP)
|
||||
packer = CANPacker("hyundai_kia_ray_pedal")
|
||||
sensor = packer.make_can_msg("GAS_SENSOR", 0, {
|
||||
"INTERCEPTOR_GAS": 0, "INTERCEPTOR_GAS2": 0,
|
||||
"STATE": 0, "COUNTER_PEDAL": 1,
|
||||
})
|
||||
for parser in parsers.values():
|
||||
parser.update([(1_000_000_000, [sensor])])
|
||||
ret, _ = state.update(parsers, SimpleNamespace())
|
||||
assert state.ray_pedal_valid
|
||||
assert state.ray_pedal_state == 0
|
||||
assert not ret.accFaulted
|
||||
|
||||
|
||||
def test_ray_driver_override_uses_physical_interceptor_tracks():
|
||||
CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
|
||||
state = CarState(CP, None)
|
||||
parsers = state.get_can_parsers(CP)
|
||||
native_gas = (0x371, bytes.fromhex("004e008000ae0700"), 0)
|
||||
physical_rest = (0x201, bytes.fromhex("010801f30cef"), 0)
|
||||
for parser in parsers.values():
|
||||
parser.update([(1_000_000_000, [native_gas, physical_rest])])
|
||||
ret, _ = state.update(parsers, SimpleNamespace())
|
||||
assert state.ray_pedal_valid
|
||||
assert not ret.gasPressed
|
||||
|
||||
packer = CANPacker("hyundai_kia_ray_pedal")
|
||||
physical_press = packer.make_can_msg("GAS_SENSOR", 0, {
|
||||
"INTERCEPTOR_GAS": (310 - 264) * 0.672,
|
||||
"INTERCEPTOR_GAS2": (593 - 497) * 0.332,
|
||||
"STATE": 0, "COUNTER_PEDAL": 13,
|
||||
})
|
||||
for parser in parsers.values():
|
||||
parser.update([(1_020_000_000, [physical_press])])
|
||||
ret, _ = state.update(parsers, SimpleNamespace())
|
||||
assert ret.gasPressed
|
||||
|
||||
|
||||
def test_ray_without_pedal_keeps_native_gas_detection():
|
||||
CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(sensor_length=8), [], False, False, False, None)
|
||||
assert not CP.enableGasInterceptorDEPRECATED
|
||||
state = CarState(CP, None)
|
||||
parsers = state.get_can_parsers(CP)
|
||||
assert Bus.party not in parsers
|
||||
native_gas = (0x371, bytes.fromhex("004e008000ae0700"), 0)
|
||||
for parser in parsers.values():
|
||||
parser.update([(1_000_000_000, [native_gas])])
|
||||
ret, _ = state.update(parsers, SimpleNamespace())
|
||||
assert ret.gasPressed
|
||||
|
||||
|
||||
def test_ray_controller_heartbeats_and_only_actuates_when_ready():
|
||||
CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0), ("CLU11", 0)], 0)
|
||||
CS = SimpleNamespace(
|
||||
lkas11=parser.vl["LKAS11"], clu11=parser.vl["CLU11"],
|
||||
out=SimpleNamespace(vEgo=12.0, gasPressed=False, brakePressed=False,
|
||||
cruiseState=SimpleNamespace(enabled=False)),
|
||||
ray_pedal_valid=True, ray_pedal_state=5, is_metric=True,
|
||||
)
|
||||
CC = SimpleNamespace(
|
||||
enabled=True, longActive=True, latActive=True,
|
||||
cruiseControl=SimpleNamespace(cancel=False, resume=False, override=False),
|
||||
)
|
||||
hud = SimpleNamespace(
|
||||
visualAlert=CarControl.HUDControl.VisualAlert.none,
|
||||
leftLaneVisible=True, rightLaneVisible=True,
|
||||
leftLaneDepart=False, rightLaneDepart=False,
|
||||
)
|
||||
actuators = SimpleNamespace(longControlState=CarControl.Actuators.LongControlState.pid)
|
||||
|
||||
def pedal_msg(accel, frame):
|
||||
controller.frame = frame
|
||||
messages = controller.create_can_msgs(True, 0, False, 0.0, accel, False,
|
||||
hud, actuators, CS, CC, 2, 0)
|
||||
return next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)
|
||||
|
||||
assert pedal_msg(2.0, 0)[:4] == bytes(4) # fault timeout: heartbeat only
|
||||
CS.ray_pedal_state = 0
|
||||
assert pedal_msg(2.0, 4)[:4] != bytes(4)
|
||||
CS.out.gasPressed = True
|
||||
assert pedal_msg(2.0, 8)[:4] == bytes(4)
|
||||
CS.out.gasPressed = False
|
||||
assert pedal_msg(-1.0, 12)[:4] == bytes(4) # decel = EV lift/regen, not gas
|
||||
CS.out.cruiseState.enabled = True
|
||||
controller.frame = 16
|
||||
messages = controller.create_can_msgs(True, 0, False, 0.0, 2.0, False,
|
||||
hud, actuators, CS, CC, 2, 0)
|
||||
assert next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)[:4] == bytes(4)
|
||||
assert any(addr == 0x4F1 and bus == 0 for addr, _, bus in messages) # cancel stock CC
|
||||
@@ -1218,6 +1218,13 @@ CANFD_ALT_BUTTONS_RESUME_CAR = {CAR.KIA_CARNIVAL_2025, CAR.KIA_CARNIVAL_HEV_4TH_
|
||||
CANFD_CORNER_RADAR_BSM_CAR = {CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5_PE, CAR.KIA_EV9}
|
||||
CANFD_RADAR_LIVE_LONGITUDINAL_CAR = {
|
||||
CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_5_PE, CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV6, CAR.KIA_EV9, CAR.GENESIS_GV60_EV_1ST_GEN,
|
||||
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
|
||||
}
|
||||
CANFD_RADAR_ECU_KEEPALIVE_CAR = {
|
||||
CAR.HYUNDAI_IONIQ_5_PE,
|
||||
CAR.HYUNDAI_IONIQ_6,
|
||||
CAR.KIA_EV9,
|
||||
CAR.GENESIS_GV60_EV_1ST_GEN,
|
||||
}
|
||||
RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | {
|
||||
CAR.HYUNDAI_IONIQ,
|
||||
|
||||
@@ -109,6 +109,7 @@ class RadarInterfaceBase(ABC):
|
||||
self.CP = CP
|
||||
self.rcp = None
|
||||
self.pts: dict[int, structs.RadarData.RadarPoint] = {}
|
||||
self.track_id: int = 0
|
||||
self.frame = 0
|
||||
|
||||
def update(self, can_packets: list[tuple[int, list[CanData]]]) -> structs.RadarDataT | None:
|
||||
@@ -232,6 +233,9 @@ class CarInterfaceBase(ABC):
|
||||
fp_ret.flags |= int(HondaStarPilotFlags.HAS_CAMERA_MESSAGES)
|
||||
|
||||
elif platform in HYUNDAI:
|
||||
if candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED:
|
||||
fp_ret.canUsePedal = True
|
||||
fp_ret.pcmCruiseSpeed = False
|
||||
if candidate in CANFD_CAR:
|
||||
hda2 = Ecu.adas in [fw.ecu for fw in car_fw]
|
||||
CAN = CanBus(None, fingerprint, bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING))
|
||||
@@ -243,7 +247,9 @@ class CarInterfaceBase(ABC):
|
||||
if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2]:
|
||||
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)
|
||||
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))
|
||||
if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise"):
|
||||
fp_ret.pcmCruiseSpeed = False
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
@@ -16,21 +16,11 @@ _SNG_ACC_MIN_DIST = 3
|
||||
_SNG_ACC_MAX_DIST = 4.5
|
||||
_LEGACY_2025_MADS_MIN_SPEED = 0.44704
|
||||
_LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0
|
||||
_LEGACY_2025_OVERRIDE_HOLD_FRAMES = 10
|
||||
_LEGACY_2025_REENGAGE_SETTLE_FRAMES = 8
|
||||
_LEGACY_2025_REENGAGE_MAX_STEER_RATE = 2.0
|
||||
_LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA = 1.0
|
||||
_LEGACY_2025_RECLAIM_FRAMES = 36
|
||||
_LEGACY_2025_RECLAIM_EXPONENT = 2.5
|
||||
_ANGLE_OVERRIDE_CONFIRM_FRAMES = 2
|
||||
_ANGLE_OVERRIDE_HOLD_FRAMES = 10
|
||||
_ANGLE_REENGAGE_SETTLE_FRAMES = 8
|
||||
_ANGLE_REENGAGE_MAX_STEER_RATE = 2.0
|
||||
_ANGLE_REENGAGE_MAX_ANGLE_DELTA = 1.0
|
||||
_ANGLE_RECLAIM_FRAMES = 36
|
||||
_ANGLE_RECLAIM_EXPONENT = 2.5
|
||||
_ANGLE_MADS_MIN_SPEED = 0.44704
|
||||
_ANGLE_MADS_MAX_STEER_ANGLE = 120.0
|
||||
_ASCENT_AOL_ARM_FRAMES = 30
|
||||
_STOP_START_STARTUP_DELAY_FRAMES = 100
|
||||
# StarPilot's first populated toggle message can arrive several seconds after
|
||||
# the car controller starts while fingerprinting and settings settle.
|
||||
@@ -53,20 +43,9 @@ class CarController(CarControllerBase):
|
||||
self.apply_steer_last = 0
|
||||
self.driver_override = False
|
||||
self.angle_override_confirm_frames = 0
|
||||
self.legacy_2025_lkas_active = False
|
||||
self.legacy_2025_handoff_active = False
|
||||
self.legacy_2025_override_hold_frames = 0
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reengage_reference_angle = 0.0
|
||||
self.legacy_2025_reclaim_frames = 0
|
||||
self.legacy_2025_reclaim_start_angle = 0.0
|
||||
self.angle_lkas_active = False
|
||||
self.angle_handoff_active = False
|
||||
self.angle_override_hold_frames = 0
|
||||
self.angle_reengage_settle_frames = 0
|
||||
self.angle_reengage_reference_angle = 0.0
|
||||
self.angle_reclaim_frames = 0
|
||||
self.angle_reclaim_start_angle = 0.0
|
||||
self.ascent_aol_arm_frames = 0
|
||||
|
||||
self.cruise_button_prev = 0
|
||||
self.steer_rate_counter = 0
|
||||
@@ -77,7 +56,7 @@ class CarController(CarControllerBase):
|
||||
self.angle_bus = CanBus.angle_for_cp(CP)
|
||||
self.status_bus = CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM_CAMERA else CanBus.main
|
||||
|
||||
if CP.flags & SubaruFlags.LKAS_ANGLE:
|
||||
if CP.flags & SubaruFlags.LKAS_ANGLE and CP.carFingerprint != CAR.SUBARU_OUTBACK_2023:
|
||||
self.VM = VehicleModel(get_safety_CP())
|
||||
|
||||
self.prev_close_distance = 0
|
||||
@@ -146,81 +125,10 @@ class CarController(CarControllerBase):
|
||||
self.stop_start_counter = (self.stop_start_counter + 1) % 0x10
|
||||
return msg
|
||||
|
||||
def _reset_legacy_2025_handoff(self):
|
||||
self.driver_override = False
|
||||
self.angle_override_confirm_frames = 0
|
||||
self.legacy_2025_handoff_active = False
|
||||
self.legacy_2025_override_hold_frames = 0
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reengage_reference_angle = 0.0
|
||||
self.legacy_2025_reclaim_frames = 0
|
||||
self.legacy_2025_reclaim_start_angle = 0.0
|
||||
|
||||
def _legacy_2025_manual_handoff(self, CS, lkas_available):
|
||||
if not lkas_available:
|
||||
self._reset_legacy_2025_handoff()
|
||||
return False
|
||||
|
||||
driver_override = self._update_angle_driver_override(CS)
|
||||
if driver_override:
|
||||
self.legacy_2025_handoff_active = True
|
||||
self.legacy_2025_override_hold_frames = _LEGACY_2025_OVERRIDE_HOLD_FRAMES
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
self.legacy_2025_reclaim_frames = 0
|
||||
return True
|
||||
|
||||
if not self.legacy_2025_handoff_active and not self.legacy_2025_lkas_active and \
|
||||
abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE:
|
||||
self.legacy_2025_handoff_active = True
|
||||
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if not self.legacy_2025_handoff_active:
|
||||
return False
|
||||
|
||||
if self.legacy_2025_override_hold_frames > 0:
|
||||
self.legacy_2025_override_hold_frames -= 1
|
||||
if self.legacy_2025_override_hold_frames == 0:
|
||||
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
return True
|
||||
|
||||
wheel_stable = abs(getattr(CS.out, "steeringRateDeg", 0.0)) <= _LEGACY_2025_REENGAGE_MAX_STEER_RATE and \
|
||||
abs(CS.out.steeringAngleDeg - self.legacy_2025_reengage_reference_angle) <= _LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA
|
||||
if wheel_stable:
|
||||
self.legacy_2025_reengage_settle_frames += 1
|
||||
else:
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if self.legacy_2025_reengage_settle_frames < _LEGACY_2025_REENGAGE_SETTLE_FRAMES:
|
||||
return True
|
||||
|
||||
self.legacy_2025_handoff_active = False
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reclaim_frames = _LEGACY_2025_RECLAIM_FRAMES
|
||||
self.legacy_2025_reclaim_start_angle = CS.out.steeringAngleDeg
|
||||
return True
|
||||
|
||||
def _legacy_2025_reclaim_target(self, target_angle):
|
||||
if self.legacy_2025_reclaim_frames <= 0:
|
||||
return target_angle
|
||||
|
||||
progress = (_LEGACY_2025_RECLAIM_FRAMES - self.legacy_2025_reclaim_frames + 1) / _LEGACY_2025_RECLAIM_FRAMES
|
||||
eased_progress = progress ** _LEGACY_2025_RECLAIM_EXPONENT
|
||||
target_angle = self.legacy_2025_reclaim_start_angle + eased_progress * \
|
||||
(target_angle - self.legacy_2025_reclaim_start_angle)
|
||||
self.legacy_2025_reclaim_frames -= 1
|
||||
return target_angle
|
||||
|
||||
def _reset_angle_handoff(self):
|
||||
self.driver_override = False
|
||||
self.angle_override_confirm_frames = 0
|
||||
self.angle_handoff_active = False
|
||||
self.angle_override_hold_frames = 0
|
||||
self.angle_reengage_settle_frames = 0
|
||||
self.angle_reengage_reference_angle = 0.0
|
||||
self.angle_reclaim_frames = 0
|
||||
self.angle_reclaim_start_angle = 0.0
|
||||
|
||||
def _angle_manual_handoff(self, CS, lat_active):
|
||||
if not lat_active:
|
||||
@@ -228,44 +136,23 @@ class CarController(CarControllerBase):
|
||||
return False
|
||||
|
||||
driver_override = self._update_angle_driver_override(CS)
|
||||
steering_rate = abs(getattr(CS.out, "steeringRateDeg", 0.0))
|
||||
if driver_override:
|
||||
self.angle_handoff_active = True
|
||||
self.angle_override_hold_frames = _ANGLE_OVERRIDE_HOLD_FRAMES
|
||||
self.angle_reengage_settle_frames = 0
|
||||
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
self.angle_reclaim_frames = 0
|
||||
return True
|
||||
|
||||
if not self.angle_handoff_active and not self.angle_lkas_active and \
|
||||
abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _ANGLE_REENGAGE_MAX_STEER_RATE:
|
||||
if self.angle_handoff_active:
|
||||
if steering_rate > _ANGLE_REENGAGE_MAX_STEER_RATE:
|
||||
return True
|
||||
|
||||
self.angle_handoff_active = False
|
||||
return True
|
||||
|
||||
if not self.angle_lkas_active and steering_rate > _ANGLE_REENGAGE_MAX_STEER_RATE:
|
||||
self.angle_handoff_active = True
|
||||
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if not self.angle_handoff_active:
|
||||
return False
|
||||
|
||||
if self.angle_override_hold_frames > 0:
|
||||
self.angle_override_hold_frames -= 1
|
||||
if self.angle_override_hold_frames == 0:
|
||||
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
return True
|
||||
|
||||
wheel_stable = abs(getattr(CS.out, "steeringRateDeg", 0.0)) <= _ANGLE_REENGAGE_MAX_STEER_RATE and \
|
||||
abs(CS.out.steeringAngleDeg - self.angle_reengage_reference_angle) <= _ANGLE_REENGAGE_MAX_ANGLE_DELTA
|
||||
if wheel_stable:
|
||||
self.angle_reengage_settle_frames += 1
|
||||
else:
|
||||
self.angle_reengage_settle_frames = 0
|
||||
self.angle_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if self.angle_reengage_settle_frames < _ANGLE_REENGAGE_SETTLE_FRAMES:
|
||||
return True
|
||||
|
||||
self.angle_handoff_active = False
|
||||
self.angle_reengage_settle_frames = 0
|
||||
self.angle_reclaim_frames = _ANGLE_RECLAIM_FRAMES
|
||||
self.angle_reclaim_start_angle = CS.out.steeringAngleDeg
|
||||
return True
|
||||
return False
|
||||
|
||||
def _update_angle_driver_override(self, CS):
|
||||
"""Debounce the higher-confidence raw torque override signal for angle cars."""
|
||||
@@ -283,16 +170,13 @@ class CarController(CarControllerBase):
|
||||
|
||||
return self.driver_override
|
||||
|
||||
def _angle_reclaim_target(self, target_angle):
|
||||
if self.angle_reclaim_frames <= 0:
|
||||
return target_angle
|
||||
def _ascent_aol_ready(self, ready):
|
||||
if not ready:
|
||||
self.ascent_aol_arm_frames = 0
|
||||
return False
|
||||
|
||||
progress = (_ANGLE_RECLAIM_FRAMES - self.angle_reclaim_frames + 1) / _ANGLE_RECLAIM_FRAMES
|
||||
eased_progress = progress ** _ANGLE_RECLAIM_EXPONENT
|
||||
target_angle = self.angle_reclaim_start_angle + eased_progress * \
|
||||
(target_angle - self.angle_reclaim_start_angle)
|
||||
self.angle_reclaim_frames -= 1
|
||||
return target_angle
|
||||
self.ascent_aol_arm_frames = min(self.ascent_aol_arm_frames + 1, _ASCENT_AOL_ARM_FRAMES)
|
||||
return self.ascent_aol_arm_frames >= _ASCENT_AOL_ARM_FRAMES
|
||||
|
||||
def lateral_angle(self, CC, CS):
|
||||
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
|
||||
@@ -302,12 +186,11 @@ class CarController(CarControllerBase):
|
||||
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
|
||||
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
|
||||
|
||||
manual_handoff = self._legacy_2025_manual_handoff(CS, lkas_available)
|
||||
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
|
||||
lkas_active = lkas_available and not manual_handoff
|
||||
|
||||
steer_target = self._legacy_2025_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
|
||||
apply_steer = apply_std_steer_angle_limits(
|
||||
steer_target,
|
||||
CC.actuators.steeringAngleDeg,
|
||||
self.apply_steer_last,
|
||||
CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg,
|
||||
@@ -315,7 +198,7 @@ class CarController(CarControllerBase):
|
||||
self.p.LEGACY_2025_ANGLE_LIMITS,
|
||||
)
|
||||
self.apply_steer_last = apply_steer
|
||||
self.legacy_2025_lkas_active = lkas_active
|
||||
self.angle_lkas_active = lkas_active
|
||||
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
|
||||
|
||||
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
||||
@@ -324,33 +207,30 @@ class CarController(CarControllerBase):
|
||||
abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE
|
||||
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
|
||||
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
|
||||
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
|
||||
if mads_only:
|
||||
cruise_available = getattr(getattr(CS.out, "cruiseState", None), "available", True)
|
||||
lkas_available = self._ascent_aol_ready(lkas_available and cruise_available)
|
||||
else:
|
||||
self.ascent_aol_arm_frames = _ASCENT_AOL_ARM_FRAMES if lkas_available else 0
|
||||
|
||||
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
|
||||
if self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023:
|
||||
manual_handoff = False
|
||||
else:
|
||||
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
|
||||
lkas_active = lkas_available and not manual_handoff
|
||||
|
||||
if lkas_active and not self.angle_lkas_active:
|
||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
||||
|
||||
steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
|
||||
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
|
||||
apply_steer = apply_std_steer_angle_limits(
|
||||
steer_target,
|
||||
self.apply_steer_last,
|
||||
CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg,
|
||||
lkas_active,
|
||||
self.p.FIXED_ANGLE_LIMITS,
|
||||
)
|
||||
else:
|
||||
apply_steer = apply_steer_angle_limits_vm(
|
||||
steer_target,
|
||||
self.apply_steer_last,
|
||||
CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg,
|
||||
lkas_active,
|
||||
self.p,
|
||||
self.VM,
|
||||
)
|
||||
apply_steer = apply_std_steer_angle_limits(
|
||||
CC.actuators.steeringAngleDeg,
|
||||
self.apply_steer_last,
|
||||
CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg,
|
||||
lkas_active,
|
||||
self.p.FIXED_ANGLE_LIMITS,
|
||||
)
|
||||
self.apply_steer_last = apply_steer
|
||||
self.angle_lkas_active = lkas_active
|
||||
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
|
||||
@@ -365,9 +245,8 @@ class CarController(CarControllerBase):
|
||||
lat_active = lkas_available and not self.driver_override and not manual_handoff
|
||||
if lat_active and not self.angle_lkas_active:
|
||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
||||
steer_target = self._angle_reclaim_target(CC.actuators.steeringAngleDeg) if lat_active else CC.actuators.steeringAngleDeg
|
||||
apply_steer = apply_steer_angle_limits_vm(
|
||||
steer_target,
|
||||
CC.actuators.steeringAngleDeg,
|
||||
self.apply_steer_last,
|
||||
CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg,
|
||||
@@ -408,6 +287,11 @@ class CarController(CarControllerBase):
|
||||
|
||||
return subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req)
|
||||
|
||||
def _lkas_status_active(self, CC):
|
||||
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
||||
return self.angle_lkas_active
|
||||
return CC.latActive
|
||||
|
||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
@@ -484,9 +368,11 @@ class CarController(CarControllerBase):
|
||||
CC.longActive, hud_control.leadVisible,
|
||||
self.status_bus))
|
||||
|
||||
can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.latActive, hud_control.visualAlert,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart, self.status_bus))
|
||||
can_sends.append(subarucan.create_es_lkas_state(
|
||||
self.packer, self.frame // 10, CS.es_lkas_state_msg, self._lkas_status_active(CC), hud_control.visualAlert,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart, self.status_bus,
|
||||
))
|
||||
|
||||
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
|
||||
can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg,
|
||||
|
||||
@@ -85,14 +85,14 @@ class CarState(CarStateBase):
|
||||
|
||||
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
|
||||
ret.steeringAngleDeg = cp_angle.vl["Steering_2"]["Steering_Angle"]
|
||||
steering_updated = len(cp_angle.vl_all["Steering_2"]["Steering_Angle"]) > 0
|
||||
steering_counter = cp_angle.vl["Steering_2"]["COUNTER"]
|
||||
else:
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
|
||||
steering_updated = len(cp.vl_all["Steering_Torque"]["Steering_Angle"]) > 0
|
||||
steering_counter = cp.vl["Steering_Torque"].get("COUNTER", 0)
|
||||
|
||||
if not (self.CP.flags & SubaruFlags.PREGLOBAL):
|
||||
# ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
|
||||
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, steering_updated)
|
||||
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, steering_counter)
|
||||
|
||||
ret.steeringTorque = cp_angle.vl["Steering_Torque"]["Steer_Torque_Sensor"]
|
||||
ret.steeringTorqueEps = cp_angle.vl["Steering_Torque"]["Steer_Torque_Output"]
|
||||
|
||||
@@ -42,7 +42,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
|
||||
if candidate in SUBARU_STOP_START_CARS:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.STOP_START_BUTTON.value
|
||||
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
|
||||
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
@@ -8,7 +8,7 @@ from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, fw_versions, gen_empty_fingerprint, structs
|
||||
from opendbc.car.fw_query_definitions import StdQueries
|
||||
from opendbc.car.subaru import subarucan
|
||||
from opendbc.car.subaru.carcontroller import CarController
|
||||
from opendbc.car.subaru.carcontroller import CarController, _ASCENT_AOL_ARM_FRAMES
|
||||
from opendbc.car.subaru.carstate import CarState
|
||||
from opendbc.car.subaru.fingerprints import FW_VERSIONS
|
||||
from opendbc.car.fw_versions import match_fw_to_car
|
||||
@@ -244,7 +244,7 @@ def test_outback_2023_uses_d_platform_bus_layout():
|
||||
assert CP.flags & SubaruFlags.D_PLATFORM
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.STOP_START_BUTTON
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS)
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS
|
||||
assert CanBus.main_for_cp(CP) == CanBus.alt
|
||||
assert CanBus.angle_for_cp(CP) == CanBus.main
|
||||
assert parsers[Bus.pt].bus == CanBus.alt
|
||||
@@ -414,7 +414,7 @@ def test_legacy_2025_engagement_continues_from_last_sent_angle():
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.47)
|
||||
|
||||
|
||||
def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging():
|
||||
def test_legacy_2025_reengages_immediately_after_manual_steering_stops():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(
|
||||
@@ -444,42 +444,20 @@ def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging():
|
||||
CS.out.steeringPressed = False
|
||||
CS.out.steeringTorque = 0.0
|
||||
CS.out.steeringAngleDeg = -113.78
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2, [msg])])
|
||||
parser.update([(3, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
for i in range(9):
|
||||
CS.out.steeringAngleDeg += 0.5
|
||||
CS.out.steeringRateDeg = 20.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
for i in range(6):
|
||||
if i % 2:
|
||||
CS.out.steeringAngleDeg += 0.5
|
||||
CS.out.steeringRateDeg = 0.0 if i % 2 == 0 else 20.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(12 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
for i in range(8):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(18 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
measured_angle = CS.out.steeringAngleDeg
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(26, [msg])])
|
||||
parser.update([(4, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
assert abs(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] - measured_angle) < 0.1
|
||||
assert -113.78 < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -100.0
|
||||
|
||||
|
||||
def test_legacy_2025_manual_handoff_reclaim_is_gradual():
|
||||
def test_legacy_2025_manual_handoff_reentry_uses_normal_angle_limits():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(
|
||||
@@ -508,22 +486,24 @@ def test_legacy_2025_manual_handoff_reclaim_is_gradual():
|
||||
CS.out.steeringPressed = False
|
||||
CS.out.steeringTorque = 0.0
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
for i in range(19):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2 + i, [msg])])
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(4, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
first_reclaim_angle = parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"]
|
||||
assert first_reclaim_angle == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
|
||||
first_reentry_angle = parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"]
|
||||
assert CC.actuators.steeringAngleDeg < first_reentry_angle < CS.out.steeringAngleDeg
|
||||
|
||||
reclaim_angles = []
|
||||
reentry_angles = []
|
||||
for i in range(6):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(20 + i, [msg])])
|
||||
reclaim_angles.append(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"])
|
||||
reentry_angles.append(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"])
|
||||
|
||||
assert all(reclaim_angles[i] >= reclaim_angles[i + 1] for i in range(len(reclaim_angles) - 1))
|
||||
assert reclaim_angles[-1] > CC.actuators.steeringAngleDeg
|
||||
assert all(reentry_angles[i] >= reentry_angles[i + 1] for i in range(len(reentry_angles) - 1))
|
||||
assert reentry_angles[-1] > CC.actuators.steeringAngleDeg
|
||||
|
||||
|
||||
def test_ascent_2023_uses_gen2_angle_bus_layout():
|
||||
@@ -546,6 +526,25 @@ def test_ascent_2023_uses_gen2_angle_bus_layout():
|
||||
assert controller.status_bus == CanBus.main
|
||||
|
||||
|
||||
def test_ascent_steering_rate_retains_last_can_sample():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
car_state = CarState(CP, None)
|
||||
parsers = car_state.get_can_parsers(CP)
|
||||
toggles = SimpleNamespace(subaru_sng=False)
|
||||
|
||||
parsers[Bus.pt].vl["Steering_2"]["Steering_Angle"] = 1.0
|
||||
parsers[Bus.pt].vl["Steering_2"]["COUNTER"] = 1
|
||||
car_state.update(parsers, toggles)
|
||||
|
||||
parsers[Bus.pt].vl["Steering_2"]["Steering_Angle"] = 2.0
|
||||
parsers[Bus.pt].vl["Steering_2"]["COUNTER"] = 2
|
||||
state, _ = car_state.update(parsers, toggles)
|
||||
assert state.steeringRateDeg == pytest.approx(50.0)
|
||||
|
||||
state, _ = car_state.update(parsers, toggles)
|
||||
assert state.steeringRateDeg == pytest.approx(50.0)
|
||||
|
||||
|
||||
def test_other_angle_platforms_keep_existing_bus_layout():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
|
||||
parsers = CarState.get_can_parsers(CP)
|
||||
@@ -622,8 +621,9 @@ def test_angle_controller_blocks_low_speed_mads_engagement():
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
|
||||
|
||||
def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023))
|
||||
def test_angle_controller_uses_fixed_angle_rate_limits(platform):
|
||||
CP = CarInterface.get_non_essential_params(platform)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-14.88))
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
@@ -643,8 +643,8 @@ def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
|
||||
assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -25.0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023))
|
||||
def test_angle_controller_yields_until_manual_steering_settles(platform):
|
||||
def test_ascent_angle_controller_reengages_immediately_after_manual_steering_stops():
|
||||
platform = CAR.SUBARU_ASCENT_2023
|
||||
CP = CarInterface.get_non_essential_params(platform)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
|
||||
@@ -671,18 +671,18 @@ def test_angle_controller_yields_until_manual_steering_settles(platform):
|
||||
CS.out.steeringTorque = 0.0
|
||||
CS.out.steeringAngleDeg = -17.91
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
for i in range(18):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(20, [msg])])
|
||||
parser.update([(4, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
|
||||
assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < CC.actuators.steeringAngleDeg
|
||||
|
||||
|
||||
def test_ascent_angle_controller_blocks_parking_lot_aol_engagement():
|
||||
def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-206.12))
|
||||
@@ -692,6 +692,7 @@ def test_ascent_angle_controller_blocks_parking_lot_aol_engagement():
|
||||
steeringRateDeg=96.0,
|
||||
steeringTorque=7.0,
|
||||
steeringPressed=False,
|
||||
cruiseState=SimpleNamespace(available=True),
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
standstill=False,
|
||||
))
|
||||
@@ -704,21 +705,77 @@ def test_ascent_angle_controller_blocks_parking_lot_aol_engagement():
|
||||
|
||||
CS.out.steeringAngleDeg = -100.0
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
for frame in range(2, _ASCENT_AOL_ARM_FRAMES + 1):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(frame, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2, [msg])])
|
||||
parser.update([(_ASCENT_AOL_ARM_FRAMES + 1, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
|
||||
CS.out.gearShifter = structs.CarState.GearShifter.reverse
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3, [msg])])
|
||||
parser.update([(_ASCENT_AOL_ARM_FRAMES + 2, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
|
||||
def test_lkas_hud_state_uses_lateral_active():
|
||||
def test_ascent_aol_does_not_arm_before_cruise_main_is_available():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=5.0))
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=10.0,
|
||||
steeringAngleDeg=0.0,
|
||||
steeringRateDeg=0.0,
|
||||
steeringTorque=0.0,
|
||||
steeringPressed=False,
|
||||
cruiseState=SimpleNamespace(available=False),
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
standstill=False,
|
||||
))
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
|
||||
for frame in range(_ASCENT_AOL_ARM_FRAMES):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(frame + 1, [msg])])
|
||||
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert controller.ascent_aol_arm_frames == 0
|
||||
|
||||
CS.out.cruiseState.available = True
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(_ASCENT_AOL_ARM_FRAMES + 1, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert controller.ascent_aol_arm_frames == 1
|
||||
|
||||
|
||||
def test_ascent_angle_controller_does_not_delay_normal_engagement():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=5.0))
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=10.0,
|
||||
steeringAngleDeg=0.0,
|
||||
steeringRateDeg=0.0,
|
||||
steeringTorque=0.0,
|
||||
steeringPressed=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
standstill=False,
|
||||
))
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(1, [msg])])
|
||||
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
|
||||
|
||||
def test_lkas_hud_state_uses_angle_request_state():
|
||||
update_source = inspect.getsource(CarController.update)
|
||||
|
||||
assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.latActive" in update_source
|
||||
assert "CS.es_lkas_state_msg, self._lkas_status_active(CC), hud_control.visualAlert" in update_source
|
||||
assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled" not in update_source
|
||||
|
||||
|
||||
@@ -736,3 +793,51 @@ def test_lkas_hud_active_bit_follows_lateral_state(enabled, expected):
|
||||
|
||||
assert parser.can_valid
|
||||
assert parser.vl["ES_LKAS_State"]["LKAS_ACTIVE"] == expected
|
||||
|
||||
|
||||
def test_outback_manual_steering_keeps_cooperative_angle_request():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(
|
||||
enabled=False,
|
||||
latActive=True,
|
||||
actuators=SimpleNamespace(steeringAngleDeg=-225.0),
|
||||
)
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=0.9,
|
||||
steeringAngleDeg=-57.0,
|
||||
steeringRateDeg=-45.0,
|
||||
steeringTorque=0.0,
|
||||
steeringPressed=True,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
standstill=False,
|
||||
))
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
|
||||
for frame, steering_torque in enumerate((-79.0, -81.0, -170.0, -250.0, -250.0, 79.0, 81.0, 170.0, 250.0, 250.0), start=1):
|
||||
CS.out.steeringTorque = steering_torque
|
||||
CS.out.steeringPressed = abs(steering_torque) > 80.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(frame, [msg])])
|
||||
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
assert CC.actuators.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < CS.out.steeringAngleDeg
|
||||
assert controller._lkas_status_active(CC)
|
||||
|
||||
|
||||
def test_ascent_hud_waits_for_angle_request():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(latActive=True)
|
||||
|
||||
assert not controller._lkas_status_active(CC)
|
||||
controller.angle_lkas_active = True
|
||||
assert controller._lkas_status_active(CC)
|
||||
|
||||
|
||||
def test_other_angle_cars_keep_lateral_status_behavior():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
|
||||
controller = CarController({}, CP)
|
||||
controller.angle_lkas_active = False
|
||||
|
||||
assert controller._lkas_status_active(SimpleNamespace(latActive=True))
|
||||
|
||||
@@ -5,9 +5,10 @@ from opendbc.car.lateral import apply_steer_angle_limits_vm
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.tesla.coop_steering import CooperativeSteeringController
|
||||
from opendbc.car.tesla.teslacan import TeslaCAN
|
||||
from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven
|
||||
from opendbc.car.tesla.preap.carcontroller import PreAPLongController, init_preap_can
|
||||
from opendbc.car.tesla.preap.stock_cc_spoofer import StockCCSpoofer
|
||||
from opendbc.car.tesla.values import CANBUS, CAR, CarControllerParams, TeslaSafetyFlags
|
||||
from opendbc.car.tesla.values import CANBUS, CAR, CarControllerParams, TeslaSafetyFlags, LEGACY_CARS
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
|
||||
def get_safety_CP():
|
||||
@@ -24,6 +25,7 @@ class CarController(CarControllerBase):
|
||||
self.coop_enabled = CP.carFingerprint == CAR.TESLA_MODEL_3 and any(
|
||||
config.safetyParam & TeslaSafetyFlags.COOP_STEERING.value for config in CP.safetyConfigs
|
||||
)
|
||||
self._clear_steering_limit_info()
|
||||
self.packer = CANPacker(dbc_names[Bus.party])
|
||||
self.tesla_can = TeslaCAN(self.packer)
|
||||
self.preap_long = None
|
||||
@@ -38,9 +40,37 @@ class CarController(CarControllerBase):
|
||||
self.stock_cc = StockCCSpoofer()
|
||||
from opendbc.car.tesla.interface import CarInterface
|
||||
self.VM = VehicleModel(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP))
|
||||
elif CP.carFingerprint in LEGACY_CARS:
|
||||
self.packers = {
|
||||
CANBUS.party: CANPacker(dbc_names[Bus.party]),
|
||||
}
|
||||
self.tesla_can = TeslaCANRaven(self.packers)
|
||||
from opendbc.car.tesla.interface import CarInterface
|
||||
self.VM = VehicleModel(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1))
|
||||
|
||||
def _clear_steering_limit_info(self):
|
||||
self.steering_limit_info_valid = False
|
||||
self.model_limit_error_deg = 0.0
|
||||
self.resume_limit_error_deg = 0.0
|
||||
self.cooperative_limit_error_deg = 0.0
|
||||
self.cooperative_offset_deg = 0.0
|
||||
self.steering_limit_mono_time = 0
|
||||
self.combined_limit_error_deg = 0.0
|
||||
|
||||
def get_steering_limit_info(self) -> dict[str, bool | float | int]:
|
||||
return {
|
||||
"valid": self.steering_limit_info_valid,
|
||||
"modelLimitErrorDeg": self.model_limit_error_deg,
|
||||
"resumeLimitErrorDeg": self.resume_limit_error_deg,
|
||||
"cooperativeLimitErrorDeg": self.cooperative_limit_error_deg,
|
||||
"cooperativeOffsetDeg": self.cooperative_offset_deg,
|
||||
"monoTime": self.steering_limit_mono_time,
|
||||
"combinedLimitErrorDeg": self.combined_limit_error_deg,
|
||||
}
|
||||
|
||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||
if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
|
||||
self._clear_steering_limit_info()
|
||||
return self._update_preap(CC, CS)
|
||||
|
||||
actuators = CC.actuators
|
||||
@@ -48,8 +78,12 @@ class CarController(CarControllerBase):
|
||||
|
||||
# Preserve the stock controller path unless cooperative steering is explicitly enabled.
|
||||
lat_active = CC.latActive and (not CS.out.steeringDisengage if self.coop_enabled else CS.hands_on_level < 3)
|
||||
if not (self.coop_enabled and lat_active):
|
||||
self._clear_steering_limit_info()
|
||||
|
||||
if self.frame % 2 == 0:
|
||||
requested_angle = actuators.steeringAngleDeg
|
||||
|
||||
# Angular rate limit based on speed
|
||||
self.apply_angle_last = apply_steer_angle_limits_vm(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg,
|
||||
lat_active, CarControllerParams, self.VM)
|
||||
@@ -57,9 +91,34 @@ class CarController(CarControllerBase):
|
||||
self.apply_angle_command_last, lat_active = self.coop_steer.update(
|
||||
self.apply_angle_last, lat_active, self.coop_enabled, CS, self.VM,
|
||||
)
|
||||
can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_command_last, lat_active))
|
||||
|
||||
if self.frame % 10 == 0:
|
||||
if self.coop_enabled and lat_active:
|
||||
model_limit_error_deg = abs(requested_angle - self.apply_angle_last)
|
||||
resume_limit_error_deg = self.coop_steer.resume_limit_error_deg
|
||||
cooperative_limit_error_deg = self.coop_steer.cooperative_limit_error_deg
|
||||
cooperative_offset_deg = self.coop_steer.cooperative_offset_deg
|
||||
combined_limit_error_deg = abs(requested_angle + cooperative_offset_deg - self.apply_angle_command_last)
|
||||
limit_values = (model_limit_error_deg, resume_limit_error_deg, cooperative_limit_error_deg,
|
||||
cooperative_offset_deg, combined_limit_error_deg)
|
||||
|
||||
if all(np.isfinite(value) for value in limit_values):
|
||||
self.steering_limit_info_valid = True
|
||||
self.model_limit_error_deg = model_limit_error_deg
|
||||
self.resume_limit_error_deg = resume_limit_error_deg
|
||||
self.cooperative_limit_error_deg = cooperative_limit_error_deg
|
||||
self.cooperative_offset_deg = cooperative_offset_deg
|
||||
self.steering_limit_mono_time = now_nanos
|
||||
self.combined_limit_error_deg = combined_limit_error_deg
|
||||
else:
|
||||
self._clear_steering_limit_info()
|
||||
|
||||
if self.CP.carFingerprint in LEGACY_CARS:
|
||||
cntr = (self.frame // 2) % 16
|
||||
can_sends.append(self.tesla_can.create_steering_control(cntr, self.apply_angle_command_last, lat_active))
|
||||
else:
|
||||
can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_command_last, lat_active))
|
||||
|
||||
if self.frame % 10 == 0 and self.CP.carFingerprint not in LEGACY_CARS:
|
||||
can_sends.append(self.tesla_can.create_steering_allowed())
|
||||
|
||||
# Longitudinal control
|
||||
@@ -68,13 +127,21 @@ class CarController(CarControllerBase):
|
||||
state = 13 if CC.cruiseControl.cancel else 4 # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
|
||||
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
cntr = (self.frame // 4) % 8
|
||||
can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, self.frame, CS.out.vEgo, CS.out.gasPressed))
|
||||
if self.CP.carFingerprint in LEGACY_CARS:
|
||||
hw1_accel = accel if CC.longActive and not CC.cruiseControl.cancel else 0.
|
||||
hw1_active = CC.longActive and not CC.cruiseControl.cancel
|
||||
can_sends.append(self.tesla_can.create_longitudinal_command(state, hw1_accel, cntr, CS.out.vEgo, hw1_active, CS.out.gasPressed))
|
||||
else:
|
||||
can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, self.frame, CS.out.vEgo, CS.out.gasPressed))
|
||||
|
||||
else:
|
||||
# Increment counter so cancel is prioritized even without openpilot longitudinal
|
||||
if CC.cruiseControl.cancel:
|
||||
cntr = (CS.das_control["DAS_controlCounter"] + 1) % 8
|
||||
can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, self.frame, CS.out.vEgo, False))
|
||||
if self.CP.carFingerprint in LEGACY_CARS:
|
||||
can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, CS.out.vEgo, False, CS.out.gasPressed))
|
||||
else:
|
||||
can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, self.frame, CS.out.vEgo, False))
|
||||
|
||||
# TODO: HUD control
|
||||
new_actuators = actuators.as_builder()
|
||||
|
||||
@@ -4,7 +4,10 @@ from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, 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
|
||||
from opendbc.car.tesla.values import (
|
||||
DBC, CANBUS, GEAR_MAP, STEER_DISENGAGE_THRESHOLD, STEER_THRESHOLD, TeslaSafetyFlags,
|
||||
CAR, LEGACY_CARS,
|
||||
)
|
||||
from opendbc.car.tesla.preap.carstate import get_preap_can_parsers, update_preap
|
||||
from opendbc.car.tesla.preap.engagement import PreAPEngagement
|
||||
from opendbc.car.tesla.preap.nap_conf import nap_conf
|
||||
@@ -25,8 +28,19 @@ class CarState(CarStateBase):
|
||||
def __init__(self, CP, FPCP):
|
||||
super().__init__(CP, FPCP)
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party])
|
||||
self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] if CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP else \
|
||||
self.can_define.dv["DI_torque2"]["DI_gear"]
|
||||
if CP.carFingerprint in LEGACY_CARS:
|
||||
self.can_define_party = CANDefine(DBC[CP.carFingerprint][Bus.party])
|
||||
self.can_define_pt = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||||
self.can_define_chassis = CANDefine(DBC[CP.carFingerprint][Bus.chassis])
|
||||
self.can_defines = {
|
||||
**self.can_define_party.dv,
|
||||
**self.can_define_pt.dv,
|
||||
**self.can_define_chassis.dv,
|
||||
}
|
||||
self.shifter_values = self.can_defines["DI_torque2"]["DI_gear"]
|
||||
else:
|
||||
self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] if CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP else \
|
||||
self.can_define.dv["DI_torque2"]["DI_gear"]
|
||||
|
||||
self.autopark = False
|
||||
self.autopark_prev = False
|
||||
@@ -75,6 +89,8 @@ class CarState(CarStateBase):
|
||||
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
|
||||
if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
|
||||
return update_preap(self, can_parsers)
|
||||
if self.CP.carFingerprint in LEGACY_CARS:
|
||||
return self.update_legacy(can_parsers)
|
||||
|
||||
cp_party = can_parsers[Bus.party]
|
||||
cp_ap_party = can_parsers[Bus.ap_party]
|
||||
@@ -173,10 +189,94 @@ class CarState(CarStateBase):
|
||||
|
||||
return ret, fp_ret
|
||||
|
||||
def update_legacy(self, can_parsers):
|
||||
cp_party = can_parsers[Bus.party]
|
||||
cp_ap_party = can_parsers[Bus.ap_party]
|
||||
cp_pt = can_parsers[Bus.pt]
|
||||
cp_ap_pt = can_parsers[Bus.ap_pt]
|
||||
cp_chassis = can_parsers[Bus.chassis]
|
||||
ret = structs.CarState()
|
||||
fp_ret = custom.StarPilotCarState.new_message()
|
||||
|
||||
# Vehicle speed
|
||||
ret.vEgoRaw = cp_chassis.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
# Gas and brake
|
||||
ret.gasPressed = cp_pt.vl["DI_torque1"]["DI_pedalPos"] > 0
|
||||
ret.brake = 0
|
||||
ret.brakePressed = cp_chassis.vl["BrakeMessage"]["driverBrakeStatus"] != 1
|
||||
|
||||
# Steering wheel and EPAS status
|
||||
epas_status = cp_chassis.vl["EPAS_sysStatus"]
|
||||
self.hands_on_level = epas_status["EPAS_handsOnLevel"]
|
||||
ret.steeringAngleDeg = -epas_status["EPAS_internalSAS"]
|
||||
ret.steeringRateDeg = -cp_chassis.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"]
|
||||
ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"]
|
||||
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > STEER_THRESHOLD, 5)
|
||||
|
||||
eac_status = self.can_defines["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(epas_status["EPAS_eacStatus"]), None)
|
||||
ret.steerFaultPermanent = eac_status == "EAC_FAULT"
|
||||
ret.steerFaultTemporary = eac_status == "EAC_INHIBITED"
|
||||
eac_error_code = self.can_defines["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(epas_status["EPAS_eacErrorCode"]), None)
|
||||
ret.steeringDisengage = self.hands_on_level >= 3 or (
|
||||
eac_status == "EAC_INHIBITED" and eac_error_code == "EAC_ERROR_HIGH_ANGLE_RATE_SAFETY"
|
||||
)
|
||||
|
||||
# Cruise
|
||||
cruise_state = self.can_defines["DI_state"]["DI_cruiseState"].get(int(cp_chassis.vl["DI_state"]["DI_cruiseState"]), None)
|
||||
speed_units = self.can_defines["DI_state"]["DI_speedUnits"].get(int(cp_chassis.vl["DI_state"]["DI_speedUnits"]), None)
|
||||
cruise_enabled = cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
|
||||
ret.cruiseState.enabled = cruise_enabled
|
||||
if speed_units == "KPH":
|
||||
ret.cruiseState.speed = max(cp_chassis.vl["DI_state"]["DI_hw1CruiseSet"] * CV.KPH_TO_MS, 1e-3)
|
||||
elif speed_units == "MPH":
|
||||
ret.cruiseState.speed = max(cp_chassis.vl["DI_state"]["DI_hw1CruiseSet"] * CV.MPH_TO_MS, 1e-3)
|
||||
ret.cruiseState.available = cruise_state == "STANDBY" or ret.cruiseState.enabled
|
||||
ret.cruiseState.standstill = False
|
||||
ret.standstill = ret.vEgoRaw < 0.1
|
||||
ret.accFaulted = cruise_state == "FAULT"
|
||||
|
||||
# Gear, body state, and safety state
|
||||
ret.gearShifter = GEAR_MAP[self.can_defines["DI_torque2"]["DI_gear"].get(
|
||||
int(cp_chassis.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")]
|
||||
|
||||
doors = ("DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE")
|
||||
ret.doorOpen = any(
|
||||
self.can_defines["GTW_carState"][door].get(int(cp_chassis.vl["GTW_carState"][door]), "OPEN") == "OPEN"
|
||||
for door in doors
|
||||
)
|
||||
ret.leftBlinker = cp_chassis.vl["GTW_carState"]["BC_indicatorLStatus"] == 1
|
||||
ret.rightBlinker = cp_chassis.vl["GTW_carState"]["BC_indicatorRStatus"] == 1
|
||||
|
||||
_ = cp_chassis.vl["SDM1"]
|
||||
_ = cp_chassis.vl["RCM_status"]
|
||||
sd_time = cp_chassis.ts_nanos["SDM1"]["SDM_bcklDrivStatus"]
|
||||
rcm_time = cp_chassis.ts_nanos["RCM_status"]["RCM_buckleDriverStatus"]
|
||||
if sd_time and cp_chassis._last_update_nanos - sd_time <= 1_000_000_000:
|
||||
ret.seatbeltUnlatched = cp_chassis.vl["SDM1"]["SDM_bcklDrivStatus"] != 1
|
||||
elif rcm_time and cp_chassis._last_update_nanos - rcm_time <= 1_000_000_000:
|
||||
ret.seatbeltUnlatched = cp_chassis.vl["RCM_status"]["RCM_buckleDriverStatus"] != 1
|
||||
else:
|
||||
ret.seatbeltUnlatched = True
|
||||
|
||||
ret.stockAeb = cp_ap_pt.vl["DAS_control"]["DAS_aebEvent"] == 1
|
||||
ret.stockLkas = cp_ap_party.vl["DAS_steeringControl"]["DAS_steeringControlType"] == 2
|
||||
self.das_control = copy.copy(cp_ap_pt.vl["DAS_control"])
|
||||
return ret, fp_ret
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP):
|
||||
if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
|
||||
return get_preap_can_parsers(CP)
|
||||
if CP.carFingerprint in LEGACY_CARS:
|
||||
return {
|
||||
Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
|
||||
Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party),
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CANBUS.party),
|
||||
Bus.ap_pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CANBUS.autopilot_party),
|
||||
Bus.chassis: CANParser(DBC[CP.carFingerprint][Bus.chassis], [("SDM1", 0), ("RCM_status", 0)], CANBUS.party),
|
||||
}
|
||||
return {
|
||||
Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
|
||||
Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party)
|
||||
|
||||
@@ -62,6 +62,9 @@ class CooperativeSteeringController:
|
||||
self.angle_override = 0.0
|
||||
self.resume_rate_limiter_delta = SteerRateLimiter()
|
||||
self.resume_rate_limiter = SteerRateLimiter()
|
||||
self.resume_limit_error_deg = 0.0
|
||||
self.cooperative_limit_error_deg = 0.0
|
||||
self.cooperative_offset_deg = 0.0
|
||||
|
||||
def reset_override_state(self, apply_angle: float) -> None:
|
||||
self.apply_angle_last = apply_angle
|
||||
@@ -105,19 +108,25 @@ class CooperativeSteeringController:
|
||||
return self.resume_rate_limiter.update(apply_angle, angle_rate_delta)
|
||||
|
||||
def update(self, apply_angle: float, lat_active: bool, enabled: bool, CS, VM: VehicleModel) -> tuple[float, bool]:
|
||||
self.resume_limit_error_deg = 0.0
|
||||
self.cooperative_limit_error_deg = 0.0
|
||||
self.cooperative_offset_deg = 0.0
|
||||
if not enabled:
|
||||
self.reset_resume_state(apply_angle)
|
||||
self.reset_override_state(apply_angle)
|
||||
return apply_angle, lat_active
|
||||
|
||||
requested_angle = apply_angle
|
||||
apply_angle = self.apply_resume_rate_limit(lat_active, apply_angle)
|
||||
self.resume_limit_error_deg = abs(requested_angle - apply_angle)
|
||||
if not lat_active:
|
||||
self.reset_override_state(apply_angle)
|
||||
return apply_angle, False
|
||||
|
||||
apply_angle_delta = apply_angle - self.apply_angle_last
|
||||
self.apply_angle_last = apply_angle
|
||||
apply_angle += self.update_override_angle(apply_angle_delta, CS.out.steeringTorque, CS.out.vEgo, VM)
|
||||
self.cooperative_offset_deg = self.update_override_angle(apply_angle_delta, CS.out.steeringTorque, CS.out.vEgo, VM)
|
||||
apply_angle += self.cooperative_offset_deg
|
||||
|
||||
limited_angle = apply_steer_angle_limits_vm(
|
||||
apply_angle,
|
||||
@@ -129,5 +138,6 @@ class CooperativeSteeringController:
|
||||
VM,
|
||||
)
|
||||
self.coop_apply_angle_last = limited_angle
|
||||
self.cooperative_limit_error_deg = abs(apply_angle - limited_angle)
|
||||
self.unwind_override_angle(apply_angle - limited_angle)
|
||||
return limited_angle, True
|
||||
|
||||
@@ -5,6 +5,12 @@ from opendbc.car.tesla.values import CAR
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.TESLA_MODEL_S_HW1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'1016704-00-HAA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x10\x00A',
|
||||
],
|
||||
},
|
||||
CAR.TESLA_MODEL_3: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'TeM3_E014p10_0.0.0 (16),E014.17.00',
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
from opendbc.car import get_safety_config, structs
|
||||
from opendbc.car import Bus, get_safety_config, structs
|
||||
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
|
||||
from opendbc.car.tesla.values import TeslaSafetyFlags, CAR, DBC, LEGACY_CARS
|
||||
from opendbc.car.tesla.preap.interface import get_preap_accel_limits, get_preap_params
|
||||
|
||||
|
||||
@@ -32,6 +32,21 @@ class CarInterface(CarInterfaceBase):
|
||||
if candidate == CAR.TESLA_MODEL_S_PREAP:
|
||||
return get_preap_params(ret)
|
||||
|
||||
if candidate in LEGACY_CARS:
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla, TeslaSafetyFlags.FLAG_HW1.value)]
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerAtStandstill = True
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||
ret.radarUnavailable = Bus.radar not in DBC[candidate]
|
||||
ret.radarTimeStepDEPRECATED = 0.125
|
||||
ret.alphaLongitudinalAvailable = True
|
||||
|
||||
if alpha_long:
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.LONG_CONTROL.value
|
||||
return ret
|
||||
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)]
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
@@ -16,7 +16,7 @@ class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
|
||||
self.radar_off_can = CP.radarUnavailable or CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP
|
||||
self.radar_off_can = CP.radarUnavailable or Bus.radar not in DBC[CP.carFingerprint]
|
||||
self.updated_messages: set[int] = set()
|
||||
self.track_id = 0
|
||||
self.radar_offset = float(nap_conf.radar_offset) if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP else 0.0
|
||||
|
||||
@@ -0,0 +1,53 @@
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.interfaces import V_CRUISE_MAX
|
||||
from opendbc.car.tesla.values import CANBUS, CarControllerParams
|
||||
|
||||
|
||||
class TeslaCANRaven:
|
||||
"""CAN commands used by the legacy Model S/X powertrain and EPAS buses."""
|
||||
|
||||
def __init__(self, packers):
|
||||
self.packers = packers
|
||||
self.CCP = CarControllerParams
|
||||
self.jerk_upper = self.CCP.JERK_LIMIT_MAX
|
||||
self.jerk_lower = self.CCP.JERK_LIMIT_MIN
|
||||
|
||||
@staticmethod
|
||||
def checksum(msg_id, dat):
|
||||
return ((msg_id & 0xFF) + ((msg_id >> 8) & 0xFF) + sum(dat)) & 0xFF
|
||||
|
||||
def create_steering_control(self, counter, angle, enabled):
|
||||
values = {
|
||||
"DAS_steeringControlCounter": counter,
|
||||
"DAS_steeringAngleRequest": -angle,
|
||||
"DAS_steeringHapticRequest": 0,
|
||||
"DAS_steeringControlType": 1 if enabled else 0,
|
||||
}
|
||||
data = self.packers[CANBUS.party].make_can_msg("DAS_steeringControl", CANBUS.party, values)[1]
|
||||
values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
|
||||
return self.packers[CANBUS.party].make_can_msg("DAS_steeringControl", CANBUS.party, values)
|
||||
|
||||
def create_longitudinal_command(self, acc_state, accel, counter, v_ego, active, gas_pressed):
|
||||
set_speed = max(v_ego * CV.MS_TO_KPH, 0)
|
||||
if active:
|
||||
set_speed = 0 if accel < 0 else V_CRUISE_MAX
|
||||
|
||||
if gas_pressed:
|
||||
self.jerk_upper = self.jerk_lower = 0.0
|
||||
else:
|
||||
self.jerk_lower = max(self.jerk_lower - self.CCP.JERK_RAMP_RATE, self.CCP.JERK_LIMIT_MIN)
|
||||
self.jerk_upper = min(self.jerk_upper + self.CCP.JERK_RAMP_RATE, self.CCP.JERK_LIMIT_MAX)
|
||||
|
||||
values = {
|
||||
"DAS_setSpeed": set_speed,
|
||||
"DAS_accState": acc_state,
|
||||
"DAS_aebEvent": 0,
|
||||
"DAS_jerkMin": self.jerk_lower,
|
||||
"DAS_jerkMax": self.jerk_upper,
|
||||
"DAS_accelMin": accel,
|
||||
"DAS_accelMax": max(accel, 0),
|
||||
"DAS_controlCounter": counter,
|
||||
}
|
||||
data = self.packers[CANBUS.party].make_can_msg("DAS_control", CANBUS.party, values)[1]
|
||||
values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
|
||||
return self.packers[CANBUS.party].make_can_msg("DAS_control", CANBUS.party, values)
|
||||
+1
File diff suppressed because one or more lines are too long
@@ -0,0 +1,148 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Offline AP1/HW1 CAN, radar, controller, and panda-safety replay.
|
||||
|
||||
Usage: PYTHONPATH=. python -m opendbc.car.tesla.tests.replay_hw1_route PATH_TO_RLOGS
|
||||
|
||||
The supplied Pre-AP recording contains stock AP commands copied to bus 0 while
|
||||
ELM327/old firmware forwarded traffic. The counterfactual run skips those copies:
|
||||
the HW1 safety mode blocks stock 0x488/0x2b9 forwarding on bus 2. This is NOT a
|
||||
physical HW1 drive; it cannot validate engagement or steering actuation on-car.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
from collections import Counter
|
||||
from pathlib import Path
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.tesla.carcontroller import CarController
|
||||
from opendbc.car.tesla.carstate import CarState
|
||||
from opendbc.car.tesla.interface import CarInterface
|
||||
from opendbc.car.tesla.radar_interface import RadarInterface
|
||||
from opendbc.car.tesla.values import CAR, DBC, TeslaSafetyFlags
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
|
||||
def replay(paths: list[Path], simulate_active: bool = False):
|
||||
fp = {0: {0x201: 5}, 1: {}, 2: {}}
|
||||
cp = CarInterface.get_params(CAR.TESLA_MODEL_S_HW1, fp, [], True, False, False, None)
|
||||
assert cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.tesla
|
||||
assert cp.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value | TeslaSafetyFlags.LONG_CONTROL.value
|
||||
safety = libsafety_py.libsafety
|
||||
assert safety.set_safety_hooks(int(structs.CarParams.SafetyModel.tesla), cp.safetyConfigs[0].safetyParam) == 0
|
||||
safety.init_tests()
|
||||
|
||||
parsers = CarState.get_can_parsers(cp)
|
||||
cs = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp)
|
||||
active_controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp) if simulate_active else None
|
||||
radar = RadarInterface(cp)
|
||||
stats = Counter()
|
||||
first_rejected = []
|
||||
active_rejected = []
|
||||
last_ap_command: dict[tuple[int, bytes], int] = {}
|
||||
suppressed_examples = []
|
||||
|
||||
for path in paths:
|
||||
for event in LogReader(str(path)):
|
||||
if event.which() != "can":
|
||||
continue
|
||||
t = event.logMonoTime
|
||||
frames = [(x.address, bytes(x.dat), x.src) for x in event.can]
|
||||
stock_in_event = {(a, d) for a, d, b in frames if b == 2 and a in (0x488, 0x2b9)}
|
||||
for a, d, b in frames:
|
||||
if b == 2 and a in (0x488, 0x2b9):
|
||||
last_ap_command[(a, d)] = t
|
||||
if b == 0 and a in (0x488, 0x2b9):
|
||||
seen = last_ap_command.get((a, d), -1)
|
||||
if (a, d) in stock_in_event or (0 <= t - seen < 250_000_000):
|
||||
stats["suppressed_bus0_stock_copies"] += 1
|
||||
continue
|
||||
stats["unmatched_bus0_stock_commands"] += 1
|
||||
if len(suppressed_examples) < 5:
|
||||
suppressed_examples.append((path.name, t, hex(a), d.hex()))
|
||||
if b < 128:
|
||||
stats["physical_rx"] += 1
|
||||
if not safety.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d)):
|
||||
stats["rx_rejected"] += 1
|
||||
if b == 2 and a in (0x488, 0x2b9):
|
||||
stats["stock_forward_blocked"] += safety.safety_fwd_hook(b, a) == -1
|
||||
|
||||
safety.set_timer((t // 1000) & 0xffffffff)
|
||||
safety.safety_tick_current_safety_config()
|
||||
stats["safety_invalid_ticks"] += not safety.safety_config_valid()
|
||||
stats["relay_malfunction_ticks"] += safety.get_relay_malfunction()
|
||||
stats["controls_allowed_ticks"] += safety.get_controls_allowed()
|
||||
|
||||
batch = [(t, frames)]
|
||||
for parser in parsers.values():
|
||||
parser.update(batch)
|
||||
stats["invalid_car_parser_ticks"] += not parser.can_valid
|
||||
out, _ = cs.update(parsers, None)
|
||||
cs.out = out
|
||||
stats["carstate_faulted_ticks"] += out.accFaulted
|
||||
stats["seatbelt_unlatched_ticks"] += out.seatbeltUnlatched
|
||||
stats["steering_inhibited_ticks"] += out.steerFaultTemporary
|
||||
stats["cruise_engaged_ticks"] += out.cruiseState.enabled
|
||||
|
||||
radar_data = radar.update(batch)
|
||||
if radar_data is not None:
|
||||
stats["radar_updates"] += 1
|
||||
stats["radar_points"] += len(radar_data.points)
|
||||
stats["radar_error_updates"] += radar_data.errors.canError or radar_data.errors.radarFault
|
||||
|
||||
cc = structs.CarControl.new_message()
|
||||
cc.actuators.steeringAngleDeg = out.steeringAngleDeg
|
||||
cc.actuators.accel = 0.
|
||||
# Do not fabricate engagement on the actual faulted/standby route.
|
||||
_, sends = controller.update(cc.as_reader(), cs, t, None)
|
||||
for a, d, b in sends:
|
||||
stats["generated_tx"] += 1
|
||||
stats[f"generated_{hex(a)}"] += 1
|
||||
if not safety.safety_tx_hook(libsafety_py.make_CANPacket(a, b, d)):
|
||||
stats["tx_rejected"] += 1
|
||||
if len(first_rejected) < 5:
|
||||
first_rejected.append((path.name, t, hex(a), d.hex(), out.steeringAngleDeg, safety.get_relay_malfunction()))
|
||||
|
||||
if active_controller is not None:
|
||||
# A synthetic gate test only. This recording never engaged cruise, so
|
||||
# enabling controls here does NOT represent an actual car-state transition.
|
||||
eligible = (not (out.steerFaultTemporary or out.steerFaultPermanent or out.steeringDisengage or out.accFaulted or
|
||||
out.gasPressed or out.brakePressed or out.stockAeb or out.stockLkas) and out.vEgoRaw > 2.)
|
||||
simulated = structs.CarControl.new_message()
|
||||
simulated.latActive = eligible
|
||||
simulated.longActive = eligible
|
||||
simulated.actuators.steeringAngleDeg = out.steeringAngleDeg
|
||||
simulated.actuators.accel = 0.5 if eligible else 0.
|
||||
_, active_sends = active_controller.update(simulated.as_reader(), cs, t, None)
|
||||
if eligible:
|
||||
stats["simulated_eligible_ticks"] += 1
|
||||
safety.set_controls_allowed(True)
|
||||
for a, d, b in active_sends:
|
||||
stats["simulated_tx"] += 1
|
||||
stats[f"simulated_{hex(a)}"] += 1
|
||||
if not safety.safety_tx_hook(libsafety_py.make_CANPacket(a, b, d)):
|
||||
stats["simulated_tx_rejected"] += 1
|
||||
if len(active_rejected) < 5:
|
||||
active_rejected.append((path.name, t, hex(a), d.hex(), out.steeringAngleDeg, out.vEgoRaw))
|
||||
safety.set_controls_allowed(False)
|
||||
stats["can_events"] += 1
|
||||
print(f"{path.name}: {dict(stats)}", flush=True)
|
||||
|
||||
print(f"unmatched bus-0 command examples: {suppressed_examples}")
|
||||
print(f"rejected TX examples: {first_rejected}")
|
||||
print(f"rejected synthetic-active TX examples: {active_rejected}")
|
||||
print(f"final: {dict(stats)}")
|
||||
return stats
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
argp = argparse.ArgumentParser(description=__doc__)
|
||||
argp.add_argument("rlogs", type=Path, help="directory containing segment rlog.zst files")
|
||||
argp.add_argument("--simulate-active", action="store_true", help="force safety engagement only on healthy standby samples")
|
||||
args = argp.parse_args()
|
||||
files = sorted(args.rlogs.glob("*.rlog.zst"))
|
||||
if not files:
|
||||
argp.error("no *.rlog.zst files found")
|
||||
replay(files, args.simulate_active)
|
||||
@@ -1,3 +1,4 @@
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
@@ -73,3 +74,110 @@ def test_safety_flag_is_model_3_only(candidate, enabled, expected):
|
||||
|
||||
if candidate != CAR.TESLA_MODEL_S_PREAP:
|
||||
assert CarController(DBC[candidate], params).coop_enabled is expected
|
||||
|
||||
|
||||
def assert_finite_nonnegative_limit_errors(controller):
|
||||
errors = (controller.resume_limit_error_deg, controller.cooperative_limit_error_deg)
|
||||
assert all(math.isfinite(error) and error >= 0.0 for error in errors)
|
||||
assert math.isfinite(controller.cooperative_offset_deg)
|
||||
|
||||
|
||||
def test_zero_torque_has_zero_cooperative_diagnostics(vehicle_model):
|
||||
controller = CooperativeSteeringController()
|
||||
|
||||
angle, lat_active = controller.update(0.0, True, True, make_car_state(), vehicle_model)
|
||||
|
||||
assert angle == 0.0
|
||||
assert lat_active
|
||||
assert controller.resume_limit_error_deg == 0.0
|
||||
assert controller.cooperative_limit_error_deg == 0.0
|
||||
assert controller.cooperative_offset_deg == 0.0
|
||||
assert_finite_nonnegative_limit_errors(controller)
|
||||
|
||||
|
||||
def test_steady_light_torque_reports_offset_without_real_limiting(vehicle_model):
|
||||
controller = CooperativeSteeringController()
|
||||
|
||||
for _ in range(100):
|
||||
controller.update(0.0, True, True, make_car_state(torque=0.9), vehicle_model)
|
||||
|
||||
assert controller.cooperative_offset_deg > 2.5
|
||||
assert controller.resume_limit_error_deg < 2.5
|
||||
assert controller.cooperative_limit_error_deg < 2.5
|
||||
assert_finite_nonnegative_limit_errors(controller)
|
||||
|
||||
|
||||
def test_torque_reversal_updates_signed_offset_without_negative_errors(vehicle_model):
|
||||
controller = CooperativeSteeringController()
|
||||
for _ in range(100):
|
||||
controller.update(0.0, True, True, make_car_state(torque=0.9), vehicle_model)
|
||||
|
||||
for _ in range(200):
|
||||
controller.update(0.0, True, True, make_car_state(torque=-0.9), vehicle_model)
|
||||
|
||||
assert controller.cooperative_offset_deg < -2.5
|
||||
assert_finite_nonnegative_limit_errors(controller)
|
||||
|
||||
|
||||
def test_release_reports_gradual_offset_unwind(vehicle_model):
|
||||
controller = CooperativeSteeringController()
|
||||
for _ in range(100):
|
||||
controller.update(0.0, True, True, make_car_state(torque=1.5), vehicle_model)
|
||||
|
||||
offsets = []
|
||||
for _ in range(100):
|
||||
controller.update(0.0, True, True, make_car_state(), vehicle_model)
|
||||
offsets.append(controller.cooperative_offset_deg)
|
||||
|
||||
assert offsets[0] > offsets[-1] >= 0.0
|
||||
assert all(next_offset <= offset for offset, next_offset in zip(offsets, offsets[1:]))
|
||||
assert offsets[-1] == pytest.approx(0.0, abs=1e-6)
|
||||
assert_finite_nonnegative_limit_errors(controller)
|
||||
|
||||
|
||||
def test_resume_ramp_reports_resume_limiting(vehicle_model):
|
||||
controller = CooperativeSteeringController()
|
||||
controller.reset_resume_state(0.0)
|
||||
controller.reset_override_state(0.0)
|
||||
|
||||
controller.update(20.0, True, True, make_car_state(), vehicle_model)
|
||||
|
||||
assert controller.resume_limit_error_deg > 2.5
|
||||
assert controller.cooperative_limit_error_deg < 2.5
|
||||
assert controller.cooperative_offset_deg == 0.0
|
||||
|
||||
|
||||
def test_final_limiter_reports_cooperative_target_clipping(vehicle_model):
|
||||
controller = CooperativeSteeringController()
|
||||
controller.reset_resume_state(20.0)
|
||||
controller.reset_override_state(0.0)
|
||||
|
||||
controller.update(20.0, True, True, make_car_state(), vehicle_model)
|
||||
|
||||
assert controller.resume_limit_error_deg == 0.0
|
||||
assert controller.cooperative_limit_error_deg > 2.5
|
||||
assert controller.cooperative_offset_deg == 0.0
|
||||
|
||||
|
||||
def test_final_limiter_remains_visible_with_light_torque(vehicle_model):
|
||||
controller = CooperativeSteeringController()
|
||||
controller.reset_resume_state(20.0)
|
||||
controller.reset_override_state(0.0)
|
||||
|
||||
controller.update(20.0, True, True, make_car_state(torque=-0.9), vehicle_model)
|
||||
|
||||
assert abs(controller.cooperative_offset_deg) > 0.0
|
||||
assert controller.cooperative_limit_error_deg > 2.5
|
||||
|
||||
|
||||
def test_diagnostics_reset_on_disabled_update(vehicle_model):
|
||||
controller = CooperativeSteeringController()
|
||||
controller.reset_resume_state(20.0)
|
||||
controller.update(20.0, True, True, make_car_state(), vehicle_model)
|
||||
assert controller.cooperative_limit_error_deg > 2.5
|
||||
|
||||
controller.update(4.0, True, False, make_car_state(torque=2.0), vehicle_model)
|
||||
|
||||
assert controller.resume_limit_error_deg == 0.0
|
||||
assert controller.cooperative_limit_error_deg == 0.0
|
||||
assert controller.cooperative_offset_deg == 0.0
|
||||
|
||||
@@ -0,0 +1,228 @@
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
import cereal.messaging as messaging
|
||||
|
||||
from cereal import car
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.car.tesla.carcontroller import CarController
|
||||
from opendbc.car.tesla.interface import CarInterface
|
||||
from opendbc.car.tesla.values import CAR, DBC
|
||||
|
||||
|
||||
BASELINE_SHA = "a80064be4fdf4b8765a5e9dec44d8a5c266f48a8"
|
||||
BASELINE_SOURCE_SHA256 = {
|
||||
"opendbc_repo/opendbc/car/tesla/coop_steering.py": "9c9d60bbfae2aaa0d8c1fca203a9fa19a14a19feef5aa89e85ecaa6f6502a9f0",
|
||||
"opendbc_repo/opendbc/car/tesla/carcontroller.py": "1ef3cf646bc4b3c398bd12010e624b90e083426152d632fafb5b2e93fd3d8661",
|
||||
}
|
||||
BASELINE_FIXTURE = Path(__file__).parent / "fixtures" / "coop_steering_baseline_a80064be.json"
|
||||
|
||||
|
||||
def make_params(candidate=CAR.TESLA_MODEL_3, cooperative=True):
|
||||
toggles = SimpleNamespace(tesla_cooperative_steering=cooperative, trailer_load_kg=0.0)
|
||||
return CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, toggles)
|
||||
|
||||
|
||||
def make_car_state(torque=0.0, speed=15.0, angle=0.0, steering_disengage=False):
|
||||
return SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
steeringTorque=torque,
|
||||
steeringAngleDeg=angle,
|
||||
steeringDisengage=steering_disengage,
|
||||
vEgo=speed,
|
||||
vEgoRaw=speed,
|
||||
gasPressed=False,
|
||||
),
|
||||
hands_on_level=0,
|
||||
das_control={"DAS_controlCounter": 0},
|
||||
)
|
||||
|
||||
|
||||
def make_control(requested_angle=0.0, lat_active=True):
|
||||
control = car.CarControl.new_message()
|
||||
control.latActive = lat_active
|
||||
control.actuators.steeringAngleDeg = requested_angle
|
||||
return control.as_reader()
|
||||
|
||||
|
||||
def make_controller(candidate=CAR.TESLA_MODEL_3, cooperative=True):
|
||||
params = make_params(candidate, cooperative)
|
||||
return CarController(DBC[candidate], params)
|
||||
|
||||
|
||||
def run_frame(controller, requested_angle=0.0, torque=0.0, speed=15.0, measured_angle=0.0,
|
||||
lat_active=True, steering_disengage=False, now_nanos=1_000_000_000):
|
||||
return controller.update(
|
||||
make_control(requested_angle, lat_active),
|
||||
make_car_state(torque, speed, measured_angle, steering_disengage),
|
||||
now_nanos,
|
||||
SimpleNamespace(),
|
||||
)
|
||||
|
||||
|
||||
def get_limit_info(controller):
|
||||
return SimpleNamespace(**controller.get_steering_limit_info())
|
||||
|
||||
|
||||
def legacy_actuator_dict(actuators):
|
||||
return actuators.to_dict()
|
||||
|
||||
|
||||
def test_steering_limit_info_defaults_to_invalid():
|
||||
controller = make_controller()
|
||||
|
||||
info = get_limit_info(controller)
|
||||
assert not info.valid
|
||||
assert info.monoTime == 0
|
||||
|
||||
|
||||
def test_steering_limit_info_round_trips_through_custom_message():
|
||||
message = messaging.new_message("starpilotCarControl", valid=True)
|
||||
info = message.starpilotCarControl.steeringLimitInfo
|
||||
info.valid = True
|
||||
info.modelLimitErrorDeg = 1.25
|
||||
info.resumeLimitErrorDeg = 0.5
|
||||
info.cooperativeLimitErrorDeg = 2.0
|
||||
info.cooperativeOffsetDeg = -4.5
|
||||
info.monoTime = 1_234_567_890
|
||||
info.combinedLimitErrorDeg = 3.75
|
||||
|
||||
restored = messaging.log_from_bytes(message.to_bytes())
|
||||
restored_info = restored.starpilotCarControl.steeringLimitInfo
|
||||
assert restored_info.valid
|
||||
assert restored_info.modelLimitErrorDeg == 1.25
|
||||
assert restored_info.resumeLimitErrorDeg == 0.5
|
||||
assert restored_info.cooperativeLimitErrorDeg == 2.0
|
||||
assert restored_info.cooperativeOffsetDeg == -4.5
|
||||
assert restored_info.monoTime == 1_234_567_890
|
||||
assert restored_info.combinedLimitErrorDeg == 3.75
|
||||
|
||||
|
||||
def test_active_cooperative_controller_reports_diagnostics():
|
||||
controller = make_controller()
|
||||
requested_angle = 20.0
|
||||
now_nanos = 1_234_567_890
|
||||
|
||||
actuators, _ = run_frame(controller, requested_angle, torque=0.9, measured_angle=0.0, now_nanos=now_nanos)
|
||||
|
||||
info = get_limit_info(controller)
|
||||
assert info.valid
|
||||
assert info.monoTime == now_nanos
|
||||
assert info.modelLimitErrorDeg == pytest.approx(abs(requested_angle - controller.apply_angle_last), abs=1e-5)
|
||||
assert info.resumeLimitErrorDeg == pytest.approx(controller.coop_steer.resume_limit_error_deg, abs=1e-5)
|
||||
assert info.cooperativeLimitErrorDeg == pytest.approx(controller.coop_steer.cooperative_limit_error_deg, abs=1e-5)
|
||||
assert info.cooperativeOffsetDeg == pytest.approx(controller.coop_steer.cooperative_offset_deg, abs=1e-5)
|
||||
assert info.combinedLimitErrorDeg == pytest.approx(
|
||||
abs(requested_angle + info.cooperativeOffsetDeg - actuators.steeringAngleDeg), abs=1e-5,
|
||||
)
|
||||
assert info.modelLimitErrorDeg > 2.5
|
||||
assert info.cooperativeOffsetDeg > 0.0
|
||||
assert info.combinedLimitErrorDeg > 2.5
|
||||
errors = (info.modelLimitErrorDeg, info.resumeLimitErrorDeg,
|
||||
info.cooperativeLimitErrorDeg, info.combinedLimitErrorDeg)
|
||||
assert all(math.isfinite(error) and error >= 0.0 for error in errors)
|
||||
|
||||
|
||||
def test_cooperative_offset_alone_does_not_become_limiter_error():
|
||||
controller = make_controller()
|
||||
actuators = None
|
||||
|
||||
for frame in range(200):
|
||||
actuators, _ = run_frame(controller, torque=0.9, now_nanos=1_000_000_000 + frame * 10_000_000)
|
||||
|
||||
assert actuators is not None
|
||||
info = get_limit_info(controller)
|
||||
assert info.valid
|
||||
assert info.cooperativeOffsetDeg > 2.5
|
||||
assert info.modelLimitErrorDeg < 2.5
|
||||
assert info.resumeLimitErrorDeg < 2.5
|
||||
assert info.cooperativeLimitErrorDeg < 2.5
|
||||
assert info.combinedLimitErrorDeg < 2.5
|
||||
|
||||
|
||||
def test_combined_error_keeps_two_same_direction_small_limits_visible():
|
||||
controller = make_controller()
|
||||
# Prime the resume limiter to the first-stage output for this literal input.
|
||||
controller.coop_steer.reset_resume_state(-0.9954867959022522)
|
||||
|
||||
actuators, _ = run_frame(controller, -2.5, torque=-1.5, speed=12.5, measured_angle=0.0)
|
||||
|
||||
info = get_limit_info(controller)
|
||||
assert info.modelLimitErrorDeg == pytest.approx(1.5045133, abs=1e-5)
|
||||
assert info.resumeLimitErrorDeg == pytest.approx(0.0, abs=1e-5)
|
||||
assert info.cooperativeLimitErrorDeg == pytest.approx(1.5045133, abs=1e-5)
|
||||
assert info.modelLimitErrorDeg < 2.5
|
||||
assert info.cooperativeLimitErrorDeg < 2.5
|
||||
assert info.combinedLimitErrorDeg == pytest.approx(3.0090265, abs=1e-5)
|
||||
assert info.combinedLimitErrorDeg > 2.5
|
||||
|
||||
|
||||
def test_intervening_100hz_frame_retains_matching_50hz_sample():
|
||||
controller = make_controller()
|
||||
first, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_000_000_000)
|
||||
first_info = controller.get_steering_limit_info()
|
||||
|
||||
second, _ = run_frame(controller, -40.0, torque=-1.5, now_nanos=1_010_000_000)
|
||||
|
||||
assert controller.get_steering_limit_info() == first_info
|
||||
assert controller.get_steering_limit_info()["monoTime"] == 1_000_000_000
|
||||
|
||||
|
||||
def test_inactive_interval_clears_sample_until_next_steering_update():
|
||||
controller = make_controller()
|
||||
active, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_000_000_000)
|
||||
assert get_limit_info(controller).valid
|
||||
|
||||
inactive, _ = run_frame(controller, 8.0, torque=0.9, lat_active=False, now_nanos=1_010_000_000)
|
||||
assert not get_limit_info(controller).valid
|
||||
assert get_limit_info(controller).monoTime == 0
|
||||
|
||||
resumed, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_020_000_000)
|
||||
assert get_limit_info(controller).valid
|
||||
assert get_limit_info(controller).monoTime == 1_020_000_000
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("candidate", "cooperative", "steering_disengage"), (
|
||||
(CAR.TESLA_MODEL_3, False, False),
|
||||
(CAR.TESLA_MODEL_Y, True, False),
|
||||
(CAR.TESLA_MODEL_3, True, True),
|
||||
))
|
||||
def test_diagnostics_invalid_when_not_in_supported_active_path(candidate, cooperative, steering_disengage):
|
||||
controller = make_controller(candidate, cooperative)
|
||||
|
||||
actuators, _ = run_frame(controller, torque=1.5, steering_disengage=steering_disengage)
|
||||
|
||||
info = get_limit_info(controller)
|
||||
assert not info.valid
|
||||
assert info.monoTime == 0
|
||||
|
||||
|
||||
def test_actual_actuators_and_steering_can_match_pinned_baseline_fixture():
|
||||
fixture = json.loads(BASELINE_FIXTURE.read_text())
|
||||
assert fixture["metadata"] == {
|
||||
"schemaVersion": 1,
|
||||
"baselineSha": BASELINE_SHA,
|
||||
"baselineSourceSha256": BASELINE_SOURCE_SHA256,
|
||||
"frameCount": 386,
|
||||
}
|
||||
|
||||
candidate = make_controller()
|
||||
for expected in fixture["frames"]:
|
||||
inputs = expected["input"]
|
||||
candidate_actuators, candidate_can = run_frame(
|
||||
candidate,
|
||||
inputs["requestedAngleDeg"],
|
||||
inputs["torqueNm"],
|
||||
inputs["speedMps"],
|
||||
inputs["measuredAngleDeg"],
|
||||
inputs["latActive"],
|
||||
inputs["steeringDisengage"],
|
||||
inputs["nowNanos"],
|
||||
)
|
||||
|
||||
assert legacy_actuator_dict(candidate_actuators) == expected["actuators"]
|
||||
assert [[address, data.hex(), bus] for address, data, bus in candidate_can] == expected["can"]
|
||||
@@ -0,0 +1,134 @@
|
||||
import pytest
|
||||
|
||||
from cereal import custom
|
||||
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.fw_versions import match_fw_to_car
|
||||
from opendbc.car.tesla.carcontroller import CarController
|
||||
from opendbc.car.tesla.carstate import CarState
|
||||
from opendbc.car.tesla.fingerprints import FW_VERSIONS
|
||||
from opendbc.car.tesla.interface import CarInterface
|
||||
from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven
|
||||
from opendbc.car.tesla.values import CANBUS, CAR, DBC, TeslaSafetyFlags
|
||||
|
||||
|
||||
def test_hw1_is_distinct_from_preap_and_does_not_change_can_buses():
|
||||
preap = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP)
|
||||
hw1 = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)
|
||||
|
||||
assert preap.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.teslaPreAP
|
||||
assert hw1.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.tesla
|
||||
assert hw1.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value
|
||||
assert hw1.radarTimeStepDEPRECATED == pytest.approx(0.125)
|
||||
assert preap.radarTimeStepDEPRECATED == pytest.approx(0.05)
|
||||
assert CANBUS.party == 0 and CANBUS.autopilot_party == 2
|
||||
assert CarState.get_can_parsers(hw1)[Bus.ap_pt].bus == 2
|
||||
assert CarState.get_can_parsers(hw1)[Bus.chassis].message_states[0x211].ignore_alive
|
||||
assert CarState.get_can_parsers(preap)[Bus.party].bus == 0
|
||||
|
||||
|
||||
def test_hw1_requires_explicit_alpha_long_for_acceleration():
|
||||
ret = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)
|
||||
hw1 = CarInterface._get_params(ret, CAR.TESLA_MODEL_S_HW1, {0: {0x201: 5}}, [], True, False, False)
|
||||
assert hw1.openpilotLongitudinalControl
|
||||
assert hw1.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value | TeslaSafetyFlags.LONG_CONTROL.value
|
||||
|
||||
|
||||
def test_ap1_eps_fw_matches_hw1_without_matching_preap():
|
||||
version = FW_VERSIONS[CAR.TESLA_MODEL_S_HW1][(structs.CarParams.Ecu.eps, 0x730, None)][0]
|
||||
fw = structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.eps, address=0x730, brand="tesla", fwVersion=version)
|
||||
exact, candidates = match_fw_to_car([fw], "", log=False)
|
||||
assert exact and candidates == {CAR.TESLA_MODEL_S_HW1}
|
||||
|
||||
|
||||
def test_hw1_display_and_cruise_bytes_do_not_change_preap_signals():
|
||||
# Captured bus-0 DI_state (0x368) from the AP1 route: display 9 MPH, set speed 10 MPH.
|
||||
parser = CANParser("tesla_can", [(0x368, 0)], 0)
|
||||
parser.message_states[0x368].ignore_counter = True
|
||||
frames = [(0x368, bytes.fromhex("84185e3009980a2d"), 0)]
|
||||
parser.update([(1_000_000_000, frames)])
|
||||
parser.update([(2_000_000_000, frames)])
|
||||
state = parser.vl["DI_state"]
|
||||
assert state["DI_hw1DigitalSpeed"] == 9
|
||||
assert state["DI_hw1CruiseSet"] == 10
|
||||
assert state["DI_digitalSpeed"] == 10
|
||||
assert state["DI_cruiseSet"] != 10
|
||||
|
||||
|
||||
def test_hw1_packer_emits_bus_zero_with_matching_checksums():
|
||||
packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.party])
|
||||
tesla_can = TeslaCANRaven({CANBUS.party: packer})
|
||||
for msg, expected_addr, checksum_index in (
|
||||
(tesla_can.create_steering_control(0, 0, False), 0x488, 3),
|
||||
(tesla_can.create_longitudinal_command(13, 0, 0, 10, False, False), 0x2b9, 7),
|
||||
):
|
||||
addr, data, bus = msg
|
||||
assert addr == expected_addr and bus == 0
|
||||
assert data[checksum_index] == TeslaCANRaven.checksum(addr, data[:checksum_index])
|
||||
|
||||
|
||||
def test_hw1_cancel_clears_acceleration_and_does_not_request_max_speed():
|
||||
cp = CarInterface._get_params(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1),
|
||||
CAR.TESLA_MODEL_S_HW1, {0: {}}, [], True, False, False)
|
||||
controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp)
|
||||
state = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
state.out.vEgo = 10.
|
||||
cc = structs.CarControl.new_message()
|
||||
cc.longActive = True
|
||||
cc.cruiseControl.cancel = True
|
||||
cc.actuators.accel = 2.
|
||||
_, sends = controller.update(cc.as_reader(), state, 0, None)
|
||||
_, data, bus = next(msg for msg in sends if msg[0] == 0x2b9)
|
||||
assert bus == 0
|
||||
parser = CANParser("tesla_can", [(0x2b9, 0)], 0)
|
||||
parser.update([(1_000_000_000, [(0x2b9, data, bus)])])
|
||||
decoded = parser.vl["DAS_control"]
|
||||
assert decoded["DAS_accState"] == 13
|
||||
assert decoded["DAS_accelMin"] == pytest.approx(0, abs=0.05)
|
||||
assert decoded["DAS_accelMax"] == pytest.approx(0, abs=0.05)
|
||||
assert decoded["DAS_setSpeed"] != 200
|
||||
|
||||
|
||||
def test_hw1_carstate_uses_ap1_powertrain_and_chassis():
|
||||
cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)
|
||||
parsers = CarState.get_can_parsers(cp)
|
||||
frames = [
|
||||
(0x155, bytes.fromhex("000000000005e308"), 0), # ESP speed 15.07 kph
|
||||
(0x368, bytes.fromhex("84185e3009980a2d"), 0),
|
||||
(0x201, bytes.fromhex("5444008df2"), 0),
|
||||
]
|
||||
for parser in parsers.values():
|
||||
for addr in (0x155, 0x368, 0x201):
|
||||
_ = parser.vl[addr]
|
||||
parser.message_states[addr].ignore_counter = True
|
||||
parser.message_states[addr].ignore_checksum = True
|
||||
parser.update([(1_000_000_000, frames)])
|
||||
parser.update([(2_000_000_000, frames)])
|
||||
state = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
ret, _ = state.update(parsers, None)
|
||||
assert not ret.cruiseState.enabled
|
||||
assert ret.cruiseState.speed == pytest.approx(10 * 0.44704)
|
||||
assert ret.vEgoRaw == pytest.approx(15.07 / 3.6)
|
||||
assert not ret.seatbeltUnlatched
|
||||
|
||||
# A stale belt frame cannot allow an engagement indefinitely.
|
||||
for parser in parsers.values():
|
||||
parser.update([(4_000_000_000, [])])
|
||||
ret, _ = state.update(parsers, None)
|
||||
assert ret.seatbeltUnlatched
|
||||
|
||||
|
||||
def test_hw1_can_use_rcm_buckle_when_sdm1_is_absent():
|
||||
cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)
|
||||
parsers = CarState.get_can_parsers(cp)
|
||||
packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.chassis])
|
||||
addr, data, bus = packer.make_can_msg("RCM_status", 0, {"RCM_buckleDriverStatus": 1})
|
||||
assert addr == 0x211 and bus == 0
|
||||
frames = [(addr, data, bus)]
|
||||
for parser in parsers.values():
|
||||
_ = parser.vl["RCM_status"]
|
||||
parser.update([(1_000_000_000, frames)])
|
||||
state = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
out, _ = state.update(parsers, None)
|
||||
assert not out.seatbeltUnlatched
|
||||
@@ -70,6 +70,16 @@ class CAR(Platforms):
|
||||
Bus.radar: 'tesla_radar_bosch_generated',
|
||||
},
|
||||
)
|
||||
TESLA_MODEL_S_HW1 = TeslaPlatformConfig(
|
||||
[CarDocs("Tesla Model S (with HW1) 2014-16", "All", support_type=SupportType.COMMUNITY, support_link="#community")],
|
||||
CarSpecs(mass=2100., wheelbase=2.960, steerRatio=15.0),
|
||||
{
|
||||
Bus.chassis: 'tesla_can',
|
||||
Bus.party: 'tesla_can',
|
||||
Bus.pt: 'tesla_can',
|
||||
Bus.radar: 'tesla_radar_bosch_generated',
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
@@ -125,10 +135,14 @@ class CarControllerParams:
|
||||
ACCEL_MAX = 2.0 # m/s^2
|
||||
ACCEL_MIN = -3.48 # m/s^2
|
||||
JERK_LIMIT_MAX = 4.9 # m/s^3, ACC faults at 5.0
|
||||
JERK_LIMIT_MIN = -4.9 # m/s^3, ACC faults at 5.0
|
||||
JERK_RAMP_RATE = JERK_LIMIT_MAX * 0.002
|
||||
|
||||
|
||||
class TeslaSafetyFlags(IntFlag):
|
||||
LONG_CONTROL = 1
|
||||
FLAG_EXTERNAL_PANDA = 4
|
||||
FLAG_HW1 = 8
|
||||
COOP_STEERING = 256
|
||||
|
||||
|
||||
@@ -157,5 +171,7 @@ class CruiseButtons:
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
|
||||
LEGACY_CARS = (CAR.TESLA_MODEL_S_HW1,)
|
||||
|
||||
STEER_THRESHOLD = 1
|
||||
STEER_DISENGAGE_THRESHOLD = 5.0
|
||||
|
||||
@@ -35,6 +35,8 @@ non_tested_cars = [
|
||||
GM.CHEVROLET_MALIBU_ASCM,
|
||||
GM.CHEVROLET_MALIBU_SDGM,
|
||||
GM.CHEVROLET_SUBURBAN,
|
||||
GM.CHEVROLET_SUBURBAN_ASCM,
|
||||
GM.CHEVROLET_SUBURBAN_CAMERA,
|
||||
GM.CHEVROLET_TRAX,
|
||||
GM.CHEVROLET_VOLT_ASCM,
|
||||
GM.CHEVROLET_VOLT_CAMERA,
|
||||
@@ -108,6 +110,7 @@ non_tested_cars = [
|
||||
TOYOTA.TOYOTA_RAV4H,
|
||||
|
||||
# No recorded routes yet
|
||||
VOLVO.VOLVO_V40,
|
||||
VOLVO.VOLVO_XC40_RECHARGE,
|
||||
VOLVO.VOLVO_S60_RECHARGE,
|
||||
VOLVO.POLESTAR_2,
|
||||
|
||||
@@ -2,7 +2,7 @@ from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.car_helpers import FRAME_FINGERPRINT, _apply_starpilot_access_policy, _get_gm_stored_candidate_fallback, can_fingerprint
|
||||
from opendbc.car.car_helpers import FRAME_FINGERPRINT, _apply_starpilot_access_policy, _get_gm_stored_candidate_fallback, _normalize_gm_suburban_camera_candidate, can_fingerprint
|
||||
from opendbc.car.fingerprints import _FINGERPRINTS as FINGERPRINTS
|
||||
from opendbc.car.gm.values import CAR as GM
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
@@ -116,3 +116,14 @@ class TestCanFingerprint:
|
||||
candidate = _apply_starpilot_access_policy("CHEVROLET_VOLT_CC", SimpleNamespace(block_user=True))
|
||||
|
||||
assert candidate == "CHEVROLET_VOLT_CC"
|
||||
|
||||
def test_gm_suburban_camera_variant_uses_vin_and_camera_bus_signature(self):
|
||||
fingerprints = {
|
||||
0: {190: 6, 201: 8, 209: 7, 211: 2, 241: 6, 304: 1, 320: 3},
|
||||
2: {0x24b: 8, 0x64b: 8},
|
||||
}
|
||||
|
||||
assert _normalize_gm_suburban_camera_candidate(None, fingerprints, "1GNSKJKJXKR148371") == "CHEVROLET_SUBURBAN_CAMERA"
|
||||
assert _normalize_gm_suburban_camera_candidate("GMC_YUKON", fingerprints, "1GNSKJKJXKR148371") == "CHEVROLET_SUBURBAN_CAMERA"
|
||||
assert _normalize_gm_suburban_camera_candidate(None, fingerprints, "1GNSKCKC5KR255194") is None
|
||||
assert _normalize_gm_suburban_camera_candidate(None, {0: fingerprints[0], 2: {0x320: 3}}, "1GNSKJKJXKR148371") is None
|
||||
|
||||
@@ -26,6 +26,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"TESLA_MODEL_3" = [nan, 2.5, nan]
|
||||
"TESLA_MODEL_Y" = [nan, 2.5, nan]
|
||||
"TESLA_MODEL_X" = [nan, 2.5, nan]
|
||||
"TESLA_MODEL_S_HW1" = [nan, 2.5, nan]
|
||||
|
||||
# Guess
|
||||
"VOLKSWAGEN_ID3_MK1" = [nan, 2.5, nan]
|
||||
@@ -145,6 +146,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"PORSCHE_MACAN_MK1" = [2.0, 2.0, 0.2]
|
||||
"VOLVO_XC40_RECHARGE" = [1.5, 1.5, 0.1]
|
||||
"VOLVO_S60_RECHARGE" = [1.5, 1.5, 0.1]
|
||||
"VOLVO_V40" = [1.5, 1.5, 0.1]
|
||||
|
||||
# Dashcam or fallback configured as ideal car
|
||||
"MOCK" = [10.0, 10, 0.0]
|
||||
|
||||
@@ -90,6 +90,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"CHEVROLET_MALIBU" = "CHEVROLET_VOLT"
|
||||
"CHEVROLET_MALIBU_ASCM" = "CHEVROLET_VOLT"
|
||||
"CHEVROLET_MALIBU_SDGM" = "CHEVROLET_VOLT"
|
||||
"CHEVROLET_SUBURBAN_ASCM" = "CHEVROLET_SUBURBAN"
|
||||
"CHEVROLET_SUBURBAN_CAMERA" = "CHEVROLET_SUBURBAN"
|
||||
"HOLDEN_ASTRA" = "CHEVROLET_VOLT"
|
||||
"CHEVROLET_VOLT_CC" = "CHEVROLET_VOLT"
|
||||
"CHEVROLET_VOLT_CAMERA" = "CHEVROLET_VOLT"
|
||||
|
||||
@@ -40,11 +40,13 @@ TOYOTA_NO_LEAD_CRUISE_SIGN_FLIP_MIN_SET_SPEED_ERROR = 0.35 # m/s
|
||||
TOYOTA_RAV4_LAUNCH_PEDAL_BLEND_SPEED = 5.0 # m/s
|
||||
TOYOTA_RAV4_LAUNCH_PEDAL_SCALE = 0.11
|
||||
TOYOTA_RAV4_LOW_SPEED_PEDAL_SCALE = 0.23
|
||||
TOYOTA_AUTO_HOLD_ACCEL = -1.0
|
||||
TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES = 100
|
||||
|
||||
# LKA limits
|
||||
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
|
||||
MAX_STEER_RATE = 100 # deg/s
|
||||
MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut
|
||||
MAX_STEER_RATE_FRAMES = 17 # tx control frames needed before torque can be cut
|
||||
|
||||
# EPS allows user torque above threshold for 50 frames before permanently faulting
|
||||
MAX_USER_TORQUE = 500
|
||||
@@ -75,6 +77,14 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
|
||||
) or highlander_sdsu)
|
||||
|
||||
|
||||
def get_toyota_lat_active(car_fingerprint, requested_active: bool, steering_torque: float,
|
||||
steering_pressed: bool) -> bool:
|
||||
if not requested_active or abs(steering_torque) >= MAX_USER_TORQUE:
|
||||
return False
|
||||
|
||||
return not (car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 and steering_pressed)
|
||||
|
||||
|
||||
def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool:
|
||||
return (
|
||||
auto_hold_enabled and
|
||||
@@ -309,30 +319,32 @@ class CarController(CarControllerBase):
|
||||
|
||||
self.last_standstill = CS.out.standstill
|
||||
|
||||
def create_auto_brake_hold_messages(self, CS: structs.CarState, brake_hold_allowed_timer: int = 100):
|
||||
can_sends = []
|
||||
brake_hold_allowed = (CS.out.standstill and CS.out.cruiseState.available and
|
||||
def update_auto_hold_state(self, CS: structs.CarState, cancel_requested: bool = False,
|
||||
activation_frames: int = TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES):
|
||||
brake_hold_allowed = (not cancel_requested and CS.out.standstill and CS.out.cruiseState.available and
|
||||
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 > brake_hold_allowed_timer
|
||||
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 self.frame % 2 == 0:
|
||||
can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active))
|
||||
return self.brake_hold_active
|
||||
|
||||
return can_sends
|
||||
def reset_auto_hold_state(self):
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
|
||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||
actuators = CC.actuators
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
|
||||
lat_active = get_toyota_lat_active(self.CP.carFingerprint, CC.latActive,
|
||||
CS.out.steeringTorque, CS.out.steeringPressed)
|
||||
|
||||
if len(CC.orientationNED) == 3:
|
||||
self.pitch.update(CC.orientationNED[1])
|
||||
@@ -423,10 +435,9 @@ 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)):
|
||||
can_sends.extend(self.create_auto_brake_hold_messages(CS))
|
||||
elif self.brake_hold_active:
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
self.update_auto_hold_state(CS, pcm_cancel_cmd)
|
||||
else:
|
||||
self.reset_auto_hold_state()
|
||||
|
||||
interceptor_gas_cmd = self._compute_interceptor_gas_cmd(CC, CS)
|
||||
|
||||
@@ -534,6 +545,11 @@ 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:
|
||||
pcm_accel_cmd = TOYOTA_AUTO_HOLD_ACCEL
|
||||
self.permit_braking = True
|
||||
self.standstill_req = True
|
||||
|
||||
main_accel_cmd = 0. if self.CP.flags & ToyotaFlags.SECOC.value else pcm_accel_cmd
|
||||
can_sends.append(toyotacan.create_accel_command(self.packer, main_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,
|
||||
CS.acc_type, fcw_alert, self.distance_button,
|
||||
|
||||
@@ -90,8 +90,6 @@ class CarState(CarStateBase):
|
||||
self.has_can_filter = self.FPCP.flags & ToyotaStarPilotFlags.RADAR_CAN_FILTER.value
|
||||
self.has_SDSU = self.FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value
|
||||
self.has_ZSS = self.FPCP.flags & ToyotaStarPilotFlags.ZSS.value
|
||||
self.auto_brake_hold = bool(self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value)
|
||||
self.pre_collision_2 = {}
|
||||
|
||||
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
|
||||
cp = can_parsers[Bus.pt]
|
||||
@@ -227,9 +225,6 @@ class CarState(CarStateBase):
|
||||
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
|
||||
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
|
||||
|
||||
if self.auto_brake_hold:
|
||||
self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
|
||||
|
||||
if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR:
|
||||
self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
|
||||
|
||||
@@ -314,9 +309,6 @@ class CarState(CarStateBase):
|
||||
if CP.carFingerprint in DISTANCE_BUTTON_CAR:
|
||||
pt_messages.append(("PCM_CRUISE_4", 1))
|
||||
|
||||
if CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value:
|
||||
cam_messages.append(("PRE_COLLISION_2", 50))
|
||||
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
|
||||
|
||||
@@ -164,8 +164,8 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.GAS_INTERCEPTOR.value
|
||||
|
||||
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
|
||||
if toyota_auto_hold and candidate in TOYOTA_AUTO_HOLD_CARS:
|
||||
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
if toyota_auto_hold and ret.openpilotLongitudinalControl and candidate in TOYOTA_AUTO_HOLD_CARS:
|
||||
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
|
||||
if not ret.openpilotLongitudinalControl:
|
||||
|
||||
@@ -11,6 +11,7 @@ from opendbc.car.toyota import toyotacan
|
||||
from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
|
||||
get_prius_positive_feedforward_scale, \
|
||||
get_rav4_interceptor_pedal_scale, \
|
||||
get_toyota_lat_active, \
|
||||
limit_interceptor_pcm_accel, \
|
||||
limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \
|
||||
limit_prius_stopping_accel, should_bypass_toyota_long_pid, supports_toyota_auto_hold, \
|
||||
@@ -196,7 +197,8 @@ class TestToyotaInterfaces:
|
||||
params.put_bool("ToyotaAutoHold", True)
|
||||
car_params = CarInterface.get_params(
|
||||
candidate,
|
||||
{bus: {} for bus in range(8)},
|
||||
{bus: ({0x2FF: 8} if candidate in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H) and bus == 0 else {})
|
||||
for bus in range(8)},
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
@@ -207,12 +209,13 @@ class TestToyotaInterfaces:
|
||||
params.remove("ToyotaAutoHold")
|
||||
|
||||
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
|
||||
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 "PRE_COLLISION_2" in can_parsers[Bus.cam].vl
|
||||
assert "PRE_COLLISION_2" not in can_parsers[Bus.cam].vl
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||
def test_auto_hold_is_disabled_by_default(self, candidate):
|
||||
@@ -229,7 +232,7 @@ class TestToyotaInterfaces:
|
||||
)
|
||||
|
||||
assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
|
||||
def test_prius_openpilot_long_uses_hybrid_long_defaults(self):
|
||||
car_params = CarInterface.get_params(
|
||||
@@ -732,6 +735,15 @@ class TestToyotaFingerprint:
|
||||
|
||||
|
||||
class TestToyotaCarController:
|
||||
def test_corolla_tss2_hands_off_immediately_when_driver_is_steering(self):
|
||||
assert not get_toyota_lat_active(CAR.TOYOTA_COROLLA_TSS2, True, 117, True)
|
||||
|
||||
def test_corolla_tss2_stays_active_without_driver_input(self):
|
||||
assert get_toyota_lat_active(CAR.TOYOTA_COROLLA_TSS2, True, 99, False)
|
||||
|
||||
def test_toyota_driver_handoff_behavior_is_corolla_only(self):
|
||||
assert get_toyota_lat_active(CAR.TOYOTA_RAV4_TSS2, True, 117, True)
|
||||
|
||||
@staticmethod
|
||||
def _make_controller(*, standstill_req=False, last_standstill=False):
|
||||
controller = CarController.__new__(CarController)
|
||||
@@ -744,6 +756,8 @@ class TestToyotaCarController:
|
||||
controller.standstill_req = standstill_req
|
||||
controller.last_standstill = last_standstill
|
||||
controller.accel = 0.0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
return controller
|
||||
|
||||
@staticmethod
|
||||
@@ -806,9 +820,6 @@ class TestToyotaCarController:
|
||||
def test_toyota_auto_hold_latches_after_brake_press_until_gas(self):
|
||||
controller = self._make_controller()
|
||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
@@ -818,28 +829,22 @@ class TestToyotaCarController:
|
||||
brakePressed=True,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
)
|
||||
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
assert controller.brake_hold_active
|
||||
|
||||
cs.out.brakePressed = False
|
||||
controller.frame = 2
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
assert controller.brake_hold_active
|
||||
|
||||
cs.out.gasPressed = True
|
||||
controller.frame = 4
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
assert not controller.brake_hold_active
|
||||
|
||||
def test_toyota_auto_hold_does_not_trigger_without_brake_press(self):
|
||||
controller = self._make_controller()
|
||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
standstill=True,
|
||||
@@ -848,10 +853,9 @@ class TestToyotaCarController:
|
||||
brakePressed=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
)
|
||||
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
assert not controller.brake_hold_active
|
||||
|
||||
def test_prius_resume_request_releases_standstill_latch(self):
|
||||
@@ -991,12 +995,9 @@ class TestToyotaCarController:
|
||||
assert parser.can_valid
|
||||
assert parser.vl["ACC_CONTROL"]["ALLOW_LONG_PRESS"] == 1
|
||||
|
||||
def test_auto_brake_hold_sends_modified_pre_collision_after_timer(self):
|
||||
def test_auto_hold_uses_acc_control_brake_path(self):
|
||||
controller = self._make_controller()
|
||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
standstill=True,
|
||||
@@ -1005,16 +1006,19 @@ class TestToyotaCarController:
|
||||
brakePressed=True,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
)
|
||||
|
||||
can_sends = controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
||||
can_sends = [toyotacan.create_accel_command(
|
||||
controller.packer, -1.0, False, True, True, False, 1, False, 0, False,
|
||||
)]
|
||||
|
||||
parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("PRE_COLLISION_2", 0)], 0)
|
||||
parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("ACC_CONTROL", 0)], 0)
|
||||
parser.update([(1, can_sends)])
|
||||
assert controller.brake_hold_active
|
||||
assert parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
|
||||
assert parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1
|
||||
assert parser.vl["ACC_CONTROL"]["ACCEL_CMD"] == -1.0
|
||||
assert parser.vl["ACC_CONTROL"]["PERMIT_BRAKING"] == 1
|
||||
assert parser.vl["ACC_CONTROL"]["RELEASE_STANDSTILL"] == 0
|
||||
|
||||
def test_interceptor_stop_and_go_holds_small_launch_at_standstill(self):
|
||||
controller = self._make_controller()
|
||||
|
||||
@@ -89,38 +89,6 @@ def create_pcs_commands(packer, accel, active, mass):
|
||||
return [msg1, msg2]
|
||||
|
||||
|
||||
def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active):
|
||||
values = {s: pre_collision_2[s] for s in [
|
||||
"DSS1GDRV",
|
||||
"DS1STAT2",
|
||||
"DS1STBK2",
|
||||
"PCSWAR",
|
||||
"PCSALM",
|
||||
"PCSOPR",
|
||||
"PCSABK",
|
||||
"PBATRGR",
|
||||
"PPTRGR",
|
||||
"IBTRGR",
|
||||
"CLEXTRGR",
|
||||
"IRLT_REQ",
|
||||
"BRKHLD",
|
||||
"AVSTRGR",
|
||||
"VGRSTRGR",
|
||||
"PREFILL",
|
||||
"PBRTRGR",
|
||||
"PCSDIS",
|
||||
"PBPREPMP",
|
||||
] if s in pre_collision_2}
|
||||
|
||||
if brake_hold_active:
|
||||
values = {
|
||||
"DSS1GDRV": 0x3FF,
|
||||
"PBRTRGR": frame % 730 < 727,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("PRE_COLLISION_2", 0, values)
|
||||
|
||||
|
||||
def create_acc_cancel_command(packer):
|
||||
values = {
|
||||
"GAS_RELEASED": 0,
|
||||
|
||||
@@ -185,6 +185,29 @@ def volkswagen_mqb_meb_checksum(address: int, sig, d: bytearray) -> int:
|
||||
return crc ^ 0xFF
|
||||
|
||||
|
||||
def volkswagen_mqb_meb_dyn_len_checksum(address: int, sig, d: bytearray, length: int, const: list[int]) -> int:
|
||||
d = d[:length]
|
||||
crc = 0xFF
|
||||
for i in range(1, len(d)):
|
||||
crc ^= d[i]
|
||||
crc = CRC8H2F[crc]
|
||||
counter = d[1] & 0x0F
|
||||
crc ^= const[counter]
|
||||
crc = CRC8H2F[crc]
|
||||
return crc ^ 0xFF
|
||||
|
||||
|
||||
def volkswagen_meb_alt_crc_checksum(address: int, sig, d: bytearray) -> int:
|
||||
entry = VOLKSWAGEN_MEB_ALT_CRC_CONSTANTS.get(address)
|
||||
if entry:
|
||||
length, const = entry
|
||||
checksum = volkswagen_mqb_meb_dyn_len_checksum(address, sig, d, length, const)
|
||||
if checksum == d[0]:
|
||||
return checksum
|
||||
|
||||
return volkswagen_mqb_meb_checksum(address, sig, d)
|
||||
|
||||
|
||||
def xor_checksum(address: int, sig, d: bytearray, initial_value: int = 0) -> int:
|
||||
checksum = initial_value
|
||||
checksum_byte = sig.start_bit // 8
|
||||
@@ -243,6 +266,8 @@ VOLKSWAGEN_MQB_MEB_CONSTANTS: dict[int, list[int]] = {
|
||||
0xC5, 0x91, 0x0F, 0x27, 0x34, 0x04, 0x7F, 0x02], # EA_02
|
||||
0x20A: [0x9D, 0xE8, 0x36, 0xA1, 0xCA, 0x3B, 0x1D, 0x33,
|
||||
0xE0, 0xD5, 0xBB, 0x5F, 0xAE, 0x3C, 0x31, 0x9F], # EML_06
|
||||
0x25D: [0xDA, 0x6B, 0x0E, 0xB2, 0x78, 0xBD, 0x5A, 0x81,
|
||||
0x7B, 0xD6, 0x41, 0x39, 0x76, 0xB6, 0xD7, 0x35], # KLR_01
|
||||
0x26B: [0xCE, 0xCC, 0xBD, 0x69, 0xA1, 0x3C, 0x18, 0x76,
|
||||
0x0F, 0x04, 0xF2, 0x3A, 0x93, 0x24, 0x19, 0x51], # TA_01
|
||||
0x30C: [0x0F] * 16, # ACC_02
|
||||
@@ -256,3 +281,19 @@ VOLKSWAGEN_MQB_MEB_CONSTANTS: dict[int, list[int]] = {
|
||||
0x65D: [0xAC, 0xB3, 0xAB, 0xEB, 0x7A, 0xE1, 0x3B, 0xF7,
|
||||
0x73, 0xBA, 0x7C, 0x9E, 0x06, 0x5F, 0x02, 0xD9], # ESP_20
|
||||
}
|
||||
|
||||
|
||||
VOLKSWAGEN_MEB_ALT_CRC_CONSTANTS: dict[int, tuple[int, list[int]]] = {
|
||||
0x0DB: (42, [0x09, 0xFA, 0xCA, 0x8E, 0x62, 0xD5, 0xD1, 0xF0,
|
||||
0x31, 0xA0, 0xAF, 0xDA, 0x4D, 0x1A, 0x0A, 0x97]), # AWV_03
|
||||
0xFC: (60, [0x69, 0xDC, 0xF9, 0x64, 0x6A, 0xCE, 0x55, 0x2C,
|
||||
0xC4, 0x38, 0x8F, 0xD1, 0xC6, 0x43, 0xB4, 0xB1]), # ESC_51
|
||||
0x102: (44, [0xD7, 0x12, 0x85, 0x7E, 0x0B, 0x34, 0xFA, 0x16,
|
||||
0x7A, 0x25, 0x2D, 0x8F, 0x04, 0x8E, 0x5D, 0x35]), # ESC_50
|
||||
0x10B: (44, [0x2C, 0xB1, 0x1A, 0x75, 0xBB, 0x65, 0x79, 0x47,
|
||||
0x81, 0x2B, 0xCC, 0x96, 0x17, 0xDB, 0xC0, 0x94]), # Motor_51
|
||||
0x139: (28, [0x96, 0x92, 0x95, 0xB5, 0x6E, 0xE3, 0xBD, 0xB4,
|
||||
0xFA, 0xAE, 0xBE, 0xCB, 0xCF, 0xA5, 0x77, 0xEF]), # VMM_02
|
||||
0x13D: (28, [0x18, 0x71, 0x10, 0x8D, 0xD7, 0xAA, 0xB0, 0x78,
|
||||
0xAC, 0x12, 0xAE, 0x0C, 0xDD, 0xF1, 0x85, 0x68]), # QFK_01
|
||||
}
|
||||
|
||||
@@ -1,10 +1,16 @@
|
||||
import random
|
||||
import re
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.can.packer import CANPacker
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.volkswagen.interface import CarInterface
|
||||
from opendbc.car.volkswagen.values import CAR, FW_QUERY_CONFIG, WMI, VolkswagenFlags, VolkswagenSafetyFlags
|
||||
from opendbc.car.volkswagen.fingerprints import FW_VERSIONS
|
||||
from opendbc.car.volkswagen.mqbcan import volkswagen_meb_alt_crc_checksum, volkswagen_mqb_meb_checksum
|
||||
from opendbc.car.volkswagen.radar_interface import RadarInterface
|
||||
from opendbc.car.volkswagen.values import CAR, DBC, FW_QUERY_CONFIG, WMI, CanBus, VolkswagenFlags, VolkswagenSafetyFlags
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
@@ -60,6 +66,47 @@ class TestVolkswagenPlatformConfigs:
|
||||
assert not cp.pcmCruise
|
||||
assert cp.safetyConfigs[-1].safetyParam & VolkswagenSafetyFlags.LONG_CONTROL
|
||||
|
||||
@pytest.mark.parametrize("data_hex", (
|
||||
"fc03fcfcfc0f0000",
|
||||
"e304fcfcfc0f0000",
|
||||
"1105fcfcfc0f0000",
|
||||
))
|
||||
def test_meb_klr_checksum(self, data_hex):
|
||||
data = bytearray.fromhex(data_hex)
|
||||
assert volkswagen_mqb_meb_checksum(0x25D, None, data) == data[0]
|
||||
|
||||
@pytest.mark.parametrize(("address", "data_hex"), (
|
||||
(0x0DB, "bb0ffcf0fefe0000fd0fffc0ff0000000200000000000000010000000000000000000000000000000000000000000000"),
|
||||
(0x0FC, "650b1f007ef0b10c0000000000000000ffff1019191c1cfefe0000000000000000e0fff40140ffeb7f0748e481af421f00000000000000000000000000000000"),
|
||||
(0x102, "9f0e7cfa010500000020cb0402000000b703a00000ec0f00000000002cd3ff1f0020a60000000020000000007d5256ab"),
|
||||
(0x10B, "9d06000000007efe000000010000ff01feff000000000000000000000090240000000000000000000000000000000000"),
|
||||
(0x139, "ac0e850b0890132000d019800000000000000000000000003002000500000000"),
|
||||
(0x13D, "2412111101d1060000d0d410d106000000000000000000000000000000000000"),
|
||||
))
|
||||
def test_meb_gen2_checksum(self, address, data_hex):
|
||||
data = bytearray.fromhex(data_hex)
|
||||
assert volkswagen_meb_alt_crc_checksum(address, None, data) == data[0]
|
||||
|
||||
def test_meb_camera_radar_tracks(self):
|
||||
cp = self._get_meb_params(CAR.SKODA_ENYAQ_MK1)
|
||||
radar = RadarInterface(cp)
|
||||
packer = CANPacker(DBC[cp.carFingerprint][Bus.radar])
|
||||
message = packer.make_can_msg("MEB_Distance_01", CanBus(cp).cam, {
|
||||
"Distance_Status": 0,
|
||||
"Same_Lane_01_ObjectID": 1,
|
||||
"Same_Lane_01_Long_Distance": 25.0,
|
||||
"Same_Lane_01_Lat_Distance": 0.5,
|
||||
"Same_Lane_01_Rel_Velo": -2.0,
|
||||
})
|
||||
|
||||
radar_data = radar.update([(1_000_000_000, [message])])
|
||||
assert radar_data is not None
|
||||
assert len(radar_data.points) == 1
|
||||
assert radar_data.points[0].trackId == 0
|
||||
assert radar_data.points[0].dRel == pytest.approx(25.0, abs=0.1)
|
||||
assert radar_data.points[0].yRel == pytest.approx(0.5, abs=0.1)
|
||||
assert radar_data.points[0].vRel == pytest.approx(-2.0, abs=0.1)
|
||||
|
||||
def test_taos_longitudinal_actuator_delay(self):
|
||||
taos_cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_TAOS_MK1)
|
||||
golf_cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_GOLF_MK7)
|
||||
|
||||
@@ -1,3 +1,5 @@
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.can.packer import CANPacker
|
||||
@@ -5,16 +7,20 @@ from opendbc.car import Bus
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.lateral import apply_std_steer_angle_limits
|
||||
from opendbc.car.volvo.helpers import LCA3CounterSync
|
||||
from opendbc.car.volvo.volvocan import (create_lca_message, create_pscm_message, create_lca_3_message, create_lca_2_message, create_lca_4_message,
|
||||
create_lca_5_message, create_lca_6_message, create_lca_7_message, create_pscm_related_message)
|
||||
from opendbc.car.volvo.values import CarControllerParams
|
||||
from opendbc.car.volvo.volvocan import (create_c1_cancel, create_c1_pscm_message, create_c1_steering_control, create_lca_message,
|
||||
create_pscm_message, create_lca_3_message, create_lca_2_message, create_lca_4_message, create_lca_5_message,
|
||||
create_lca_6_message, create_lca_7_message, create_pscm_related_message)
|
||||
from opendbc.car.volvo.values import CAR, CarControllerParams, VolvoC1PlatformConfig
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP):
|
||||
super().__init__(dbc_names, CP)
|
||||
self.packer = CANPacker(dbc_names[Bus.party])
|
||||
self.is_c1 = isinstance(CAR(CP.carFingerprint).config, VolvoC1PlatformConfig)
|
||||
self.packer = CANPacker(dbc_names[Bus.pt] if self.is_c1 else dbc_names[Bus.party])
|
||||
self.apply_angle_last = 0.0 # Track last applied steering angle
|
||||
self.c1_torque_samples = deque(maxlen=CarControllerParams.C1_N_ZERO_TORQUE)
|
||||
self.c1_recovery_until = -1
|
||||
|
||||
self.gear_acc = 60
|
||||
self.lca_4_acc = 0 # Bresenham accumulator for 29 Hz
|
||||
@@ -62,6 +68,9 @@ class CarController(CarControllerBase):
|
||||
self.lca_auth_drv_mag_filt = 0.0
|
||||
|
||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||
if self.is_c1:
|
||||
return self._update_c1(CC, CS)
|
||||
|
||||
can_sends = []
|
||||
actuators = CC.actuators
|
||||
|
||||
@@ -285,3 +294,50 @@ class CarController(CarControllerBase):
|
||||
self.frame += 1
|
||||
self.last_lat_active = CC.latActive
|
||||
return new_actuators, can_sends
|
||||
|
||||
def _update_c1(self, CC, CS):
|
||||
can_sends = []
|
||||
actuators = CC.actuators
|
||||
|
||||
if self.frame % 2 == 0: # stock FSM1 and PSCM1 messages are 50 Hz
|
||||
requested_active = CC.latActive and CS.out.vEgo > self.CP.minSteerSpeed
|
||||
recovering = requested_active and self.frame < self.c1_recovery_until
|
||||
|
||||
if not requested_active:
|
||||
self.c1_torque_samples.clear()
|
||||
self.c1_recovery_until = -1
|
||||
elif recovering:
|
||||
self.c1_torque_samples.clear()
|
||||
else:
|
||||
if self.c1_recovery_until >= 0:
|
||||
self.c1_recovery_until = -1
|
||||
self.c1_torque_samples.clear()
|
||||
self.c1_torque_samples.append(CS.c1_lka_torque)
|
||||
if (len(self.c1_torque_samples) == CarControllerParams.C1_N_ZERO_TORQUE and
|
||||
all(torque == 0 for torque in self.c1_torque_samples)):
|
||||
self.c1_recovery_until = self.frame + 100
|
||||
self.c1_torque_samples.clear()
|
||||
recovering = True
|
||||
|
||||
lat_active = requested_active and not recovering
|
||||
desired_angle = float(np.clip(
|
||||
actuators.steeringAngleDeg,
|
||||
CS.out.steeringAngleDeg - CarControllerParams.C1_ANGLE_ERROR,
|
||||
CS.out.steeringAngleDeg + CarControllerParams.C1_ANGLE_ERROR,
|
||||
))
|
||||
apply_angle = apply_std_steer_angle_limits(
|
||||
desired_angle, self.apply_angle_last, CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg, lat_active, CarControllerParams.C1_ANGLE_LIMITS,
|
||||
)
|
||||
|
||||
can_sends.append(create_c1_pscm_message(self.packer, CS.c1_msg_pscm))
|
||||
can_sends.append(create_c1_steering_control(self.packer, apply_angle, lat_active))
|
||||
self.apply_angle_last = apply_angle
|
||||
|
||||
if CC.cruiseControl.cancel and self.frame % 10 == 0:
|
||||
can_sends.append(create_c1_cancel(self.packer))
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.steeringAngleDeg = self.apply_angle_last
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
from cereal import custom
|
||||
from opendbc.car import structs, Bus
|
||||
from opendbc.car import Bus, ButtonType, create_button_events, structs
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car.volvo.values import DBC, VolvoSPAPlatformConfig, CAR
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.volvo.values import CAR, DBC, VolvoC1PlatformConfig, VolvoSPAPlatformConfig
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
@@ -16,6 +17,7 @@ STEERING_PRESSED_THRESHOLD = 2
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, FPCP):
|
||||
super().__init__(CP, FPCP)
|
||||
self.is_c1 = isinstance(CAR(CP.carFingerprint).config, VolvoC1PlatformConfig)
|
||||
self.is_spa = isinstance(CAR(CP.carFingerprint).config, VolvoSPAPlatformConfig)
|
||||
self.gas_pressed_prev = False
|
||||
self.dispatch_lca_2_msg = False
|
||||
@@ -34,8 +36,22 @@ class CarState(CarStateBase):
|
||||
self.msg_lca_4 = {}
|
||||
self.msg_lca_6 = {}
|
||||
self.msg_lca_7 = {}
|
||||
self.c1_msg_pscm = {}
|
||||
self.c1_lka_torque = 0
|
||||
self.c1_button_states = {
|
||||
"ACCOnOffBtn": False,
|
||||
"ACCStopBtn": False,
|
||||
"ACCSetBtn": False,
|
||||
"ACCResumeBtn": False,
|
||||
"ACCMinusBtn": False,
|
||||
"TimeGapIncreaseBtn": False,
|
||||
"TimeGapDecreaseBtn": False,
|
||||
}
|
||||
|
||||
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
|
||||
if self.is_c1:
|
||||
return self._update_c1(can_parsers)
|
||||
|
||||
cp_main = can_parsers[Bus.main]
|
||||
cp_pt = can_parsers[Bus.pt]
|
||||
cp_party = can_parsers[Bus.party]
|
||||
@@ -137,8 +153,83 @@ class CarState(CarStateBase):
|
||||
fp_ret = custom.StarPilotCarState.new_message()
|
||||
return ret, fp_ret
|
||||
|
||||
def _update_c1(self, can_parsers):
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
ret = structs.CarState()
|
||||
|
||||
ret.vEgoRaw = cp.vl["VehicleSpeed1"]["VehicleSpeed"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.vEgoRaw < 0.1
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["PSCM1"]["SteeringAngleServo"]
|
||||
ret.steeringTorque = cp.vl["PSCM1"]["LKATorque"]
|
||||
ret.steeringPressed = False
|
||||
|
||||
ret.gasPressed = cp.vl["PedalandBrake"]["AccPedal"] > 5.0
|
||||
ret.brakePressed = bool(cp.vl["PedalandBrake"]["BrakePedalActive2"] or
|
||||
cp.vl["PedalandBrake"]["BrakePedalActive"])
|
||||
|
||||
ret.gearShifter = {
|
||||
0: GearShifter.park,
|
||||
1: GearShifter.reverse,
|
||||
2: GearShifter.neutral,
|
||||
3: GearShifter.drive,
|
||||
}.get(int(cp.vl["TCM0"]["GearShifter"]), GearShifter.unknown)
|
||||
|
||||
ret.cruiseState.available = bool(cp_cam.vl["FSM0"]["ACCStatusOnOff"])
|
||||
ret.cruiseState.enabled = bool(cp_cam.vl["FSM0"]["ACCStatusActive"])
|
||||
ret.cruiseState.speed = cp.vl["ACC"]["SpeedTargetACC"] * CV.KPH_TO_MS
|
||||
ret.cruiseState.nonAdaptive = False
|
||||
ret.cruiseState.standstill = ret.standstill
|
||||
|
||||
turn_signal = int(cp.vl["MiscCarInfo"]["TurnSignal"])
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
|
||||
50, turn_signal == 1, turn_signal == 3)
|
||||
ret.doorOpen = False
|
||||
ret.seatbeltUnlatched = False
|
||||
|
||||
button_types = {
|
||||
"ACCOnOffBtn": ButtonType.mainCruise,
|
||||
"ACCStopBtn": ButtonType.cancel,
|
||||
"ACCSetBtn": ButtonType.setCruise,
|
||||
"ACCResumeBtn": ButtonType.resumeCruise,
|
||||
"ACCMinusBtn": ButtonType.decelCruise,
|
||||
"TimeGapIncreaseBtn": ButtonType.gapAdjustCruise,
|
||||
"TimeGapDecreaseBtn": ButtonType.gapAdjustCruise,
|
||||
}
|
||||
button_events = []
|
||||
for signal, button_type in button_types.items():
|
||||
pressed = bool(cp.vl["CCButtons"][signal])
|
||||
button_events.extend(create_button_events(pressed, self.c1_button_states[signal], {True: button_type}))
|
||||
self.c1_button_states[signal] = pressed
|
||||
ret.buttonEvents = button_events
|
||||
|
||||
self.c1_msg_pscm = cp.vl["PSCM1"]
|
||||
self.c1_lka_torque = int(cp.vl["PSCM1"]["LKATorque"])
|
||||
|
||||
fp_ret = custom.StarPilotCarState.new_message()
|
||||
return ret, fp_ret
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP):
|
||||
if isinstance(CAR(CP.carFingerprint).config, VolvoC1PlatformConfig):
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [
|
||||
("VehicleSpeed1", 50),
|
||||
("CCButtons", 100),
|
||||
("PSCM1", 50),
|
||||
("PedalandBrake", 100),
|
||||
("TCM0", 10),
|
||||
("ACC", 17),
|
||||
("MiscCarInfo", 25),
|
||||
], 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.cam], [
|
||||
("FSM0", 100),
|
||||
("FSM1", 50),
|
||||
], 2),
|
||||
}
|
||||
|
||||
return {
|
||||
Bus.main: CANParser(DBC[CP.carFingerprint][Bus.main], [], 0),
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1),
|
||||
|
||||
@@ -3,6 +3,14 @@
|
||||
from opendbc.car.volvo.values import CAR
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.VOLVO_V40: [
|
||||
# V40 2017
|
||||
{8: 8, 16: 8, 48: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 208: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 352: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 624: 8, 640: 8, 648: 8, 652: 8, 656: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 832: 8, 848: 8, 853: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1061: 8, 1072: 8, 1409: 8},
|
||||
# V40 2015
|
||||
{8: 8, 16: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 648: 8, 652: 8, 656: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 832: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1061: 8, 1072: 8, 1409: 8},
|
||||
# V40 2014
|
||||
{8: 8, 16: 8, 64: 8, 85: 8, 101: 8, 112: 8, 114: 8, 117: 8, 128: 8, 176: 8, 192: 8, 224: 8, 240: 8, 245: 8, 256: 8, 272: 8, 288: 8, 291: 8, 293: 8, 304: 8, 325: 8, 336: 8, 424: 8, 432: 8, 437: 8, 464: 8, 472: 8, 480: 8, 528: 8, 608: 8, 648: 8, 652: 8, 657: 8, 681: 8, 693: 8, 704: 8, 707: 8, 709: 8, 816: 8, 864: 8, 880: 8, 912: 8, 928: 8, 943: 8, 944: 8, 968: 8, 970: 8, 976: 8, 992: 8, 997: 8, 1024: 8, 1029: 8, 1072: 8, 1409: 8},
|
||||
],
|
||||
CAR.VOLVO_XC40_RECHARGE: [{
|
||||
21: 8, 22: 8, 23: 8, 26: 8, 69: 8, 85: 8, 87: 8, 88: 8, 96: 8, 103: 8, 104: 8, 105: 8, 128: 8, 144: 8, 146: 8, 147: 8, 151: 8, 304: 8, 336: 8, 339: 8, 341: 8, 394: 8, 395: 8, 640: 8, 656: 8, 666: 8, 773: 8, 778: 8, 1072: 8, 1120: 8, 1302: 8, 1336: 8, 1407: 8, 1422: 8, 1423: 8, 1424: 8, 1425: 8, 1426: 8
|
||||
}],
|
||||
|
||||
@@ -2,11 +2,10 @@ from opendbc.car import structs, get_safety_config
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.volvo.carcontroller import CarController
|
||||
from opendbc.car.volvo.carstate import CarState
|
||||
from opendbc.car.volvo.values import VolvoSPAPlatformConfig, CAR
|
||||
from opendbc.car.volvo.values import CAR, VolvoC1PlatformConfig, VolvoSafetyFlags, VolvoSPAPlatformConfig
|
||||
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
|
||||
VOLVO_FLAG_SPA = 1
|
||||
SAFETY_VOLVO = structs.CarParams.SafetyModel.volvo
|
||||
|
||||
|
||||
@@ -18,17 +17,20 @@ class CarInterface(CarInterfaceBase):
|
||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||
ret.brand = 'volvo'
|
||||
|
||||
platform = CAR(candidate).config
|
||||
safety_param = 0
|
||||
if isinstance(CAR(candidate).config, VolvoSPAPlatformConfig):
|
||||
safety_param = VOLVO_FLAG_SPA
|
||||
if isinstance(platform, VolvoSPAPlatformConfig):
|
||||
safety_param = VolvoSafetyFlags.SPA.value
|
||||
elif isinstance(platform, VolvoC1PlatformConfig):
|
||||
safety_param = VolvoSafetyFlags.C1.value
|
||||
ret.safetyConfigs = [get_safety_config(SAFETY_VOLVO, safety_param)]
|
||||
#ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput)]
|
||||
|
||||
ret.dashcamOnly = False
|
||||
|
||||
ret.steerActuatorDelay = 0.3
|
||||
ret.steerActuatorDelay = 0.2 if isinstance(platform, VolvoC1PlatformConfig) else 0.3
|
||||
ret.steerLimitTimer = 0.1
|
||||
ret.steerAtStandstill = True
|
||||
ret.steerAtStandstill = not isinstance(platform, VolvoC1PlatformConfig)
|
||||
|
||||
# Use angle-based steering control for Volvo CMA platform
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||
@@ -39,4 +41,7 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
ret.pcmCruise = True
|
||||
|
||||
if isinstance(platform, VolvoC1PlatformConfig):
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
|
||||
return ret
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
import pytest
|
||||
|
||||
from cereal import custom
|
||||
from opendbc.can.packer import CANPacker
|
||||
from opendbc.car import Bus, ButtonType, CanData, structs
|
||||
from opendbc.car.volvo.carstate import CarState
|
||||
from opendbc.car.volvo.interface import CarInterface
|
||||
from opendbc.car.volvo.values import CAR, DBC
|
||||
|
||||
|
||||
def _can_data(msg):
|
||||
address, data, bus = msg
|
||||
return CanData(address, data, bus)
|
||||
|
||||
|
||||
def test_c1_carstate_decodes_vehicle_and_cruise_signals():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLVO_V40)
|
||||
cs = CarState(cp, custom.StarPilotCarParams.new_message())
|
||||
parsers = CarState.get_can_parsers(cp)
|
||||
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
|
||||
|
||||
messages = [
|
||||
packer.make_can_msg("VehicleSpeed1", 0, {"VehicleSpeed": 72}),
|
||||
packer.make_can_msg("CCButtons", 0, {"ACCStopBtn": 1, "ACCSetBtn": 1}),
|
||||
packer.make_can_msg("PSCM1", 0, {"SteeringAngleServo": -12.5, "LKATorque": 7}),
|
||||
packer.make_can_msg("PedalandBrake", 0, {"AccPedal": 6, "BrakePedalActive2": 1}),
|
||||
packer.make_can_msg("TCM0", 0, {"GearShifter": 3}),
|
||||
packer.make_can_msg("ACC", 0, {"SpeedTargetACC": 100}),
|
||||
packer.make_can_msg("MiscCarInfo", 0, {"TurnSignal": 1}),
|
||||
packer.make_can_msg("FSM0", 2, {"ACCStatusOnOff": 1, "ACCStatusActive": 1}),
|
||||
packer.make_can_msg("FSM1", 2, {}),
|
||||
]
|
||||
packets = [(1_000_000, [_can_data(msg) for msg in messages])]
|
||||
for parser in parsers.values():
|
||||
parser.update(packets)
|
||||
|
||||
ret, _ = cs.update(parsers, None)
|
||||
assert ret.vEgoRaw == pytest.approx(20.0)
|
||||
assert ret.steeringAngleDeg == pytest.approx(-12.5, abs=0.05)
|
||||
assert ret.steeringTorque == 7
|
||||
assert ret.gasPressed and ret.brakePressed
|
||||
assert ret.gearShifter == structs.CarState.GearShifter.drive
|
||||
assert ret.cruiseState.available and ret.cruiseState.enabled
|
||||
assert ret.cruiseState.speed == pytest.approx(100 / 3.6)
|
||||
assert ret.leftBlinker and not ret.rightBlinker
|
||||
assert len(ret.buttonEvents) == 2
|
||||
assert any(event.type == ButtonType.cancel and event.pressed for event in ret.buttonEvents)
|
||||
assert any(event.type == ButtonType.setCruise and event.pressed for event in ret.buttonEvents)
|
||||
|
||||
release = packer.make_can_msg("CCButtons", 0, {})
|
||||
packets = [(2_000_000, [_can_data(release)])]
|
||||
for parser in parsers.values():
|
||||
parser.update(packets)
|
||||
ret, _ = cs.update(parsers, None)
|
||||
assert len(ret.buttonEvents) == 2
|
||||
assert any(event.type == ButtonType.cancel and not event.pressed for event in ret.buttonEvents)
|
||||
assert any(event.type == ButtonType.setCruise and not event.pressed for event in ret.buttonEvents)
|
||||
@@ -4,7 +4,8 @@ from types import SimpleNamespace
|
||||
from opendbc.car.volvo.carcontroller import CarController
|
||||
from opendbc.car.volvo.helpers import checksum_lca_5_message
|
||||
from opendbc.car.volvo.interface import CarInterface
|
||||
from opendbc.car.volvo.values import DBC
|
||||
from opendbc.car.volvo.values import CAR, DBC
|
||||
from opendbc.car.volvo.volvocan import create_c1_checksum
|
||||
|
||||
|
||||
def _zero_message():
|
||||
@@ -67,3 +68,70 @@ def test_controller_relays_stock_lca5_angle_when_inactive():
|
||||
if raw & (1 << 14):
|
||||
raw -= 1 << 15
|
||||
assert abs(raw * 0.05596 - 12.0) < 0.1
|
||||
|
||||
|
||||
def _c1_state():
|
||||
return SimpleNamespace(
|
||||
out=SimpleNamespace(steeringAngleDeg=10.0, vEgo=12.0, vEgoRaw=12.0),
|
||||
c1_lka_torque=5,
|
||||
c1_msg_pscm=_zero_message(),
|
||||
)
|
||||
|
||||
|
||||
def test_c1_controller_emits_checked_steering_and_pscm_relay():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLVO_V40)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
cc = SimpleNamespace(
|
||||
latActive=True,
|
||||
actuators=_Actuators(),
|
||||
cruiseControl=SimpleNamespace(cancel=False),
|
||||
)
|
||||
|
||||
actuators, can_sends = controller.update(cc, _c1_state(), 0, None)
|
||||
assert [(msg[0], msg[2]) for msg in can_sends] == [(0x125, 2), (0xD0, 0)]
|
||||
|
||||
fsm = can_sends[1][1]
|
||||
assert fsm[7] & 0x3 == 3
|
||||
assert fsm[6] == create_c1_checksum(fsm)
|
||||
assert 0.0 < actuators.steeringAngleDeg <= 2.0
|
||||
|
||||
|
||||
def test_c1_controller_sends_only_cancel_button():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLVO_V40)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
cc = SimpleNamespace(
|
||||
latActive=False,
|
||||
actuators=_Actuators(),
|
||||
cruiseControl=SimpleNamespace(cancel=True),
|
||||
)
|
||||
|
||||
_, can_sends = controller.update(cc, _c1_state(), 0, None)
|
||||
buttons = next(msg for msg in can_sends if msg[0] == 0x10)
|
||||
assert buttons[2] == 0
|
||||
assert buttons[1][7] == 0x10
|
||||
assert buttons[1][6] == 0
|
||||
|
||||
|
||||
def test_c1_controller_temporarily_drops_steering_on_zero_torque_fault():
|
||||
cp = CarInterface.get_non_essential_params(CAR.VOLVO_V40)
|
||||
controller = CarController(DBC[cp.carFingerprint], cp)
|
||||
cs = _c1_state()
|
||||
cs.c1_lka_torque = 0
|
||||
cc = SimpleNamespace(
|
||||
latActive=True,
|
||||
actuators=_Actuators(),
|
||||
cruiseControl=SimpleNamespace(cancel=False),
|
||||
)
|
||||
|
||||
directions = []
|
||||
for _ in range(23):
|
||||
_, can_sends = controller.update(cc, cs, 0, None)
|
||||
directions.extend(msg[1][7] & 0x3 for msg in can_sends if msg[0] == 0xD0)
|
||||
|
||||
assert directions[:-1] == [3] * 11
|
||||
assert directions[-1] == 0
|
||||
|
||||
while controller.frame <= 122:
|
||||
_, can_sends = controller.update(cc, cs, 0, None)
|
||||
fsm = next(msg for msg in can_sends if msg[0] == 0xD0)
|
||||
assert fsm[1][7] & 0x3 == 3
|
||||
|
||||
@@ -1,13 +1,23 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntFlag
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
|
||||
from opendbc.car.lateral import AngleSteeringLimits
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.docs_definitions import CarDocs, CarHarness, CarParts
|
||||
from opendbc.car.fw_query_definitions import FwQueryConfig
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
# C1 support is adapted from the original dragonpilot V40 port:
|
||||
# https://github.com/dragonpilot/dragonpilot/commit/773dce507082d931236b64dca8024dce9625446f
|
||||
|
||||
|
||||
class VolvoSafetyFlags(IntFlag):
|
||||
SPA = 1
|
||||
C1 = 2
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_STEP = 1 # 100 Hz LCA command frequency (controlsd runs at 100 Hz)
|
||||
@@ -81,6 +91,19 @@ class CarControllerParams:
|
||||
([0., 5., 25.], [5., 2., .3]), # rate down limits at different speeds
|
||||
)
|
||||
|
||||
C1_STEER_NO = 0
|
||||
C1_STEER = 3
|
||||
C1_N_ZERO_TORQUE = 12
|
||||
C1_ANGLE_ERROR = 20.0
|
||||
C1_ANGLE_DELTA_BP = [0., 8.33, 13.89, 19.44, 25., 30.55, 36.1]
|
||||
C1_ANGLE_DELTA_UP = [2., 1.2, .25, .20, .15, .10, .10]
|
||||
C1_ANGLE_DELTA_DOWN = [2., 1.2, .25, .20, .15, .10, .10]
|
||||
C1_ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
|
||||
359.9,
|
||||
(C1_ANGLE_DELTA_BP, C1_ANGLE_DELTA_UP),
|
||||
(C1_ANGLE_DELTA_BP, C1_ANGLE_DELTA_DOWN),
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class VolvoCarDocs(CarDocs):
|
||||
@@ -105,7 +128,26 @@ class VolvoSPAPlatformConfig(PlatformConfig):
|
||||
})
|
||||
|
||||
|
||||
@dataclass
|
||||
class VolvoC1PlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {
|
||||
Bus.pt: 'volvo_v40_2017_pt',
|
||||
Bus.cam: 'volvo_v40_2017_pt',
|
||||
})
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
VOLVO_V40 = VolvoC1PlatformConfig(
|
||||
[VolvoCarDocs("Volvo V40 2013-19")],
|
||||
CarSpecs(
|
||||
mass=1610,
|
||||
wheelbase=2.647,
|
||||
steerRatio=14.7,
|
||||
centerToFrontRatio=0.44,
|
||||
minSteerSpeed=1.0 * CV.KPH_TO_MS,
|
||||
),
|
||||
)
|
||||
|
||||
VOLVO_XC40_RECHARGE = VolvoCMAPlatformConfig(
|
||||
[VolvoCarDocs("Volvo XC40 Recharge 2021-23")],
|
||||
CarSpecs(
|
||||
|
||||
@@ -2,6 +2,47 @@ from opendbc.car.volvo.helpers import (checksum_lca_2_message, checksum_2_0x69_m
|
||||
checksum_2_pscm_related_message, checksum_lca_5_message)
|
||||
from opendbc.car.carlog import carlog
|
||||
|
||||
|
||||
def create_c1_pscm_message(packer, msg_pscm: dict):
|
||||
values = {
|
||||
"LKATorque": 0,
|
||||
"SteeringAngleServo": msg_pscm["SteeringAngleServo"],
|
||||
"byte0": msg_pscm["byte0"],
|
||||
"byte3": msg_pscm["byte3"],
|
||||
"byte4": msg_pscm["byte4"],
|
||||
"byte7": msg_pscm["byte7"],
|
||||
"LKAActive": int(msg_pscm["LKAActive"]) & 0xD,
|
||||
}
|
||||
return packer.make_can_msg("PSCM1", 2, values)
|
||||
|
||||
|
||||
def create_c1_checksum(data: bytes) -> int:
|
||||
angle_raw = ((data[4] & 0x3F) << 8) | data[5]
|
||||
direction = data[7] & 0x3
|
||||
checksum_sum = (data[3] + direction + angle_raw + (angle_raw >> 8)) & 0xFF
|
||||
return checksum_sum ^ 0xFF
|
||||
|
||||
|
||||
def create_c1_steering_control(packer, apply_angle: float, lat_active: bool):
|
||||
values = {
|
||||
"SET_X_E3": 0xE3,
|
||||
"SET_X_B4": 0xB4,
|
||||
"SET_X_08": 0x08,
|
||||
"TrqLim": 0,
|
||||
"LKAAngleReq": apply_angle,
|
||||
"LKASteerDirection": 3 if lat_active else 0,
|
||||
"SET_X_25": 0x25,
|
||||
"SET_X_02": 0x02,
|
||||
}
|
||||
data = packer.make_can_msg("FSM1", 0, values)[1]
|
||||
values["Checksum"] = create_c1_checksum(data)
|
||||
return packer.make_can_msg("FSM1", 0, values)
|
||||
|
||||
|
||||
def create_c1_cancel(packer):
|
||||
return packer.make_can_msg("CCButtons", 0, {"ACCStopBtn": 1})
|
||||
|
||||
|
||||
def create_lca_message(packer, lat_active: bool, apply_angle: float, msg_lca: dict,
|
||||
authority_pos: int = 614, authority_neg: int = -614,
|
||||
overrides: dict | None = None):
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1480,6 +1480,7 @@ BO_ 905 SCC14: 8 SCC
|
||||
SG_ JerkLowerLimit : 19|7@1+ (0.1,0) [0|12.7] "m/s^3" ESC
|
||||
SG_ ACCMode : 32|3@1+ (1,0) [0|7] "" CLU,HUD,LDWS_LKAS,ESC
|
||||
SG_ ObjGap : 56|8@1+ (1,0) [0|255] "" CLU,HUD,ESC
|
||||
SG_ ObjDistStat : 42|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 1157 LFAHDA_MFC: 8 XXX
|
||||
SG_ HDA_USM : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
@@ -1497,7 +1498,7 @@ BO_ 913 BCM_PO_11: 8 Vector__XXX
|
||||
SG_ BCM_Door_Dri_Status : 5|1@0+ (1,0) [0|1] "" PT_ESC_ABS
|
||||
SG_ BCM_Shift_R_MT_SW_Status : 39|2@0+ (1,0) [0|3] "" PT_ESC_ABS
|
||||
SG_ LDA_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RAY_LKAS_BTN : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ RAY_LKAS_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1426 LABEL11: 8 XXX
|
||||
SG_ CC_React : 34|1@1+ (1,0) [0|1] "" XXX
|
||||
@@ -1671,6 +1672,7 @@ VAL_ 871 CF_Lvr_IsgState 0 "enabled" 1 "activated" 2 "unknown" 3 "disabled";
|
||||
VAL_ 871 CF_Lvr_Gear 12 "T" 5 "D" 8 "S" 6 "N" 7 "R" 0 "P";
|
||||
VAL_ 882 Elect_Gear_Shifter 4 "S" 5 "D" 8 "S" 6 "N" 7 "R" 0 "P";
|
||||
VAL_ 905 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault" 4 "cancelled";
|
||||
VAL_ 905 ObjDistStat 0 "no_object" 1 "receding" 2 "approaching";
|
||||
VAL_ 909 CF_VSM_Warn 2 "FCW" 3 "AEB";
|
||||
VAL_ 916 ACCEnable 0 "SCC ready" 1 "SCC temp fault" 2 "SCC permanent fault" 3 "SCC permanent fault, communication issue";
|
||||
VAL_ 1056 SCCInfoDisplay 0 "No Message" 2 "Cruise Control" 3 "Lost Lead" 4 "Standstill";
|
||||
|
||||
@@ -1480,6 +1480,7 @@ BO_ 905 SCC14: 8 SCC
|
||||
SG_ JerkLowerLimit : 19|7@1+ (0.1,0) [0|12.7] "m/s^3" ESC
|
||||
SG_ ACCMode : 32|3@1+ (1,0) [0|7] "" CLU,HUD,LDWS_LKAS,ESC
|
||||
SG_ ObjGap : 56|8@1+ (1,0) [0|255] "" CLU,HUD,ESC
|
||||
SG_ ObjDistStat : 42|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 1157 LFAHDA_MFC: 4 XXX
|
||||
SG_ HDA_USM : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
@@ -1671,6 +1672,7 @@ VAL_ 871 CF_Lvr_IsgState 0 "enabled" 1 "activated" 2 "unknown" 3 "disabled";
|
||||
VAL_ 871 CF_Lvr_Gear 12 "T" 5 "D" 8 "S" 6 "N" 7 "R" 0 "P";
|
||||
VAL_ 882 Elect_Gear_Shifter 4 "S" 5 "D" 8 "S" 6 "N" 7 "R" 0 "P";
|
||||
VAL_ 905 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault" 4 "cancelled";
|
||||
VAL_ 905 ObjDistStat 0 "no_object" 1 "receding" 2 "approaching";
|
||||
VAL_ 909 CF_VSM_Warn 2 "FCW" 3 "AEB";
|
||||
VAL_ 916 ACCEnable 0 "SCC ready" 1 "SCC temp fault" 2 "SCC permanent fault" 3 "SCC permanent fault, communication issue";
|
||||
VAL_ 1056 SCCInfoDisplay 0 "No Message" 2 "Cruise Control" 3 "Lost Lead" 4 "Standstill";
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
VERSION ""
|
||||
|
||||
NS_ :
|
||||
BS_:
|
||||
BU_: INTERCEPTOR NEO
|
||||
|
||||
BO_ 512 GAS_COMMAND: 6 NEO
|
||||
SG_ GAS_COMMAND : 7|16@0+ (0.672,-177.408) [0|255] "" INTERCEPTOR
|
||||
SG_ GAS_COMMAND2 : 23|16@0+ (0.332,-165.004) [0|255] "" INTERCEPTOR
|
||||
SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR
|
||||
SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR
|
||||
SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" INTERCEPTOR
|
||||
|
||||
BO_ 513 GAS_SENSOR: 6 INTERCEPTOR
|
||||
SG_ INTERCEPTOR_GAS : 7|16@0+ (0.672,-177.408) [0|255] "" NEO
|
||||
SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.332,-165.004) [0|255] "" NEO
|
||||
SG_ STATE : 39|4@0+ (1,0) [0|15] "" NEO
|
||||
SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" NEO
|
||||
SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" NEO
|
||||
|
||||
VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ;
|
||||
|
||||
CM_ "Kia Ray EV comma pedal uses the standard 0x200/0x201 rolling counter and CRC8 protocol. Channel scaling is Ray-only, estimated from the September 15 route: sensor rest raw 264/497, slopes about 3.79/7.68 raw counts per native E_EMS11 pedal unit, and 100 native units mapped to 255 comma pedal command units. Confirm against installed Ray firmware before increasing the command cap.";
|
||||
File diff suppressed because it is too large
Load Diff
@@ -217,6 +217,9 @@ BO_ 513 SDM1: 5 GTW
|
||||
SG_ SDM_bcklPassStatus : 3|2@0+ (1,0) [0|3] "" NEO
|
||||
SG_ SDM_bcklDrivStatus : 5|2@0+ (1,0) [0|3] "" NEO
|
||||
|
||||
BO_ 529 RCM_status: 8 RCM
|
||||
SG_ RCM_buckleDriverStatus : 15|2@0+ (1,0) [0|3] "" GTW,OCS,DAS
|
||||
|
||||
BO_ 532 EPB_epasControl: 3 EPB
|
||||
SG_ EPB_epasControlChecksum : 23|8@0+ (1,0) [0|255] "" NEO,EPAS
|
||||
SG_ EPB_epasControlCounter : 11|4@0+ (1,0) [0|15] "" NEO,EPAS
|
||||
@@ -256,9 +259,11 @@ BO_ 872 DI_state: 8 DI
|
||||
SG_ DI_immobilizerState : 28|3@1+ (1,0) [0|0] "" NEO
|
||||
SG_ DI_speedUnits : 31|1@1+ (1,0) [0|1] "" NEO
|
||||
SG_ DI_cruiseSet : 32|9@1+ (0.5,0) [0|255.5] "speed" NEO
|
||||
SG_ DI_hw1DigitalSpeed : 32|8@1+ (1,0) [0|250] "speed" NEO
|
||||
SG_ DI_aebState : 41|3@1+ (1,0) [0|0] "" NEO
|
||||
SG_ DI_stateCounter : 44|4@1+ (1,0) [0|0] "" NEO
|
||||
SG_ DI_digitalSpeed : 48|8@1+ (1,0) [0|250] "" NEO
|
||||
SG_ DI_hw1CruiseSet : 48|8@1+ (1,0) [0|250] "speed" NEO
|
||||
SG_ DI_stateChecksum : 56|8@1+ (1,0) [0|0] "" NEO
|
||||
|
||||
BO_ 109 SBW_RQ_SCCM: 4 STW
|
||||
@@ -906,4 +911,3 @@ VAL_ 1001 DAS_turnIndicatorRequestReason 6 "DAS_ACTIVE_COMMANDED_LANE_CHANGE" 5
|
||||
VAL_ 1160 DAS_steeringAngleRequest 16384 "ZERO_ANGLE" ;
|
||||
VAL_ 1160 DAS_steeringControlType 1 "ANGLE_CONTROL" 3 "DISABLED" 0 "NONE" 2 "RESERVED" ;
|
||||
VAL_ 1160 DAS_steeringHapticRequest 1 "ACTIVE" 0 "IDLE" ;
|
||||
|
||||
|
||||
@@ -11,3 +11,4 @@ class ALTERNATIVE_EXPERIENCE:
|
||||
|
||||
ALWAYS_ON_LATERAL = 32
|
||||
GM_REMAP_CANCEL_TO_DISTANCE = 64
|
||||
TOYOTA_AUTO_HOLD = 128
|
||||
|
||||
@@ -339,6 +339,7 @@ extern bool gm_remote_start_boots_comma;
|
||||
|
||||
#define ALT_EXP_ALWAYS_ON_LATERAL 32
|
||||
#define ALT_EXP_GM_REMAP_CANCEL_TO_DISTANCE 64
|
||||
#define ALT_EXP_TOYOTA_AUTO_HOLD 128
|
||||
|
||||
extern int alternative_experience;
|
||||
|
||||
@@ -383,3 +384,4 @@ extern const safety_hooks rivian_hooks;
|
||||
extern const safety_hooks psa_hooks;
|
||||
extern const safety_hooks volvo_hooks;
|
||||
extern const safety_hooks tesla_preap_hooks;
|
||||
extern const safety_hooks tesla_legacy_hooks;
|
||||
|
||||
@@ -60,6 +60,7 @@ static bool gm_panda_3d1_sched = false;
|
||||
static bool gm_panda_paddle_sched = false;
|
||||
static bool gm_bolt_2022_pedal = false;
|
||||
static bool gm_alt_brake = false;
|
||||
static bool gm_volt_cc_gateway = false;
|
||||
static bool gm_volt_auto_hold = false;
|
||||
static bool gm_volt_one_pedal = false;
|
||||
|
||||
@@ -261,7 +262,8 @@ static void gm_rx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
if ((msg->addr == 0xF1U) && gm_alt_brake) {
|
||||
brake_pressed = msg->data[1] >= 6U;
|
||||
const uint8_t brake_threshold = gm_volt_cc_gateway ? 21U : 6U;
|
||||
brake_pressed = msg->data[1] >= brake_threshold;
|
||||
}
|
||||
|
||||
if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM) && !gm_force_brake_c9) {
|
||||
@@ -720,7 +722,7 @@ static safety_config gm_init(uint16_t param) {
|
||||
gm_cc_long = GET_FLAG(param, GM_PARAM_CC_LONG);
|
||||
gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC);
|
||||
gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG);
|
||||
const bool gm_volt_cc_gateway = GET_FLAG(param, GM_PARAM_VOLT_CC_GATEWAY) && gm_no_camera && !gm_pedal_long && !gm_has_acc;
|
||||
gm_volt_cc_gateway = GET_FLAG(param, GM_PARAM_VOLT_CC_GATEWAY) && gm_no_camera && !gm_pedal_long && !gm_has_acc;
|
||||
enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR);
|
||||
gm_force_ascm = GET_FLAG(param, GM_PARAM_HW_ASCM_LONG);
|
||||
gm_force_brake_c9 = GET_FLAG(param, GM_PARAM_FORCE_BRAKE_C9);
|
||||
|
||||
@@ -67,6 +67,9 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
|
||||
#define HYUNDAI_NON_SCC_EV_ADDR_CHECK \
|
||||
{.msg = {{0x592U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
#define HYUNDAI_RAY_PEDAL_ADDR_CHECK \
|
||||
{.msg = {{0x201U, 0, 6, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
|
||||
static const CanMsg HYUNDAI_TX_MSGS[] = {
|
||||
HYUNDAI_COMMON_TX_MSGS(0, false)
|
||||
};
|
||||
@@ -75,6 +78,11 @@ static const CanMsg HYUNDAI_REFRESH_TX_MSGS[] = {
|
||||
HYUNDAI_COMMON_TX_MSGS(0, true)
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_RAY_PEDAL_TX_MSGS[] = {
|
||||
HYUNDAI_COMMON_TX_MSGS(0, true)
|
||||
{0x200, 0, 6, .check_relay = false}, // comma pedal only, not Hyundai EMS20
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
|
||||
HYUNDAI_LONG_COMMON_TX_MSGS(0, false)
|
||||
{0x38D, 0, 8, .check_relay = false}, // FCA11 Bus 0
|
||||
@@ -90,6 +98,7 @@ static const CanMsg HYUNDAI_LONG_REFRESH_TX_MSGS[] = {
|
||||
};
|
||||
|
||||
static bool hyundai_legacy = false;
|
||||
static bool hyundai_ray_pedal = false;
|
||||
static bool hyundai_can_canfd_blended_hda2 = false;
|
||||
static bool hyundai_acc_main_on_rx_prev = false;
|
||||
|
||||
@@ -120,6 +129,8 @@ static uint8_t hyundai_get_counter(const CANPacket_t *msg) {
|
||||
cnt = byte_421 & 0xFU;
|
||||
} else if (msg->addr == 0x4F1U) {
|
||||
cnt = (msg->data[3] >> 4) & 0xFU;
|
||||
} else if (hyundai_ray_pedal && (msg->addr == 0x201U)) {
|
||||
cnt = msg->data[4] & 0xFU;
|
||||
} else {
|
||||
}
|
||||
return cnt;
|
||||
@@ -136,11 +147,24 @@ static uint32_t hyundai_get_checksum(const CANPacket_t *msg) {
|
||||
chksum = msg->data[6] & 0xFU;
|
||||
} else if (msg->addr == 0x421U) {
|
||||
chksum = hyundai_can_canfd_blended ? msg->data[0] : msg->data[7] >> 4;
|
||||
} else if (hyundai_ray_pedal && (msg->addr == 0x201U)) {
|
||||
chksum = msg->data[5];
|
||||
} else {
|
||||
}
|
||||
return chksum;
|
||||
}
|
||||
|
||||
static uint8_t hyundai_ray_pedal_checksum(const CANPacket_t *msg) {
|
||||
uint8_t crc = 0xFFU;
|
||||
for (int i = 4; i >= 0; i--) {
|
||||
crc ^= msg->data[i];
|
||||
for (int j = 0; j < 8; j++) {
|
||||
crc = (crc & 0x80U) ? (uint8_t)((crc << 1U) ^ 0xD5U) : (uint8_t)(crc << 1U);
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
static void hyundai_rx_all_hook(const CANPacket_t *msg) {
|
||||
if ((msg->addr == 0x53EU) && (msg->bus == 2U) && (GET_LEN(msg) == 6U)) {
|
||||
hyundai_has_lkas12 = true;
|
||||
@@ -148,6 +172,10 @@ static void hyundai_rx_all_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) {
|
||||
if (hyundai_ray_pedal && (msg->addr == 0x201U)) {
|
||||
return hyundai_ray_pedal_checksum(msg);
|
||||
}
|
||||
|
||||
uint8_t chksum = 0;
|
||||
if (msg->addr == 0x386U) {
|
||||
// count the bits
|
||||
@@ -231,7 +259,11 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
// gas press, different for EV, hybrid, and ICE models
|
||||
if ((msg->addr == 0x371U) && hyundai_ev_gas_signal) {
|
||||
if ((msg->addr == 0x201U) && hyundai_ray_pedal) {
|
||||
const uint16_t track1 = ((uint16_t)msg->data[0] << 8U) | msg->data[1];
|
||||
const uint16_t track2 = ((uint16_t)msg->data[2] << 8U) | msg->data[3];
|
||||
gas_pressed = (track1 > 272U) || (track2 > 513U);
|
||||
} else if ((msg->addr == 0x371U) && hyundai_ev_gas_signal && !hyundai_ray_pedal) {
|
||||
gas_pressed = (((msg->data[4] & 0x7FU) << 1) | (msg->data[3] >> 7)) != 0U;
|
||||
} else if ((msg->addr == 0x371U) && hyundai_hybrid_gas_signal) {
|
||||
gas_pressed = msg->data[7] != 0U;
|
||||
@@ -291,6 +323,22 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
|
||||
|
||||
bool tx = true;
|
||||
|
||||
if (hyundai_ray_pedal && (msg->addr == 0x200U)) {
|
||||
const uint16_t track1 = ((uint16_t)msg->data[0] << 8U) | msg->data[1];
|
||||
const uint16_t track2 = ((uint16_t)msg->data[2] << 8U) | msg->data[3];
|
||||
const bool enabled = (msg->data[4] & 0x80U) != 0U;
|
||||
const int expected_track2 = 497 + (2 * ((int)track1 - 264));
|
||||
if ((msg->data[4] & 0x70U) != 0U ||
|
||||
(msg->data[5] != hyundai_ray_pedal_checksum(msg)) ||
|
||||
(enabled && (track1 < 264U || track1 > 397U || track2 < 497U || track2 > 766U ||
|
||||
SAFETY_ABS((int)track2 - expected_track2) > 40)) ||
|
||||
(!enabled && ((track1 != 0U) || (track2 != 0U))) ||
|
||||
longitudinal_interceptor_checks(msg) ||
|
||||
(enabled && (!get_longitudinal_allowed() || brake_pressed_prev))) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
if ((msg->addr == 0x53EU) && !hyundai_has_lkas12) {
|
||||
tx = false;
|
||||
}
|
||||
@@ -390,6 +438,10 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
|
||||
return tx;
|
||||
}
|
||||
|
||||
static bool hyundai_fwd_hook(int bus_num, int addr) {
|
||||
return (bus_num == 2) && (addr == 0x53E) && hyundai_has_lkas12;
|
||||
}
|
||||
|
||||
static safety_config hyundai_init(uint16_t param) {
|
||||
static const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = {
|
||||
HYUNDAI_COMMON_TX_MSGS(2, false)
|
||||
@@ -457,6 +509,10 @@ static safety_config hyundai_init(uint16_t param) {
|
||||
};
|
||||
|
||||
hyundai_common_init(param);
|
||||
hyundai_ray_pedal = (param & (uint16_t)~(32U | 128U | 2048U)) == 0x9405U;
|
||||
if (hyundai_ray_pedal) {
|
||||
hyundai_longitudinal = true; // button engagement; no Hyundai SCC TX
|
||||
}
|
||||
hyundai_legacy = false;
|
||||
hyundai_can_canfd_blended_hda2 = hyundai_can_canfd_blended && hyundai_canfd_lka_steering;
|
||||
hyundai_aol_main_lkas_sync = GET_FLAG(param, 32U);
|
||||
@@ -467,6 +523,17 @@ static safety_config hyundai_init(uint16_t param) {
|
||||
}
|
||||
|
||||
safety_config ret;
|
||||
if (hyundai_ray_pedal) {
|
||||
static RxCheck hyundai_ray_pedal_rx_checks[] = {
|
||||
HYUNDAI_COMMON_RX_CHECKS(false)
|
||||
HYUNDAI_NON_SCC_EV_ADDR_CHECK
|
||||
HYUNDAI_LDA_BUTTON_ADDR_CHECK
|
||||
HYUNDAI_RAY_PEDAL_ADDR_CHECK
|
||||
};
|
||||
SET_RX_CHECKS(hyundai_ray_pedal_rx_checks, ret);
|
||||
SET_TX_MSGS(HYUNDAI_RAY_PEDAL_TX_MSGS, ret);
|
||||
return ret;
|
||||
}
|
||||
if (hyundai_longitudinal) {
|
||||
// Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
|
||||
static RxCheck hyundai_long_rx_checks[] = {
|
||||
@@ -696,6 +763,7 @@ static safety_config hyundai_legacy_init(uint16_t param) {
|
||||
|
||||
hyundai_common_init(param);
|
||||
hyundai_legacy = true;
|
||||
hyundai_ray_pedal = false;
|
||||
hyundai_can_canfd_blended_hda2 = false;
|
||||
hyundai_camera_scc = false;
|
||||
hyundai_can_refresh_msgs = false;
|
||||
@@ -711,6 +779,7 @@ const safety_hooks hyundai_hooks = {
|
||||
.get_counter = hyundai_get_counter,
|
||||
.get_checksum = hyundai_get_checksum,
|
||||
.compute_checksum = hyundai_compute_checksum,
|
||||
.fwd = hyundai_fwd_hook,
|
||||
};
|
||||
|
||||
const safety_hooks hyundai_legacy_hooks = {
|
||||
@@ -721,4 +790,5 @@ const safety_hooks hyundai_legacy_hooks = {
|
||||
.get_counter = hyundai_get_counter,
|
||||
.get_checksum = hyundai_get_checksum,
|
||||
.compute_checksum = hyundai_compute_checksum,
|
||||
.fwd = hyundai_fwd_hook,
|
||||
};
|
||||
|
||||
@@ -0,0 +1,241 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define TESLA_LEGACY_FLAG_HW1 8U
|
||||
|
||||
static bool tesla_external_panda = false;
|
||||
static bool tesla_hw1 = false;
|
||||
static bool tesla_hw2 = false;
|
||||
static bool tesla_hw3 = false;
|
||||
static bool tesla_legacy_longitudinal = false;
|
||||
|
||||
static int chassis_bus = 0U;
|
||||
static int das_control_msg = 0x2bfU;
|
||||
static int di_torque1_msg = 0x106U;
|
||||
|
||||
static bool tesla_legacy_stock_aeb = false;
|
||||
static bool tesla_legacy_stock_lkas = false;
|
||||
static bool tesla_legacy_stock_lkas_prev = false;
|
||||
|
||||
static void tesla_legacy_rx_hook(const CANPacket_t *msg) {
|
||||
// EPAS_sysStatus: steering angle, driver hands, and EAC status.
|
||||
if (!tesla_external_panda && (msg->bus == 0U) && (msg->addr == 0x370U)) {
|
||||
const int angle_meas_new = (((msg->data[4] & 0x3FU) << 8) | msg->data[5]) - 8192U;
|
||||
update_sample(&angle_meas, angle_meas_new);
|
||||
|
||||
const int hands_on_level = msg->data[4] >> 6;
|
||||
const int eac_status = msg->data[6] >> 5;
|
||||
const int eac_error_code = msg->data[2] >> 4;
|
||||
steering_disengage = (hands_on_level >= 3) || ((eac_status == 0) && (eac_error_code == 9));
|
||||
}
|
||||
|
||||
// ESP_B: ESP_vehicleSpeed.
|
||||
if (!tesla_external_panda && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x155U)) {
|
||||
const float speed = ((msg->data[6] | (msg->data[5] << 8)) * 0.01) * KPH_TO_MS;
|
||||
UPDATE_VEHICLE_SPEED(speed);
|
||||
}
|
||||
|
||||
// DI_torque1: pedal position. HW1 uses the 0x108 message variant.
|
||||
if ((tesla_external_panda || tesla_hw1) && (msg->bus == 0U) && (msg->addr == di_torque1_msg)) {
|
||||
gas_pressed = msg->data[6] != 0U;
|
||||
}
|
||||
|
||||
if (((tesla_external_panda) && (msg->bus == 0U) && (msg->addr == 0x1f8U)) ||
|
||||
((!tesla_external_panda) && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x20aU))) {
|
||||
brake_pressed = (((msg->data[0] & 0x0CU) >> 2) != 1U);
|
||||
}
|
||||
|
||||
// DI_state: cruise state.
|
||||
if (((tesla_external_panda) && (msg->bus == 0U) && (msg->addr == 0x256U)) ||
|
||||
((!tesla_external_panda) && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x368U))) {
|
||||
const int cruise_state = (msg->data[1] >> 4) & 0x07U;
|
||||
const bool cruise_engaged = (cruise_state == 2) || (cruise_state == 3) || (cruise_state == 4) ||
|
||||
(cruise_state == 6) || (cruise_state == 7);
|
||||
vehicle_moving = cruise_state != 3;
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
|
||||
if (msg->bus == 2U) {
|
||||
if ((tesla_external_panda || tesla_hw1) && msg->addr == das_control_msg) {
|
||||
tesla_legacy_stock_aeb = (msg->data[2] & 0x03U) == 1U;
|
||||
}
|
||||
|
||||
if (!tesla_external_panda && msg->addr == 0x488U) {
|
||||
const int steering_control_type = msg->data[2] >> 6;
|
||||
const bool stock_lkas_now = steering_control_type == 2;
|
||||
if (stock_lkas_now && !tesla_legacy_stock_lkas_prev && !controls_allowed) {
|
||||
tesla_legacy_stock_lkas = true;
|
||||
}
|
||||
if (!stock_lkas_now) {
|
||||
tesla_legacy_stock_lkas = false;
|
||||
}
|
||||
tesla_legacy_stock_lkas_prev = stock_lkas_now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool tesla_legacy_tx_hook(const CANPacket_t *msg) {
|
||||
const AngleSteeringLimits TESLA_STEERING_LIMITS = {
|
||||
.max_angle = 3600,
|
||||
.angle_deg_to_can = 10,
|
||||
.frequency = 50U,
|
||||
};
|
||||
|
||||
const AngleSteeringParams TESLA_LEGACY_STEERING_PARAMS = {
|
||||
.slip_factor = -0.0005666493436310427,
|
||||
.steer_ratio = 15.,
|
||||
.wheelbase = 2.96,
|
||||
};
|
||||
|
||||
const LongitudinalLimits TESLA_LONG_LIMITS = {
|
||||
.max_accel = 425,
|
||||
.min_accel = 288,
|
||||
.inactive_accel = 375,
|
||||
};
|
||||
|
||||
bool violation = false;
|
||||
|
||||
// DAS_steeringControl: angle is encoded in 0.1 degree units with a 1638.35 offset.
|
||||
if (!tesla_external_panda && (msg->addr == 0x488U)) {
|
||||
const int raw_angle_can = ((msg->data[0] & 0x7FU) << 8) | msg->data[1];
|
||||
const int desired_angle = raw_angle_can - 16384;
|
||||
const int steer_control_type = msg->data[2] >> 6;
|
||||
const bool steer_control_enabled = steer_control_type == 1;
|
||||
|
||||
violation |= steer_angle_cmd_checks_vm(desired_angle, steer_control_enabled,
|
||||
TESLA_STEERING_LIMITS, TESLA_LEGACY_STEERING_PARAMS);
|
||||
|
||||
const bool valid_steer_control_type = (steer_control_type == 0) || (steer_control_type == 1);
|
||||
violation |= !valid_steer_control_type;
|
||||
violation |= tesla_legacy_stock_lkas;
|
||||
}
|
||||
|
||||
// DAS_control: HW1 longitudinal control is sent to the powertrain bus (bus 0).
|
||||
if ((tesla_external_panda || tesla_hw1) && (msg->addr == das_control_msg)) {
|
||||
const int aeb_event = msg->data[2] & 0x03U;
|
||||
violation |= aeb_event != 0;
|
||||
violation |= tesla_legacy_stock_aeb;
|
||||
|
||||
const int raw_accel_max = ((msg->data[6] & 0x1FU) << 4) | (msg->data[5] >> 4);
|
||||
const int raw_accel_min = ((msg->data[5] & 0x0FU) << 5) | (msg->data[4] >> 3);
|
||||
if (tesla_legacy_longitudinal) {
|
||||
violation |= (raw_accel_max < TESLA_LONG_LIMITS.inactive_accel) &&
|
||||
(raw_accel_min < TESLA_LONG_LIMITS.inactive_accel);
|
||||
violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS);
|
||||
violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS);
|
||||
} else {
|
||||
// Stock ACC may only be cancelled, never spoofed or accelerated.
|
||||
const int acc_state = msg->data[1] >> 4;
|
||||
violation |= acc_state != 13;
|
||||
violation |= (raw_accel_max != TESLA_LONG_LIMITS.inactive_accel) ||
|
||||
(raw_accel_min != TESLA_LONG_LIMITS.inactive_accel);
|
||||
}
|
||||
}
|
||||
|
||||
return !violation;
|
||||
}
|
||||
|
||||
static bool tesla_legacy_fwd_hook(int bus_num, int addr) {
|
||||
bool block_msg = false;
|
||||
|
||||
if (bus_num == 2) {
|
||||
if (!tesla_external_panda && !tesla_hw1 && (addr == 0x27dU)) {
|
||||
block_msg = true;
|
||||
}
|
||||
if (!tesla_external_panda && (addr == 0x488U) && !tesla_legacy_stock_lkas) {
|
||||
block_msg = true;
|
||||
}
|
||||
if ((tesla_external_panda || tesla_hw1) && (addr == das_control_msg) && !tesla_legacy_stock_aeb) {
|
||||
block_msg = true;
|
||||
}
|
||||
}
|
||||
|
||||
return block_msg;
|
||||
}
|
||||
|
||||
static safety_config tesla_legacy_init(uint16_t param) {
|
||||
const int TESLA_FLAG_EXTERNAL_PANDA = 4;
|
||||
const int TESLA_FLAG_HW2 = 16;
|
||||
const int TESLA_FLAG_HW3 = 32;
|
||||
|
||||
tesla_external_panda = GET_FLAG(param, TESLA_FLAG_EXTERNAL_PANDA);
|
||||
tesla_hw1 = GET_FLAG(param, TESLA_LEGACY_FLAG_HW1);
|
||||
tesla_hw2 = GET_FLAG(param, TESLA_FLAG_HW2);
|
||||
tesla_hw3 = GET_FLAG(param, TESLA_FLAG_HW3);
|
||||
tesla_legacy_longitudinal = GET_FLAG(param, 1);
|
||||
|
||||
tesla_legacy_stock_aeb = false;
|
||||
tesla_legacy_stock_lkas = false;
|
||||
tesla_legacy_stock_lkas_prev = false;
|
||||
chassis_bus = 0U;
|
||||
di_torque1_msg = 0x106U;
|
||||
das_control_msg = tesla_external_panda ? 0x2bfU : 0x2b9U;
|
||||
|
||||
static const CanMsg TESLA_TX_LEGACY_MSGS[] = {
|
||||
{0x488, 0, 4, .check_relay = true, .disable_static_blocking = true},
|
||||
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true},
|
||||
};
|
||||
|
||||
static const CanMsg TESLA_LEGACY_PT_MSGS[] = {
|
||||
{0x2bf, 0, 8, .check_relay = true, .disable_static_blocking = true},
|
||||
};
|
||||
|
||||
static const CanMsg TESLA_TX_LEGACY_HW1_MSGS[] = {
|
||||
{0x488, 0, 4, .check_relay = true, .disable_static_blocking = true},
|
||||
{0x2b9, 0, 8, .check_relay = true, .disable_static_blocking = true},
|
||||
};
|
||||
|
||||
static RxCheck tesla_legacy_pt_rx_checks[] = {
|
||||
{.msg = {{0x106, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x1f8, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x2bf, 2, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x256, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
};
|
||||
|
||||
static RxCheck tesla_legacy_hw1_rx_checks[] = {
|
||||
{.msg = {{0x108, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x2b9, 2, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x370, 0, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x155, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x20a, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x368, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
};
|
||||
|
||||
static RxCheck tesla_legacy_hw2_rx_checks[] = {
|
||||
{.msg = {{0x370, 0, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x155, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x20a, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x368, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
};
|
||||
|
||||
static RxCheck tesla_legacy_hw3_rx_checks[] = {
|
||||
{.msg = {{0x370, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x155, 1, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x20a, 1, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x368, 1, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
{.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
|
||||
};
|
||||
|
||||
if (tesla_external_panda && (tesla_hw3 || tesla_hw2)) {
|
||||
return BUILD_SAFETY_CFG(tesla_legacy_pt_rx_checks, TESLA_LEGACY_PT_MSGS);
|
||||
}
|
||||
if (tesla_hw3) {
|
||||
chassis_bus = 1U;
|
||||
return BUILD_SAFETY_CFG(tesla_legacy_hw3_rx_checks, TESLA_TX_LEGACY_MSGS);
|
||||
}
|
||||
if (tesla_hw1) {
|
||||
di_torque1_msg = 0x108U;
|
||||
return BUILD_SAFETY_CFG(tesla_legacy_hw1_rx_checks, TESLA_TX_LEGACY_HW1_MSGS);
|
||||
}
|
||||
return BUILD_SAFETY_CFG(tesla_legacy_hw2_rx_checks, TESLA_TX_LEGACY_MSGS);
|
||||
}
|
||||
|
||||
const safety_hooks tesla_legacy_hooks = {
|
||||
.init = tesla_legacy_init,
|
||||
.rx = tesla_legacy_rx_hook,
|
||||
.tx = tesla_legacy_tx_hook,
|
||||
.fwd = tesla_legacy_fwd_hook,
|
||||
};
|
||||
@@ -2,6 +2,8 @@
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define TOYOTA_AUTO_HOLD_ACCEL -1000 // -1.0 m/s^2 in ACC_CONTROL units
|
||||
|
||||
// Stock longitudinal
|
||||
#define TOYOTA_BASE_TX_MSGS \
|
||||
{0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false}, /* LKAS + LTA + PCM cancel cmd */ \
|
||||
@@ -233,9 +235,9 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
|
||||
// the EPS faults when the steering angle rate is above a certain threshold for too long. to prevent this,
|
||||
// we allow setting STEER_REQUEST bit to 0 while maintaining the requested torque value for a single frame
|
||||
.min_valid_request_frames = 18,
|
||||
.min_valid_request_frames = 17,
|
||||
.max_invalid_request_frames = 1,
|
||||
.min_valid_request_rt_interval = 171000, // 171ms; a ~10% buffer on cutting every 19 frames
|
||||
.min_valid_request_rt_interval = 162000, // 162ms; a ~10% buffer on cutting every 18 frames
|
||||
.has_steer_req_tolerance = true,
|
||||
};
|
||||
|
||||
@@ -277,7 +279,15 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
// SecOC cars move accel to 0x183. Only allow inactive accel on 0x343 to match stock behavior
|
||||
violation = desired_accel != TOYOTA_LONG_LIMITS.inactive_accel;
|
||||
}
|
||||
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
|
||||
|
||||
bool toyota_auto_hold =
|
||||
!toyota_stock_longitudinal &&
|
||||
((alternative_experience & ALT_EXP_TOYOTA_AUTO_HOLD) != 0) &&
|
||||
!vehicle_moving && !gas_pressed && acc_main_on &&
|
||||
(desired_accel == TOYOTA_AUTO_HOLD_ACCEL) &&
|
||||
GET_BIT(msg, 30U) && !GET_BIT(msg, 31U) && !GET_BIT(msg, 24U);
|
||||
|
||||
violation |= !toyota_auto_hold && longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
|
||||
|
||||
// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
|
||||
if (toyota_stock_longitudinal) {
|
||||
@@ -394,12 +404,7 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
// Auto brake hold replaces the camera AEB message only while stopped.
|
||||
if ((msg->addr == 0x344U) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0)) {
|
||||
if (vehicle_moving || gas_pressed || !acc_main_on) {
|
||||
tx = false;
|
||||
}
|
||||
} else if ((msg->addr == 0x344U) && toyota_stock_longitudinal) {
|
||||
if ((msg->addr == 0x344U) && toyota_stock_longitudinal) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
@@ -566,21 +571,11 @@ static safety_config toyota_init(uint16_t param) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
static bool toyota_fwd_hook(int bus_num, int addr) {
|
||||
bool block_msg = false;
|
||||
if (bus_num == 2) {
|
||||
block_msg = (addr == 0x344) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0) &&
|
||||
!vehicle_moving && !gas_pressed && acc_main_on;
|
||||
}
|
||||
return block_msg;
|
||||
}
|
||||
|
||||
const safety_hooks toyota_hooks = {
|
||||
.init = toyota_init,
|
||||
.rx = toyota_rx_hook,
|
||||
.rx_all = toyota_rx_all_hook,
|
||||
.tx = toyota_tx_hook,
|
||||
.fwd = toyota_fwd_hook,
|
||||
.get_checksum = toyota_get_checksum,
|
||||
.compute_checksum = toyota_compute_checksum,
|
||||
.get_quality_flag_valid = toyota_get_quality_flag_valid,
|
||||
|
||||
@@ -2,9 +2,11 @@
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// safetyParam: 0 = CMA (XC40 Recharge), 1 = SPA (S60 Recharge, Polestar 2)
|
||||
// safetyParam: 0 = CMA (XC40 Recharge), 1 = SPA (S60 Recharge, Polestar 2),
|
||||
// 2 = C1 (V40)
|
||||
// Polestar 2 is technically CMA, but appears to use SPA DBC for CAN 1 bus
|
||||
#define VOLVO_FLAG_SPA 1U
|
||||
#define VOLVO_FLAG_C1 2U
|
||||
|
||||
// Volvo CAN message addresses shared between CMA and SPA
|
||||
#define VOLVO_LCA_STEER 0x58U // TX from VCU1 to PSCM, LCA steering command (0x58)
|
||||
@@ -24,6 +26,15 @@
|
||||
#define VOLVO_LCA_6 0x97U // TX LCA_6 message
|
||||
#define VOLVO_LCA_7 0x92U // TX LCA_7 message
|
||||
|
||||
// C1-specific addresses (V40). The V40 powertrain bus is bus 0 and its
|
||||
// forward-camera bus is bus 2; bus 1 is unused by this port.
|
||||
#define VOLVO_C1_BUTTONS 0x10U
|
||||
#define VOLVO_C1_FSM_0 0x30U
|
||||
#define VOLVO_C1_FSM_1 0xD0U
|
||||
#define VOLVO_C1_PSCM_1 0x125U
|
||||
#define VOLVO_C1_PEDAL_AND_BRAKE 0x55U
|
||||
#define VOLVO_C1_SPEED 0x150U
|
||||
|
||||
// CMA-specific PT bus addresses
|
||||
#define VOLVO_CMA_BUS1_SPEED 0x70U // RX vehicle speed
|
||||
#define VOLVO_CMA_ECM_1 0x250U // RX accelerator pedal position
|
||||
@@ -44,6 +55,10 @@
|
||||
#define VOLVO_MAX_ANGLE_CAN 9650
|
||||
#define VOLVO_RELAY_ANGLE_TOLERANCE 54 // approximately 3 degrees
|
||||
|
||||
#define VOLVO_C1_ANGLE_DEG_TO_CAN 22.753128f
|
||||
#define VOLVO_C1_MAX_ANGLE_CAN 8189
|
||||
#define VOLVO_C1_RELAY_ANGLE_TOLERANCE 2
|
||||
|
||||
|
||||
// CAN bus definitions for Volvo
|
||||
// Using same naming as carstate.py for consistency: main, pt, party
|
||||
@@ -54,6 +69,7 @@
|
||||
// Runtime addresses set by volvo_init based on safetyParam
|
||||
static uint16_t volvo_ecm_1_addr;
|
||||
static uint16_t volvo_bus1_cruise_control_addr;
|
||||
static bool volvo_c1;
|
||||
|
||||
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]);
|
||||
@@ -67,6 +83,21 @@ static int volvo_lca_5_angle(const CANPacket_t *msg) {
|
||||
return to_signed(volvo_be_15(msg, 6U), 15);
|
||||
}
|
||||
|
||||
static int volvo_c1_pscm_angle(const CANPacket_t *msg) {
|
||||
return (int)(((uint16_t)msg->data[5] << 8U) | msg->data[6]) - 32768;
|
||||
}
|
||||
|
||||
static int volvo_c1_fsm_angle(const CANPacket_t *msg) {
|
||||
return (int)(((uint16_t)(msg->data[4] & 0x3FU) << 8U) | msg->data[5]) - 8192;
|
||||
}
|
||||
|
||||
static uint8_t volvo_c1_fsm_checksum(const CANPacket_t *msg) {
|
||||
const uint16_t angle_raw = ((uint16_t)(msg->data[4] & 0x3FU) << 8U) | msg->data[5];
|
||||
const uint8_t direction = msg->data[7] & 0x3U;
|
||||
const uint8_t checksum_sum = (msg->data[3] + direction + angle_raw + (angle_raw >> 8U)) & 0xFFU;
|
||||
return checksum_sum ^ 0xFFU;
|
||||
}
|
||||
|
||||
static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = {
|
||||
.max_angle = VOLVO_MAX_ANGLE_CAN,
|
||||
.angle_deg_to_can = VOLVO_ANGLE_DEG_TO_CAN,
|
||||
@@ -81,8 +112,51 @@ static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = {
|
||||
.frequency = 50U,
|
||||
};
|
||||
|
||||
static const AngleSteeringLimits VOLVO_C1_ANGLE_STEERING_LIMITS = {
|
||||
.max_angle = VOLVO_C1_MAX_ANGLE_CAN,
|
||||
.angle_deg_to_can = VOLVO_C1_ANGLE_DEG_TO_CAN,
|
||||
.angle_rate_up_lookup = {
|
||||
{7.0f, 17.0f, 36.0f},
|
||||
{2.0f, 0.25f, 0.1f},
|
||||
},
|
||||
.angle_rate_down_lookup = {
|
||||
{7.0f, 17.0f, 36.0f},
|
||||
{2.0f, 0.25f, 0.1f},
|
||||
},
|
||||
.max_angle_error = 455, // 20 degrees
|
||||
.angle_error_min_speed = 0.0f,
|
||||
.frequency = 50U,
|
||||
.enforce_angle_error = true,
|
||||
};
|
||||
|
||||
static void volvo_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
if (volvo_c1) {
|
||||
if (msg->bus == VOLVO_MAIN_BUS) {
|
||||
if (msg->addr == VOLVO_C1_PSCM_1) {
|
||||
update_sample(&angle_meas, volvo_c1_pscm_angle(msg));
|
||||
}
|
||||
|
||||
if (msg->addr == VOLVO_C1_SPEED) {
|
||||
const uint16_t speed_raw = ((uint16_t)msg->data[6] << 8U) | msg->data[7];
|
||||
const float speed = ((float)speed_raw * 0.01f) / 3.6f;
|
||||
vehicle_moving = speed > 0.1f;
|
||||
UPDATE_VEHICLE_SPEED(speed);
|
||||
}
|
||||
|
||||
if (msg->addr == VOLVO_C1_PEDAL_AND_BRAKE) {
|
||||
const uint16_t gas_raw = ((uint16_t)(msg->data[1] & 0x3U) << 8U) | msg->data[2];
|
||||
gas_pressed = gas_raw > 50U; // DBC factor 0.1: greater than 5 percent
|
||||
brake_pressed = GET_BIT(msg, 24U) || GET_BIT(msg, 38U);
|
||||
}
|
||||
}
|
||||
|
||||
if ((msg->bus == VOLVO_PARTY_BUS) && (msg->addr == VOLVO_C1_FSM_0)) {
|
||||
pcm_cruise_check(GET_BIT(msg, 58U));
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Main bus (bus 0) messages
|
||||
if (msg->bus == VOLVO_MAIN_BUS) {
|
||||
// Update brake pedal and cruise state from BCM2
|
||||
@@ -158,6 +232,33 @@ static void volvo_rx_hook(const CANPacket_t *msg) {
|
||||
static bool volvo_tx_hook(const CANPacket_t *msg) {
|
||||
bool tx = true;
|
||||
|
||||
if (volvo_c1) {
|
||||
if (msg->addr == VOLVO_C1_FSM_1) {
|
||||
const int desired_angle = volvo_c1_fsm_angle(msg);
|
||||
const uint8_t direction = msg->data[7] & 0x3U;
|
||||
const bool steer_control_enabled = direction != 0U;
|
||||
tx &= SAFETY_ABS(desired_angle) <= VOLVO_C1_MAX_ANGLE_CAN;
|
||||
tx &= !steer_angle_cmd_checks(desired_angle, steer_control_enabled, VOLVO_C1_ANGLE_STEERING_LIMITS);
|
||||
tx &= (direction == 0U) || (direction == 3U);
|
||||
tx &= (msg->data[0] == 0xE3U) && (msg->data[1] == 0xB4U) && (msg->data[2] == 0x08U);
|
||||
tx &= (msg->data[3] == 0x80U) && ((msg->data[4] & 0xC0U) == 0x80U) && ((msg->data[7] & 0xFCU) == 0x94U);
|
||||
tx &= msg->data[6] == volvo_c1_fsm_checksum(msg);
|
||||
}
|
||||
|
||||
if (msg->addr == VOLVO_C1_PSCM_1) {
|
||||
const int relayed_angle = volvo_c1_pscm_angle(msg);
|
||||
const int measured_max = angle_meas.max + VOLVO_C1_RELAY_ANGLE_TOLERANCE;
|
||||
const int measured_min = angle_meas.min - VOLVO_C1_RELAY_ANGLE_TOLERANCE;
|
||||
tx &= !safety_max_limit_check(relayed_angle, measured_max, measured_min);
|
||||
}
|
||||
|
||||
// Only ACC cancel (byte 7 bit 4) may be synthesized.
|
||||
if (msg->addr == VOLVO_C1_BUTTONS) {
|
||||
tx &= ((msg->data[7] & 0xEFU) == 0U) && (msg->data[6] == 0U);
|
||||
}
|
||||
return tx;
|
||||
}
|
||||
|
||||
// LCA_5 carries the actual angle command used by the controller. The stock
|
||||
// LCA frame also contains an angle-shaped field, but the imported controller
|
||||
// deliberately leaves that field at the observed vehicle value.
|
||||
@@ -255,6 +356,22 @@ 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);
|
||||
|
||||
if (volvo_c1) {
|
||||
static const CanMsg VOLVO_C1_TX_MSGS[] = {
|
||||
{VOLVO_C1_FSM_1, VOLVO_MAIN_BUS, 8, .check_relay = true},
|
||||
{VOLVO_C1_PSCM_1, VOLVO_PARTY_BUS, 8, .check_relay = true},
|
||||
{VOLVO_C1_BUTTONS, VOLVO_MAIN_BUS, 8, .check_relay = false},
|
||||
};
|
||||
static RxCheck volvo_c1_rx_checks[] = {
|
||||
{.msg = {{VOLVO_C1_PSCM_1, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{VOLVO_C1_FSM_0, VOLVO_PARTY_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{VOLVO_C1_PEDAL_AND_BRAKE, VOLVO_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{VOLVO_C1_SPEED, VOLVO_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
return BUILD_SAFETY_CFG(volvo_c1_rx_checks, VOLVO_C1_TX_MSGS);
|
||||
}
|
||||
|
||||
// Set PT bus addresses based on platform
|
||||
volvo_ecm_1_addr = spa ? VOLVO_SPA_ECM_1 : VOLVO_CMA_ECM_1;
|
||||
|
||||
@@ -12,6 +12,7 @@
|
||||
#include "opendbc/safety/modes/toyota.h"
|
||||
#include "opendbc/safety/modes/tesla.h"
|
||||
#include "opendbc/safety/modes/tesla_preap.h"
|
||||
#include "opendbc/safety/modes/tesla_legacy.h"
|
||||
#include "opendbc/safety/modes/gm.h"
|
||||
#include "opendbc/safety/modes/ford.h"
|
||||
#include "opendbc/safety/modes/hyundai.h"
|
||||
@@ -502,7 +503,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
|
||||
int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
|
||||
for (int i = 0; i < hook_config_count; i++) {
|
||||
if (safety_hook_registry[i].id == mode) {
|
||||
current_hooks = safety_hook_registry[i].hooks;
|
||||
current_hooks = ((mode == SAFETY_TESLA) && GET_FLAG(param, TESLA_LEGACY_FLAG_HW1)) ?
|
||||
&tesla_legacy_hooks : safety_hook_registry[i].hooks;
|
||||
current_safety_mode = mode;
|
||||
current_safety_param = param;
|
||||
set_status = 0; // set
|
||||
|
||||
@@ -654,6 +654,31 @@ class TestGmCcLongitudinalNoCameraSafety(TestGmCcLongitudinalSafety):
|
||||
self.safety.init_tests()
|
||||
|
||||
|
||||
def test_gm_volt_cc_gateway_brake_threshold_matches_carstate():
|
||||
safety = libsafety_py.libsafety
|
||||
safety.set_safety_hooks(
|
||||
CarParams.SafetyModel.gm,
|
||||
GMSafetyFlags.FLAG_GM_NO_CAMERA |
|
||||
GMSafetyFlags.FLAG_GM_NO_ACC |
|
||||
GMSafetyFlags.FLAG_GM_CC_LONG |
|
||||
GMSafetyFlags.FLAG_GM_VOLT_CC_GATEWAY,
|
||||
)
|
||||
safety.init_tests()
|
||||
safety.set_controls_allowed(True)
|
||||
|
||||
cruise = common.make_msg(0, 0x3D1, 8, bytes([0, 0, 0, 0, 0x80, 0, 0, 0]))
|
||||
safety.safety_rx_hook(cruise)
|
||||
assert safety.get_controls_allowed()
|
||||
|
||||
noisy_brake = libsafety_py.make_CANPacket(0xF1, 0, b"\x34\x06\x05\x40\x00\x00")
|
||||
safety.safety_rx_hook(noisy_brake)
|
||||
assert safety.get_controls_allowed()
|
||||
|
||||
pressed_brake = libsafety_py.make_CANPacket(0xF1, 0, b"\x34\x15\x05\x40\x00\x00")
|
||||
safety.safety_rx_hook(pressed_brake)
|
||||
assert not safety.get_controls_allowed()
|
||||
|
||||
|
||||
class TestGmCcLongitudinalPandaSchedSafety(TestGmCcLongitudinalSafety):
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0x180, 0x370], 0: [0x184, 0x3D1]}
|
||||
INTERCEPTOR_GAS_PRESSED = 596
|
||||
|
||||
@@ -434,9 +434,11 @@ def test_hyundai_lkas12_tx_requires_stock_camera_message():
|
||||
|
||||
lkas12 = libsafety_py.make_CANPacket(0x53E, 0, bytes(6))
|
||||
assert not safety.safety_tx_hook(lkas12)
|
||||
assert safety.safety_fwd_hook(2, 0x53E) == 0
|
||||
|
||||
safety.safety_rx_hook(libsafety_py.make_CANPacket(0x53E, 2, bytes(6)))
|
||||
assert safety.safety_tx_hook(lkas12)
|
||||
assert safety.safety_fwd_hook(2, 0x53E) == -1
|
||||
|
||||
|
||||
class TestHyundaiLongitudinalSafety(HyundaiLongitudinalBase, TestHyundaiSafety):
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import create_gas_interceptor_command
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
|
||||
@pytest.mark.parametrize("param", [0x9405, 0x9C05, 0x9401, 0x1005, 0])
|
||||
def test_ray_pedal_tx_isolation_and_limits(param):
|
||||
safety = libsafety_py.libsafety
|
||||
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, param)
|
||||
safety.init_tests()
|
||||
safety.set_controls_allowed(True)
|
||||
packer = CANPacker("hyundai_kia_ray_pedal")
|
||||
|
||||
def tx(gas):
|
||||
addr, dat, bus = create_gas_interceptor_command(packer, gas, 3)
|
||||
return safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
|
||||
|
||||
has_ray_signature = param in (0x9405, 0x9C05)
|
||||
assert tx(0) is has_ray_signature
|
||||
assert tx(0.35) is has_ray_signature
|
||||
assert not tx(0.36) # above the Ray-only initial command cap
|
||||
assert not tx(1.0)
|
||||
|
||||
if has_ray_signature:
|
||||
safety.set_controls_allowed(False)
|
||||
assert tx(0)
|
||||
assert not tx(0.1)
|
||||
safety.set_controls_allowed(True)
|
||||
safety.set_gas_pressed_prev(True)
|
||||
assert not tx(0.1)
|
||||
safety.set_gas_pressed_prev(False)
|
||||
addr, dat, bus = create_gas_interceptor_command(packer, 0.1, 3)
|
||||
bad_crc = bytearray(dat)
|
||||
bad_crc[-1] ^= 1
|
||||
assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, bytes(bad_crc)))
|
||||
|
||||
|
||||
def test_ray_pedal_rx_crc_is_checked_only_for_ray_signature():
|
||||
safety = libsafety_py.libsafety
|
||||
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405)
|
||||
safety.init_tests()
|
||||
dat = bytes.fromhex("01f403d55de8")
|
||||
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0x201, 0, dat))
|
||||
bad_crc = bytearray(dat)
|
||||
bad_crc[-1] ^= 1
|
||||
assert not safety.safety_rx_hook(libsafety_py.make_CANPacket(0x201, 0, bytes(bad_crc)))
|
||||
|
||||
|
||||
def test_ray_native_commanded_gas_does_not_cancel_driver_override_safety():
|
||||
safety = libsafety_py.libsafety
|
||||
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405)
|
||||
safety.init_tests()
|
||||
safety.set_controls_allowed(True)
|
||||
packer = CANPacker("hyundai_kia_ray_pedal")
|
||||
|
||||
physical_rest = bytes.fromhex("010801f30cef")
|
||||
native_gas = bytes.fromhex("004e008000ae0700")
|
||||
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0x201, 0, physical_rest))
|
||||
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0x371, 0, native_gas))
|
||||
assert not safety.get_gas_pressed_prev()
|
||||
addr, dat, bus = create_gas_interceptor_command(packer, 0.1, 3)
|
||||
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
|
||||
|
||||
physical_press = packer.make_can_msg("GAS_SENSOR", 0, {
|
||||
"INTERCEPTOR_GAS": (310 - 264) * 0.672,
|
||||
"INTERCEPTOR_GAS2": (593 - 497) * 0.332,
|
||||
"STATE": 0, "COUNTER_PEDAL": 13,
|
||||
})
|
||||
press_addr, press_dat, press_bus = physical_press
|
||||
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(press_addr, press_bus, press_dat))
|
||||
assert safety.get_gas_pressed_prev()
|
||||
assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat))
|
||||
|
||||
|
||||
def test_non_ray_hyundai_ev_keeps_native_driver_gas_detection():
|
||||
safety = libsafety_py.libsafety
|
||||
safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x1001)
|
||||
safety.init_tests()
|
||||
native_gas = bytes.fromhex("004e008000ae0700")
|
||||
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0x371, 0, native_gas))
|
||||
assert safety.get_gas_pressed_prev()
|
||||
@@ -417,6 +417,18 @@ class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, Test
|
||||
return self.packer.make_can_msg_safety("Steering_2", SUBARU_MAIN_BUS, {"Steering_Angle": angle})
|
||||
|
||||
|
||||
class TestSubaruDPlatformFixedAngleSafety(TestSubaruDPlatformAngleSafety):
|
||||
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | \
|
||||
SubaruSafetyFlags.FIXED_ANGLE_LIMITS
|
||||
STEER_ANGLE_MAX = 545
|
||||
ANGLE_RATE_BP = [0., 5., 35.]
|
||||
ANGLE_RATE_UP = [5., .8, .15]
|
||||
ANGLE_RATE_DOWN = [5., .8, .15]
|
||||
|
||||
def test_rt_limits(self):
|
||||
raise unittest.SkipTest("Breakpoint angle limits do not enforce a real-time message frequency")
|
||||
|
||||
|
||||
class TestSubaruDPlatformStopStartSafety(TestSubaruDPlatformAngleSafety):
|
||||
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM | \
|
||||
SubaruSafetyFlags.STOP_START_BUTTON
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven
|
||||
from opendbc.car.tesla.values import CANBUS, CAR, DBC, TeslaSafetyFlags
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def legacy_safety():
|
||||
safety = libsafety_py.libsafety
|
||||
packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.pt])
|
||||
return safety, TeslaCANRaven({CANBUS.party: packer})
|
||||
|
||||
|
||||
def tx(safety, msg):
|
||||
addr, data, bus = msg
|
||||
return safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, data))
|
||||
|
||||
|
||||
def test_hw1_steering_requires_controls_allowed(legacy_safety):
|
||||
safety, can = legacy_safety
|
||||
safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value)
|
||||
safety.init_tests()
|
||||
safety.set_angle_meas(0, 0)
|
||||
safety.set_controls_allowed(False)
|
||||
assert tx(safety, can.create_steering_control(0, 0, False))
|
||||
assert not tx(safety, can.create_steering_control(0, 0, True))
|
||||
safety.set_controls_allowed(True)
|
||||
assert tx(safety, can.create_steering_control(0, 0, True))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("alpha_long", [False, True])
|
||||
def test_hw1_accel_only_allowed_with_alpha_long_and_engagement(legacy_safety, alpha_long):
|
||||
safety, can = legacy_safety
|
||||
param = TeslaSafetyFlags.FLAG_HW1.value | (TeslaSafetyFlags.LONG_CONTROL.value if alpha_long else 0)
|
||||
safety.set_safety_hooks(10, param)
|
||||
safety.init_tests()
|
||||
safety.set_controls_allowed(True)
|
||||
assert tx(safety, can.create_longitudinal_command(13, 0, 0, 10, False, False))
|
||||
assert tx(safety, can.create_longitudinal_command(4, 1, 0, 10, True, False)) == alpha_long
|
||||
safety.set_controls_allowed(False)
|
||||
assert not tx(safety, can.create_longitudinal_command(4, 1, 0, 10, True, False))
|
||||
|
||||
|
||||
def test_hw1_stock_ap_steer_and_acc_are_blocked_from_forwarding(legacy_safety):
|
||||
safety, _ = legacy_safety
|
||||
safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value)
|
||||
safety.init_tests()
|
||||
assert safety.safety_fwd_hook(2, 0x488) == -1
|
||||
assert safety.safety_fwd_hook(2, 0x2b9) == -1
|
||||
assert safety.safety_fwd_hook(2, 0x370) == 0
|
||||
|
||||
|
||||
def test_hw1_flag_dispatch_does_not_change_modern_or_preap_hooks(legacy_safety):
|
||||
safety, can = legacy_safety
|
||||
steer = can.create_steering_control(0, 0, False)
|
||||
safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value)
|
||||
safety.init_tests()
|
||||
assert tx(safety, steer)
|
||||
|
||||
# APS monitor exists only in the modern Tesla TX whitelist; HW1 must not
|
||||
# accidentally inherit it from the unflagged hook.
|
||||
monitor = libsafety_py.make_CANPacket(0x27d, 0, bytes(3))
|
||||
assert not safety.safety_tx_hook(monitor)
|
||||
safety.set_safety_hooks(10, 0)
|
||||
safety.init_tests()
|
||||
assert safety.safety_tx_hook(monitor)
|
||||
|
||||
safety.set_safety_hooks(35, 0)
|
||||
safety.init_tests()
|
||||
assert safety.safety_fwd_hook(2, 0x370) == -1
|
||||
@@ -97,29 +97,55 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT
|
||||
msg = libsafety_py.make_CANPacket(0x283, 0, bytes(dat))
|
||||
self.assertEqual(not bad and not stock_longitudinal, self._tx(msg))
|
||||
|
||||
def test_auto_brake_hold_aeb_replacement_only_at_standstill(self):
|
||||
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALLOW_AEB)
|
||||
hold_msg = libsafety_py.make_CANPacket(0x344, 0, b"\xfd\x80\x00\x00\x00\x00\x00\xcc")
|
||||
def test_auto_hold_acc_control_is_narrowly_allowed_only_at_standstill(self):
|
||||
if (not self.LONGITUDINAL or
|
||||
self.safety.get_current_safety_param() & (ToyotaSafetyFlags.STOCK_LONGITUDINAL.value | ToyotaSafetyFlags.SECOC.value)):
|
||||
raise unittest.SkipTest("Toyota Auto Hold requires non-SecOC openpilot longitudinal control")
|
||||
|
||||
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD)
|
||||
hold_msg = self.packer.make_can_msg_safety("ACC_CONTROL", 0, {
|
||||
"ACCEL_CMD": -1.0,
|
||||
"PERMIT_BRAKING": 1,
|
||||
"RELEASE_STANDSTILL": 0,
|
||||
"CANCEL_REQ": 0,
|
||||
})
|
||||
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._toggle_aol(True))
|
||||
self._rx(self._user_gas_msg(False))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertTrue(self._tx(hold_msg))
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x344))
|
||||
|
||||
self.assertFalse(self._tx(self.packer.make_can_msg_safety("ACC_CONTROL", 0, {
|
||||
"ACCEL_CMD": -1.1,
|
||||
"PERMIT_BRAKING": 1,
|
||||
"RELEASE_STANDSTILL": 0,
|
||||
})))
|
||||
|
||||
self._rx(self._speed_msg(1.0))
|
||||
self.assertFalse(self._tx(hold_msg))
|
||||
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
|
||||
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._user_gas_msg(True))
|
||||
self.assertFalse(self._tx(hold_msg))
|
||||
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
|
||||
|
||||
self._rx(self._user_gas_msg(False))
|
||||
self._rx(self._toggle_aol(False))
|
||||
self.assertFalse(self._tx(hold_msg))
|
||||
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
|
||||
|
||||
def test_auto_hold_acc_control_is_blocked_without_toyota_hold_toggle(self):
|
||||
hold_msg = self.packer.make_can_msg_safety("ACC_CONTROL", 0, {
|
||||
"ACCEL_CMD": -1.0,
|
||||
"PERMIT_BRAKING": 1,
|
||||
"RELEASE_STANDSTILL": 0,
|
||||
"CANCEL_REQ": 0,
|
||||
})
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._toggle_aol(True))
|
||||
self._rx(self._user_gas_msg(False))
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.safety.set_alternative_experience(0)
|
||||
self.assertFalse(self._tx(hold_msg))
|
||||
|
||||
# Only allow LTA msgs with no actuation
|
||||
def test_lta_steer_cmd(self):
|
||||
@@ -228,7 +254,7 @@ class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSaf
|
||||
TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conservative for rounding
|
||||
|
||||
# Safety around steering req bit
|
||||
MIN_VALID_STEERING_FRAMES = 18
|
||||
MIN_VALID_STEERING_FRAMES = 17
|
||||
MAX_INVALID_STEERING_FRAMES = 1
|
||||
|
||||
def setUp(self):
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Safety tests for Volvo CMA/SPA.
|
||||
Safety tests for Volvo C1/CMA/SPA.
|
||||
|
||||
The safety mode lives at ``opendbc/safety/modes/volvo.h`` and is parameterized
|
||||
by ``safetyParam``:
|
||||
@@ -8,14 +8,13 @@ by ``safetyParam``:
|
||||
- ``safetyParam == 0`` → CMA platform (Volvo XC40 Recharge)
|
||||
- ``safetyParam == VOLVO_FLAG_SPA`` → SPA platform (Volvo S60 Recharge,
|
||||
Polestar 2)
|
||||
- ``safetyParam == VOLVO_FLAG_C1`` → C1 platform (Volvo V40)
|
||||
|
||||
The two platforms share LCA/PSCM/etc. addresses on the main and party buses
|
||||
but use *different* PT-bus addresses and signal scales for ECM_1 and
|
||||
BUS1_CRUISE_CONTROL. Vehicle speed is read from main-bus SPEED on both, so it
|
||||
is not platform-dependent. This test file exercises both platforms through the
|
||||
same generic ``CarSafetyTest`` harness so that any future divergence between
|
||||
``carstate.py`` and ``volvo.h`` — e.g. a threshold drifting out of sync — is
|
||||
caught on a laptop instead of in the car.
|
||||
CMA and SPA share LCA/PSCM/etc. addresses on the main and party buses but use
|
||||
different PT-bus addresses and signal scales. C1 uses the V40's legacy CAN
|
||||
layout and its own safety allowlist. The tests exercise all three through the
|
||||
generic ``CarSafetyTest`` harness so divergence between ``carstate.py`` and
|
||||
``volvo.h`` is caught before running in a car.
|
||||
|
||||
Companion to: ``opendbc/car/volvo/carstate.py`` (must agree on thresholds).
|
||||
"""
|
||||
@@ -25,13 +24,16 @@ import re
|
||||
import unittest
|
||||
|
||||
from opendbc.car.volvo.interface import SAFETY_VOLVO
|
||||
from opendbc.car.volvo.values import VolvoSafetyFlags
|
||||
from opendbc.car.volvo.volvocan import create_c1_checksum, create_c1_steering_control
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
import opendbc.safety.tests.common as common
|
||||
from opendbc.safety.tests.common import CANPackerSafety
|
||||
|
||||
|
||||
# Must match VOLVO_FLAG_SPA in opendbc/safety/modes/volvo.h
|
||||
VOLVO_FLAG_SPA = 1
|
||||
# Must match the flags in opendbc/safety/modes/volvo.h
|
||||
VOLVO_FLAG_SPA = VolvoSafetyFlags.SPA.value
|
||||
VOLVO_FLAG_C1 = VolvoSafetyFlags.C1.value
|
||||
|
||||
# Must match VOLVO_SPEED_TO_MS in volvo.h and SPEED_TO_MS in carstate.py
|
||||
VOLVO_SPEED_TO_MS = 0.003977
|
||||
@@ -332,5 +334,124 @@ class TestVolvoSPA(TestVolvoSafetyBase):
|
||||
"BUS1_CRUISE_CONTROL", VOLVO_PT_BUS, values)
|
||||
|
||||
|
||||
class TestVolvoC1(common.CarSafetyTest, common.AngleSteeringSafetyTest):
|
||||
TX_MSGS = [[0xD0, VOLVO_MAIN_BUS], [0x125, VOLVO_PARTY_BUS], [0x10, VOLVO_MAIN_BUS]]
|
||||
RELAY_MALFUNCTION_ADDRS = {
|
||||
VOLVO_MAIN_BUS: (0xD0,),
|
||||
VOLVO_PARTY_BUS: (0x125,),
|
||||
}
|
||||
FWD_BLACKLISTED_ADDRS = {
|
||||
VOLVO_MAIN_BUS: [0x125],
|
||||
VOLVO_PARTY_BUS: [0xD0],
|
||||
}
|
||||
STANDSTILL_THRESHOLD = 0.1
|
||||
GAS_PRESSED_THRESHOLD = 5.0
|
||||
|
||||
STEER_ANGLE_MAX = 359.9
|
||||
STEER_ANGLE_TEST_MAX = 350.0
|
||||
DEG_TO_CAN = 1 / 0.04395
|
||||
ANGLE_RATE_BP = [7.0, 17.0, 36.0]
|
||||
ANGLE_RATE_UP = [2.0, 0.25, 0.1]
|
||||
ANGLE_RATE_DOWN = [2.0, 0.25, 0.1]
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerSafety("volvo_v40_2017_pt")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(SAFETY_VOLVO, VOLVO_FLAG_C1)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _angle_cmd_msg(self, angle: float, enabled: bool, increment_timer: bool = True):
|
||||
values = {
|
||||
"SET_X_E3": 0xE3,
|
||||
"SET_X_B4": 0xB4,
|
||||
"SET_X_08": 0x08,
|
||||
"LKAAngleReq": angle,
|
||||
"LKASteerDirection": 3 if enabled else 0,
|
||||
"TrqLim": 0,
|
||||
"SET_X_25": 0x25,
|
||||
"SET_X_02": 0x02,
|
||||
}
|
||||
|
||||
def fix_checksum(msg):
|
||||
address, data, bus = msg
|
||||
data = bytearray(data)
|
||||
data[6] = create_c1_checksum(data)
|
||||
return address, data, bus
|
||||
|
||||
return self.packer.make_can_msg_safety("FSM1", VOLVO_MAIN_BUS, values, fix_checksum)
|
||||
|
||||
def _angle_meas_msg(self, angle: float):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"PSCM1", VOLVO_MAIN_BUS, {"SteeringAngleServo": angle})
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"VehicleSpeed1", VOLVO_MAIN_BUS, {"VehicleSpeed": speed * 3.6})
|
||||
|
||||
def _speed_msg_2(self, speed):
|
||||
return None
|
||||
|
||||
def _user_brake_msg(self, brake):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"PedalandBrake", VOLVO_MAIN_BUS, {"BrakePedalActive2": bool(brake)})
|
||||
|
||||
def _user_gas_msg(self, gas):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"PedalandBrake", VOLVO_MAIN_BUS, {"AccPedal": gas})
|
||||
|
||||
def _pcm_status_msg(self, enable):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"FSM0", VOLVO_PARTY_BUS, {"ACCStatusActive": bool(enable)})
|
||||
|
||||
def test_cancel_button_only(self):
|
||||
allowed = self.packer.make_can_msg_safety(
|
||||
"CCButtons", VOLVO_MAIN_BUS, {"ACCStopBtn": 1})
|
||||
self.assertTrue(self._tx(allowed))
|
||||
|
||||
for signal in ("ACCOnOffBtn", "ACCSetBtn", "ACCResumeBtn", "ACCMinusBtn",
|
||||
"TimeGapIncreaseBtn", "TimeGapDecreaseBtn"):
|
||||
msg = self.packer.make_can_msg_safety("CCButtons", VOLVO_MAIN_BUS, {signal: 1})
|
||||
self.assertFalse(self._tx(msg), signal)
|
||||
|
||||
def test_pscm_relay_cannot_invent_angle(self):
|
||||
for _ in range(common.MAX_SAMPLE_VALS):
|
||||
self._rx(self._angle_meas_msg(10))
|
||||
valid = self.packer.make_can_msg_safety(
|
||||
"PSCM1", VOLVO_PARTY_BUS, {"SteeringAngleServo": 10})
|
||||
invalid = self.packer.make_can_msg_safety(
|
||||
"PSCM1", VOLVO_PARTY_BUS, {"SteeringAngleServo": 20})
|
||||
self.assertTrue(self._tx(valid))
|
||||
self.assertFalse(self._tx(invalid))
|
||||
|
||||
def test_pscm_relay_preserves_full_lock_angle(self):
|
||||
for angle in (-720, 500):
|
||||
for _ in range(common.MAX_SAMPLE_VALS):
|
||||
self._rx(self._angle_meas_msg(angle))
|
||||
relayed = self.packer.make_can_msg_safety(
|
||||
"PSCM1", VOLVO_PARTY_BUS, {"SteeringAngleServo": angle})
|
||||
self.assertTrue(self._tx(relayed), angle)
|
||||
|
||||
def test_steering_static_fields_and_checksum(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._reset_angle_measurement(0)
|
||||
self._reset_speed_measurement(10)
|
||||
self._set_prev_desired_angle(0)
|
||||
valid = self._angle_cmd_msg(0, True)
|
||||
self.assertTrue(self._tx(valid))
|
||||
|
||||
for byte_index in (0, 1, 2, 3, 4, 6, 7):
|
||||
invalid = self._angle_cmd_msg(0, True)
|
||||
invalid[0].data[byte_index] ^= 0x4 if byte_index in (4, 7) else 0x1
|
||||
self.assertFalse(self._tx(invalid), byte_index)
|
||||
|
||||
def test_controller_steering_message_is_allowed(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._reset_angle_measurement(0)
|
||||
self._reset_speed_measurement(10)
|
||||
self._set_prev_desired_angle(0)
|
||||
address, data, bus = create_c1_steering_control(self.packer, 0, True)
|
||||
self.assertTrue(self._tx(libsafety_py.make_CANPacket(address, bus, data)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
@@ -130,4 +130,5 @@ flake8-implicit-str-concat.allow-multiline=false
|
||||
include-package-data = true
|
||||
|
||||
[tool.setuptools.package-data]
|
||||
"opendbc.dbc" = ["hyundai_kia_ray_pedal.dbc"]
|
||||
"opendbc.safety" = ["*.h", "board/*.h", "board/drivers/*.h", "modes/*.h"]
|
||||
|
||||
@@ -181,6 +181,11 @@ build_project("panda_h7_remote_can_ignition_only", base_project_h7, "./board/mai
|
||||
build_project("panda_hkg_remote_can_ignition_only", base_project_f4, "./board/main.c", ["-DPANDA_HKG_REMOTE_START", "-DPANDA_IGNORE_IGNITION_LINE"])
|
||||
build_project("panda_h7_hkg_remote_can_ignition_only", base_project_h7, "./board/main.c", ["-DPANDA_HKG_REMOTE_START", "-DPANDA_IGNORE_IGNITION_LINE"])
|
||||
|
||||
build_project("panda_tesla_wake", base_project_f4, "./board/main.c", ["-DPANDA_TESLA_WAKE_ON_CAN"])
|
||||
build_project("panda_h7_tesla_wake", base_project_h7, "./board/main.c", ["-DPANDA_TESLA_WAKE_ON_CAN"])
|
||||
build_project("panda_tesla_wake_can_ignition_only", base_project_f4, "./board/main.c", ["-DPANDA_TESLA_WAKE_ON_CAN", "-DPANDA_IGNORE_IGNITION_LINE"])
|
||||
build_project("panda_h7_tesla_wake_can_ignition_only", base_project_h7, "./board/main.c", ["-DPANDA_TESLA_WAKE_ON_CAN", "-DPANDA_IGNORE_IGNITION_LINE"])
|
||||
|
||||
# panda jungle fw
|
||||
flags = [
|
||||
"-DPANDA_JUNGLE",
|
||||
|
||||
@@ -2,18 +2,18 @@
|
||||
|
||||
bool bootkick_reset_triggered = false;
|
||||
|
||||
void bootkick_tick(bool ignition, bool recent_heartbeat) {
|
||||
void bootkick_tick(bool ignition, bool recent_heartbeat, bool wake) {
|
||||
static uint16_t bootkick_last_serial_ptr = 0;
|
||||
static uint8_t waiting_to_boot_countdown = 0;
|
||||
static uint8_t boot_reset_countdown = 0;
|
||||
static uint8_t bootkick_harness_status_prev = HARNESS_STATUS_NC;
|
||||
static bool bootkick_ign_prev = false;
|
||||
static bool bootkick_wake_prev = false;
|
||||
static BootState boot_state = BOOT_BOOTKICK;
|
||||
BootState boot_state_prev = boot_state;
|
||||
const bool harness_inserted = (harness.status != bootkick_harness_status_prev) && (harness.status != HARNESS_STATUS_NC);
|
||||
|
||||
if ((ignition && !bootkick_ign_prev) || harness_inserted) {
|
||||
// bootkick on rising edge of ignition or harness insertion
|
||||
if ((ignition && !bootkick_ign_prev) || harness_inserted || (wake && !bootkick_wake_prev && !ignition)) {
|
||||
boot_state = BOOT_BOOTKICK;
|
||||
} else if (recent_heartbeat) {
|
||||
// disable bootkick once openpilot is up
|
||||
@@ -56,6 +56,7 @@ void bootkick_tick(bool ignition, bool recent_heartbeat) {
|
||||
|
||||
// update state
|
||||
bootkick_ign_prev = ignition;
|
||||
bootkick_wake_prev = wake;
|
||||
bootkick_harness_status_prev = harness.status;
|
||||
bootkick_last_serial_ptr = uart_ring_som_debug.w_ptr_tx;
|
||||
if (waiting_to_boot_countdown > 0U) {
|
||||
|
||||
@@ -2,4 +2,4 @@
|
||||
|
||||
extern bool bootkick_reset_triggered;
|
||||
|
||||
void bootkick_tick(bool ignition, bool recent_heartbeat);
|
||||
void bootkick_tick(bool ignition, bool recent_heartbeat, bool wake);
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user