mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-11 18:53:47 +08:00
Compare commits
3 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| ac3fb6a4d8 | |||
| 79b791a26b | |||
| 1c35e376e9 |
@@ -27,10 +27,6 @@ add_panda_targets() {
|
|||||||
panda_h7_remote_can_ignition_only
|
panda_h7_remote_can_ignition_only
|
||||||
panda_hkg_remote_can_ignition_only
|
panda_hkg_remote_can_ignition_only
|
||||||
panda_h7_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
|
panda_jungle_h7
|
||||||
body_h7
|
body_h7
|
||||||
)
|
)
|
||||||
|
|||||||
Binary file not shown.
@@ -110,7 +110,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"LocationFilterInitialState", {PERSISTENT, BYTES}},
|
{"LocationFilterInitialState", {PERSISTENT, BYTES}},
|
||||||
{"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
|
{"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
|
||||||
{"LongitudinalPersonality", {PERSISTENT, INT, std::to_string(static_cast<int>(cereal::LongitudinalPersonality::STANDARD))}},
|
{"LongitudinalPersonality", {PERSISTENT, INT, std::to_string(static_cast<int>(cereal::LongitudinalPersonality::STANDARD))}},
|
||||||
{"LongitudinalPersonalityProfiles", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
|
|
||||||
{"NetworkMetered", {PERSISTENT, BOOL}},
|
{"NetworkMetered", {PERSISTENT, BOOL}},
|
||||||
{"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
{"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
||||||
{"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
{"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
||||||
@@ -352,7 +351,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"DrivingModelVersion", {PERSISTENT, STRING, "v15", "v15", 1}},
|
{"DrivingModelVersion", {PERSISTENT, STRING, "v15", "v15", 1}},
|
||||||
{"DynamicPathWidth", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
{"DynamicPathWidth", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||||
{"DynamicPedalsOnUI", {PERSISTENT, BOOL, "1", "0", 1, 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}},
|
{"EngageVolume", {PERSISTENT, INT, "101", "101", 2, SETTINGS_SIMPLE}},
|
||||||
{"EVTuning", {PERSISTENT, BOOL, "0", "0", 3}},
|
{"EVTuning", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||||
{"Fahrenheit", {PERSISTENT, BOOL, "0", "0", 3}},
|
{"Fahrenheit", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||||
@@ -363,7 +361,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"IgnoreIgnitionLine", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
{"IgnoreIgnitionLine", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||||
{"LongPitch", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
|
{"LongPitch", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
|
||||||
{"RemoteStartBootsComma", {PERSISTENT, BOOL, "0", "0", 0, 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}},
|
{"RemapCancelToDistance", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||||
{"NAPAdaptiveAccel", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},
|
{"NAPAdaptiveAccel", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},
|
||||||
{"NAPFollowDistance", {PERSISTENT, INT, "4", "4"}},
|
{"NAPFollowDistance", {PERSISTENT, INT, "4", "4"}},
|
||||||
@@ -468,7 +465,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"LeadDetectionThreshold", {PERSISTENT, INT, "35", "50", 3}},
|
{"LeadDetectionThreshold", {PERSISTENT, INT, "35", "50", 3}},
|
||||||
{"LeadIndicator", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
{"LeadIndicator", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||||
{"LeadInfo", {PERSISTENT, BOOL, "0", "0", 3}},
|
{"LeadInfo", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||||
{"LeadInfoMode", {PERSISTENT, INT, "2", "2", 3}},
|
|
||||||
{"LKASButtonControl", {PERSISTENT, INT, "5", "0", 2, SETTINGS_SIMPLE}},
|
{"LKASButtonControl", {PERSISTENT, INT, "5", "0", 2, SETTINGS_SIMPLE}},
|
||||||
{"LockDoors", {PERSISTENT, BOOL, "1", "0", 0}},
|
{"LockDoors", {PERSISTENT, BOOL, "1", "0", 0}},
|
||||||
{"LockDoorsTimer", {PERSISTENT, INT, "0", "0", 0}},
|
{"LockDoorsTimer", {PERSISTENT, INT, "0", "0", 0}},
|
||||||
|
|||||||
Binary file not shown.
@@ -5,7 +5,7 @@ import threading
|
|||||||
import time
|
import time
|
||||||
import uuid
|
import uuid
|
||||||
|
|
||||||
from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType, UnknownKeyName
|
from openpilot.common.params import Params, ParamKeyFlag, UnknownKeyName
|
||||||
|
|
||||||
class TestParams:
|
class TestParams:
|
||||||
def setup_method(self):
|
def setup_method(self):
|
||||||
@@ -128,31 +128,6 @@ class TestParams:
|
|||||||
assert self.params.get("LiveParameters") is None
|
assert self.params.get("LiveParameters") is None
|
||||||
assert self.params.get("LiveParameters", return_default=True) 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):
|
def test_params_get_type(self):
|
||||||
# json
|
# json
|
||||||
self.params.put("ApiCache_DriveStats", {"a": 0})
|
self.params.put("ApiCache_DriveStats", {"a": 0})
|
||||||
|
|||||||
@@ -194,7 +194,7 @@ def get_checksum_state(dbc_name: str) -> ChecksumState | None:
|
|||||||
return ChecksumState(4, 2, 3, 5, False, SignalType.HONDA_CHECKSUM, honda_checksum)
|
return ChecksumState(4, 2, 3, 5, False, SignalType.HONDA_CHECKSUM, honda_checksum)
|
||||||
elif dbc_name.startswith(("toyota_", "lexus_")):
|
elif dbc_name.startswith(("toyota_", "lexus_")):
|
||||||
return ChecksumState(8, -1, 7, -1, False, SignalType.TOYOTA_CHECKSUM, toyota_checksum)
|
return ChecksumState(8, -1, 7, -1, False, SignalType.TOYOTA_CHECKSUM, toyota_checksum)
|
||||||
elif dbc_name.startswith(("hyundai_canfd_generated", "hyundai_radar_210_21f_generated")):
|
elif dbc_name.startswith("hyundai_canfd_generated"):
|
||||||
return ChecksumState(16, -1, 0, -1, True, SignalType.HKG_CAN_FD_CHECKSUM, hkg_can_fd_checksum)
|
return ChecksumState(16, -1, 0, -1, True, SignalType.HKG_CAN_FD_CHECKSUM, hkg_can_fd_checksum)
|
||||||
elif dbc_name.startswith(("vw_mqb", "vw_mqbevo", "vw_meb")):
|
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)
|
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_mqb_meb_checksum)
|
||||||
|
|||||||
@@ -6,7 +6,7 @@ from opendbc.car.lateral import apply_driver_steer_torque_limits
|
|||||||
from opendbc.car.gm import gmcan
|
from opendbc.car.gm import gmcan
|
||||||
from opendbc.car.common.conversions import Conversions as CV
|
from opendbc.car.common.conversions import Conversions as CV
|
||||||
from opendbc.car.gm.values import (
|
from opendbc.car.gm.values import (
|
||||||
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,
|
ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams,
|
||||||
CruiseButtons, GMFlags, GMSafetyFlags,
|
CruiseButtons, GMFlags, GMSafetyFlags,
|
||||||
)
|
)
|
||||||
from opendbc.car.interfaces import CarControllerBase
|
from opendbc.car.interfaces import CarControllerBase
|
||||||
@@ -309,7 +309,7 @@ def supports_volt_auto_hold(CP, auto_hold_enabled: bool):
|
|||||||
auto_hold_enabled and
|
auto_hold_enabled and
|
||||||
getattr(CP, "openpilotLongitudinalControl", False) and
|
getattr(CP, "openpilotLongitudinalControl", False) and
|
||||||
stock_hold_safety_ready and
|
stock_hold_safety_ready and
|
||||||
CP.carFingerprint in GM_AUTO_HOLD_CARS
|
CP.carFingerprint in AUTO_HOLD_VOLT_CARS
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -18,7 +18,6 @@ from opendbc.car.gm.values import (
|
|||||||
AccState,
|
AccState,
|
||||||
CanBus,
|
CanBus,
|
||||||
CruiseButtons,
|
CruiseButtons,
|
||||||
GM_AUTO_HOLD_CARS,
|
|
||||||
GMFlags,
|
GMFlags,
|
||||||
SDGM_CAR,
|
SDGM_CAR,
|
||||||
STEER_THRESHOLD,
|
STEER_THRESHOLD,
|
||||||
@@ -33,7 +32,6 @@ STANDSTILL_THRESHOLD = 10 * 0.0311
|
|||||||
VOLT_EBCM_BRAKE_PRESSED_THRESHOLD = 6 / 0xd0
|
VOLT_EBCM_BRAKE_PRESSED_THRESHOLD = 6 / 0xd0
|
||||||
AUTO_HOLD_MIN_DRIVE_TIME_S = 3.0
|
AUTO_HOLD_MIN_DRIVE_TIME_S = 3.0
|
||||||
AUTO_HOLD_REGEN_RELEASE_COOLDOWN_S = 1.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,
|
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||||
CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel}
|
CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||||
@@ -69,36 +67,6 @@ def update_auto_hold_drive_timers(in_drive_for_hold: bool, moving_for_hold: bool
|
|||||||
return auto_hold_drive_time, one_pedal_drive_time
|
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):
|
class CarState(CarStateBase):
|
||||||
def __init__(self, CP, FPCP):
|
def __init__(self, CP, FPCP):
|
||||||
super().__init__(CP, FPCP)
|
super().__init__(CP, FPCP)
|
||||||
@@ -137,8 +105,6 @@ class CarState(CarStateBase):
|
|||||||
self.lkas_previously_enabled = 0
|
self.lkas_previously_enabled = 0
|
||||||
self.lkas_enabled = 0
|
self.lkas_enabled = 0
|
||||||
self.pcm_acc_status = AccState.OFF
|
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.stock_fcw_alert = 0
|
||||||
self.car_gps_config = get_car_gps_config(CP)
|
self.car_gps_config = get_car_gps_config(CP)
|
||||||
self.car_gps_supported = self.car_gps_config is not None
|
self.car_gps_supported = self.car_gps_config is not None
|
||||||
@@ -384,18 +350,8 @@ class CarState(CarStateBase):
|
|||||||
|
|
||||||
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
||||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
||||||
acc_state = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
|
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
|
||||||
friction_brake_unavailable = pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1
|
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.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
|
||||||
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
|
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
|
||||||
@@ -444,11 +400,6 @@ class CarState(CarStateBase):
|
|||||||
self.auto_hold_fault_suppression_timer = max(self.auto_hold_fault_suppression_timer - DT_CTRL, 0.0)
|
self.auto_hold_fault_suppression_timer = max(self.auto_hold_fault_suppression_timer - DT_CTRL, 0.0)
|
||||||
ret.accFaulted = False
|
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:
|
if self.CP.enableBsm and not sdgm_non_volt:
|
||||||
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
|
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
|
||||||
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
|
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
|
||||||
|
|||||||
@@ -31,8 +31,6 @@ BOLT_CC_DIRECTION_MEMORY_S = 1.5
|
|||||||
VOLT_CC_CARS = {
|
VOLT_CC_CARS = {
|
||||||
CAR.CHEVROLET_VOLT_CC,
|
CAR.CHEVROLET_VOLT_CC,
|
||||||
}
|
}
|
||||||
VOLT_CC_TARGET_DEADBAND_MPH = 5.0
|
|
||||||
VOLT_CC_ACCEL_DEADBAND_MS2 = 0.15
|
|
||||||
|
|
||||||
|
|
||||||
def malibu_phase_map_for_button(button):
|
def malibu_phase_map_for_button(button):
|
||||||
@@ -346,15 +344,7 @@ def _create_volt_cc_spam_command(CS, actuators, ms_convert):
|
|||||||
speed_setpoint = int(round(CS.out.cruiseState.speed * ms_convert))
|
speed_setpoint = int(round(CS.out.cruiseState.speed * ms_convert))
|
||||||
ego_speed = CS.out.vEgo * ms_convert
|
ego_speed = CS.out.vEgo * ms_convert
|
||||||
|
|
||||||
v_cruise_kph = float(getattr(CS.out, "vCruise", 0.0))
|
if accel == 0.0:
|
||||||
if 0.0 < v_cruise_kph < 255.0:
|
|
||||||
is_metric = ms_convert == CV.MS_TO_KPH
|
|
||||||
target_setpoint = v_cruise_kph if is_metric else v_cruise_kph * CV.KPH_TO_MPH
|
|
||||||
target_deadband = VOLT_CC_TARGET_DEADBAND_MPH * (CV.MPH_TO_KPH if is_metric else 1.0)
|
|
||||||
if abs(target_setpoint - speed_setpoint) <= target_deadband:
|
|
||||||
return CruiseButtons.INIT, float("inf")
|
|
||||||
|
|
||||||
if abs(accel) <= VOLT_CC_ACCEL_DEADBAND_MS2:
|
|
||||||
return CruiseButtons.INIT, float("inf")
|
return CruiseButtons.INIT, float("inf")
|
||||||
|
|
||||||
if accel < 0.0:
|
if accel < 0.0:
|
||||||
|
|||||||
@@ -15,7 +15,6 @@ from opendbc.car.gm.values import (
|
|||||||
CC_ONLY_CAR,
|
CC_ONLY_CAR,
|
||||||
CC_REGEN_PADDLE_CAR,
|
CC_REGEN_PADDLE_CAR,
|
||||||
EV_CAR,
|
EV_CAR,
|
||||||
GM_AUTO_HOLD_CARS,
|
|
||||||
SDGM_CAR,
|
SDGM_CAR,
|
||||||
CarControllerParams,
|
CarControllerParams,
|
||||||
CanBus,
|
CanBus,
|
||||||
@@ -711,19 +710,18 @@ class CarInterface(CarInterfaceBase):
|
|||||||
if remote_start_boots_comma:
|
if remote_start_boots_comma:
|
||||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_REMOTE_START_BOOTS_COMMA.value
|
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_REMOTE_START_BOOTS_COMMA.value
|
||||||
|
|
||||||
gm_stock_friction_brake_safety = (
|
volt_stock_friction_brake_safety = (
|
||||||
ret.openpilotLongitudinalControl and
|
ret.openpilotLongitudinalControl and
|
||||||
(
|
(gm_auto_hold or volt_one_pedal_mode) and
|
||||||
(gm_auto_hold and candidate in GM_AUTO_HOLD_CARS) or
|
candidate in {
|
||||||
(volt_one_pedal_mode and candidate in {
|
CAR.CHEVROLET_VOLT,
|
||||||
CAR.CHEVROLET_VOLT,
|
CAR.CHEVROLET_VOLT_2019,
|
||||||
CAR.CHEVROLET_VOLT_2019,
|
CAR.CHEVROLET_VOLT_ASCM,
|
||||||
CAR.CHEVROLET_VOLT_ASCM,
|
CAR.CHEVROLET_VOLT_CAMERA,
|
||||||
CAR.CHEVROLET_VOLT_CAMERA,
|
}
|
||||||
})
|
|
||||||
)
|
|
||||||
)
|
)
|
||||||
if gm_stock_friction_brake_safety:
|
if volt_stock_friction_brake_safety:
|
||||||
|
# Reuse the paddle-scheduler safety bit as a Volt stock friction-brake
|
||||||
# marker on non-pedal paths. Auto hold and one-pedal can run while OP
|
# 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
|
# longitudinal is configured but not currently active, so the bit must
|
||||||
# be present regardless of the current long-control mode. Do not expose
|
# be present regardless of the current long-control mode. Do not expose
|
||||||
|
|||||||
@@ -431,15 +431,6 @@ def test_volt_auto_hold_requires_toggle_supported_non_cc_only_volt_and_stock_saf
|
|||||||
),
|
),
|
||||||
True,
|
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(
|
assert not supports_volt_auto_hold(
|
||||||
SimpleNamespace(
|
SimpleNamespace(
|
||||||
carFingerprint=CAR.CHEVROLET_VOLT,
|
carFingerprint=CAR.CHEVROLET_VOLT,
|
||||||
|
|||||||
@@ -8,13 +8,7 @@ from opendbc.can import CANPacker, CANParser
|
|||||||
from opendbc.car import Bus, DT_CTRL, structs
|
from opendbc.car import Bus, DT_CTRL, structs
|
||||||
from opendbc.car.car_helpers import interfaces
|
from opendbc.car.car_helpers import interfaces
|
||||||
from opendbc.car.gm import gmcan
|
from opendbc.car.gm import gmcan
|
||||||
from opendbc.car.gm.carstate import (
|
from opendbc.car.gm.carstate import CarState as GMCarState, get_hard_cruise_buttons, update_auto_hold_drive_timers
|
||||||
CarState as GMCarState,
|
|
||||||
get_hard_cruise_buttons,
|
|
||||||
is_gm_auto_hold_active,
|
|
||||||
update_auto_hold_drive_timers,
|
|
||||||
update_startup_acc_fault_suppression,
|
|
||||||
)
|
|
||||||
from opendbc.car.gm.carcontroller import (
|
from opendbc.car.gm.carcontroller import (
|
||||||
VisualAlert,
|
VisualAlert,
|
||||||
get_acc_dashboard_always_one,
|
get_acc_dashboard_always_one,
|
||||||
@@ -210,63 +204,6 @@ class TestBoltGps:
|
|||||||
assert all(message in parsers[Bus.pt].vl for message in CHEVROLET_BOLT_GPS_MESSAGES)
|
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:
|
class TestGMInterface:
|
||||||
def test_lacrosse_obd_and_ascm_integrations_remain_separate(self):
|
def test_lacrosse_obd_and_ascm_integrations_remain_separate(self):
|
||||||
obd_params = interfaces[CAR.BUICK_LACROSSE].get_params(
|
obd_params = interfaces[CAR.BUICK_LACROSSE].get_params(
|
||||||
@@ -584,43 +521,6 @@ class TestGMInterface:
|
|||||||
assert car_params.openpilotLongitudinalControl
|
assert car_params.openpilotLongitudinalControl
|
||||||
assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
|
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):
|
def test_volt_auto_hold_does_not_set_stock_hold_safety_bit_with_op_long_disabled(self):
|
||||||
CarInterface = interfaces[CAR.CHEVROLET_VOLT_ASCM]
|
CarInterface = interfaces[CAR.CHEVROLET_VOLT_ASCM]
|
||||||
fingerprint = _empty_fingerprint()
|
fingerprint = _empty_fingerprint()
|
||||||
@@ -947,63 +847,6 @@ class TestGMCarController:
|
|||||||
|
|
||||||
assert len(msgs) == 1
|
assert len(msgs) == 1
|
||||||
|
|
||||||
def test_volt_cc_redneck_holds_when_stock_setpoint_is_within_target_deadband(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=99.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 == 99
|
|
||||||
|
|
||||||
def test_volt_cc_redneck_catches_up_when_target_exceeds_deadband(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)
|
|
||||||
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=90.0 * CV.KPH_TO_MS,
|
|
||||||
cruiseState=SimpleNamespace(speed=90.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 == []
|
|
||||||
|
|
||||||
controller.frame = int(0.7 / DT_CTRL)
|
|
||||||
msgs = gmcan.create_gm_cc_spam_command(
|
|
||||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
|
||||||
)
|
|
||||||
|
|
||||||
assert len(msgs) == 1
|
|
||||||
assert controller.apply_speed == 91
|
|
||||||
|
|
||||||
def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self):
|
def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self):
|
||||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
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)
|
controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||||
|
|||||||
@@ -533,14 +533,6 @@ EV_CAR = {
|
|||||||
CAR.CHEVROLET_MALIBU_HYBRID_CC,
|
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)
|
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
|
||||||
CAMERA_ACC_CAR = {
|
CAMERA_ACC_CAR = {
|
||||||
CAR.CHEVROLET_BOLT_ACC_2022_2023,
|
CAR.CHEVROLET_BOLT_ACC_2022_2023,
|
||||||
|
|||||||
@@ -23,20 +23,6 @@ from openpilot.common.params import Params
|
|||||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
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:
|
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))
|
torque_delta = abs(float(torque_cmd) - float(prev_torque_cmd))
|
||||||
@@ -252,7 +238,6 @@ class CarController(CarControllerBase):
|
|||||||
self.steering_pressed_filter_s = 0.0
|
self.steering_pressed_filter_s = 0.0
|
||||||
self.steering_pressed_robust_prev = False
|
self.steering_pressed_robust_prev = False
|
||||||
self.bosch_last_gas = 0.0
|
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_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 = self.param_store.get_float("HondaWindFactorParams", default=1.0)
|
||||||
self.bosch_wind_factor_before_brake = self.bosch_wind_factor
|
self.bosch_wind_factor_before_brake = self.bosch_wind_factor
|
||||||
@@ -487,16 +472,12 @@ class CarController(CarControllerBase):
|
|||||||
self.bosch_last_gas = self.gas
|
self.bosch_last_gas = self.gas
|
||||||
|
|
||||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
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
|
self.stopping_counter = self.stopping_counter + 1 if stopping else 0
|
||||||
if not self.mvl_accord_mode or mvl_radar_owned:
|
if not self.mvl_accord_mode or mvl_radar_owned:
|
||||||
can_sends.extend(
|
can_sends.extend(
|
||||||
hondacan.create_acc_commands(
|
hondacan.create_acc_commands(
|
||||||
self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas, self.stopping_counter, self.CP,
|
self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas, self.stopping_counter, self.CP,
|
||||||
gas_force=gas_pedal_force, braking=bosch_braking,
|
gas_force=gas_pedal_force if self.mvl_accord_mode else None,
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
else:
|
else:
|
||||||
|
|||||||
@@ -71,18 +71,16 @@ def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_ca
|
|||||||
return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values)
|
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, braking=None):
|
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, CP, gas_force=None):
|
||||||
commands = []
|
commands = []
|
||||||
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
|
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
|
||||||
|
|
||||||
control_on = 5 if enabled else 0
|
control_on = 5 if enabled else 0
|
||||||
if gas_force is None:
|
if gas_force is None:
|
||||||
gas_force = accel
|
gas_force = accel
|
||||||
if braking is None:
|
gas_command = gas if active and gas_force > min_gas_accel else -30000
|
||||||
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
|
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 = 1 if active and stopping_counter > 0 else 0
|
||||||
standstill_release = 1 if active and stopping_counter == 0 else 0
|
standstill_release = 1 if active and stopping_counter == 0 else 0
|
||||||
|
|
||||||
|
|||||||
@@ -7,16 +7,13 @@ from opendbc.car.structs import CarParams
|
|||||||
from opendbc.car import gen_empty_fingerprint
|
from opendbc.car import gen_empty_fingerprint
|
||||||
from opendbc.car.honda.interface import CarInterface
|
from opendbc.car.honda.interface import CarInterface
|
||||||
from opendbc.car.honda.carcontroller import (
|
from opendbc.car.honda.carcontroller import (
|
||||||
BOSCH_BRAKE_FORCE_ON,
|
|
||||||
BOSCH_BRAKE_FORCE_RELEASE,
|
|
||||||
CarController,
|
CarController,
|
||||||
get_civic_bosch_modified_steering_pressed,
|
get_civic_bosch_modified_steering_pressed,
|
||||||
get_civic_bosch_modified_torque_lpf_tau,
|
get_civic_bosch_modified_torque_lpf_tau,
|
||||||
get_honda_bosch_wind_brake_mps2,
|
get_honda_bosch_wind_brake_mps2,
|
||||||
update_honda_bosch_braking,
|
|
||||||
update_honda_bosch_live_learning,
|
update_honda_bosch_live_learning,
|
||||||
)
|
)
|
||||||
from opendbc.car.honda.hondacan import create_acc_commands, create_lkas_hud
|
from opendbc.car.honda.hondacan import create_lkas_hud
|
||||||
from opendbc.car.honda.fingerprints import FW_VERSIONS
|
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, \
|
from opendbc.car.honda.values import CAR, DBC, HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL, CarControllerParams, HondaFlags, HondaSafetyFlags, \
|
||||||
HondaStarPilotFlags
|
HondaStarPilotFlags
|
||||||
@@ -29,67 +26,6 @@ def get_test_toggles() -> SimpleNamespace:
|
|||||||
|
|
||||||
|
|
||||||
class TestHondaFingerprint:
|
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):
|
def test_honda_lkas_hud_shows_lane_lines_when_lateral_only_is_active(self):
|
||||||
class FakePacker:
|
class FakePacker:
|
||||||
@staticmethod
|
@staticmethod
|
||||||
|
|||||||
@@ -10,13 +10,13 @@ from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_an
|
|||||||
from opendbc.car.common.conversions import Conversions as CV
|
from opendbc.car.common.conversions import Conversions as CV
|
||||||
from opendbc.car.hyundai import hyundaicanfd, hyundaican
|
from opendbc.car.hyundai import hyundaicanfd, hyundaican
|
||||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||||
from opendbc.car.hyundai.values import HyundaiFlags, HyundaiSafetyFlags, HyundaiStarPilotFlags, Buttons, CarControllerParams, CAR, CANFD_ANGLE_LONGITUDINAL_CAR, \
|
from opendbc.car.hyundai.values import HyundaiFlags, HyundaiSafetyFlags, HyundaiStarPilotFlags, Buttons, CarControllerParams, CAR, \
|
||||||
CANFD_RADAR_ECU_KEEPALIVE_CAR, CANFD_ALT_BUTTONS_RESUME_CAR, kia_ev6_gt_line_longitudinal_tuning, \
|
CANFD_ANGLE_LONGITUDINAL_CAR, 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
|
KIA_EV6_GT_LINE_LONG_TUNING_TESTING_GROUND_ID
|
||||||
from opendbc.car.interfaces import CarControllerBase
|
from opendbc.car.interfaces import CarControllerBase
|
||||||
from opendbc.car.vehicle_model import VehicleModel
|
from opendbc.car.vehicle_model import VehicleModel
|
||||||
from openpilot.common.params import Params
|
from openpilot.common.params import Params
|
||||||
from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_hyundai_canfd_scc_jerk_limits
|
|
||||||
from openpilot.starpilot.common.testing_grounds import testing_ground
|
from openpilot.starpilot.common.testing_grounds import testing_ground
|
||||||
|
|
||||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||||
@@ -860,11 +860,7 @@ class CarController(CarControllerBase):
|
|||||||
can_sends = []
|
can_sends = []
|
||||||
|
|
||||||
lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
||||||
persistent_lfa_status_cars = (
|
persistent_lfa_status_cars = (CAR.HYUNDAI_IONIQ_6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, CAR.KIA_EV6)
|
||||||
CAR.HYUNDAI_IONIQ_6,
|
|
||||||
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
|
|
||||||
CAR.KIA_EV6,
|
|
||||||
)
|
|
||||||
lfa_longitudinal_active = self.CP.openpilotLongitudinalControl \
|
lfa_longitudinal_active = self.CP.openpilotLongitudinalControl \
|
||||||
if self.CP.carFingerprint in persistent_lfa_status_cars else self.long_active_ecu
|
if self.CP.carFingerprint in persistent_lfa_status_cars else self.long_active_ecu
|
||||||
lka_steering_long = lka_steering and lfa_longitudinal_active
|
lka_steering_long = lka_steering and lfa_longitudinal_active
|
||||||
@@ -1023,11 +1019,7 @@ class CarController(CarControllerBase):
|
|||||||
CC.rightBlinker))
|
CC.rightBlinker))
|
||||||
if self.frame % 2 == 0:
|
if self.frame % 2 == 0:
|
||||||
if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN:
|
if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN:
|
||||||
scc_jerk_limits = get_hyundai_canfd_scc_jerk_limits(self.CP)
|
acc_kwargs = {}
|
||||||
acc_kwargs = {
|
|
||||||
"jerk_upper": scc_jerk_limits[0],
|
|
||||||
"jerk_lower": scc_jerk_limits[1],
|
|
||||||
}
|
|
||||||
else:
|
else:
|
||||||
lead_visible, lead_distance, lead_rel_speed = self._get_canfd_scc_lead_state(CC, CS, now_nanos)
|
lead_visible, lead_distance, lead_rel_speed = self._get_canfd_scc_lead_state(CC, CS, now_nanos)
|
||||||
acc_kwargs = {
|
acc_kwargs = {
|
||||||
|
|||||||
@@ -185,7 +185,6 @@ FW_VERSIONS = {
|
|||||||
],
|
],
|
||||||
(Ecu.abs, 0x7d1, None): [
|
(Ecu.abs, 0x7d1, None): [
|
||||||
b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300',
|
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 \x03 100 \x08\x01 58910-L0300',
|
||||||
b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100',
|
b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100',
|
||||||
b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100',
|
b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100',
|
||||||
@@ -209,7 +208,6 @@ FW_VERSIONS = {
|
|||||||
b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC102',
|
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-L1010 4DNDC103',
|
||||||
b'\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1030 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 4DNAP100',
|
||||||
b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP101',
|
b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP101',
|
||||||
b'\xf1\x00DN8 MDPS R 1.00 1.02 57700-L1000 4DNDP105',
|
b'\xf1\x00DN8 MDPS R 1.00 1.02 57700-L1000 4DNDP105',
|
||||||
@@ -217,7 +215,6 @@ FW_VERSIONS = {
|
|||||||
(Ecu.fwdCamera, 0x7c4, None): [
|
(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.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.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 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.00 99211-L0000 190716',
|
||||||
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L0000 191016',
|
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L0000 191016',
|
||||||
|
|||||||
@@ -19,9 +19,6 @@ MRR30_RADAR_START_ADDR = 0x210
|
|||||||
MRR30_RADAR_MSG_COUNT = 16
|
MRR30_RADAR_MSG_COUNT = 16
|
||||||
MRR35_RADAR_START_ADDR = 0x3A5
|
MRR35_RADAR_START_ADDR = 0x3A5
|
||||||
MRR35_RADAR_MSG_COUNT = 32
|
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)
|
@dataclass(frozen=True)
|
||||||
@@ -33,7 +30,6 @@ class RadarTrackConfig:
|
|||||||
frequency: int = 50
|
frequency: int = 50
|
||||||
parser_msg_count: int | None = None
|
parser_msg_count: int | None = None
|
||||||
expected_length: int | None = None
|
expected_length: int | None = None
|
||||||
dbc_name: str | None = None
|
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def can_parser_msg_count(self) -> int:
|
def can_parser_msg_count(self) -> int:
|
||||||
@@ -51,10 +47,6 @@ RADAR_TRACK_CONFIGS = {
|
|||||||
|
|
||||||
|
|
||||||
def get_radar_track_config(car_fingerprint, flags: int = 0) -> RadarTrackConfig | None:
|
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)
|
radar_dbc = DBC[car_fingerprint].get(Bus.radar)
|
||||||
if car_fingerprint == CAR.GENESIS_G90 and radar_dbc == HYUNDAI_MANDO_FRONT_RADAR_DBC:
|
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)
|
return RadarTrackConfig(RADAR_START_ADDR, G90_RADAR_MSG_COUNT, "mando", parser_msg_count=RADAR_MSG_COUNT)
|
||||||
@@ -73,10 +65,6 @@ def radar_tracks_available(radar_config: RadarTrackConfig | None, fingerprint) -
|
|||||||
if radar_config is None:
|
if radar_config is None:
|
||||||
return False
|
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)
|
msg_len = fingerprint[radar_config.bus].get(radar_config.start_addr)
|
||||||
if msg_len is None:
|
if msg_len is None:
|
||||||
return False
|
return False
|
||||||
@@ -90,8 +78,7 @@ def get_radar_can_parser(CP, radar_config):
|
|||||||
|
|
||||||
messages = [(f"RADAR_TRACK_{addr:x}", radar_config.frequency)
|
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)]
|
for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.can_parser_msg_count)]
|
||||||
dbc_name = radar_config.dbc_name or DBC[CP.carFingerprint][Bus.radar]
|
return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, radar_config.bus)
|
||||||
return CANParser(dbc_name, messages, radar_config.bus)
|
|
||||||
|
|
||||||
|
|
||||||
class RadarInterface(RadarInterfaceBase):
|
class RadarInterface(RadarInterfaceBase):
|
||||||
@@ -236,27 +223,6 @@ class RadarInterface(RadarInterfaceBase):
|
|||||||
del self.pts[track_key]
|
del self.pts[track_key]
|
||||||
continue
|
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":
|
if radar_type == "mrrevo14f":
|
||||||
for i in ("1", "2"):
|
for i in ("1", "2"):
|
||||||
track_key = addr * 2 + int(i) - 1
|
track_key = addr * 2 + int(i) - 1
|
||||||
|
|||||||
@@ -4,7 +4,7 @@ from types import SimpleNamespace
|
|||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
from opendbc.can import CANPacker, CANParser
|
from opendbc.can import CANPacker, CANParser
|
||||||
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs
|
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs, uds
|
||||||
from opendbc.car.structs import CarControl, CarParams
|
from opendbc.car.structs import CarControl, CarParams
|
||||||
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
|
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
|
||||||
from opendbc.car.hyundai.carcontroller import CarController, CANCEL_BUTTON_DELAY_FRAMES, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \
|
from opendbc.car.hyundai.carcontroller import CarController, CANCEL_BUTTON_DELAY_FRAMES, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \
|
||||||
@@ -28,13 +28,12 @@ from opendbc.car.hyundai.interface import CarInterface, KIA_EV9_ACCEL_MAX, get_c
|
|||||||
from opendbc.car.hyundai import hyundaican, hyundaicanfd
|
from opendbc.car.hyundai import hyundaican, hyundaicanfd
|
||||||
from opendbc.car.hyundai.hyundaicanfd import CanBus, hkg_can_fd_checksum
|
from opendbc.car.hyundai.hyundaicanfd import CanBus, hkg_can_fd_checksum
|
||||||
from opendbc.car.hyundai.radar_interface import MRREVO14F_RADAR_START_ADDR, MRR30_RADAR_START_ADDR, MRR35_RADAR_START_ADDR, \
|
from opendbc.car.hyundai.radar_interface import MRREVO14F_RADAR_START_ADDR, MRR30_RADAR_START_ADDR, MRR35_RADAR_START_ADDR, \
|
||||||
RADAR_START_ADDR, RadarInterface, get_radar_track_config, radar_tracks_available
|
RADAR_START_ADDR, get_radar_track_config
|
||||||
from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, DATELESS_FUZZY_CARS, \
|
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, \
|
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \
|
||||||
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
|
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
|
||||||
LEGACY_LONGITUDINAL_CAR, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \
|
LEGACY_LONGITUDINAL_CAR, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \
|
||||||
HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, \
|
HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, kia_ev6_gt_line_longitudinal_tuning
|
||||||
CANFD_RADAR_ECU_KEEPALIVE_CAR, CANFD_RADAR_LIVE_LONGITUDINAL_CAR, kia_ev6_gt_line_longitudinal_tuning
|
|
||||||
|
|
||||||
LongCtrlState = CarControl.Actuators.LongControlState
|
LongCtrlState = CarControl.Actuators.LongControlState
|
||||||
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
|
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
|
||||||
@@ -130,22 +129,17 @@ def get_test_toggles() -> SimpleNamespace:
|
|||||||
|
|
||||||
|
|
||||||
class TestHyundaiFingerprint:
|
class TestHyundaiFingerprint:
|
||||||
def test_egmp_communication_control_paths(self):
|
@pytest.mark.parametrize("car", [
|
||||||
stock_request = bytes([0x28, 0x83, 0x01])
|
CAR.HYUNDAI_IONIQ_5,
|
||||||
radar_keepalive_request = bytes([0x28, 0x01, 0x01])
|
CAR.KIA_EV6,
|
||||||
|
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
|
||||||
assert CAR.KIA_EV6 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
|
])
|
||||||
assert CAR.KIA_EV6 not in CANFD_RADAR_ECU_KEEPALIVE_CAR
|
def test_hda2_cars_without_radar_keepalive_use_stock_communication_control(self, car):
|
||||||
assert get_communication_control_request(CAR.KIA_EV6) == stock_request
|
stock_request = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x83, uds.MESSAGE_TYPE.NORMAL])
|
||||||
|
radar_keepalive_request = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL,
|
||||||
assert CAR.HYUNDAI_IONIQ_5 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
|
uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
|
||||||
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) == stock_request
|
||||||
assert get_communication_control_request(CAR.HYUNDAI_IONIQ_6) == radar_keepalive_request
|
assert get_communication_control_request(CAR.HYUNDAI_IONIQ_6) == radar_keepalive_request
|
||||||
|
|
||||||
def test_carnival_hev_low_speed_torque_rate_limits(self):
|
def test_carnival_hev_low_speed_torque_rate_limits(self):
|
||||||
@@ -445,42 +439,6 @@ class TestHyundaiFingerprint:
|
|||||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_LKA_STEERING
|
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_LKA_STEERING
|
||||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.EV_GAS
|
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:
|
for candidate in HYUNDAI_NON_SCC_CARS:
|
||||||
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, None)
|
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, None)
|
||||||
assert bool(CP.flags & HyundaiFlags.NON_SCC)
|
assert bool(CP.flags & HyundaiFlags.NON_SCC)
|
||||||
@@ -2564,7 +2522,6 @@ class TestHyundaiFingerprint:
|
|||||||
"DAMP_FACTOR": 100,
|
"DAMP_FACTOR": 100,
|
||||||
}
|
}
|
||||||
cc = SimpleNamespace(enabled=True, latActive=True,
|
cc = SimpleNamespace(enabled=True, latActive=True,
|
||||||
longActive=False,
|
|
||||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||||
leftBlinker=False, rightBlinker=False,
|
leftBlinker=False, rightBlinker=False,
|
||||||
hudControl=SimpleNamespace())
|
hudControl=SimpleNamespace())
|
||||||
@@ -2608,44 +2565,8 @@ class TestHyundaiFingerprint:
|
|||||||
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
|
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
|
||||||
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
||||||
|
|
||||||
@pytest.mark.parametrize(("car", "powertrain_flag"), [
|
@pytest.mark.parametrize("car", [CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV6, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN])
|
||||||
(CAR.HYUNDAI_IONIQ_5, HyundaiFlags.EV),
|
def test_egmp_keeps_lfa_status_when_longitudinal_is_inactive(self, car):
|
||||||
(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 = CarParams.new_message()
|
||||||
CP.carFingerprint = car
|
CP.carFingerprint = car
|
||||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
|
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
|
||||||
@@ -2661,7 +2582,6 @@ class TestHyundaiFingerprint:
|
|||||||
)
|
)
|
||||||
cs = SimpleNamespace(
|
cs = SimpleNamespace(
|
||||||
stock_lfa_msg=None, stock_lkas_msg=None,
|
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),
|
out=SimpleNamespace(gearShifter=structs.CarState.GearShifter.park),
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -2669,6 +2589,38 @@ class TestHyundaiFingerprint:
|
|||||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
|
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
|
||||||
assert any(addr == 0x12A for addr, _, _ in msgs)
|
assert any(addr == 0x12A for addr, _, _ in msgs)
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(("car", "powertrain_flag"), [
|
||||||
|
(CAR.HYUNDAI_IONIQ_5, HyundaiFlags.EV),
|
||||||
|
(CAR.KIA_CARNIVAL_2025, 0),
|
||||||
|
(CAR.KIA_CARNIVAL_HEV_4TH_GEN, HyundaiFlags.HYBRID),
|
||||||
|
])
|
||||||
|
def test_hda2_keeps_lfa_status_while_longitudinal_ecu_is_disabled(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)
|
||||||
|
controller.frame = 1
|
||||||
|
controller.long_active_ecu = True
|
||||||
|
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(
|
||||||
|
brakePressed=False, gasPressed=False,
|
||||||
|
gearShifter=structs.CarState.GearShifter.park,
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
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)
|
||||||
|
|
||||||
def test_gv70_electrified_longitudinal_uses_hda2_scc_contract(self):
|
def test_gv70_electrified_longitudinal_uses_hda2_scc_contract(self):
|
||||||
CP = CarParams.new_message()
|
CP = CarParams.new_message()
|
||||||
CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
|
CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
|
||||||
|
|||||||
@@ -109,7 +109,6 @@ class RadarInterfaceBase(ABC):
|
|||||||
self.CP = CP
|
self.CP = CP
|
||||||
self.rcp = None
|
self.rcp = None
|
||||||
self.pts: dict[int, structs.RadarData.RadarPoint] = {}
|
self.pts: dict[int, structs.RadarData.RadarPoint] = {}
|
||||||
self.track_id: int = 0
|
|
||||||
self.frame = 0
|
self.frame = 0
|
||||||
|
|
||||||
def update(self, can_packets: list[tuple[int, list[CanData]]]) -> structs.RadarDataT | None:
|
def update(self, can_packets: list[tuple[int, list[CanData]]]) -> structs.RadarDataT | None:
|
||||||
|
|||||||
@@ -16,11 +16,21 @@ _SNG_ACC_MIN_DIST = 3
|
|||||||
_SNG_ACC_MAX_DIST = 4.5
|
_SNG_ACC_MAX_DIST = 4.5
|
||||||
_LEGACY_2025_MADS_MIN_SPEED = 0.44704
|
_LEGACY_2025_MADS_MIN_SPEED = 0.44704
|
||||||
_LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0
|
_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_CONFIRM_FRAMES = 2
|
||||||
|
_ANGLE_OVERRIDE_HOLD_FRAMES = 10
|
||||||
|
_ANGLE_REENGAGE_SETTLE_FRAMES = 8
|
||||||
_ANGLE_REENGAGE_MAX_STEER_RATE = 2.0
|
_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_MIN_SPEED = 0.44704
|
||||||
_ANGLE_MADS_MAX_STEER_ANGLE = 120.0
|
_ANGLE_MADS_MAX_STEER_ANGLE = 120.0
|
||||||
_ASCENT_AOL_ARM_FRAMES = 30
|
|
||||||
_STOP_START_STARTUP_DELAY_FRAMES = 100
|
_STOP_START_STARTUP_DELAY_FRAMES = 100
|
||||||
# StarPilot's first populated toggle message can arrive several seconds after
|
# StarPilot's first populated toggle message can arrive several seconds after
|
||||||
# the car controller starts while fingerprinting and settings settle.
|
# the car controller starts while fingerprinting and settings settle.
|
||||||
@@ -43,9 +53,20 @@ class CarController(CarControllerBase):
|
|||||||
self.apply_steer_last = 0
|
self.apply_steer_last = 0
|
||||||
self.driver_override = False
|
self.driver_override = False
|
||||||
self.angle_override_confirm_frames = 0
|
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_lkas_active = False
|
||||||
self.angle_handoff_active = False
|
self.angle_handoff_active = False
|
||||||
self.ascent_aol_arm_frames = 0
|
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.cruise_button_prev = 0
|
self.cruise_button_prev = 0
|
||||||
self.steer_rate_counter = 0
|
self.steer_rate_counter = 0
|
||||||
@@ -125,36 +146,126 @@ class CarController(CarControllerBase):
|
|||||||
self.stop_start_counter = (self.stop_start_counter + 1) % 0x10
|
self.stop_start_counter = (self.stop_start_counter + 1) % 0x10
|
||||||
return msg
|
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):
|
def _reset_angle_handoff(self):
|
||||||
self.driver_override = False
|
self.driver_override = False
|
||||||
self.angle_override_confirm_frames = 0
|
self.angle_override_confirm_frames = 0
|
||||||
self.angle_handoff_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
|
||||||
|
|
||||||
def _angle_manual_handoff(self, CS, lat_active, use_steering_pressed=False):
|
def _angle_manual_handoff(self, CS, lat_active):
|
||||||
if not lat_active:
|
if not lat_active:
|
||||||
self._reset_angle_handoff()
|
self._reset_angle_handoff()
|
||||||
return False
|
return False
|
||||||
|
|
||||||
driver_override = self._update_angle_driver_override(CS)
|
driver_override = self._update_angle_driver_override(CS)
|
||||||
if use_steering_pressed:
|
|
||||||
driver_override = driver_override or getattr(CS.out, "steeringPressed", False)
|
|
||||||
steering_rate = abs(getattr(CS.out, "steeringRateDeg", 0.0))
|
|
||||||
if driver_override:
|
if driver_override:
|
||||||
self.angle_handoff_active = True
|
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
|
return True
|
||||||
|
|
||||||
if self.angle_handoff_active:
|
if not self.angle_handoff_active and not self.angle_lkas_active and \
|
||||||
if steering_rate > _ANGLE_REENGAGE_MAX_STEER_RATE:
|
abs(getattr(CS.out, "steeringRateDeg", 0.0)) > _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_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
|
return True
|
||||||
|
|
||||||
return False
|
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
|
||||||
|
|
||||||
def _update_angle_driver_override(self, CS):
|
def _update_angle_driver_override(self, CS):
|
||||||
"""Debounce the higher-confidence raw torque override signal for angle cars."""
|
"""Debounce the higher-confidence raw torque override signal for angle cars."""
|
||||||
@@ -172,13 +283,16 @@ class CarController(CarControllerBase):
|
|||||||
|
|
||||||
return self.driver_override
|
return self.driver_override
|
||||||
|
|
||||||
def _ascent_aol_ready(self, ready):
|
def _angle_reclaim_target(self, target_angle):
|
||||||
if not ready:
|
if self.angle_reclaim_frames <= 0:
|
||||||
self.ascent_aol_arm_frames = 0
|
return target_angle
|
||||||
return False
|
|
||||||
|
|
||||||
self.ascent_aol_arm_frames = min(self.ascent_aol_arm_frames + 1, _ASCENT_AOL_ARM_FRAMES)
|
progress = (_ANGLE_RECLAIM_FRAMES - self.angle_reclaim_frames + 1) / _ANGLE_RECLAIM_FRAMES
|
||||||
return self.ascent_aol_arm_frames >= _ASCENT_AOL_ARM_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
|
||||||
|
|
||||||
def lateral_angle(self, CC, CS):
|
def lateral_angle(self, CC, CS):
|
||||||
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
|
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
|
||||||
@@ -188,11 +302,12 @@ class CarController(CarControllerBase):
|
|||||||
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
|
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
|
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
|
||||||
|
|
||||||
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
|
manual_handoff = self._legacy_2025_manual_handoff(CS, lkas_available)
|
||||||
lkas_active = lkas_available and not manual_handoff
|
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(
|
apply_steer = apply_std_steer_angle_limits(
|
||||||
CC.actuators.steeringAngleDeg,
|
steer_target,
|
||||||
self.apply_steer_last,
|
self.apply_steer_last,
|
||||||
CS.out.vEgoRaw,
|
CS.out.vEgoRaw,
|
||||||
CS.out.steeringAngleDeg,
|
CS.out.steeringAngleDeg,
|
||||||
@@ -200,7 +315,7 @@ class CarController(CarControllerBase):
|
|||||||
self.p.LEGACY_2025_ANGLE_LIMITS,
|
self.p.LEGACY_2025_ANGLE_LIMITS,
|
||||||
)
|
)
|
||||||
self.apply_steer_last = apply_steer
|
self.apply_steer_last = apply_steer
|
||||||
self.angle_lkas_active = lkas_active
|
self.legacy_2025_lkas_active = lkas_active
|
||||||
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
|
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):
|
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
||||||
@@ -209,24 +324,17 @@ class CarController(CarControllerBase):
|
|||||||
abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE
|
abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE
|
||||||
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
|
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
|
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(
|
manual_handoff = self._angle_manual_handoff(CS, lkas_available)
|
||||||
CS, lkas_available, use_steering_pressed=self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023,
|
|
||||||
)
|
|
||||||
lkas_active = lkas_available and not manual_handoff
|
lkas_active = lkas_available and not manual_handoff
|
||||||
|
|
||||||
if lkas_active and not self.angle_lkas_active:
|
if lkas_active and not self.angle_lkas_active:
|
||||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
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 in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
|
||||||
apply_steer = apply_std_steer_angle_limits(
|
apply_steer = apply_std_steer_angle_limits(
|
||||||
CC.actuators.steeringAngleDeg,
|
steer_target,
|
||||||
self.apply_steer_last,
|
self.apply_steer_last,
|
||||||
CS.out.vEgoRaw,
|
CS.out.vEgoRaw,
|
||||||
CS.out.steeringAngleDeg,
|
CS.out.steeringAngleDeg,
|
||||||
@@ -257,8 +365,9 @@ class CarController(CarControllerBase):
|
|||||||
lat_active = lkas_available and not self.driver_override and not manual_handoff
|
lat_active = lkas_available and not self.driver_override and not manual_handoff
|
||||||
if lat_active and not self.angle_lkas_active:
|
if lat_active and not self.angle_lkas_active:
|
||||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
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(
|
apply_steer = apply_steer_angle_limits_vm(
|
||||||
CC.actuators.steeringAngleDeg,
|
steer_target,
|
||||||
self.apply_steer_last,
|
self.apply_steer_last,
|
||||||
CS.out.vEgoRaw,
|
CS.out.vEgoRaw,
|
||||||
CS.out.steeringAngleDeg,
|
CS.out.steeringAngleDeg,
|
||||||
@@ -299,11 +408,6 @@ class CarController(CarControllerBase):
|
|||||||
|
|
||||||
return subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req)
|
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):
|
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||||
actuators = CC.actuators
|
actuators = CC.actuators
|
||||||
hud_control = CC.hudControl
|
hud_control = CC.hudControl
|
||||||
@@ -380,7 +484,7 @@ class CarController(CarControllerBase):
|
|||||||
CC.longActive, hud_control.leadVisible,
|
CC.longActive, hud_control.leadVisible,
|
||||||
self.status_bus))
|
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,
|
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.leftLaneVisible, hud_control.rightLaneVisible,
|
||||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart, self.status_bus))
|
hud_control.leftLaneDepart, hud_control.rightLaneDepart, self.status_bus))
|
||||||
|
|
||||||
|
|||||||
@@ -85,14 +85,14 @@ class CarState(CarStateBase):
|
|||||||
|
|
||||||
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
|
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
|
||||||
ret.steeringAngleDeg = cp_angle.vl["Steering_2"]["Steering_Angle"]
|
ret.steeringAngleDeg = cp_angle.vl["Steering_2"]["Steering_Angle"]
|
||||||
steering_counter = cp_angle.vl["Steering_2"]["COUNTER"]
|
steering_updated = len(cp_angle.vl_all["Steering_2"]["Steering_Angle"]) > 0
|
||||||
else:
|
else:
|
||||||
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
|
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
|
||||||
steering_counter = cp.vl["Steering_Torque"].get("COUNTER", 0)
|
steering_updated = len(cp.vl_all["Steering_Torque"]["Steering_Angle"]) > 0
|
||||||
|
|
||||||
if not (self.CP.flags & SubaruFlags.PREGLOBAL):
|
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
|
# 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_counter)
|
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, steering_updated)
|
||||||
|
|
||||||
ret.steeringTorque = cp_angle.vl["Steering_Torque"]["Steer_Torque_Sensor"]
|
ret.steeringTorque = cp_angle.vl["Steering_Torque"]["Steer_Torque_Sensor"]
|
||||||
ret.steeringTorqueEps = cp_angle.vl["Steering_Torque"]["Steer_Torque_Output"]
|
ret.steeringTorqueEps = cp_angle.vl["Steering_Torque"]["Steer_Torque_Output"]
|
||||||
|
|||||||
@@ -8,7 +8,7 @@ from opendbc.can import CANPacker, CANParser
|
|||||||
from opendbc.car import Bus, fw_versions, gen_empty_fingerprint, structs
|
from opendbc.car import Bus, fw_versions, gen_empty_fingerprint, structs
|
||||||
from opendbc.car.fw_query_definitions import StdQueries
|
from opendbc.car.fw_query_definitions import StdQueries
|
||||||
from opendbc.car.subaru import subarucan
|
from opendbc.car.subaru import subarucan
|
||||||
from opendbc.car.subaru.carcontroller import CarController, _ASCENT_AOL_ARM_FRAMES
|
from opendbc.car.subaru.carcontroller import CarController
|
||||||
from opendbc.car.subaru.carstate import CarState
|
from opendbc.car.subaru.carstate import CarState
|
||||||
from opendbc.car.subaru.fingerprints import FW_VERSIONS
|
from opendbc.car.subaru.fingerprints import FW_VERSIONS
|
||||||
from opendbc.car.fw_versions import match_fw_to_car
|
from opendbc.car.fw_versions import match_fw_to_car
|
||||||
@@ -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)
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.47)
|
||||||
|
|
||||||
|
|
||||||
def test_legacy_2025_reengages_immediately_after_manual_steering_stops():
|
def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging():
|
||||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||||
controller = CarController({}, CP)
|
controller = CarController({}, CP)
|
||||||
CC = SimpleNamespace(
|
CC = SimpleNamespace(
|
||||||
@@ -444,20 +444,42 @@ def test_legacy_2025_reengages_immediately_after_manual_steering_stops():
|
|||||||
CS.out.steeringPressed = False
|
CS.out.steeringPressed = False
|
||||||
CS.out.steeringTorque = 0.0
|
CS.out.steeringTorque = 0.0
|
||||||
CS.out.steeringAngleDeg = -113.78
|
CS.out.steeringAngleDeg = -113.78
|
||||||
CS.out.steeringRateDeg = 0.0
|
|
||||||
msg = controller.lateral_angle(CC, CS)
|
msg = controller.lateral_angle(CC, CS)
|
||||||
parser.update([(3, [msg])])
|
parser.update([(2, [msg])])
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
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
|
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)
|
msg = controller.lateral_angle(CC, CS)
|
||||||
parser.update([(4, [msg])])
|
parser.update([(26, [msg])])
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||||
assert -113.78 < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -100.0
|
assert abs(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] - measured_angle) < 0.1
|
||||||
|
|
||||||
|
|
||||||
def test_legacy_2025_manual_handoff_reentry_uses_normal_angle_limits():
|
def test_legacy_2025_manual_handoff_reclaim_is_gradual():
|
||||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||||
controller = CarController({}, CP)
|
controller = CarController({}, CP)
|
||||||
CC = SimpleNamespace(
|
CC = SimpleNamespace(
|
||||||
@@ -486,24 +508,22 @@ def test_legacy_2025_manual_handoff_reentry_uses_normal_angle_limits():
|
|||||||
CS.out.steeringPressed = False
|
CS.out.steeringPressed = False
|
||||||
CS.out.steeringTorque = 0.0
|
CS.out.steeringTorque = 0.0
|
||||||
CS.out.steeringRateDeg = 0.0
|
CS.out.steeringRateDeg = 0.0
|
||||||
msg = controller.lateral_angle(CC, CS)
|
for i in range(19):
|
||||||
parser.update([(3, [msg])])
|
msg = controller.lateral_angle(CC, CS)
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
parser.update([(2 + i, [msg])])
|
||||||
|
|
||||||
msg = controller.lateral_angle(CC, CS)
|
|
||||||
parser.update([(4, [msg])])
|
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||||
first_reentry_angle = parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"]
|
first_reclaim_angle = parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"]
|
||||||
assert CC.actuators.steeringAngleDeg < first_reentry_angle < CS.out.steeringAngleDeg
|
assert first_reclaim_angle == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
|
||||||
|
|
||||||
reentry_angles = []
|
reclaim_angles = []
|
||||||
for i in range(6):
|
for i in range(6):
|
||||||
msg = controller.lateral_angle(CC, CS)
|
msg = controller.lateral_angle(CC, CS)
|
||||||
parser.update([(20 + i, [msg])])
|
parser.update([(20 + i, [msg])])
|
||||||
reentry_angles.append(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"])
|
reclaim_angles.append(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"])
|
||||||
|
|
||||||
assert all(reentry_angles[i] >= reentry_angles[i + 1] for i in range(len(reentry_angles) - 1))
|
assert all(reclaim_angles[i] >= reclaim_angles[i + 1] for i in range(len(reclaim_angles) - 1))
|
||||||
assert reentry_angles[-1] > CC.actuators.steeringAngleDeg
|
assert reclaim_angles[-1] > CC.actuators.steeringAngleDeg
|
||||||
|
|
||||||
|
|
||||||
def test_ascent_2023_uses_gen2_angle_bus_layout():
|
def test_ascent_2023_uses_gen2_angle_bus_layout():
|
||||||
@@ -526,25 +546,6 @@ def test_ascent_2023_uses_gen2_angle_bus_layout():
|
|||||||
assert controller.status_bus == CanBus.main
|
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():
|
def test_other_angle_platforms_keep_existing_bus_layout():
|
||||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
|
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
|
||||||
parsers = CarState.get_can_parsers(CP)
|
parsers = CarState.get_can_parsers(CP)
|
||||||
@@ -644,7 +645,7 @@ def test_angle_controller_uses_fixed_angle_rate_limits(platform):
|
|||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023))
|
@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023))
|
||||||
def test_angle_controller_reengages_immediately_after_manual_steering_stops(platform):
|
def test_angle_controller_yields_until_manual_steering_settles(platform):
|
||||||
CP = CarInterface.get_non_essential_params(platform)
|
CP = CarInterface.get_non_essential_params(platform)
|
||||||
controller = CarController({}, CP)
|
controller = CarController({}, CP)
|
||||||
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
|
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
|
||||||
@@ -671,18 +672,18 @@ def test_angle_controller_reengages_immediately_after_manual_steering_stops(plat
|
|||||||
CS.out.steeringTorque = 0.0
|
CS.out.steeringTorque = 0.0
|
||||||
CS.out.steeringAngleDeg = -17.91
|
CS.out.steeringAngleDeg = -17.91
|
||||||
CS.out.steeringRateDeg = 0.0
|
CS.out.steeringRateDeg = 0.0
|
||||||
msg = controller.lateral_angle(CC, CS)
|
for i in range(18):
|
||||||
parser.update([(3, [msg])])
|
msg = controller.lateral_angle(CC, CS)
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
parser.update([(2 + i, [msg])])
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||||
|
|
||||||
msg = controller.lateral_angle(CC, CS)
|
msg = controller.lateral_angle(CC, CS)
|
||||||
parser.update([(4, [msg])])
|
parser.update([(20, [msg])])
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||||
assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < CC.actuators.steeringAngleDeg
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
|
||||||
|
|
||||||
|
|
||||||
def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope():
|
def test_ascent_angle_controller_blocks_parking_lot_aol_engagement():
|
||||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||||
controller = CarController({}, CP)
|
controller = CarController({}, CP)
|
||||||
CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-206.12))
|
CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-206.12))
|
||||||
@@ -692,7 +693,6 @@ def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope():
|
|||||||
steeringRateDeg=96.0,
|
steeringRateDeg=96.0,
|
||||||
steeringTorque=7.0,
|
steeringTorque=7.0,
|
||||||
steeringPressed=False,
|
steeringPressed=False,
|
||||||
cruiseState=SimpleNamespace(available=True),
|
|
||||||
gearShifter=structs.CarState.GearShifter.drive,
|
gearShifter=structs.CarState.GearShifter.drive,
|
||||||
standstill=False,
|
standstill=False,
|
||||||
))
|
))
|
||||||
@@ -705,77 +705,21 @@ def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope():
|
|||||||
|
|
||||||
CS.out.steeringAngleDeg = -100.0
|
CS.out.steeringAngleDeg = -100.0
|
||||||
CS.out.steeringRateDeg = 0.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)
|
msg = controller.lateral_angle(CC, CS)
|
||||||
parser.update([(_ASCENT_AOL_ARM_FRAMES + 1, [msg])])
|
parser.update([(2, [msg])])
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||||
|
|
||||||
CS.out.gearShifter = structs.CarState.GearShifter.reverse
|
CS.out.gearShifter = structs.CarState.GearShifter.reverse
|
||||||
msg = controller.lateral_angle(CC, CS)
|
msg = controller.lateral_angle(CC, CS)
|
||||||
parser.update([(_ASCENT_AOL_ARM_FRAMES + 2, [msg])])
|
parser.update([(3, [msg])])
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||||
|
|
||||||
|
|
||||||
def test_ascent_aol_does_not_arm_before_cruise_main_is_available():
|
def test_lkas_hud_state_uses_lateral_active():
|
||||||
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)
|
update_source = inspect.getsource(CarController.update)
|
||||||
|
|
||||||
assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, self._lkas_status_active(CC)" in update_source
|
assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.latActive" in update_source
|
||||||
assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled" not in update_source
|
assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled" not in update_source
|
||||||
|
|
||||||
|
|
||||||
@@ -793,48 +737,3 @@ def test_lkas_hud_active_bit_follows_lateral_state(enabled, expected):
|
|||||||
|
|
||||||
assert parser.can_valid
|
assert parser.can_valid
|
||||||
assert parser.vl["ES_LKAS_State"]["LKAS_ACTIVE"] == expected
|
assert parser.vl["ES_LKAS_State"]["LKAS_ACTIVE"] == expected
|
||||||
|
|
||||||
|
|
||||||
def test_outback_manual_steering_releases_angle_request_before_lkas_fault():
|
|
||||||
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=-127.0,
|
|
||||||
steeringPressed=True,
|
|
||||||
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"] == 0
|
|
||||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
|
||||||
assert not 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))
|
|
||||||
|
|||||||
@@ -108,7 +108,6 @@ non_tested_cars = [
|
|||||||
TOYOTA.TOYOTA_RAV4H,
|
TOYOTA.TOYOTA_RAV4H,
|
||||||
|
|
||||||
# No recorded routes yet
|
# No recorded routes yet
|
||||||
VOLVO.VOLVO_V40,
|
|
||||||
VOLVO.VOLVO_XC40_RECHARGE,
|
VOLVO.VOLVO_XC40_RECHARGE,
|
||||||
VOLVO.VOLVO_S60_RECHARGE,
|
VOLVO.VOLVO_S60_RECHARGE,
|
||||||
VOLVO.POLESTAR_2,
|
VOLVO.POLESTAR_2,
|
||||||
|
|||||||
@@ -145,7 +145,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
|||||||
"PORSCHE_MACAN_MK1" = [2.0, 2.0, 0.2]
|
"PORSCHE_MACAN_MK1" = [2.0, 2.0, 0.2]
|
||||||
"VOLVO_XC40_RECHARGE" = [1.5, 1.5, 0.1]
|
"VOLVO_XC40_RECHARGE" = [1.5, 1.5, 0.1]
|
||||||
"VOLVO_S60_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
|
# Dashcam or fallback configured as ideal car
|
||||||
"MOCK" = [10.0, 10, 0.0]
|
"MOCK" = [10.0, 10, 0.0]
|
||||||
|
|||||||
@@ -40,8 +40,6 @@ 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_BLEND_SPEED = 5.0 # m/s
|
||||||
TOYOTA_RAV4_LAUNCH_PEDAL_SCALE = 0.11
|
TOYOTA_RAV4_LAUNCH_PEDAL_SCALE = 0.11
|
||||||
TOYOTA_RAV4_LOW_SPEED_PEDAL_SCALE = 0.23
|
TOYOTA_RAV4_LOW_SPEED_PEDAL_SCALE = 0.23
|
||||||
TOYOTA_AUTO_HOLD_ACCEL = -1.0
|
|
||||||
TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES = 100
|
|
||||||
|
|
||||||
# LKA limits
|
# LKA limits
|
||||||
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
|
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
|
||||||
@@ -311,24 +309,23 @@ class CarController(CarControllerBase):
|
|||||||
|
|
||||||
self.last_standstill = CS.out.standstill
|
self.last_standstill = CS.out.standstill
|
||||||
|
|
||||||
def update_auto_hold_state(self, CS: structs.CarState, cancel_requested: bool = False,
|
def create_auto_brake_hold_messages(self, CS: structs.CarState, brake_hold_allowed_timer: int = 100):
|
||||||
activation_frames: int = TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES):
|
can_sends = []
|
||||||
brake_hold_allowed = (not cancel_requested and CS.out.standstill and CS.out.cruiseState.available and
|
brake_hold_allowed = (CS.out.standstill and CS.out.cruiseState.available and
|
||||||
not CS.out.gasPressed and not CS.out.cruiseState.enabled and
|
not CS.out.gasPressed and not CS.out.cruiseState.enabled and
|
||||||
CS.out.gearShifter not in (PARK, REVERSE))
|
CS.out.gearShifter not in (PARK, REVERSE))
|
||||||
|
|
||||||
if brake_hold_allowed and not self.brake_hold_active and CS.out.brakePressed:
|
if brake_hold_allowed and not self.brake_hold_active and CS.out.brakePressed:
|
||||||
self._brake_hold_counter += 1
|
self._brake_hold_counter += 1
|
||||||
self.brake_hold_active = self._brake_hold_counter > activation_frames
|
self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer
|
||||||
elif not brake_hold_allowed:
|
elif not brake_hold_allowed:
|
||||||
self._brake_hold_counter = 0
|
self._brake_hold_counter = 0
|
||||||
self.brake_hold_active = False
|
self.brake_hold_active = False
|
||||||
|
|
||||||
return self.brake_hold_active
|
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))
|
||||||
|
|
||||||
def reset_auto_hold_state(self):
|
return can_sends
|
||||||
self._brake_hold_counter = 0
|
|
||||||
self.brake_hold_active = False
|
|
||||||
|
|
||||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||||
actuators = CC.actuators
|
actuators = CC.actuators
|
||||||
@@ -426,9 +423,10 @@ class CarController(CarControllerBase):
|
|||||||
|
|
||||||
self._update_standstill_request(CC, CS, actuators, starpilot_toggles)
|
self._update_standstill_request(CC, CS, actuators, starpilot_toggles)
|
||||||
if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)):
|
if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)):
|
||||||
self.update_auto_hold_state(CS, pcm_cancel_cmd)
|
can_sends.extend(self.create_auto_brake_hold_messages(CS))
|
||||||
else:
|
elif self.brake_hold_active:
|
||||||
self.reset_auto_hold_state()
|
self._brake_hold_counter = 0
|
||||||
|
self.brake_hold_active = False
|
||||||
|
|
||||||
interceptor_gas_cmd = self._compute_interceptor_gas_cmd(CC, CS)
|
interceptor_gas_cmd = self._compute_interceptor_gas_cmd(CC, CS)
|
||||||
|
|
||||||
@@ -536,11 +534,6 @@ class CarController(CarControllerBase):
|
|||||||
|
|
||||||
pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
|
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
|
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,
|
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,
|
CS.acc_type, fcw_alert, self.distance_button,
|
||||||
|
|||||||
@@ -90,6 +90,8 @@ class CarState(CarStateBase):
|
|||||||
self.has_can_filter = self.FPCP.flags & ToyotaStarPilotFlags.RADAR_CAN_FILTER.value
|
self.has_can_filter = self.FPCP.flags & ToyotaStarPilotFlags.RADAR_CAN_FILTER.value
|
||||||
self.has_SDSU = self.FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value
|
self.has_SDSU = self.FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value
|
||||||
self.has_ZSS = self.FPCP.flags & ToyotaStarPilotFlags.ZSS.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:
|
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
|
||||||
cp = can_parsers[Bus.pt]
|
cp = can_parsers[Bus.pt]
|
||||||
@@ -225,6 +227,9 @@ class CarState(CarStateBase):
|
|||||||
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
|
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
|
||||||
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
|
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:
|
if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR:
|
||||||
self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
|
self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
|
||||||
|
|
||||||
@@ -309,6 +314,9 @@ class CarState(CarStateBase):
|
|||||||
if CP.carFingerprint in DISTANCE_BUTTON_CAR:
|
if CP.carFingerprint in DISTANCE_BUTTON_CAR:
|
||||||
pt_messages.append(("PCM_CRUISE_4", 1))
|
pt_messages.append(("PCM_CRUISE_4", 1))
|
||||||
|
|
||||||
|
if CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value:
|
||||||
|
cam_messages.append(("PRE_COLLISION_2", 50))
|
||||||
|
|
||||||
return {
|
return {
|
||||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
||||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
|
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
|
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.GAS_INTERCEPTOR.value
|
||||||
|
|
||||||
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
|
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
|
||||||
if toyota_auto_hold and ret.openpilotLongitudinalControl and candidate in TOYOTA_AUTO_HOLD_CARS:
|
if toyota_auto_hold and candidate in TOYOTA_AUTO_HOLD_CARS:
|
||||||
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||||
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||||
|
|
||||||
if not ret.openpilotLongitudinalControl:
|
if not ret.openpilotLongitudinalControl:
|
||||||
|
|||||||
@@ -196,8 +196,7 @@ class TestToyotaInterfaces:
|
|||||||
params.put_bool("ToyotaAutoHold", True)
|
params.put_bool("ToyotaAutoHold", True)
|
||||||
car_params = CarInterface.get_params(
|
car_params = CarInterface.get_params(
|
||||||
candidate,
|
candidate,
|
||||||
{bus: ({0x2FF: 8} if candidate in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H) and bus == 0 else {})
|
{bus: {} for bus in range(8)},
|
||||||
for bus in range(8)},
|
|
||||||
[],
|
[],
|
||||||
alpha_long=False,
|
alpha_long=False,
|
||||||
is_release=False,
|
is_release=False,
|
||||||
@@ -208,13 +207,12 @@ class TestToyotaInterfaces:
|
|||||||
params.remove("ToyotaAutoHold")
|
params.remove("ToyotaAutoHold")
|
||||||
|
|
||||||
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||||
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
|
||||||
|
|
||||||
can_parsers = CarState.get_can_parsers(car_params)
|
can_parsers = CarState.get_can_parsers(car_params)
|
||||||
car_state = CarState(car_params, SimpleNamespace(flags=0))
|
car_state = CarState(car_params, SimpleNamespace(flags=0))
|
||||||
car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0))
|
car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0))
|
||||||
assert "PRE_COLLISION_2" not in can_parsers[Bus.cam].vl
|
assert "PRE_COLLISION_2" in can_parsers[Bus.cam].vl
|
||||||
|
|
||||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||||
def test_auto_hold_is_disabled_by_default(self, candidate):
|
def test_auto_hold_is_disabled_by_default(self, candidate):
|
||||||
@@ -231,7 +229,7 @@ class TestToyotaInterfaces:
|
|||||||
)
|
)
|
||||||
|
|
||||||
assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||||
|
|
||||||
def test_prius_openpilot_long_uses_hybrid_long_defaults(self):
|
def test_prius_openpilot_long_uses_hybrid_long_defaults(self):
|
||||||
car_params = CarInterface.get_params(
|
car_params = CarInterface.get_params(
|
||||||
@@ -746,8 +744,6 @@ class TestToyotaCarController:
|
|||||||
controller.standstill_req = standstill_req
|
controller.standstill_req = standstill_req
|
||||||
controller.last_standstill = last_standstill
|
controller.last_standstill = last_standstill
|
||||||
controller.accel = 0.0
|
controller.accel = 0.0
|
||||||
controller.brake_hold_active = False
|
|
||||||
controller._brake_hold_counter = 0
|
|
||||||
return controller
|
return controller
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
@@ -810,6 +806,9 @@ class TestToyotaCarController:
|
|||||||
def test_toyota_auto_hold_latches_after_brake_press_until_gas(self):
|
def test_toyota_auto_hold_latches_after_brake_press_until_gas(self):
|
||||||
controller = self._make_controller()
|
controller = self._make_controller()
|
||||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
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(
|
cs = SimpleNamespace(
|
||||||
out=SimpleNamespace(
|
out=SimpleNamespace(
|
||||||
@@ -819,22 +818,28 @@ class TestToyotaCarController:
|
|||||||
brakePressed=True,
|
brakePressed=True,
|
||||||
gearShifter=structs.CarState.GearShifter.drive,
|
gearShifter=structs.CarState.GearShifter.drive,
|
||||||
),
|
),
|
||||||
|
pre_collision_2={},
|
||||||
)
|
)
|
||||||
|
|
||||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||||
assert controller.brake_hold_active
|
assert controller.brake_hold_active
|
||||||
|
|
||||||
cs.out.brakePressed = False
|
cs.out.brakePressed = False
|
||||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
controller.frame = 2
|
||||||
|
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||||
assert controller.brake_hold_active
|
assert controller.brake_hold_active
|
||||||
|
|
||||||
cs.out.gasPressed = True
|
cs.out.gasPressed = True
|
||||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
controller.frame = 4
|
||||||
|
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||||
assert not controller.brake_hold_active
|
assert not controller.brake_hold_active
|
||||||
|
|
||||||
def test_toyota_auto_hold_does_not_trigger_without_brake_press(self):
|
def test_toyota_auto_hold_does_not_trigger_without_brake_press(self):
|
||||||
controller = self._make_controller()
|
controller = self._make_controller()
|
||||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
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(
|
cs = SimpleNamespace(
|
||||||
out=SimpleNamespace(
|
out=SimpleNamespace(
|
||||||
standstill=True,
|
standstill=True,
|
||||||
@@ -843,9 +848,10 @@ class TestToyotaCarController:
|
|||||||
brakePressed=False,
|
brakePressed=False,
|
||||||
gearShifter=structs.CarState.GearShifter.drive,
|
gearShifter=structs.CarState.GearShifter.drive,
|
||||||
),
|
),
|
||||||
|
pre_collision_2={},
|
||||||
)
|
)
|
||||||
|
|
||||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||||
assert not controller.brake_hold_active
|
assert not controller.brake_hold_active
|
||||||
|
|
||||||
def test_prius_resume_request_releases_standstill_latch(self):
|
def test_prius_resume_request_releases_standstill_latch(self):
|
||||||
@@ -985,9 +991,12 @@ class TestToyotaCarController:
|
|||||||
assert parser.can_valid
|
assert parser.can_valid
|
||||||
assert parser.vl["ACC_CONTROL"]["ALLOW_LONG_PRESS"] == 1
|
assert parser.vl["ACC_CONTROL"]["ALLOW_LONG_PRESS"] == 1
|
||||||
|
|
||||||
def test_auto_hold_uses_acc_control_brake_path(self):
|
def test_auto_brake_hold_sends_modified_pre_collision_after_timer(self):
|
||||||
controller = self._make_controller()
|
controller = self._make_controller()
|
||||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
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(
|
cs = SimpleNamespace(
|
||||||
out=SimpleNamespace(
|
out=SimpleNamespace(
|
||||||
standstill=True,
|
standstill=True,
|
||||||
@@ -996,19 +1005,16 @@ class TestToyotaCarController:
|
|||||||
brakePressed=True,
|
brakePressed=True,
|
||||||
gearShifter=structs.CarState.GearShifter.drive,
|
gearShifter=structs.CarState.GearShifter.drive,
|
||||||
),
|
),
|
||||||
|
pre_collision_2={},
|
||||||
)
|
)
|
||||||
|
|
||||||
controller.update_auto_hold_state(cs, activation_frames=0)
|
can_sends = controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=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], [("ACC_CONTROL", 0)], 0)
|
parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("PRE_COLLISION_2", 0)], 0)
|
||||||
parser.update([(1, can_sends)])
|
parser.update([(1, can_sends)])
|
||||||
assert controller.brake_hold_active
|
assert controller.brake_hold_active
|
||||||
assert parser.vl["ACC_CONTROL"]["ACCEL_CMD"] == -1.0
|
assert parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
|
||||||
assert parser.vl["ACC_CONTROL"]["PERMIT_BRAKING"] == 1
|
assert parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1
|
||||||
assert parser.vl["ACC_CONTROL"]["RELEASE_STANDSTILL"] == 0
|
|
||||||
|
|
||||||
def test_interceptor_stop_and_go_holds_small_launch_at_standstill(self):
|
def test_interceptor_stop_and_go_holds_small_launch_at_standstill(self):
|
||||||
controller = self._make_controller()
|
controller = self._make_controller()
|
||||||
|
|||||||
@@ -89,6 +89,38 @@ def create_pcs_commands(packer, accel, active, mass):
|
|||||||
return [msg1, msg2]
|
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):
|
def create_acc_cancel_command(packer):
|
||||||
values = {
|
values = {
|
||||||
"GAS_RELEASED": 0,
|
"GAS_RELEASED": 0,
|
||||||
|
|||||||
@@ -243,8 +243,6 @@ VOLKSWAGEN_MQB_MEB_CONSTANTS: dict[int, list[int]] = {
|
|||||||
0xC5, 0x91, 0x0F, 0x27, 0x34, 0x04, 0x7F, 0x02], # EA_02
|
0xC5, 0x91, 0x0F, 0x27, 0x34, 0x04, 0x7F, 0x02], # EA_02
|
||||||
0x20A: [0x9D, 0xE8, 0x36, 0xA1, 0xCA, 0x3B, 0x1D, 0x33,
|
0x20A: [0x9D, 0xE8, 0x36, 0xA1, 0xCA, 0x3B, 0x1D, 0x33,
|
||||||
0xE0, 0xD5, 0xBB, 0x5F, 0xAE, 0x3C, 0x31, 0x9F], # EML_06
|
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,
|
0x26B: [0xCE, 0xCC, 0xBD, 0x69, 0xA1, 0x3C, 0x18, 0x76,
|
||||||
0x0F, 0x04, 0xF2, 0x3A, 0x93, 0x24, 0x19, 0x51], # TA_01
|
0x0F, 0x04, 0xF2, 0x3A, 0x93, 0x24, 0x19, 0x51], # TA_01
|
||||||
0x30C: [0x0F] * 16, # ACC_02
|
0x30C: [0x0F] * 16, # ACC_02
|
||||||
|
|||||||
@@ -1,16 +1,10 @@
|
|||||||
import random
|
import random
|
||||||
import re
|
import re
|
||||||
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from opendbc.can.packer import CANPacker
|
|
||||||
from opendbc.car import Bus
|
|
||||||
from opendbc.car.structs import CarParams
|
from opendbc.car.structs import CarParams
|
||||||
from opendbc.car.volkswagen.interface import CarInterface
|
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.fingerprints import FW_VERSIONS
|
||||||
from opendbc.car.volkswagen.mqbcan import 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
|
Ecu = CarParams.Ecu
|
||||||
|
|
||||||
@@ -66,35 +60,6 @@ class TestVolkswagenPlatformConfigs:
|
|||||||
assert not cp.pcmCruise
|
assert not cp.pcmCruise
|
||||||
assert cp.safetyConfigs[-1].safetyParam & VolkswagenSafetyFlags.LONG_CONTROL
|
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]
|
|
||||||
|
|
||||||
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):
|
def test_taos_longitudinal_actuator_delay(self):
|
||||||
taos_cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_TAOS_MK1)
|
taos_cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_TAOS_MK1)
|
||||||
golf_cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_GOLF_MK7)
|
golf_cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_GOLF_MK7)
|
||||||
|
|||||||
@@ -1,5 +1,3 @@
|
|||||||
from collections import deque
|
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
from opendbc.can.packer import CANPacker
|
from opendbc.can.packer import CANPacker
|
||||||
@@ -7,20 +5,16 @@ from opendbc.car import Bus
|
|||||||
from opendbc.car.interfaces import CarControllerBase
|
from opendbc.car.interfaces import CarControllerBase
|
||||||
from opendbc.car.lateral import apply_std_steer_angle_limits
|
from opendbc.car.lateral import apply_std_steer_angle_limits
|
||||||
from opendbc.car.volvo.helpers import LCA3CounterSync
|
from opendbc.car.volvo.helpers import LCA3CounterSync
|
||||||
from opendbc.car.volvo.volvocan import (create_c1_cancel, create_c1_pscm_message, create_c1_steering_control, create_lca_message,
|
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_pscm_message, create_lca_3_message, create_lca_2_message, create_lca_4_message, create_lca_5_message,
|
create_lca_5_message, create_lca_6_message, create_lca_7_message, create_pscm_related_message)
|
||||||
create_lca_6_message, create_lca_7_message, create_pscm_related_message)
|
from opendbc.car.volvo.values import CarControllerParams
|
||||||
from opendbc.car.volvo.values import CAR, CarControllerParams, VolvoC1PlatformConfig
|
|
||||||
|
|
||||||
|
|
||||||
class CarController(CarControllerBase):
|
class CarController(CarControllerBase):
|
||||||
def __init__(self, dbc_names, CP):
|
def __init__(self, dbc_names, CP):
|
||||||
super().__init__(dbc_names, CP)
|
super().__init__(dbc_names, CP)
|
||||||
self.is_c1 = isinstance(CAR(CP.carFingerprint).config, VolvoC1PlatformConfig)
|
self.packer = CANPacker(dbc_names[Bus.party])
|
||||||
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.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.gear_acc = 60
|
||||||
self.lca_4_acc = 0 # Bresenham accumulator for 29 Hz
|
self.lca_4_acc = 0 # Bresenham accumulator for 29 Hz
|
||||||
@@ -68,9 +62,6 @@ class CarController(CarControllerBase):
|
|||||||
self.lca_auth_drv_mag_filt = 0.0
|
self.lca_auth_drv_mag_filt = 0.0
|
||||||
|
|
||||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||||
if self.is_c1:
|
|
||||||
return self._update_c1(CC, CS)
|
|
||||||
|
|
||||||
can_sends = []
|
can_sends = []
|
||||||
actuators = CC.actuators
|
actuators = CC.actuators
|
||||||
|
|
||||||
@@ -294,50 +285,3 @@ class CarController(CarControllerBase):
|
|||||||
self.frame += 1
|
self.frame += 1
|
||||||
self.last_lat_active = CC.latActive
|
self.last_lat_active = CC.latActive
|
||||||
return new_actuators, can_sends
|
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,9 +1,8 @@
|
|||||||
from cereal import custom
|
from cereal import custom
|
||||||
from opendbc.car import Bus, ButtonType, create_button_events, structs
|
from opendbc.car import structs, Bus
|
||||||
from opendbc.can.parser import CANParser
|
from opendbc.can.parser import CANParser
|
||||||
from opendbc.car.common.conversions import Conversions as CV
|
from opendbc.car.volvo.values import DBC, VolvoSPAPlatformConfig, CAR
|
||||||
from opendbc.car.interfaces import CarStateBase
|
from opendbc.car.interfaces import CarStateBase
|
||||||
from opendbc.car.volvo.values import CAR, DBC, VolvoC1PlatformConfig, VolvoSPAPlatformConfig
|
|
||||||
|
|
||||||
GearShifter = structs.CarState.GearShifter
|
GearShifter = structs.CarState.GearShifter
|
||||||
TransmissionType = structs.CarParams.TransmissionType
|
TransmissionType = structs.CarParams.TransmissionType
|
||||||
@@ -17,7 +16,6 @@ STEERING_PRESSED_THRESHOLD = 2
|
|||||||
class CarState(CarStateBase):
|
class CarState(CarStateBase):
|
||||||
def __init__(self, CP, FPCP):
|
def __init__(self, CP, FPCP):
|
||||||
super().__init__(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.is_spa = isinstance(CAR(CP.carFingerprint).config, VolvoSPAPlatformConfig)
|
||||||
self.gas_pressed_prev = False
|
self.gas_pressed_prev = False
|
||||||
self.dispatch_lca_2_msg = False
|
self.dispatch_lca_2_msg = False
|
||||||
@@ -36,22 +34,8 @@ class CarState(CarStateBase):
|
|||||||
self.msg_lca_4 = {}
|
self.msg_lca_4 = {}
|
||||||
self.msg_lca_6 = {}
|
self.msg_lca_6 = {}
|
||||||
self.msg_lca_7 = {}
|
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:
|
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_main = can_parsers[Bus.main]
|
||||||
cp_pt = can_parsers[Bus.pt]
|
cp_pt = can_parsers[Bus.pt]
|
||||||
cp_party = can_parsers[Bus.party]
|
cp_party = can_parsers[Bus.party]
|
||||||
@@ -153,83 +137,8 @@ class CarState(CarStateBase):
|
|||||||
fp_ret = custom.StarPilotCarState.new_message()
|
fp_ret = custom.StarPilotCarState.new_message()
|
||||||
return ret, fp_ret
|
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
|
@staticmethod
|
||||||
def get_can_parsers(CP):
|
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 {
|
return {
|
||||||
Bus.main: CANParser(DBC[CP.carFingerprint][Bus.main], [], 0),
|
Bus.main: CANParser(DBC[CP.carFingerprint][Bus.main], [], 0),
|
||||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1),
|
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1),
|
||||||
|
|||||||
@@ -3,14 +3,6 @@
|
|||||||
from opendbc.car.volvo.values import CAR
|
from opendbc.car.volvo.values import CAR
|
||||||
|
|
||||||
FINGERPRINTS = {
|
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: [{
|
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
|
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,10 +2,11 @@ from opendbc.car import structs, get_safety_config
|
|||||||
from opendbc.car.interfaces import CarInterfaceBase
|
from opendbc.car.interfaces import CarInterfaceBase
|
||||||
from opendbc.car.volvo.carcontroller import CarController
|
from opendbc.car.volvo.carcontroller import CarController
|
||||||
from opendbc.car.volvo.carstate import CarState
|
from opendbc.car.volvo.carstate import CarState
|
||||||
from opendbc.car.volvo.values import CAR, VolvoC1PlatformConfig, VolvoSafetyFlags, VolvoSPAPlatformConfig
|
from opendbc.car.volvo.values import VolvoSPAPlatformConfig, CAR
|
||||||
|
|
||||||
TransmissionType = structs.CarParams.TransmissionType
|
TransmissionType = structs.CarParams.TransmissionType
|
||||||
|
|
||||||
|
VOLVO_FLAG_SPA = 1
|
||||||
SAFETY_VOLVO = structs.CarParams.SafetyModel.volvo
|
SAFETY_VOLVO = structs.CarParams.SafetyModel.volvo
|
||||||
|
|
||||||
|
|
||||||
@@ -17,20 +18,17 @@ class CarInterface(CarInterfaceBase):
|
|||||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||||
ret.brand = 'volvo'
|
ret.brand = 'volvo'
|
||||||
|
|
||||||
platform = CAR(candidate).config
|
|
||||||
safety_param = 0
|
safety_param = 0
|
||||||
if isinstance(platform, VolvoSPAPlatformConfig):
|
if isinstance(CAR(candidate).config, VolvoSPAPlatformConfig):
|
||||||
safety_param = VolvoSafetyFlags.SPA.value
|
safety_param = VOLVO_FLAG_SPA
|
||||||
elif isinstance(platform, VolvoC1PlatformConfig):
|
|
||||||
safety_param = VolvoSafetyFlags.C1.value
|
|
||||||
ret.safetyConfigs = [get_safety_config(SAFETY_VOLVO, safety_param)]
|
ret.safetyConfigs = [get_safety_config(SAFETY_VOLVO, safety_param)]
|
||||||
#ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput)]
|
#ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput)]
|
||||||
|
|
||||||
ret.dashcamOnly = False
|
ret.dashcamOnly = False
|
||||||
|
|
||||||
ret.steerActuatorDelay = 0.2 if isinstance(platform, VolvoC1PlatformConfig) else 0.3
|
ret.steerActuatorDelay = 0.3
|
||||||
ret.steerLimitTimer = 0.1
|
ret.steerLimitTimer = 0.1
|
||||||
ret.steerAtStandstill = not isinstance(platform, VolvoC1PlatformConfig)
|
ret.steerAtStandstill = True
|
||||||
|
|
||||||
# Use angle-based steering control for Volvo CMA platform
|
# Use angle-based steering control for Volvo CMA platform
|
||||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||||
@@ -41,7 +39,4 @@ class CarInterface(CarInterfaceBase):
|
|||||||
|
|
||||||
ret.pcmCruise = True
|
ret.pcmCruise = True
|
||||||
|
|
||||||
if isinstance(platform, VolvoC1PlatformConfig):
|
|
||||||
ret.transmissionType = TransmissionType.automatic
|
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|||||||
@@ -1,57 +0,0 @@
|
|||||||
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,8 +4,7 @@ from types import SimpleNamespace
|
|||||||
from opendbc.car.volvo.carcontroller import CarController
|
from opendbc.car.volvo.carcontroller import CarController
|
||||||
from opendbc.car.volvo.helpers import checksum_lca_5_message
|
from opendbc.car.volvo.helpers import checksum_lca_5_message
|
||||||
from opendbc.car.volvo.interface import CarInterface
|
from opendbc.car.volvo.interface import CarInterface
|
||||||
from opendbc.car.volvo.values import CAR, DBC
|
from opendbc.car.volvo.values import DBC
|
||||||
from opendbc.car.volvo.volvocan import create_c1_checksum
|
|
||||||
|
|
||||||
|
|
||||||
def _zero_message():
|
def _zero_message():
|
||||||
@@ -68,70 +67,3 @@ def test_controller_relays_stock_lca5_angle_when_inactive():
|
|||||||
if raw & (1 << 14):
|
if raw & (1 << 14):
|
||||||
raw -= 1 << 15
|
raw -= 1 << 15
|
||||||
assert abs(raw * 0.05596 - 12.0) < 0.1
|
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,23 +1,13 @@
|
|||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
from enum import IntFlag
|
|
||||||
|
|
||||||
from opendbc.car.structs import CarParams
|
from opendbc.car.structs import CarParams
|
||||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
|
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
|
||||||
from opendbc.car.lateral import AngleSteeringLimits
|
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.docs_definitions import CarDocs, CarHarness, CarParts
|
||||||
from opendbc.car.fw_query_definitions import FwQueryConfig
|
from opendbc.car.fw_query_definitions import FwQueryConfig
|
||||||
|
|
||||||
Ecu = CarParams.Ecu
|
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:
|
class CarControllerParams:
|
||||||
STEER_STEP = 1 # 100 Hz LCA command frequency (controlsd runs at 100 Hz)
|
STEER_STEP = 1 # 100 Hz LCA command frequency (controlsd runs at 100 Hz)
|
||||||
@@ -91,19 +81,6 @@ class CarControllerParams:
|
|||||||
([0., 5., 25.], [5., 2., .3]), # rate down limits at different speeds
|
([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
|
@dataclass
|
||||||
class VolvoCarDocs(CarDocs):
|
class VolvoCarDocs(CarDocs):
|
||||||
@@ -128,26 +105,7 @@ 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):
|
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(
|
VOLVO_XC40_RECHARGE = VolvoCMAPlatformConfig(
|
||||||
[VolvoCarDocs("Volvo XC40 Recharge 2021-23")],
|
[VolvoCarDocs("Volvo XC40 Recharge 2021-23")],
|
||||||
CarSpecs(
|
CarSpecs(
|
||||||
|
|||||||
@@ -2,47 +2,6 @@ from opendbc.car.volvo.helpers import (checksum_lca_2_message, checksum_2_0x69_m
|
|||||||
checksum_2_pscm_related_message, checksum_lca_5_message)
|
checksum_2_pscm_related_message, checksum_lca_5_message)
|
||||||
from opendbc.car.carlog import carlog
|
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,
|
def create_lca_message(packer, lat_active: bool, apply_angle: float, msg_lca: dict,
|
||||||
authority_pos: int = 614, authority_neg: int = -614,
|
authority_pos: int = 614, authority_neg: int = -614,
|
||||||
overrides: dict | None = None):
|
overrides: dict | None = None):
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -1497,7 +1497,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_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_ 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_ LDA_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
||||||
SG_ RAY_LKAS_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
SG_ RAY_LKAS_BTN : 0|2@1+ (1,0) [0|3] "" XXX
|
||||||
|
|
||||||
BO_ 1426 LABEL11: 8 XXX
|
BO_ 1426 LABEL11: 8 XXX
|
||||||
SG_ CC_React : 34|1@1+ (1,0) [0|1] "" XXX
|
SG_ CC_React : 34|1@1+ (1,0) [0|1] "" XXX
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -11,4 +11,3 @@ class ALTERNATIVE_EXPERIENCE:
|
|||||||
|
|
||||||
ALWAYS_ON_LATERAL = 32
|
ALWAYS_ON_LATERAL = 32
|
||||||
GM_REMAP_CANCEL_TO_DISTANCE = 64
|
GM_REMAP_CANCEL_TO_DISTANCE = 64
|
||||||
TOYOTA_AUTO_HOLD = 128
|
|
||||||
|
|||||||
@@ -339,7 +339,6 @@ extern bool gm_remote_start_boots_comma;
|
|||||||
|
|
||||||
#define ALT_EXP_ALWAYS_ON_LATERAL 32
|
#define ALT_EXP_ALWAYS_ON_LATERAL 32
|
||||||
#define ALT_EXP_GM_REMAP_CANCEL_TO_DISTANCE 64
|
#define ALT_EXP_GM_REMAP_CANCEL_TO_DISTANCE 64
|
||||||
#define ALT_EXP_TOYOTA_AUTO_HOLD 128
|
|
||||||
|
|
||||||
extern int alternative_experience;
|
extern int alternative_experience;
|
||||||
|
|
||||||
|
|||||||
@@ -60,7 +60,6 @@ static bool gm_panda_3d1_sched = false;
|
|||||||
static bool gm_panda_paddle_sched = false;
|
static bool gm_panda_paddle_sched = false;
|
||||||
static bool gm_bolt_2022_pedal = false;
|
static bool gm_bolt_2022_pedal = false;
|
||||||
static bool gm_alt_brake = 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_auto_hold = false;
|
||||||
static bool gm_volt_one_pedal = false;
|
static bool gm_volt_one_pedal = false;
|
||||||
|
|
||||||
@@ -262,8 +261,7 @@ static void gm_rx_hook(const CANPacket_t *msg) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
if ((msg->addr == 0xF1U) && gm_alt_brake) {
|
if ((msg->addr == 0xF1U) && gm_alt_brake) {
|
||||||
const uint8_t brake_threshold = gm_volt_cc_gateway ? 21U : 6U;
|
brake_pressed = msg->data[1] >= 6U;
|
||||||
brake_pressed = msg->data[1] >= brake_threshold;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM) && !gm_force_brake_c9) {
|
if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM) && !gm_force_brake_c9) {
|
||||||
@@ -722,7 +720,7 @@ static safety_config gm_init(uint16_t param) {
|
|||||||
gm_cc_long = GET_FLAG(param, GM_PARAM_CC_LONG);
|
gm_cc_long = GET_FLAG(param, GM_PARAM_CC_LONG);
|
||||||
gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC);
|
gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC);
|
||||||
gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG);
|
gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG);
|
||||||
gm_volt_cc_gateway = GET_FLAG(param, GM_PARAM_VOLT_CC_GATEWAY) && gm_no_camera && !gm_pedal_long && !gm_has_acc;
|
const bool 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);
|
enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR);
|
||||||
gm_force_ascm = GET_FLAG(param, GM_PARAM_HW_ASCM_LONG);
|
gm_force_ascm = GET_FLAG(param, GM_PARAM_HW_ASCM_LONG);
|
||||||
gm_force_brake_c9 = GET_FLAG(param, GM_PARAM_FORCE_BRAKE_C9);
|
gm_force_brake_c9 = GET_FLAG(param, GM_PARAM_FORCE_BRAKE_C9);
|
||||||
|
|||||||
@@ -2,8 +2,6 @@
|
|||||||
|
|
||||||
#include "opendbc/safety/declarations.h"
|
#include "opendbc/safety/declarations.h"
|
||||||
|
|
||||||
#define TOYOTA_AUTO_HOLD_ACCEL -1000 // -1.0 m/s^2 in ACC_CONTROL units
|
|
||||||
|
|
||||||
// Stock longitudinal
|
// Stock longitudinal
|
||||||
#define TOYOTA_BASE_TX_MSGS \
|
#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 */ \
|
{0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false}, /* LKAS + LTA + PCM cancel cmd */ \
|
||||||
@@ -279,15 +277,7 @@ 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
|
// 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 = 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
|
// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
|
||||||
if (toyota_stock_longitudinal) {
|
if (toyota_stock_longitudinal) {
|
||||||
@@ -404,7 +394,12 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
|||||||
tx = false;
|
tx = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((msg->addr == 0x344U) && toyota_stock_longitudinal) {
|
// 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) {
|
||||||
tx = false;
|
tx = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -571,11 +566,21 @@ static safety_config toyota_init(uint16_t param) {
|
|||||||
return ret;
|
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 = {
|
const safety_hooks toyota_hooks = {
|
||||||
.init = toyota_init,
|
.init = toyota_init,
|
||||||
.rx = toyota_rx_hook,
|
.rx = toyota_rx_hook,
|
||||||
.rx_all = toyota_rx_all_hook,
|
.rx_all = toyota_rx_all_hook,
|
||||||
.tx = toyota_tx_hook,
|
.tx = toyota_tx_hook,
|
||||||
|
.fwd = toyota_fwd_hook,
|
||||||
.get_checksum = toyota_get_checksum,
|
.get_checksum = toyota_get_checksum,
|
||||||
.compute_checksum = toyota_compute_checksum,
|
.compute_checksum = toyota_compute_checksum,
|
||||||
.get_quality_flag_valid = toyota_get_quality_flag_valid,
|
.get_quality_flag_valid = toyota_get_quality_flag_valid,
|
||||||
|
|||||||
@@ -2,11 +2,9 @@
|
|||||||
|
|
||||||
#include "opendbc/safety/declarations.h"
|
#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
|
// Polestar 2 is technically CMA, but appears to use SPA DBC for CAN 1 bus
|
||||||
#define VOLVO_FLAG_SPA 1U
|
#define VOLVO_FLAG_SPA 1U
|
||||||
#define VOLVO_FLAG_C1 2U
|
|
||||||
|
|
||||||
// Volvo CAN message addresses shared between CMA and SPA
|
// Volvo CAN message addresses shared between CMA and SPA
|
||||||
#define VOLVO_LCA_STEER 0x58U // TX from VCU1 to PSCM, LCA steering command (0x58)
|
#define VOLVO_LCA_STEER 0x58U // TX from VCU1 to PSCM, LCA steering command (0x58)
|
||||||
@@ -26,15 +24,6 @@
|
|||||||
#define VOLVO_LCA_6 0x97U // TX LCA_6 message
|
#define VOLVO_LCA_6 0x97U // TX LCA_6 message
|
||||||
#define VOLVO_LCA_7 0x92U // TX LCA_7 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
|
// CMA-specific PT bus addresses
|
||||||
#define VOLVO_CMA_BUS1_SPEED 0x70U // RX vehicle speed
|
#define VOLVO_CMA_BUS1_SPEED 0x70U // RX vehicle speed
|
||||||
#define VOLVO_CMA_ECM_1 0x250U // RX accelerator pedal position
|
#define VOLVO_CMA_ECM_1 0x250U // RX accelerator pedal position
|
||||||
@@ -55,10 +44,6 @@
|
|||||||
#define VOLVO_MAX_ANGLE_CAN 9650
|
#define VOLVO_MAX_ANGLE_CAN 9650
|
||||||
#define VOLVO_RELAY_ANGLE_TOLERANCE 54 // approximately 3 degrees
|
#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
|
// CAN bus definitions for Volvo
|
||||||
// Using same naming as carstate.py for consistency: main, pt, party
|
// Using same naming as carstate.py for consistency: main, pt, party
|
||||||
@@ -69,7 +54,6 @@
|
|||||||
// Runtime addresses set by volvo_init based on safetyParam
|
// Runtime addresses set by volvo_init based on safetyParam
|
||||||
static uint16_t volvo_ecm_1_addr;
|
static uint16_t volvo_ecm_1_addr;
|
||||||
static uint16_t volvo_bus1_cruise_control_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) {
|
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]);
|
return (int)(((uint16_t)(msg->data[byte] & 0x7FU) << 8U) | msg->data[byte + 1U]);
|
||||||
@@ -83,21 +67,6 @@ static int volvo_lca_5_angle(const CANPacket_t *msg) {
|
|||||||
return to_signed(volvo_be_15(msg, 6U), 15);
|
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 = {
|
static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = {
|
||||||
.max_angle = VOLVO_MAX_ANGLE_CAN,
|
.max_angle = VOLVO_MAX_ANGLE_CAN,
|
||||||
.angle_deg_to_can = VOLVO_ANGLE_DEG_TO_CAN,
|
.angle_deg_to_can = VOLVO_ANGLE_DEG_TO_CAN,
|
||||||
@@ -112,51 +81,8 @@ static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = {
|
|||||||
.frequency = 50U,
|
.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) {
|
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
|
// Main bus (bus 0) messages
|
||||||
if (msg->bus == VOLVO_MAIN_BUS) {
|
if (msg->bus == VOLVO_MAIN_BUS) {
|
||||||
// Update brake pedal and cruise state from BCM2
|
// Update brake pedal and cruise state from BCM2
|
||||||
@@ -232,33 +158,6 @@ static void volvo_rx_hook(const CANPacket_t *msg) {
|
|||||||
static bool volvo_tx_hook(const CANPacket_t *msg) {
|
static bool volvo_tx_hook(const CANPacket_t *msg) {
|
||||||
bool tx = true;
|
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_5 carries the actual angle command used by the controller. The stock
|
||||||
// LCA frame also contains an angle-shaped field, but the imported controller
|
// LCA frame also contains an angle-shaped field, but the imported controller
|
||||||
// deliberately leaves that field at the observed vehicle value.
|
// deliberately leaves that field at the observed vehicle value.
|
||||||
@@ -356,22 +255,6 @@ static bool volvo_tx_hook(const CANPacket_t *msg) {
|
|||||||
|
|
||||||
static safety_config volvo_init(uint16_t param) {
|
static safety_config volvo_init(uint16_t param) {
|
||||||
bool spa = GET_FLAG(param, VOLVO_FLAG_SPA);
|
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
|
// Set PT bus addresses based on platform
|
||||||
volvo_ecm_1_addr = spa ? VOLVO_SPA_ECM_1 : VOLVO_CMA_ECM_1;
|
volvo_ecm_1_addr = spa ? VOLVO_SPA_ECM_1 : VOLVO_CMA_ECM_1;
|
||||||
|
|||||||
@@ -654,31 +654,6 @@ class TestGmCcLongitudinalNoCameraSafety(TestGmCcLongitudinalSafety):
|
|||||||
self.safety.init_tests()
|
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):
|
class TestGmCcLongitudinalPandaSchedSafety(TestGmCcLongitudinalSafety):
|
||||||
FWD_BLACKLISTED_ADDRS = {2: [0x180, 0x370], 0: [0x184, 0x3D1]}
|
FWD_BLACKLISTED_ADDRS = {2: [0x180, 0x370], 0: [0x184, 0x3D1]}
|
||||||
INTERCEPTOR_GAS_PRESSED = 596
|
INTERCEPTOR_GAS_PRESSED = 596
|
||||||
|
|||||||
@@ -97,55 +97,29 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT
|
|||||||
msg = libsafety_py.make_CANPacket(0x283, 0, bytes(dat))
|
msg = libsafety_py.make_CANPacket(0x283, 0, bytes(dat))
|
||||||
self.assertEqual(not bad and not stock_longitudinal, self._tx(msg))
|
self.assertEqual(not bad and not stock_longitudinal, self._tx(msg))
|
||||||
|
|
||||||
def test_auto_hold_acc_control_is_narrowly_allowed_only_at_standstill(self):
|
def test_auto_brake_hold_aeb_replacement_only_at_standstill(self):
|
||||||
if (not self.LONGITUDINAL or
|
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALLOW_AEB)
|
||||||
self.safety.get_current_safety_param() & (ToyotaSafetyFlags.STOCK_LONGITUDINAL.value | ToyotaSafetyFlags.SECOC.value)):
|
hold_msg = libsafety_py.make_CANPacket(0x344, 0, b"\xfd\x80\x00\x00\x00\x00\x00\xcc")
|
||||||
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._speed_msg(0))
|
||||||
self._rx(self._toggle_aol(True))
|
self._rx(self._toggle_aol(True))
|
||||||
self._rx(self._user_gas_msg(False))
|
self._rx(self._user_gas_msg(False))
|
||||||
self.safety.set_controls_allowed(False)
|
|
||||||
self.assertTrue(self._tx(hold_msg))
|
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._rx(self._speed_msg(1.0))
|
||||||
self.assertFalse(self._tx(hold_msg))
|
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._speed_msg(0))
|
||||||
self._rx(self._user_gas_msg(True))
|
self._rx(self._user_gas_msg(True))
|
||||||
self.assertFalse(self._tx(hold_msg))
|
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._user_gas_msg(False))
|
||||||
self._rx(self._toggle_aol(False))
|
self._rx(self._toggle_aol(False))
|
||||||
self.assertFalse(self._tx(hold_msg))
|
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
|
# Only allow LTA msgs with no actuation
|
||||||
def test_lta_steer_cmd(self):
|
def test_lta_steer_cmd(self):
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""
|
"""
|
||||||
Safety tests for Volvo C1/CMA/SPA.
|
Safety tests for Volvo CMA/SPA.
|
||||||
|
|
||||||
The safety mode lives at ``opendbc/safety/modes/volvo.h`` and is parameterized
|
The safety mode lives at ``opendbc/safety/modes/volvo.h`` and is parameterized
|
||||||
by ``safetyParam``:
|
by ``safetyParam``:
|
||||||
@@ -8,13 +8,14 @@ by ``safetyParam``:
|
|||||||
- ``safetyParam == 0`` → CMA platform (Volvo XC40 Recharge)
|
- ``safetyParam == 0`` → CMA platform (Volvo XC40 Recharge)
|
||||||
- ``safetyParam == VOLVO_FLAG_SPA`` → SPA platform (Volvo S60 Recharge,
|
- ``safetyParam == VOLVO_FLAG_SPA`` → SPA platform (Volvo S60 Recharge,
|
||||||
Polestar 2)
|
Polestar 2)
|
||||||
- ``safetyParam == VOLVO_FLAG_C1`` → C1 platform (Volvo V40)
|
|
||||||
|
|
||||||
CMA and SPA share LCA/PSCM/etc. addresses on the main and party buses but use
|
The two platforms share LCA/PSCM/etc. addresses on the main and party buses
|
||||||
different PT-bus addresses and signal scales. C1 uses the V40's legacy CAN
|
but use *different* PT-bus addresses and signal scales for ECM_1 and
|
||||||
layout and its own safety allowlist. The tests exercise all three through the
|
BUS1_CRUISE_CONTROL. Vehicle speed is read from main-bus SPEED on both, so it
|
||||||
generic ``CarSafetyTest`` harness so divergence between ``carstate.py`` and
|
is not platform-dependent. This test file exercises both platforms through the
|
||||||
``volvo.h`` is caught before running in a car.
|
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.
|
||||||
|
|
||||||
Companion to: ``opendbc/car/volvo/carstate.py`` (must agree on thresholds).
|
Companion to: ``opendbc/car/volvo/carstate.py`` (must agree on thresholds).
|
||||||
"""
|
"""
|
||||||
@@ -24,16 +25,13 @@ import re
|
|||||||
import unittest
|
import unittest
|
||||||
|
|
||||||
from opendbc.car.volvo.interface import SAFETY_VOLVO
|
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
|
from opendbc.safety.tests.libsafety import libsafety_py
|
||||||
import opendbc.safety.tests.common as common
|
import opendbc.safety.tests.common as common
|
||||||
from opendbc.safety.tests.common import CANPackerSafety
|
from opendbc.safety.tests.common import CANPackerSafety
|
||||||
|
|
||||||
|
|
||||||
# Must match the flags in opendbc/safety/modes/volvo.h
|
# Must match VOLVO_FLAG_SPA in opendbc/safety/modes/volvo.h
|
||||||
VOLVO_FLAG_SPA = VolvoSafetyFlags.SPA.value
|
VOLVO_FLAG_SPA = 1
|
||||||
VOLVO_FLAG_C1 = VolvoSafetyFlags.C1.value
|
|
||||||
|
|
||||||
# Must match VOLVO_SPEED_TO_MS in volvo.h and SPEED_TO_MS in carstate.py
|
# Must match VOLVO_SPEED_TO_MS in volvo.h and SPEED_TO_MS in carstate.py
|
||||||
VOLVO_SPEED_TO_MS = 0.003977
|
VOLVO_SPEED_TO_MS = 0.003977
|
||||||
@@ -334,124 +332,5 @@ class TestVolvoSPA(TestVolvoSafetyBase):
|
|||||||
"BUS1_CRUISE_CONTROL", VOLVO_PT_BUS, values)
|
"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__":
|
if __name__ == "__main__":
|
||||||
unittest.main()
|
unittest.main()
|
||||||
|
|||||||
@@ -181,11 +181,6 @@ 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_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_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
|
# panda jungle fw
|
||||||
flags = [
|
flags = [
|
||||||
"-DPANDA_JUNGLE",
|
"-DPANDA_JUNGLE",
|
||||||
|
|||||||
@@ -2,18 +2,18 @@
|
|||||||
|
|
||||||
bool bootkick_reset_triggered = false;
|
bool bootkick_reset_triggered = false;
|
||||||
|
|
||||||
void bootkick_tick(bool ignition, bool recent_heartbeat, bool wake) {
|
void bootkick_tick(bool ignition, bool recent_heartbeat) {
|
||||||
static uint16_t bootkick_last_serial_ptr = 0;
|
static uint16_t bootkick_last_serial_ptr = 0;
|
||||||
static uint8_t waiting_to_boot_countdown = 0;
|
static uint8_t waiting_to_boot_countdown = 0;
|
||||||
static uint8_t boot_reset_countdown = 0;
|
static uint8_t boot_reset_countdown = 0;
|
||||||
static uint8_t bootkick_harness_status_prev = HARNESS_STATUS_NC;
|
static uint8_t bootkick_harness_status_prev = HARNESS_STATUS_NC;
|
||||||
static bool bootkick_ign_prev = false;
|
static bool bootkick_ign_prev = false;
|
||||||
static bool bootkick_wake_prev = false;
|
|
||||||
static BootState boot_state = BOOT_BOOTKICK;
|
static BootState boot_state = BOOT_BOOTKICK;
|
||||||
BootState boot_state_prev = boot_state;
|
BootState boot_state_prev = boot_state;
|
||||||
const bool harness_inserted = (harness.status != bootkick_harness_status_prev) && (harness.status != HARNESS_STATUS_NC);
|
const bool harness_inserted = (harness.status != bootkick_harness_status_prev) && (harness.status != HARNESS_STATUS_NC);
|
||||||
|
|
||||||
if ((ignition && !bootkick_ign_prev) || harness_inserted || (wake && !bootkick_wake_prev && !ignition)) {
|
if ((ignition && !bootkick_ign_prev) || harness_inserted) {
|
||||||
|
// bootkick on rising edge of ignition or harness insertion
|
||||||
boot_state = BOOT_BOOTKICK;
|
boot_state = BOOT_BOOTKICK;
|
||||||
} else if (recent_heartbeat) {
|
} else if (recent_heartbeat) {
|
||||||
// disable bootkick once openpilot is up
|
// disable bootkick once openpilot is up
|
||||||
@@ -56,7 +56,6 @@ void bootkick_tick(bool ignition, bool recent_heartbeat, bool wake) {
|
|||||||
|
|
||||||
// update state
|
// update state
|
||||||
bootkick_ign_prev = ignition;
|
bootkick_ign_prev = ignition;
|
||||||
bootkick_wake_prev = wake;
|
|
||||||
bootkick_harness_status_prev = harness.status;
|
bootkick_harness_status_prev = harness.status;
|
||||||
bootkick_last_serial_ptr = uart_ring_som_debug.w_ptr_tx;
|
bootkick_last_serial_ptr = uart_ring_som_debug.w_ptr_tx;
|
||||||
if (waiting_to_boot_countdown > 0U) {
|
if (waiting_to_boot_countdown > 0U) {
|
||||||
|
|||||||
@@ -2,4 +2,4 @@
|
|||||||
|
|
||||||
extern bool bootkick_reset_triggered;
|
extern bool bootkick_reset_triggered;
|
||||||
|
|
||||||
void bootkick_tick(bool ignition, bool recent_heartbeat, bool wake);
|
void bootkick_tick(bool ignition, bool recent_heartbeat);
|
||||||
|
|||||||
@@ -7,9 +7,7 @@ uint32_t rx_buffer_overflow = 0;
|
|||||||
|
|
||||||
can_health_t can_health[PANDA_CAN_CNT] = {{0}, {0}, {0}};
|
can_health_t can_health[PANDA_CAN_CNT] = {{0}, {0}, {0}};
|
||||||
|
|
||||||
bool wake_on_can = false;
|
// Ignition detected from CAN meessages
|
||||||
uint32_t wake_on_can_cnt = 0U;
|
|
||||||
|
|
||||||
bool ignition_can = false;
|
bool ignition_can = false;
|
||||||
uint32_t ignition_can_cnt = 0U;
|
uint32_t ignition_can_cnt = 0U;
|
||||||
#ifdef PANDA_HKG_REMOTE_START
|
#ifdef PANDA_HKG_REMOTE_START
|
||||||
@@ -227,23 +225,6 @@ void ignition_can_hook(CANPacket_t *msg) {
|
|||||||
ignition_can_cnt = 0U;
|
ignition_can_cnt = 0U;
|
||||||
}
|
}
|
||||||
prev_counter_tesla = counter;
|
prev_counter_tesla = counter;
|
||||||
|
|
||||||
#ifdef PANDA_TESLA_WAKE_ON_CAN
|
|
||||||
uint32_t checksum = (msg->addr & 0xFFU) + (msg->addr >> 8U);
|
|
||||||
for (uint8_t i = 0U; i < 7U; i++) {
|
|
||||||
checksum += msg->data[i];
|
|
||||||
}
|
|
||||||
static int prev_counter_tesla_wake = -1;
|
|
||||||
if (!msg->extended && (msg->data[7] == (checksum & 0xFFU))) {
|
|
||||||
if ((prev_counter_tesla_wake != -1) && (counter == ((prev_counter_tesla_wake + 1) % 16))) {
|
|
||||||
wake_on_can = ((msg->data[0] >> 5U) & 0x3U) != 0U;
|
|
||||||
wake_on_can_cnt = 0U;
|
|
||||||
}
|
|
||||||
prev_counter_tesla_wake = counter;
|
|
||||||
} else {
|
|
||||||
prev_counter_tesla_wake = -1;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Tesla Model S pre-AP exception
|
// Tesla Model S pre-AP exception
|
||||||
@@ -268,19 +249,6 @@ void ignition_can_hook(CANPacket_t *msg) {
|
|||||||
ignition_can_cnt = 0U;
|
ignition_can_cnt = 0U;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Volkswagen MEB exception
|
|
||||||
if ((msg->addr == 0x3C0U) && (len == 4)) {
|
|
||||||
int counter = msg->data[1] & 0xFU;
|
|
||||||
|
|
||||||
static int prev_counter_vw_meb = -1;
|
|
||||||
if ((counter == ((prev_counter_vw_meb + 1) % 16)) && (prev_counter_vw_meb != -1)) {
|
|
||||||
// Klemmen_Status_01->ZAS_Kl_15
|
|
||||||
ignition_can = ((msg->data[2] >> 1) & 1U) != 0U;
|
|
||||||
ignition_can_cnt = 0U;
|
|
||||||
}
|
|
||||||
prev_counter_vw_meb = counter;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -28,9 +28,7 @@ extern uint32_t rx_buffer_overflow;
|
|||||||
|
|
||||||
extern can_health_t can_health[PANDA_CAN_CNT];
|
extern can_health_t can_health[PANDA_CAN_CNT];
|
||||||
|
|
||||||
extern bool wake_on_can;
|
// Ignition detected from CAN meessages
|
||||||
extern uint32_t wake_on_can_cnt;
|
|
||||||
|
|
||||||
extern bool ignition_can;
|
extern bool ignition_can;
|
||||||
extern uint32_t ignition_can_cnt;
|
extern uint32_t ignition_can_cnt;
|
||||||
|
|
||||||
|
|||||||
+1
-5
@@ -192,7 +192,7 @@ static void tick_handler(void) {
|
|||||||
#ifdef PANDA_HKG_REMOTE_START
|
#ifdef PANDA_HKG_REMOTE_START
|
||||||
started = started || hkg_remote_climate_wake;
|
started = started || hkg_remote_climate_wake;
|
||||||
#endif
|
#endif
|
||||||
bootkick_tick(started, recent_heartbeat, wake_on_can);
|
bootkick_tick(started, recent_heartbeat);
|
||||||
|
|
||||||
// increase heartbeat counter and cap it at the uint32 limit
|
// increase heartbeat counter and cap it at the uint32 limit
|
||||||
if (heartbeat_counter < UINT32_MAX) {
|
if (heartbeat_counter < UINT32_MAX) {
|
||||||
@@ -270,9 +270,6 @@ static void tick_handler(void) {
|
|||||||
if (ignition_can_cnt > 2U) {
|
if (ignition_can_cnt > 2U) {
|
||||||
ignition_can = false;
|
ignition_can = false;
|
||||||
}
|
}
|
||||||
if (wake_on_can_cnt > 2U) {
|
|
||||||
wake_on_can = false;
|
|
||||||
}
|
|
||||||
#ifdef PANDA_HKG_REMOTE_START
|
#ifdef PANDA_HKG_REMOTE_START
|
||||||
if (hkg_remote_climate_wake_cnt > 2U) {
|
if (hkg_remote_climate_wake_cnt > 2U) {
|
||||||
hkg_remote_climate_wake = false;
|
hkg_remote_climate_wake = false;
|
||||||
@@ -283,7 +280,6 @@ static void tick_handler(void) {
|
|||||||
uptime_cnt += 1U;
|
uptime_cnt += 1U;
|
||||||
safety_mode_cnt += 1U;
|
safety_mode_cnt += 1U;
|
||||||
ignition_can_cnt += 1U;
|
ignition_can_cnt += 1U;
|
||||||
wake_on_can_cnt += 1U;
|
|
||||||
#ifdef PANDA_HKG_REMOTE_START
|
#ifdef PANDA_HKG_REMOTE_START
|
||||||
hkg_remote_climate_wake_cnt += 1U;
|
hkg_remote_climate_wake_cnt += 1U;
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
|||||||
extern const uint8_t gitversion[19];
|
extern const uint8_t gitversion[19];
|
||||||
const uint8_t gitversion[19] = "DEV-bf00f88b-DEBUG";
|
const uint8_t gitversion[19] = "DEV-c03d06b4-DEBUG";
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user