mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-25 02:03:43 +08:00
Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 firestar5683
|
28ec3ccb80 | ||
|
firestar5683
|
5f65219aed |
@@ -265,7 +265,6 @@ struct StarPilotSelfdriveState @0xf416ec09499d9d19 {
|
||||
alertSize @3 :AlertSize;
|
||||
alertType @4 :Text;
|
||||
alertSound @5 :Car.CarControl.HUDControl.AudibleAlert;
|
||||
vEgo @6 :Float32;
|
||||
|
||||
enum AlertStatus {
|
||||
normal @0;
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -66,6 +66,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"HondaLateralPidKiScale", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
|
||||
{"HondaLateralPidKpScale", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
|
||||
{"HondaWindFactorParams", {PERSISTENT, FLOAT}},
|
||||
{"HomeScreenName", {PERSISTENT, STRING, "StarPilot", "StarPilot", 3}},
|
||||
{"InstallDate", {PERSISTENT, TIME}},
|
||||
{"IsDriverViewEnabled", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
{"IsEngaged", {PERSISTENT, BOOL}},
|
||||
@@ -345,7 +346,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4", "0.4", 2}},
|
||||
{"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4", "0.4", 2}},
|
||||
{"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85", "0.85", 2}},
|
||||
{"FordHandsFreeCluster", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"FordHumanTurnDetection", {PERSISTENT, BOOL, "1", "1", 2}},
|
||||
{"FordLateralMode", {PERSISTENT, INT, "1", "1", 2}},
|
||||
{"FLMActiveOverrides", {PERSISTENT, JSON, "{}", "{}", 2}},
|
||||
@@ -680,7 +680,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"TuningLevel", {PERSISTENT, INT, "0", "0", 0}},
|
||||
{"TuningLevelConfirmed", {PERSISTENT, BOOL, "0", "0", 0}},
|
||||
{"TurnDesires", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"TurnSteeringLimitMuteSpeed", {PERSISTENT, INT, "0", "0", 0}},
|
||||
{"UnlockDoors", {PERSISTENT, BOOL, "1", "0", 0}},
|
||||
{"Updated", {PERSISTENT, STRING, "0", "0"}},
|
||||
{"UpdateSpeedLimits", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}},
|
||||
|
||||
Binary file not shown.
+94
-3
@@ -147,10 +147,101 @@ function launch {
|
||||
while true; do sleep 1; done
|
||||
fi
|
||||
|
||||
function prebuilt_runtime_compatible {
|
||||
python3 - <<'PY'
|
||||
import importlib
|
||||
import os
|
||||
from pathlib import Path
|
||||
import sys
|
||||
import time
|
||||
|
||||
from openpilot.common.file_chunker import get_existing_chunks
|
||||
|
||||
start = time.monotonic()
|
||||
last = start
|
||||
log_path = os.environ.get("SP_BOOT_TIMING_LOG")
|
||||
|
||||
def emit(line):
|
||||
print(line, flush=True)
|
||||
if log_path:
|
||||
try:
|
||||
with open(log_path, "a") as f:
|
||||
f.write(line + "\n")
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
def log_step(label):
|
||||
global last
|
||||
now = time.monotonic()
|
||||
emit(f"SP_BOOT_TIMING prebuilt_compat {label} +{now - last:.3f}s total={now - start:.3f}s")
|
||||
last = now
|
||||
|
||||
mods = [
|
||||
"openpilot.common.params_pyx",
|
||||
"msgq.ipc_pyx",
|
||||
"msgq.visionipc.visionipc_pyx",
|
||||
"openpilot.common.transformations.transformations",
|
||||
"openpilot.selfdrive.pandad.pandad_api_impl",
|
||||
"openpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx",
|
||||
"openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx",
|
||||
]
|
||||
|
||||
for mod in mods:
|
||||
try:
|
||||
importlib.import_module(mod)
|
||||
except Exception as e:
|
||||
print(f"Prebuilt compatibility failure in {mod}: {e}", file=sys.stderr)
|
||||
raise
|
||||
log_step(f"import:{mod}")
|
||||
|
||||
repo_root = Path.cwd().parents[1]
|
||||
required_model_artifacts = [
|
||||
repo_root / "selfdrive/modeld/models/driving_tinygrad.pkl",
|
||||
repo_root / "selfdrive/modeld/models/dmonitoring_model_metadata.pkl",
|
||||
repo_root / "selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl",
|
||||
repo_root / "selfdrive/modeld/models/dm_warp_1928x1208_tinygrad.pkl",
|
||||
repo_root / "selfdrive/modeld/models/dm_warp_1344x760_tinygrad.pkl",
|
||||
]
|
||||
required_files = [
|
||||
repo_root / "selfdrive/pandad/pandad_api_impl.so",
|
||||
repo_root / "selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so",
|
||||
repo_root / "selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so",
|
||||
repo_root / "selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so",
|
||||
repo_root / "selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so",
|
||||
repo_root / "opendbc_repo/opendbc/dbc/gm_global_a_powertrain_generated.dbc",
|
||||
]
|
||||
|
||||
for path in required_model_artifacts:
|
||||
try:
|
||||
artifact_paths = [Path(p) for p in get_existing_chunks(path)]
|
||||
except Exception as e:
|
||||
raise FileNotFoundError(f"Missing prebuilt runtime artifact: {path}") from e
|
||||
missing_chunks = [p for p in artifact_paths if not p.is_file()]
|
||||
if missing_chunks:
|
||||
missing = ", ".join(str(p) for p in missing_chunks)
|
||||
raise FileNotFoundError(f"Missing prebuilt runtime artifact chunks for {path}: {missing}")
|
||||
log_step("required_model_artifacts")
|
||||
|
||||
for path in required_files:
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"Missing prebuilt runtime artifact: {path}")
|
||||
log_step("required_files")
|
||||
PY
|
||||
}
|
||||
|
||||
USE_PREBUILT=1
|
||||
if [ -f /data/params/d/UsePrebuilt ]; then
|
||||
USE_PREBUILT=$(tr -d '\n' < /data/params/d/UsePrebuilt)
|
||||
fi
|
||||
|
||||
sp_launch_timing "prebuilt_decision_done"
|
||||
# Published trees carry this marker and must never compile on-device.
|
||||
# Developers can remove it explicitly when working from a source tree.
|
||||
if [ ! -f "$DIR/prebuilt" ]; then
|
||||
if [ "$USE_PREBUILT" = "1" ] && [ -f $DIR/prebuilt ] && ! prebuilt_runtime_compatible; then
|
||||
echo "Prebuilt runtime artifacts are incompatible on this device; rebuilding locally."
|
||||
USE_PREBUILT=0
|
||||
fi
|
||||
sp_launch_timing "prebuilt_compat_done"
|
||||
|
||||
if [ "$USE_PREBUILT" != "1" ] || [ ! -f $DIR/prebuilt ]; then
|
||||
sp_launch_timing "build_start"
|
||||
./build.py
|
||||
sp_launch_timing "build_done"
|
||||
|
||||
@@ -255,15 +255,9 @@ class CarController(CarControllerBase):
|
||||
|
||||
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
show_distance_bars = self.frame - self.distance_bar_frame < 400
|
||||
hands_free_cluster = bool(
|
||||
self.ford_lateral is not None
|
||||
and self.ford_lateral.mode != FordLateralMode.native
|
||||
and self.ford_lateral.mode == self.ford_lateral_announced_mode
|
||||
and self.ford_lateral.hands_free_cluster_enabled)
|
||||
can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
|
||||
fcw_alert, CS.out.cruiseState.standstill, show_distance_bars,
|
||||
hud_control, CS.acc_tja_status_stock_values,
|
||||
hands_free_cluster))
|
||||
hud_control, CS.acc_tja_status_stock_values))
|
||||
|
||||
self.main_on_last = main_on
|
||||
self.lkas_enabled_last = CC.latActive
|
||||
|
||||
@@ -181,7 +181,7 @@ def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: fl
|
||||
|
||||
|
||||
def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
|
||||
show_distance_bars: bool, hud_control, stock_values: dict, hands_free_cluster: bool = False):
|
||||
show_distance_bars: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
|
||||
assist status.
|
||||
@@ -197,8 +197,6 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw
|
||||
status = 3 # ActiveInterventionLeft
|
||||
elif hud_control.rightLaneDepart:
|
||||
status = 4 # ActiveInterventionRight
|
||||
elif hands_free_cluster:
|
||||
status = 7 # Hands-free assistance display
|
||||
else:
|
||||
status = 2 # Active
|
||||
elif main_on:
|
||||
|
||||
@@ -1,13 +1,10 @@
|
||||
import random
|
||||
from collections.abc import Iterable
|
||||
from types import SimpleNamespace
|
||||
|
||||
from hypothesis import settings, given, strategies as st
|
||||
from parameterized import parameterized
|
||||
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car.ford import fordcan
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.fw_versions import build_fw_dict
|
||||
from opendbc.car.ford.interface import CarInterface
|
||||
@@ -175,29 +172,3 @@ def test_mach_e_longitudinal_toggle_controls_stock_acc_selection():
|
||||
assert enhanced.alphaLongitudinalAvailable
|
||||
assert enhanced.openpilotLongitudinalControl
|
||||
assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL
|
||||
|
||||
|
||||
def test_hands_free_cluster_status_is_opt_in():
|
||||
packer = CANPacker("ford_lincoln_base_pt")
|
||||
CAN = SimpleNamespace(main=0)
|
||||
CP = SimpleNamespace(openpilotLongitudinalControl=False)
|
||||
hud = SimpleNamespace(leftLaneDepart=False, rightLaneDepart=False)
|
||||
stock_values = dict.fromkeys([
|
||||
"HaDsply_No_Cs", "HaDsply_No_Cnt", "AccStopStat_D_Dsply", "AccTrgDist2_D_Dsply",
|
||||
"AccStopRes_B_Dsply", "TjaWarn_D_Rq", "TjaMsgTxt_D_Dsply", "IaccLamp_D_Rq",
|
||||
"AccMsgTxt_D2_Rq", "FcwDeny_B_Dsply", "FcwMemStat_B_Actl", "AccTGap_B_Dsply",
|
||||
"CadsAlignIncplt_B_Actl", "AccFllwMde_B_Dsply", "CadsRadrBlck_B_Actl",
|
||||
"CmbbPostEvnt_B_Dsply", "AccStopMde_B_Dsply", "FcwMemSens_D_Actl",
|
||||
"FcwMsgTxt_D_Rq", "AccWarn_D_Dsply", "FcwVisblWarn_B_Rq", "FcwAudioWarn_B_Rq",
|
||||
"AccTGap_D_Dsply", "AccMemEnbl_B_RqDrv", "FdaMem_B_Stat",
|
||||
], 0)
|
||||
|
||||
regular = fordcan.create_acc_ui_msg(
|
||||
packer, CAN, CP, True, True, False, False, False, hud, stock_values)
|
||||
hands_free = fordcan.create_acc_ui_msg(
|
||||
packer, CAN, CP, True, True, False, False, False, hud, stock_values, True)
|
||||
expected_regular = packer.make_can_msg("ACCDATA_3", 0, {"Tja_D_Stat": 2})
|
||||
expected_hands_free = packer.make_can_msg("ACCDATA_3", 0, {"Tja_D_Stat": 7})
|
||||
|
||||
assert regular == expected_regular
|
||||
assert hands_free == expected_hands_free
|
||||
|
||||
@@ -988,45 +988,6 @@ class TestHyundaiFingerprint:
|
||||
assert long_xceed.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.HYBRID_GAS
|
||||
assert long_xceed.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
|
||||
|
||||
def test_g80_alpha_long_preserves_legacy_safety(self):
|
||||
toggles = get_test_toggles()
|
||||
|
||||
stock_g80 = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], False, False, False, toggles)
|
||||
assert CAR.GENESIS_G80 in LEGACY_LONGITUDINAL_CAR
|
||||
assert stock_g80.alphaLongitudinalAvailable
|
||||
assert not stock_g80.openpilotLongitudinalControl
|
||||
assert stock_g80.pcmCruise
|
||||
assert stock_g80.safetyConfigs[-1].safetyModel == CarParams.SafetyModel.hyundaiLegacy
|
||||
assert not (stock_g80.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG)
|
||||
|
||||
long_g80 = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], True, False, False, toggles)
|
||||
assert long_g80.alphaLongitudinalAvailable
|
||||
assert long_g80.openpilotLongitudinalControl
|
||||
assert not long_g80.pcmCruise
|
||||
assert long_g80.safetyConfigs[-1].safetyModel == CarParams.SafetyModel.hyundaiLegacy
|
||||
assert long_g80.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
|
||||
|
||||
@pytest.mark.parametrize("ecu_disabled", (True, False))
|
||||
def test_g80_alpha_long_disables_stock_scc(self, monkeypatch, ecu_disabled):
|
||||
toggles = get_test_toggles()
|
||||
CP = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], True, False, False, toggles)
|
||||
|
||||
called = {}
|
||||
|
||||
def fake_disable_ecu(*args, **kwargs):
|
||||
called.update(kwargs)
|
||||
return ecu_disabled
|
||||
|
||||
monkeypatch.setattr("opendbc.car.hyundai.interface.disable_ecu", fake_disable_ecu)
|
||||
CarInterface.init(CP, None, None)
|
||||
|
||||
assert called["addr"] == 0x7d0
|
||||
assert called["bus"] == 0
|
||||
assert called["reset"] is False
|
||||
assert CP.openpilotLongitudinalControl == ecu_disabled
|
||||
assert CP.pcmCruise != ecu_disabled
|
||||
assert bool(CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG) == ecu_disabled
|
||||
|
||||
def test_xceed_phev_disable_failure_falls_back_to_stock_acc(self, monkeypatch):
|
||||
toggles = get_test_toggles()
|
||||
CP = CarInterface.get_params(CAR.KIA_XCEED_PHEV, gen_empty_fingerprint(), [], True, False, False, toggles)
|
||||
|
||||
@@ -1213,9 +1213,6 @@ NON_SCC_CAR = CAR.with_flags(HyundaiFlags.NON_SCC)
|
||||
# HyundaiFlags.CANFD_RADAR_SCC | HyundaiFlags.CANFD_NO_RADAR_DISABLE | )
|
||||
UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.LEGACY) | CAR.with_flags(HyundaiFlags.UNSUPPORTED_LONGITUDINAL)
|
||||
|
||||
LEGACY_LONGITUDINAL_CAR = {
|
||||
CAR.GENESIS_G80,
|
||||
CAR.KIA_XCEED_PHEV,
|
||||
}
|
||||
LEGACY_LONGITUDINAL_CAR = {CAR.KIA_XCEED_PHEV}
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
|
||||
@@ -15,7 +15,6 @@ LEAF_ADAS_ECU_BUS = 0
|
||||
LEAF_ADAS_COMMAND_BUS = 1
|
||||
LEAF_ADAS_COMMAND_ADDRS = frozenset((0x1C3, 0x2B0))
|
||||
LEAF_2025_SV_PLUS_CAMERA_FW = b'6WK2CDB\x04\x18\x00\x00\x00\x00\x00R=1\x18\x99\x10\x00\x00\x00\x80'
|
||||
LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED = False
|
||||
|
||||
LEAF_KWP_EXTENDED_REQUEST = b"\x10\xC0"
|
||||
LEAF_KWP_EXTENDED_RESPONSE = b"\x50\xC0"
|
||||
@@ -27,7 +26,7 @@ LEAF_KWP_TAKEOVER_SESSIONS = (
|
||||
|
||||
|
||||
def is_leaf_2025_sv_plus_longitudinal(candidate, car_fw):
|
||||
return LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED and candidate == CAR.NISSAN_LEAF and any(
|
||||
return candidate == CAR.NISSAN_LEAF and any(
|
||||
fw.address == LEAF_ADAS_ECU_ADDR and bytes(fw.fwVersion) == LEAF_2025_SV_PLUS_CAMERA_FW
|
||||
for fw in car_fw
|
||||
)
|
||||
|
||||
@@ -4,7 +4,6 @@ import pytest
|
||||
|
||||
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.nissan import interface as nissan_interface
|
||||
from opendbc.car.nissan.carstate import CarState
|
||||
from opendbc.car.nissan.interface import CarInterface, LEAF_2025_SV_PLUS_CAMERA_FW, leaf_adas_commands_present, \
|
||||
leaf_adas_commands_silent, restore_leaf_adas_tx
|
||||
@@ -19,12 +18,6 @@ SUPPORTED_LEAF_FW = [structs.CarParams.CarFw(
|
||||
)]
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def experimental_leaf_long(monkeypatch):
|
||||
"""Exercise the dormant implementation without making it available in production."""
|
||||
monkeypatch.setattr(nissan_interface, "LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED", True)
|
||||
|
||||
|
||||
def run_controller(alpha_long, accel=0.0, long_active=True, long_state=structs.CarControl.Actuators.LongControlState.pid):
|
||||
CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, alpha_long, False, False, TEST_TOGGLES)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, CP, TEST_TOGGLES)
|
||||
@@ -40,28 +33,7 @@ def run_controller(alpha_long, accel=0.0, long_active=True, long_state=structs.C
|
||||
return {msg[0]: msg for msg in can_sends}
|
||||
|
||||
|
||||
def test_leaf_2025_sv_plus_alpha_long_is_disabled(monkeypatch):
|
||||
stock = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, False, False, False, None)
|
||||
alpha_long = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
|
||||
|
||||
assert not stock.alphaLongitudinalAvailable
|
||||
assert not stock.openpilotLongitudinalControl
|
||||
assert stock.pcmCruise
|
||||
assert not (stock.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL)
|
||||
|
||||
assert not alpha_long.alphaLongitudinalAvailable
|
||||
assert not alpha_long.openpilotLongitudinalControl
|
||||
assert alpha_long.pcmCruise
|
||||
assert not alpha_long.autoResumeSng
|
||||
assert not (alpha_long.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL)
|
||||
|
||||
disable_calls = []
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: disable_calls.append((args, kwargs)))
|
||||
CarInterface.init(alpha_long, None, None)
|
||||
assert not disable_calls
|
||||
|
||||
|
||||
def test_dormant_leaf_2025_sv_plus_alpha_long_params(experimental_leaf_long):
|
||||
def test_leaf_2025_sv_plus_alpha_long_params():
|
||||
stock = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, False, False, False, None)
|
||||
alpha_long = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
|
||||
|
||||
@@ -107,13 +79,7 @@ def test_stock_controller_does_not_send_longitudinal_messages():
|
||||
assert not ({0x2B0, 0x1C3, 0x707} & can_sends.keys())
|
||||
|
||||
|
||||
def test_disabled_alpha_long_controller_does_not_send_longitudinal_messages():
|
||||
can_sends = run_controller(True)
|
||||
|
||||
assert not ({0x2B0, 0x1C3, 0x707} & can_sends.keys())
|
||||
|
||||
|
||||
def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive(experimental_leaf_long):
|
||||
def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive():
|
||||
can_sends = run_controller(True)
|
||||
|
||||
assert can_sends[0x2B0][1].hex() == "ff6090ac5b000e03"
|
||||
@@ -123,13 +89,13 @@ def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive(experim
|
||||
assert can_sends[0x707][2] == 0
|
||||
|
||||
|
||||
def test_alpha_long_controller_clamps_to_panda_accel_limit(experimental_leaf_long):
|
||||
def test_alpha_long_controller_clamps_to_panda_accel_limit():
|
||||
can_sends = run_controller(True, accel=5.0)
|
||||
|
||||
assert can_sends[0x2B0][1].hex() == "007f8fac5b000e0c"
|
||||
|
||||
|
||||
def test_alpha_long_controller_blends_friction_brake_below_regen_limit(experimental_leaf_long):
|
||||
def test_alpha_long_controller_blends_friction_brake_below_regen_limit():
|
||||
can_sends = run_controller(True, accel=-2.0)
|
||||
|
||||
assert can_sends[0x2B0][1].hex() == "a827d5ac5b000e09"
|
||||
@@ -138,7 +104,7 @@ def test_alpha_long_controller_blends_friction_brake_below_regen_limit(experimen
|
||||
assert brake[5] & 0x84 == 0x84
|
||||
|
||||
|
||||
def test_alpha_long_controller_sends_inactive_commands_when_disengaged(experimental_leaf_long):
|
||||
def test_alpha_long_controller_sends_inactive_commands_when_disengaged():
|
||||
can_sends = run_controller(True, accel=1.0, long_active=False)
|
||||
|
||||
assert can_sends[0x2B0][1].hex() == "dc53a2ac1b000e03"
|
||||
@@ -147,7 +113,7 @@ def test_alpha_long_controller_sends_inactive_commands_when_disengaged(experimen
|
||||
|
||||
@pytest.mark.parametrize(("signal", "button_type"), [("SET_BUTTON", ButtonType.decelCruise),
|
||||
("RES_BUTTON", ButtonType.accelCruise)])
|
||||
def test_leaf_set_resume_release_enables_alpha_long(signal, button_type, experimental_leaf_long):
|
||||
def test_leaf_set_resume_release_enables_alpha_long(signal, button_type):
|
||||
CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, TEST_TOGGLES)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, CP, TEST_TOGGLES)
|
||||
CS = CarState(CP, FPCP)
|
||||
@@ -164,7 +130,7 @@ def test_leaf_set_resume_release_enables_alpha_long(signal, button_type, experim
|
||||
|
||||
|
||||
@pytest.mark.parametrize("ecu_disabled", [False, True])
|
||||
def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled, experimental_leaf_long):
|
||||
def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled):
|
||||
CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
|
||||
calls = []
|
||||
|
||||
@@ -192,7 +158,7 @@ def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled, ex
|
||||
assert bool(CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) is ecu_disabled
|
||||
|
||||
|
||||
def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch, experimental_leaf_long):
|
||||
def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch):
|
||||
CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
|
||||
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: True)
|
||||
@@ -205,7 +171,7 @@ def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch, exper
|
||||
assert CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL
|
||||
|
||||
|
||||
def test_leaf_positive_disable_response_without_command_silence_falls_back(monkeypatch, experimental_leaf_long):
|
||||
def test_leaf_positive_disable_response_without_command_silence_falls_back(monkeypatch):
|
||||
CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
|
||||
restore_calls = []
|
||||
|
||||
|
||||
@@ -165,18 +165,13 @@ class TestCarInterfaces:
|
||||
"Center_Stack_2": {"LKAS_Button": 1},
|
||||
}, is_ram=True)
|
||||
|
||||
@pytest.mark.parametrize("candidate", (
|
||||
CHRYSLER_CAR.CHRYSLER_PACIFICA_2020,
|
||||
CHRYSLER_CAR.JEEP_GRAND_CHEROKEE,
|
||||
CHRYSLER_CAR.JEEP_GRAND_CHEROKEE_2019,
|
||||
))
|
||||
def test_chrysler_steer_to_zero_module(self, candidate):
|
||||
def test_chrysler_wd_mod_enables_steer_to_zero(self):
|
||||
fingerprint = {bus: {} for bus in range(8)}
|
||||
fingerprint[0][0x4FF] = 8
|
||||
toggles = get_test_starpilot_toggles()
|
||||
|
||||
car_params = ChryslerCarInterface.get_params(
|
||||
candidate,
|
||||
CHRYSLER_CAR.CHRYSLER_PACIFICA_2020,
|
||||
fingerprint,
|
||||
[],
|
||||
alpha_long=False,
|
||||
@@ -187,7 +182,7 @@ class TestCarInterfaces:
|
||||
assert car_params.minSteerSpeed > 0.
|
||||
|
||||
fp_car_params = ChryslerCarInterface.get_starpilot_params(
|
||||
candidate,
|
||||
CHRYSLER_CAR.CHRYSLER_PACIFICA_2020,
|
||||
fingerprint,
|
||||
[],
|
||||
car_params,
|
||||
@@ -211,14 +206,11 @@ class TestCarInterfaces:
|
||||
button_message="CRUISE_BUTTONS",
|
||||
auto_high_beam=0,
|
||||
)
|
||||
controller.update(CC, CS, 0, toggles)
|
||||
controller.update(CC, CS, 0, toggles)
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
|
||||
lkas_parser = CANParser(CHRYSLER_DBC[car_params.carFingerprint][Bus.pt], [("LKAS_COMMAND", 50)], 0)
|
||||
lkas_parser.update([0, can_sends])
|
||||
assert lkas_parser.vl["LKAS_COMMAND"]["LKAS_CONTROL_BIT"] == 1
|
||||
assert lkas_parser.vl["LKAS_COMMAND"]["STEERING_TORQUE"] != 0
|
||||
|
||||
@pytest.mark.parametrize("candidate", (CHRYSLER_CAR.JEEP_GRAND_CHEROKEE, CHRYSLER_CAR.JEEP_GRAND_CHEROKEE_2019))
|
||||
def test_jeep_brake_hold_safety_capability_is_provisioned(self, candidate):
|
||||
|
||||
@@ -1103,7 +1103,6 @@ class SafetyTest(SafetyTestBase):
|
||||
'TestHyundaiSafetyFCEVLong', 'TestHyundaiLongitudinalAolLkasOnEngageSafety',
|
||||
'TestHyundaiSafetyCanRefreshLong', 'TestHyundaiSafetyCanRefreshLongCameraSCC',
|
||||
'TestHyundaiCanCanfdBlendedLongitudinalSafety',
|
||||
'TestHyundaiLegacyLongitudinalSafety',
|
||||
'TestHyundaiLegacyLongitudinalSafetyHEV'}):
|
||||
continue
|
||||
volkswagen_shared = ('TestVolkswagenMqb', 'TestVolkswagenMlb', 'TestVolkswagenMeb')
|
||||
@@ -1157,7 +1156,6 @@ class SafetyTest(SafetyTestBase):
|
||||
if attr.startswith('TestHyundaiLongitudinal') or attr in ('TestHyundaiSafetyFCEVLong',
|
||||
'TestHyundaiLongitudinalAolLkasOnEngageSafety',
|
||||
'TestHyundaiCanCanfdBlendedLongitudinalSafety',
|
||||
'TestHyundaiLegacyLongitudinalSafety',
|
||||
'TestHyundaiLegacyLongitudinalSafetyHEV'):
|
||||
# exceptions for common msgs across different Hyundai CAN platforms
|
||||
tx = list(filter(lambda m: m[0] not in [0x420, 0x50A, 0x389, 0x4A2], tx))
|
||||
|
||||
@@ -597,14 +597,6 @@ class TestHyundaiSafetyFCEVLong(TestHyundaiLongitudinalSafety, TestHyundaiSafety
|
||||
self.safety.init_tests()
|
||||
|
||||
|
||||
class TestHyundaiLegacyLongitudinalSafety(TestHyundaiLongitudinalSafety, TestHyundaiLegacySafety):
|
||||
def setUp(self):
|
||||
self.packer = CANPackerSafety("hyundai_kia_generic")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, HyundaiSafetyFlags.LONG)
|
||||
self.safety.init_tests()
|
||||
|
||||
|
||||
class TestHyundaiLegacyLongitudinalSafetyHEV(TestHyundaiLongitudinalSafety, TestHyundaiLegacySafetyHEV):
|
||||
def setUp(self):
|
||||
self.packer = CANPackerSafety("hyundai_kia_generic")
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-e28f72c6-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-284dbddd-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
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 +1 @@
|
||||
DEV-e28f72c6-DEBUG
|
||||
DEV-284dbddd-DEBUG
|
||||
@@ -1,17 +1,99 @@
|
||||
info face="como-heavy" size=-200 bold=0 italic=0 charset="" unicode=1 stretchH=100 smooth=0 aa=1 padding=0,0,0,0 spacing=0,0 outline=0
|
||||
common lineHeight=200 base=200 scaleW=1024 scaleH=512 pages=1 packed=0 alphaChnl=0 redChnl=4 greenChnl=4 blueChnl=4
|
||||
common lineHeight=200 base=200 scaleW=2048 scaleH=2048 pages=1 packed=0 alphaChnl=0 redChnl=4 greenChnl=4 blueChnl=4
|
||||
page id=0 file="como-heavy.png"
|
||||
chars count=13
|
||||
chars count=95
|
||||
char id=32 x=6 y=6 width=39 height=200 xoffset=0 yoffset=0 xadvance=39 page=0 chnl=15
|
||||
char id=63 x=57 y=6 width=81 height=115 xoffset=0 yoffset=47 xadvance=83 page=0 chnl=15
|
||||
char id=80 x=150 y=6 width=100 height=113 xoffset=8 yoffset=49 xadvance=110 page=0 chnl=15
|
||||
char id=83 x=262 y=6 width=101 height=116 xoffset=4 yoffset=47 xadvance=108 page=0 chnl=15
|
||||
char id=97 x=375 y=6 width=88 height=85 xoffset=5 yoffset=78 xadvance=100 page=0 chnl=15
|
||||
char id=105 x=475 y=6 width=31 height=121 xoffset=6 yoffset=41 xadvance=43 page=0 chnl=15
|
||||
char id=108 x=518 y=6 width=29 height=119 xoffset=7 yoffset=43 xadvance=43 page=0 chnl=15
|
||||
char id=111 x=559 y=6 width=88 height=85 xoffset=5 yoffset=78 xadvance=98 page=0 chnl=15
|
||||
char id=112 x=659 y=6 width=88 height=115 xoffset=7 yoffset=78 xadvance=100 page=0 chnl=15
|
||||
char id=114 x=759 y=6 width=63 height=83 xoffset=7 yoffset=79 xadvance=69 page=0 chnl=15
|
||||
char id=115 x=834 y=6 width=77 height=85 xoffset=4 yoffset=78 xadvance=84 page=0 chnl=15
|
||||
char id=116 x=923 y=6 width=63 height=111 xoffset=6 yoffset=52 xadvance=70 page=0 chnl=15
|
||||
char id=122 x=6 y=218 width=79 height=81 xoffset=3 yoffset=80 xadvance=85 page=0 chnl=15
|
||||
char id=33 x=57 y=6 width=32 height=114 xoffset=8 yoffset=48 xadvance=48 page=0 chnl=15
|
||||
char id=34 x=101 y=6 width=68 height=64 xoffset=6 yoffset=48 xadvance=80 page=0 chnl=15
|
||||
char id=35 x=181 y=6 width=101 height=111 xoffset=3 yoffset=51 xadvance=107 page=0 chnl=15
|
||||
char id=36 x=294 y=6 width=90 height=136 xoffset=8 yoffset=40 xadvance=107 page=0 chnl=15
|
||||
char id=37 x=396 y=6 width=115 height=113 xoffset=3 yoffset=50 xadvance=121 page=0 chnl=15
|
||||
char id=38 x=523 y=6 width=109 height=116 xoffset=3 yoffset=47 xadvance=113 page=0 chnl=15
|
||||
char id=39 x=644 y=6 width=30 height=64 xoffset=6 yoffset=48 xadvance=42 page=0 chnl=15
|
||||
char id=40 x=686 y=6 width=53 height=131 xoffset=8 yoffset=48 xadvance=67 page=0 chnl=15
|
||||
char id=41 x=751 y=6 width=53 height=131 xoffset=6 yoffset=48 xadvance=67 page=0 chnl=15
|
||||
char id=42 x=816 y=6 width=82 height=79 xoffset=4 yoffset=43 xadvance=90 page=0 chnl=15
|
||||
char id=43 x=910 y=6 width=97 height=98 xoffset=5 yoffset=63 xadvance=107 page=0 chnl=15
|
||||
char id=44 x=1019 y=6 width=42 height=60 xoffset=-3 yoffset=129 xadvance=48 page=0 chnl=15
|
||||
char id=45 x=1073 y=6 width=61 height=29 xoffset=5 yoffset=102 xadvance=71 page=0 chnl=15
|
||||
char id=46 x=1146 y=6 width=33 height=33 xoffset=8 yoffset=129 xadvance=48 page=0 chnl=15
|
||||
char id=47 x=1191 y=6 width=57 height=114 xoffset=1 yoffset=48 xadvance=59 page=0 chnl=15
|
||||
char id=48 x=1260 y=6 width=95 height=113 xoffset=6 yoffset=50 xadvance=107 page=0 chnl=15
|
||||
char id=49 x=1367 y=6 width=63 height=110 xoffset=20 yoffset=52 xadvance=107 page=0 chnl=15
|
||||
char id=50 x=1442 y=6 width=87 height=111 xoffset=11 yoffset=50 xadvance=107 page=0 chnl=15
|
||||
char id=51 x=1541 y=6 width=87 height=113 xoffset=11 yoffset=50 xadvance=107 page=0 chnl=15
|
||||
char id=52 x=1640 y=6 width=99 height=111 xoffset=5 yoffset=51 xadvance=107 page=0 chnl=15
|
||||
char id=53 x=1751 y=6 width=88 height=111 xoffset=10 yoffset=52 xadvance=107 page=0 chnl=15
|
||||
char id=54 x=1851 y=6 width=91 height=113 xoffset=8 yoffset=50 xadvance=107 page=0 chnl=15
|
||||
char id=55 x=6 y=218 width=92 height=110 xoffset=9 yoffset=52 xadvance=107 page=0 chnl=15
|
||||
char id=56 x=110 y=218 width=94 height=113 xoffset=7 yoffset=50 xadvance=107 page=0 chnl=15
|
||||
char id=57 x=216 y=218 width=90 height=113 xoffset=9 yoffset=50 xadvance=107 page=0 chnl=15
|
||||
char id=58 x=318 y=218 width=33 height=83 xoffset=8 yoffset=79 xadvance=48 page=0 chnl=15
|
||||
char id=59 x=363 y=218 width=44 height=110 xoffset=-3 yoffset=79 xadvance=48 page=0 chnl=15
|
||||
char id=60 x=419 y=218 width=95 height=90 xoffset=6 yoffset=67 xadvance=107 page=0 chnl=15
|
||||
char id=61 x=526 y=218 width=95 height=66 xoffset=6 yoffset=79 xadvance=107 page=0 chnl=15
|
||||
char id=62 x=633 y=218 width=94 height=90 xoffset=7 yoffset=67 xadvance=107 page=0 chnl=15
|
||||
char id=63 x=739 y=218 width=81 height=115 xoffset=0 yoffset=47 xadvance=83 page=0 chnl=15
|
||||
char id=64 x=832 y=218 width=115 height=117 xoffset=5 yoffset=55 xadvance=124 page=0 chnl=15
|
||||
char id=65 x=959 y=218 width=107 height=115 xoffset=7 yoffset=47 xadvance=120 page=0 chnl=15
|
||||
char id=66 x=1078 y=218 width=103 height=112 xoffset=8 yoffset=49 xadvance=114 page=0 chnl=15
|
||||
char id=67 x=1193 y=218 width=110 height=116 xoffset=5 yoffset=47 xadvance=116 page=0 chnl=15
|
||||
char id=68 x=1315 y=218 width=108 height=112 xoffset=8 yoffset=49 xadvance=121 page=0 chnl=15
|
||||
char id=69 x=1435 y=218 width=99 height=112 xoffset=8 yoffset=49 xadvance=109 page=0 chnl=15
|
||||
char id=70 x=1546 y=218 width=97 height=113 xoffset=8 yoffset=49 xadvance=105 page=0 chnl=15
|
||||
char id=71 x=1655 y=218 width=114 height=116 xoffset=5 yoffset=47 xadvance=123 page=0 chnl=15
|
||||
char id=72 x=1781 y=218 width=108 height=114 xoffset=8 yoffset=48 xadvance=124 page=0 chnl=15
|
||||
char id=73 x=1901 y=218 width=32 height=114 xoffset=8 yoffset=48 xadvance=48 page=0 chnl=15
|
||||
char id=74 x=6 y=430 width=95 height=115 xoffset=1 yoffset=48 xadvance=102 page=0 chnl=15
|
||||
char id=75 x=113 y=430 width=101 height=114 xoffset=8 yoffset=48 xadvance=112 page=0 chnl=15
|
||||
char id=76 x=226 y=430 width=96 height=113 xoffset=8 yoffset=48 xadvance=103 page=0 chnl=15
|
||||
char id=77 x=334 y=430 width=129 height=114 xoffset=8 yoffset=48 xadvance=145 page=0 chnl=15
|
||||
char id=78 x=475 y=430 width=107 height=114 xoffset=8 yoffset=48 xadvance=123 page=0 chnl=15
|
||||
char id=79 x=594 y=430 width=119 height=116 xoffset=5 yoffset=47 xadvance=128 page=0 chnl=15
|
||||
char id=80 x=725 y=430 width=100 height=113 xoffset=8 yoffset=49 xadvance=110 page=0 chnl=15
|
||||
char id=81 x=837 y=430 width=119 height=129 xoffset=5 yoffset=47 xadvance=128 page=0 chnl=15
|
||||
char id=82 x=968 y=430 width=100 height=113 xoffset=8 yoffset=49 xadvance=112 page=0 chnl=15
|
||||
char id=83 x=1080 y=430 width=101 height=116 xoffset=4 yoffset=47 xadvance=108 page=0 chnl=15
|
||||
char id=84 x=1193 y=430 width=111 height=113 xoffset=0 yoffset=49 xadvance=110 page=0 chnl=15
|
||||
char id=85 x=1316 y=430 width=101 height=116 xoffset=7 yoffset=48 xadvance=114 page=0 chnl=15
|
||||
char id=86 x=1429 y=430 width=115 height=114 xoffset=0 yoffset=48 xadvance=115 page=0 chnl=15
|
||||
char id=87 x=1556 y=430 width=162 height=114 xoffset=1 yoffset=48 xadvance=164 page=0 chnl=15
|
||||
char id=88 x=1730 y=430 width=106 height=114 xoffset=3 yoffset=48 xadvance=112 page=0 chnl=15
|
||||
char id=89 x=1848 y=430 width=111 height=114 xoffset=0 yoffset=48 xadvance=110 page=0 chnl=15
|
||||
char id=90 x=6 y=642 width=102 height=112 xoffset=3 yoffset=49 xadvance=108 page=0 chnl=15
|
||||
char id=91 x=120 y=642 width=51 height=129 xoffset=10 yoffset=49 xadvance=67 page=0 chnl=15
|
||||
char id=92 x=183 y=642 width=57 height=114 xoffset=1 yoffset=48 xadvance=59 page=0 chnl=15
|
||||
char id=93 x=252 y=642 width=52 height=129 xoffset=6 yoffset=49 xadvance=67 page=0 chnl=15
|
||||
char id=94 x=316 y=642 width=97 height=75 xoffset=5 yoffset=48 xadvance=107 page=0 chnl=15
|
||||
char id=95 x=425 y=642 width=84 height=14 xoffset=0 yoffset=168 xadvance=83 page=0 chnl=15
|
||||
char id=96 x=521 y=642 width=40 height=36 xoffset=16 yoffset=36 xadvance=83 page=0 chnl=15
|
||||
char id=97 x=573 y=642 width=88 height=85 xoffset=5 yoffset=78 xadvance=100 page=0 chnl=15
|
||||
char id=98 x=673 y=642 width=88 height=120 xoffset=7 yoffset=43 xadvance=100 page=0 chnl=15
|
||||
char id=99 x=773 y=642 width=80 height=85 xoffset=5 yoffset=78 xadvance=87 page=0 chnl=15
|
||||
char id=100 x=865 y=642 width=88 height=120 xoffset=5 yoffset=43 xadvance=100 page=0 chnl=15
|
||||
char id=101 x=965 y=642 width=85 height=85 xoffset=5 yoffset=78 xadvance=93 page=0 chnl=15
|
||||
char id=102 x=1062 y=642 width=64 height=120 xoffset=7 yoffset=42 xadvance=69 page=0 chnl=15
|
||||
char id=103 x=1138 y=642 width=88 height=116 xoffset=5 yoffset=78 xadvance=100 page=0 chnl=15
|
||||
char id=104 x=1238 y=642 width=83 height=119 xoffset=7 yoffset=43 xadvance=96 page=0 chnl=15
|
||||
char id=105 x=1333 y=642 width=31 height=121 xoffset=6 yoffset=41 xadvance=43 page=0 chnl=15
|
||||
char id=106 x=1376 y=642 width=46 height=152 xoffset=-9 yoffset=41 xadvance=43 page=0 chnl=15
|
||||
char id=107 x=1434 y=642 width=79 height=119 xoffset=7 yoffset=43 xadvance=88 page=0 chnl=15
|
||||
char id=108 x=1525 y=642 width=29 height=119 xoffset=7 yoffset=43 xadvance=43 page=0 chnl=15
|
||||
char id=109 x=1566 y=642 width=126 height=84 xoffset=7 yoffset=78 xadvance=138 page=0 chnl=15
|
||||
char id=110 x=1704 y=642 width=83 height=84 xoffset=7 yoffset=78 xadvance=96 page=0 chnl=15
|
||||
char id=111 x=1799 y=642 width=88 height=85 xoffset=5 yoffset=78 xadvance=98 page=0 chnl=15
|
||||
char id=112 x=1899 y=642 width=88 height=115 xoffset=7 yoffset=78 xadvance=100 page=0 chnl=15
|
||||
char id=113 x=6 y=854 width=88 height=115 xoffset=5 yoffset=78 xadvance=100 page=0 chnl=15
|
||||
char id=114 x=106 y=854 width=63 height=83 xoffset=7 yoffset=79 xadvance=69 page=0 chnl=15
|
||||
char id=115 x=181 y=854 width=77 height=85 xoffset=4 yoffset=78 xadvance=84 page=0 chnl=15
|
||||
char id=116 x=270 y=854 width=63 height=111 xoffset=6 yoffset=52 xadvance=70 page=0 chnl=15
|
||||
char id=117 x=345 y=854 width=83 height=84 xoffset=6 yoffset=79 xadvance=96 page=0 chnl=15
|
||||
char id=118 x=440 y=854 width=86 height=83 xoffset=0 yoffset=79 xadvance=86 page=0 chnl=15
|
||||
char id=119 x=538 y=854 width=129 height=83 xoffset=0 yoffset=79 xadvance=128 page=0 chnl=15
|
||||
char id=120 x=679 y=854 width=81 height=83 xoffset=3 yoffset=79 xadvance=87 page=0 chnl=15
|
||||
char id=121 x=772 y=854 width=83 height=115 xoffset=6 yoffset=79 xadvance=96 page=0 chnl=15
|
||||
char id=122 x=867 y=854 width=79 height=81 xoffset=3 yoffset=80 xadvance=85 page=0 chnl=15
|
||||
char id=123 x=958 y=854 width=62 height=129 xoffset=2 yoffset=49 xadvance=70 page=0 chnl=15
|
||||
char id=124 x=1032 y=854 width=22 height=150 xoffset=16 yoffset=43 xadvance=53 page=0 chnl=15
|
||||
char id=125 x=1066 y=854 width=62 height=129 xoffset=6 yoffset=49 xadvance=70 page=0 chnl=15
|
||||
char id=126 x=1140 y=854 width=97 height=38 xoffset=5 yoffset=92 xadvance=107 page=0 chnl=15
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 24 KiB After Width: | Height: | Size: 192 KiB |
@@ -12,7 +12,7 @@ LANGUAGES_FILE = TRANSLATIONS_DIR / "languages.json"
|
||||
GLYPH_PADDING = 6
|
||||
EXTRA_CHARS = "–‑✓×°§•X⚙✕◀▶✔⌫⇧␣○●↳çêüñ–‑✓×°§•€£¥²⚠ⓘ"
|
||||
UNIFONT_LANGUAGES = {"ar", "th", "zh-CHT", "zh-CHS", "ko", "ja"}
|
||||
BRAND_FONT_CHARS = " StarPilotstarpilot?z"
|
||||
BRAND_FONT_CHARS = "".join(chr(codepoint) for codepoint in range(32, 127))
|
||||
|
||||
|
||||
def _languages():
|
||||
|
||||
@@ -469,8 +469,6 @@ class LatControlTorque(LatControl):
|
||||
friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD
|
||||
friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
|
||||
friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper)
|
||||
if self.is_honda_accord:
|
||||
ff *= get_honda_accord_ff_scale(setpoint)
|
||||
if flm_surface_active and self.flm_surface_profile_key and not ioniq_6_active:
|
||||
universal_flm_profile = self.flm_surface_profile_key == FLM_UNIVERSAL_PROFILE_KEY
|
||||
flm_full_surface_center_taper = get_flm_full_surface_center_taper_scale(self.flm_surface_profile_key, setpoint, CS.vEgo,
|
||||
@@ -582,8 +580,6 @@ class LatControlTorque(LatControl):
|
||||
output_torque *= get_ram_1500_center_output_scale(setpoint, CS.vEgo)
|
||||
if output_torque * setpoint > 0.0:
|
||||
output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
if setpoint * desired_lateral_jerk < 0.0:
|
||||
output_torque *= get_ram_1500_unwind_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
elif self.is_kona_non_scc:
|
||||
output_torque *= get_kona_non_scc_center_taper_scale(setpoint, CS.vEgo)
|
||||
rapid_reversal = setpoint * desired_lateral_jerk < 0.0
|
||||
|
||||
@@ -76,9 +76,6 @@ BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS
|
||||
HONDA_ACCORD_STEER_RATIO_SCALE = 14.0 / 16.33
|
||||
HONDA_ACCORD_TORQUE_KP = 0.8
|
||||
HONDA_ACCORD_TORQUE_KI = 0.15
|
||||
HONDA_ACCORD_TURN_FF_REDUCTION_MAX = 0.10
|
||||
HONDA_ACCORD_TURN_FF_ONSET = 0.45
|
||||
HONDA_ACCORD_TURN_FF_WIDTH = 0.12
|
||||
VOLT_STANDARD_CARS = (
|
||||
GM_CAR.CHEVROLET_VOLT,
|
||||
GM_CAR.CHEVROLET_VOLT_2019,
|
||||
@@ -436,7 +433,7 @@ GMC_YUKON_CC_UNWIND_FF_REDUCTION = 0.12
|
||||
|
||||
SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05
|
||||
SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09
|
||||
SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.26
|
||||
SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.22
|
||||
SONATA_HYBRID_FF_ONSET = 0.18
|
||||
SONATA_HYBRID_FF_ONSET_WIDTH = 0.08
|
||||
SONATA_HYBRID_FF_CUTOFF = 1.35
|
||||
@@ -447,7 +444,7 @@ SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12
|
||||
SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.02
|
||||
SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18
|
||||
SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10
|
||||
SONATA_HYBRID_CENTER_TAPER_MAX = 0.10
|
||||
SONATA_HYBRID_CENTER_TAPER_MAX = 0.07
|
||||
SONATA_HYBRID_CENTER_TAPER_LAT = 0.16
|
||||
SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025
|
||||
SONATA_HYBRID_CENTER_TAPER_SPEED = 22.0
|
||||
@@ -1181,13 +1178,6 @@ RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET = 5.5
|
||||
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET_WIDTH = 1.5
|
||||
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX = 16.0
|
||||
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX_WIDTH = 2.0
|
||||
RAM_1500_UNWIND_OUTPUT_TAPER_MAX = 0.18
|
||||
RAM_1500_UNWIND_OUTPUT_SPEED_ONSET = 20.0
|
||||
RAM_1500_UNWIND_OUTPUT_SPEED_FULL = 29.0
|
||||
RAM_1500_UNWIND_OUTPUT_JERK_ONSET = 0.50
|
||||
RAM_1500_UNWIND_OUTPUT_JERK_FULL = 1.80
|
||||
RAM_1500_UNWIND_OUTPUT_LAT_ONSET = 0.65
|
||||
RAM_1500_UNWIND_OUTPUT_LAT_WIDTH = 0.30
|
||||
|
||||
# The Kona route is exceptionally accurate below highway speed, but Pop V2
|
||||
# reverses the requested lateral acceleration roughly once per second at
|
||||
@@ -1797,23 +1787,6 @@ def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_l
|
||||
return 1.0 - (RAM_1500_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
|
||||
|
||||
|
||||
def get_ram_1500_unwind_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float,
|
||||
v_ego: float) -> float:
|
||||
"""Soften only rapid high-speed unwind reversals on the RAM 1500."""
|
||||
if desired_lateral_accel * desired_lateral_jerk >= 0.0:
|
||||
return 1.0
|
||||
|
||||
speed_weight = float(np.interp(v_ego,
|
||||
[RAM_1500_UNWIND_OUTPUT_SPEED_ONSET, RAM_1500_UNWIND_OUTPUT_SPEED_FULL],
|
||||
[0.0, 1.0]))
|
||||
jerk_weight = float(np.interp(abs(desired_lateral_jerk),
|
||||
[RAM_1500_UNWIND_OUTPUT_JERK_ONSET, RAM_1500_UNWIND_OUTPUT_JERK_FULL],
|
||||
[0.0, 1.0]))
|
||||
curve_weight = _sigmoid((abs(desired_lateral_accel) - RAM_1500_UNWIND_OUTPUT_LAT_ONSET) /
|
||||
RAM_1500_UNWIND_OUTPUT_LAT_WIDTH)
|
||||
return 1.0 - (RAM_1500_UNWIND_OUTPUT_TAPER_MAX * speed_weight * jerk_weight * curve_weight)
|
||||
|
||||
|
||||
def get_ram_1500_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
|
||||
"""Damp only center corrections at low/mid speed, not turn authority."""
|
||||
center_weight = _sigmoid((RAM_1500_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
|
||||
@@ -2032,13 +2005,6 @@ def get_honda_accord_steer_ratio_scale(_v_ego: float) -> float:
|
||||
return HONDA_ACCORD_STEER_RATIO_SCALE
|
||||
|
||||
|
||||
def get_honda_accord_ff_scale(desired_lateral_accel: float) -> float:
|
||||
"""Taper only sharp-turn feedforward where the Accord carries excess curvature."""
|
||||
turn_weight = _sigmoid((abs(desired_lateral_accel) - HONDA_ACCORD_TURN_FF_ONSET) /
|
||||
HONDA_ACCORD_TURN_FF_WIDTH)
|
||||
return 1.0 - (HONDA_ACCORD_TURN_FF_REDUCTION_MAX * turn_weight)
|
||||
|
||||
|
||||
def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
|
||||
center_window = _bolt_2017_sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH)
|
||||
return 1.0 - (BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window)
|
||||
|
||||
@@ -923,8 +923,7 @@ class LongitudinalMpc:
|
||||
def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow,
|
||||
personality=log.LongitudinalPersonality.standard, tracking_lead=True,
|
||||
optional_far_lead_comfort=True, smooth_duplicate_vision=False,
|
||||
stop_x=None, silverado_early_follow=False, modelV2=None,
|
||||
lead_obstacle_bias=(0.0, 0.0)):
|
||||
stop_x=None, silverado_early_follow=False, modelV2=None):
|
||||
v_ego = self.x0[1]
|
||||
lead_one = radarstate.leadOne
|
||||
lead_two = radarstate.leadTwo
|
||||
@@ -944,8 +943,6 @@ class LongitudinalMpc:
|
||||
# and then treat that as a stopped car/obstacle at this new distance.
|
||||
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
|
||||
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
|
||||
lead_0_obstacle -= float(lead_obstacle_bias[0])
|
||||
lead_1_obstacle -= float(lead_obstacle_bias[1])
|
||||
|
||||
self.params[:,0] = ACCEL_MIN
|
||||
self.params[:,1] = max(0.0, self.max_a)
|
||||
|
||||
@@ -21,8 +21,6 @@ from openpilot.selfdrive.controls.lib.lead_follow_policy import is_nonurgent_dup
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
|
||||
get_far_follow_output_slew_rates,
|
||||
get_follow_prebrake_min_headway,
|
||||
get_honda_accord_lead_departure_tune,
|
||||
get_toyota_rav4_tss2_lead_departure_tune,
|
||||
get_force_stop_distance_bias,
|
||||
get_force_stop_handoff_distance,
|
||||
allow_radar_standstill_gap_settle,
|
||||
@@ -32,7 +30,6 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
|
||||
is_toyota_rav4_tss2_radar_follow_lead,
|
||||
get_toyota_sienna_post_departure_restop_cap,
|
||||
get_untracked_slow_lead_decel_scale,
|
||||
get_toyota_prius_stopped_lead_obstacle_bias,
|
||||
)
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
@@ -1508,14 +1505,9 @@ class LongitudinalPlanner:
|
||||
max(LEAD_DEPART_ACCEL_HOLD_FULL_GAP - LEAD_DEPART_ACCEL_HOLD_MIN_GAP, 0.1), 0.0, 1.0))
|
||||
lead_factor = float(np.clip((lead_speed - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED) /
|
||||
max(LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED, 0.1), 0.0, 1.0))
|
||||
departure_tune = get_honda_accord_lead_departure_tune(self.CP)
|
||||
if departure_tune is None:
|
||||
departure_tune = get_toyota_rav4_tss2_lead_departure_tune(self.CP)
|
||||
max_accel = LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL if departure_tune is None else departure_tune[0]
|
||||
assist = LEAD_DEPART_ACCEL_ASSIST if departure_tune is None else departure_tune[1]
|
||||
accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (max_accel - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip(
|
||||
accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip(
|
||||
0.55 * lead_factor + 0.45 * gap_factor, 0.0, 1.0)
|
||||
assisted_model_accel = float(model_desired_accel) + assist
|
||||
assisted_model_accel = float(model_desired_accel) + LEAD_DEPART_ACCEL_ASSIST
|
||||
return min(accel_cap, max(assisted_model_accel, LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL))
|
||||
|
||||
def get_reusable_lead_depart_accel_floor(self, lead, v_ego, t_follow):
|
||||
@@ -2209,19 +2201,6 @@ class LongitudinalPlanner:
|
||||
get_force_stop_distance_bias(self.CP.carFingerprint)
|
||||
)
|
||||
|
||||
prius_lead_obstacle_bias = (0.0, 0.0)
|
||||
if (
|
||||
self.mode == 'acc' and
|
||||
not bool(getattr(sm['modelV2'].action, 'shouldStop', False)) and
|
||||
not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
|
||||
not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
|
||||
not bool(getattr(sm['carState'], 'standstill', False))
|
||||
):
|
||||
prius_lead_obstacle_bias = (
|
||||
get_toyota_prius_stopped_lead_obstacle_bias(self.CP, self.lead_one, scene_v_ego),
|
||||
get_toyota_prius_stopped_lead_obstacle_bias(self.CP, self.lead_two, scene_v_ego),
|
||||
)
|
||||
|
||||
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,
|
||||
sm['starpilotPlan'].dangerFactor, effective_t_follow,
|
||||
personality=personality, tracking_lead=lead_control_active,
|
||||
@@ -2229,8 +2208,7 @@ class LongitudinalPlanner:
|
||||
smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass,
|
||||
stop_x=force_stop_x,
|
||||
silverado_early_follow=early_truck_follow,
|
||||
modelV2=sm['modelV2'],
|
||||
lead_obstacle_bias=prius_lead_obstacle_bias)
|
||||
modelV2=sm['modelV2'])
|
||||
|
||||
self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
|
||||
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
|
||||
|
||||
@@ -4,8 +4,6 @@ import numpy as np
|
||||
HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0
|
||||
HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0
|
||||
HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35
|
||||
HONDA_ACCORD_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.85
|
||||
HONDA_ACCORD_LEAD_DEPART_ACCEL_ASSIST = 0.25
|
||||
HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25
|
||||
GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED = 18.0
|
||||
GM_SILVERADO_EARLY_FOLLOW_MAX_DISTANCE = 130.0
|
||||
@@ -38,15 +36,6 @@ TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_OFFSET = 32.0
|
||||
TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_LATERAL_OFFSET = 1.75
|
||||
TOYOTA_RAV4_TSS2_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5
|
||||
TOYOTA_RAV4_TSS2_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75
|
||||
TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.70
|
||||
TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST = 0.20
|
||||
TOYOTA_PRIUS_STOPPED_LEAD_OBSTACLE_BIAS_M = 1.5
|
||||
TOYOTA_PRIUS_STOPPED_LEAD_MAX_EGO_SPEED = 22.0
|
||||
TOYOTA_PRIUS_STOPPED_LEAD_MAX_SPEED = 1.0
|
||||
TOYOTA_PRIUS_STOPPED_LEAD_MIN_CLOSING_SPEED = 0.15
|
||||
TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE = 80.0
|
||||
TOYOTA_PRIUS_STOPPED_LEAD_RAMP_DISTANCE = 10.0
|
||||
TOYOTA_PRIUS_STOPPED_LEAD_MAX_LATERAL_OFFSET = 1.75
|
||||
TOYOTA_CAMRY_TSS2_FORCE_STOP_HANDOFF_M = 4.5
|
||||
# The Camry's force-stop path otherwise consumes the model endpoint before the
|
||||
# normal MPC stop-distance margin can be applied. Keep it within the forward
|
||||
@@ -55,35 +44,6 @@ TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M = 6.0
|
||||
DEFAULT_FORCE_STOP_HANDOFF_M = 6.0
|
||||
|
||||
|
||||
def get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego):
|
||||
"""Move the ordinary Prius stopped-lead target back without touching stop targets."""
|
||||
if (
|
||||
getattr(CP, "brand", "") != "toyota" or
|
||||
str(getattr(CP, "carFingerprint", "")) != "TOYOTA_PRIUS" or
|
||||
lead is None or not bool(getattr(lead, "status", False)) or
|
||||
float(v_ego) <= 0.0 or float(v_ego) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_EGO_SPEED or
|
||||
float(getattr(lead, "vLead", 0.0)) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_SPEED or
|
||||
abs(float(getattr(lead, "yRel", 0.0))) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_LATERAL_OFFSET
|
||||
):
|
||||
return 0.0
|
||||
|
||||
distance = float(getattr(lead, "dRel", float("inf")))
|
||||
closing_speed = float(v_ego) - float(getattr(lead, "vLead", 0.0))
|
||||
if (
|
||||
distance <= 0.0 or distance > TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE or
|
||||
closing_speed < TOYOTA_PRIUS_STOPPED_LEAD_MIN_CLOSING_SPEED
|
||||
):
|
||||
return 0.0
|
||||
|
||||
strength = np.clip(
|
||||
(TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE - distance) /
|
||||
(TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE - TOYOTA_PRIUS_STOPPED_LEAD_RAMP_DISTANCE),
|
||||
0.0, 1.0,
|
||||
)
|
||||
bias = TOYOTA_PRIUS_STOPPED_LEAD_OBSTACLE_BIAS_M * strength
|
||||
return float(min(bias, max(distance - 0.5, 0.0)))
|
||||
|
||||
|
||||
def is_toyota_rav4_tss2_post_departure_tune(CP):
|
||||
"""Identify RAV4 TSS2 variants that need normal catch-up caps after departure."""
|
||||
return (
|
||||
@@ -92,15 +52,6 @@ def is_toyota_rav4_tss2_post_departure_tune(CP):
|
||||
)
|
||||
|
||||
|
||||
def get_toyota_rav4_tss2_lead_departure_tune(CP):
|
||||
if is_toyota_rav4_tss2_post_departure_tune(CP):
|
||||
return (
|
||||
TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL,
|
||||
TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST,
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
def get_toyota_rav4_tss2_early_lead_cap(CP, lead, v_ego, accel_min):
|
||||
"""Start a mild RAV4 coast/brake response before a hard lead approach."""
|
||||
if (
|
||||
@@ -204,15 +155,6 @@ def get_lead_follow_jerk_scale(CP):
|
||||
return 1.0
|
||||
|
||||
|
||||
def get_honda_accord_lead_departure_tune(CP):
|
||||
if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD":
|
||||
return (
|
||||
HONDA_ACCORD_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL,
|
||||
HONDA_ACCORD_LEAD_DEPART_ACCEL_ASSIST,
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
def is_gm_silverado_early_follow_lead(CP, lead, v_ego):
|
||||
"""Admit a credible centered vision lead before it becomes a close lead."""
|
||||
if (
|
||||
|
||||
@@ -43,7 +43,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
|
||||
get_gmc_yukon_cc_ff_scale,
|
||||
get_ram_1500_center_output_scale,
|
||||
get_ram_1500_transition_output_scale,
|
||||
get_ram_1500_unwind_output_scale,
|
||||
get_ram_1500_ff_scale,
|
||||
get_rav4_tss2_pid_output,
|
||||
get_subaru_impreza_pid_output_scale,
|
||||
@@ -87,7 +86,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_genesis_gv70_friction_threshold,
|
||||
get_genesis_gv70_high_speed_error_scale,
|
||||
get_genesis_gv70_unwind_ff_scale,
|
||||
get_honda_accord_ff_scale,
|
||||
get_elantra_non_scc_ff_scale,
|
||||
get_honda_accord_steer_ratio_scale,
|
||||
get_palisade_ff_scale,
|
||||
@@ -1081,18 +1079,6 @@ class TestLatControl:
|
||||
assert crawl > center
|
||||
assert center > 0.85
|
||||
|
||||
def test_ram_1500_unwind_output_taper_is_high_speed_and_phase_gated(self):
|
||||
turn_in = get_ram_1500_unwind_output_scale(1.2, 1.1, 25.0)
|
||||
low_speed = get_ram_1500_unwind_output_scale(1.2, -1.1, 15.0)
|
||||
high_speed = get_ram_1500_unwind_output_scale(1.2, -1.1, 25.0)
|
||||
sharp_reversal = get_ram_1500_unwind_output_scale(2.4, -2.0, 29.0)
|
||||
|
||||
assert turn_in == pytest.approx(1.0)
|
||||
assert low_speed == pytest.approx(1.0)
|
||||
assert 0.95 < high_speed < 1.0
|
||||
assert sharp_reversal < high_speed
|
||||
assert sharp_reversal > 0.80
|
||||
|
||||
def test_ram_1500_phase_feedforward_curve(self):
|
||||
assert get_ram_1500_ff_scale(0.0, 1.0, 15.0) == pytest.approx(1.0)
|
||||
assert get_ram_1500_ff_scale(1.2, 1.1, 17.0) > 1.0
|
||||
@@ -1775,11 +1761,6 @@ class TestLatControl:
|
||||
assert get_honda_accord_steer_ratio_scale(0.0) == pytest.approx(expected_scale)
|
||||
assert get_honda_accord_steer_ratio_scale(20.0) == pytest.approx(expected_scale)
|
||||
|
||||
def test_honda_accord_turn_feedforward_taper(self):
|
||||
assert get_honda_accord_ff_scale(0.0) > get_honda_accord_ff_scale(0.8)
|
||||
assert get_honda_accord_ff_scale(-0.8) == pytest.approx(get_honda_accord_ff_scale(0.8))
|
||||
assert get_honda_accord_ff_scale(0.0) == pytest.approx(1.0, abs=0.01)
|
||||
|
||||
def test_subaru_impreza_pid_output_scale_preserves_small_errors(self):
|
||||
assert get_subaru_impreza_pid_output_scale(0.0) == 1.0
|
||||
assert get_subaru_impreza_pid_output_scale(0.75) == 1.0
|
||||
|
||||
@@ -27,9 +27,6 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
|
||||
allow_radar_standstill_gap_settle,
|
||||
get_far_follow_output_slew_rates,
|
||||
get_follow_prebrake_min_headway,
|
||||
get_honda_accord_lead_departure_tune,
|
||||
get_toyota_prius_stopped_lead_obstacle_bias,
|
||||
get_toyota_rav4_tss2_lead_departure_tune,
|
||||
get_toyota_rav4_tss2_early_lead_cap,
|
||||
get_toyota_sienna_post_departure_restop_cap,
|
||||
is_toyota_rav4_tss2_radar_follow_lead,
|
||||
@@ -94,28 +91,6 @@ def test_mpc_duplicate_lead_filters_do_not_cross_contaminate_tracks():
|
||||
assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0)
|
||||
|
||||
|
||||
def test_prius_stopped_lead_obstacle_bias_is_small_and_vehicle_specific():
|
||||
prius = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_PRIUS)
|
||||
other = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
stopped_lead = make_lead(status=True, d_rel=18.0, v_lead=0.2, model_prob=0.99)
|
||||
|
||||
bias = get_toyota_prius_stopped_lead_obstacle_bias(prius, stopped_lead, v_ego=8.0)
|
||||
assert 0.0 < bias < 1.5
|
||||
assert get_toyota_prius_stopped_lead_obstacle_bias(other, stopped_lead, v_ego=8.0) == pytest.approx(0.0)
|
||||
assert get_toyota_prius_stopped_lead_obstacle_bias(
|
||||
prius, make_lead(status=True, d_rel=18.0, v_lead=4.0), v_ego=8.0,
|
||||
) == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_prius_stopped_lead_obstacle_bias_does_not_apply_at_standstill_or_to_departures():
|
||||
prius = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_PRIUS)
|
||||
stopped_lead = make_lead(status=True, d_rel=4.0, v_lead=0.0, model_prob=0.99)
|
||||
departing_lead = make_lead(status=True, d_rel=18.0, v_lead=2.0, model_prob=0.99)
|
||||
|
||||
assert get_toyota_prius_stopped_lead_obstacle_bias(prius, stopped_lead, v_ego=0.0) == pytest.approx(0.0)
|
||||
assert get_toyota_prius_stopped_lead_obstacle_bias(prius, departing_lead, v_ego=8.0) == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_mpc_duplicate_vision_filter_smooths_distance_jumps_per_track():
|
||||
mpc = LongitudinalMpc()
|
||||
mpc.set_cur_state(27.0, 0.0)
|
||||
@@ -2504,55 +2479,6 @@ def test_route_251682_rav4_confirmed_depart_adds_bounded_accel_assist():
|
||||
assert floor <= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL
|
||||
|
||||
|
||||
def test_honda_accord_lead_departure_assist_is_stronger_but_vehicle_scoped():
|
||||
accord = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD)
|
||||
civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(accord, init_v=0.0)
|
||||
lead = make_lead(
|
||||
status=True,
|
||||
d_rel=7.7,
|
||||
v_lead=2.0,
|
||||
a_lead=1.79,
|
||||
radar=False,
|
||||
model_prob=1.0,
|
||||
)
|
||||
|
||||
accord_floor = planner.get_lead_depart_accel_floor(lead, v_ego=0.0, model_desired_accel=0.44)
|
||||
civic_floor = LongitudinalPlanner(civic, init_v=0.0).get_lead_depart_accel_floor(
|
||||
lead, v_ego=0.0, model_desired_accel=0.44,
|
||||
)
|
||||
|
||||
assert get_honda_accord_lead_departure_tune(accord) is not None
|
||||
assert get_honda_accord_lead_departure_tune(civic) is None
|
||||
assert accord_floor > civic_floor
|
||||
assert accord_floor <= get_honda_accord_lead_departure_tune(accord)[0]
|
||||
|
||||
|
||||
def test_rav4_tss2_lead_departure_assist_is_vehicle_scoped():
|
||||
rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
|
||||
civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
lead = make_lead(
|
||||
status=True,
|
||||
d_rel=7.7,
|
||||
v_lead=2.0,
|
||||
a_lead=1.79,
|
||||
radar=False,
|
||||
model_prob=1.0,
|
||||
)
|
||||
|
||||
rav4_floor = LongitudinalPlanner(rav4, init_v=0.0).get_lead_depart_accel_floor(
|
||||
lead, v_ego=0.0, model_desired_accel=0.44,
|
||||
)
|
||||
civic_floor = LongitudinalPlanner(civic, init_v=0.0).get_lead_depart_accel_floor(
|
||||
lead, v_ego=0.0, model_desired_accel=0.44,
|
||||
)
|
||||
|
||||
assert get_toyota_rav4_tss2_lead_departure_tune(rav4) is not None
|
||||
assert get_toyota_rav4_tss2_lead_departure_tune(civic) is None
|
||||
assert rav4_floor > civic_floor
|
||||
assert rav4_floor == pytest.approx(0.64)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
|
||||
def test_standstill_depart_accel_hold_reuses_floor_through_softening_lead_delta(model_version):
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
|
||||
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
|
||||
* *
|
||||
* This file is part of 'ekf' *
|
||||
******************************************************************************/
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_8339538066892951453) {
|
||||
out_8339538066892951453[0] = delta_x[0] + nom_x[0];
|
||||
out_8339538066892951453[1] = delta_x[1] + nom_x[1];
|
||||
out_8339538066892951453[2] = delta_x[2] + nom_x[2];
|
||||
out_8339538066892951453[3] = delta_x[3] + nom_x[3];
|
||||
out_8339538066892951453[4] = delta_x[4] + nom_x[4];
|
||||
out_8339538066892951453[5] = delta_x[5] + nom_x[5];
|
||||
out_8339538066892951453[6] = delta_x[6] + nom_x[6];
|
||||
out_8339538066892951453[7] = delta_x[7] + nom_x[7];
|
||||
out_8339538066892951453[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_4436222918597397805) {
|
||||
out_4436222918597397805[0] = delta_x[0] + nom_x[0];
|
||||
out_4436222918597397805[1] = delta_x[1] + nom_x[1];
|
||||
out_4436222918597397805[2] = delta_x[2] + nom_x[2];
|
||||
out_4436222918597397805[3] = delta_x[3] + nom_x[3];
|
||||
out_4436222918597397805[4] = delta_x[4] + nom_x[4];
|
||||
out_4436222918597397805[5] = delta_x[5] + nom_x[5];
|
||||
out_4436222918597397805[6] = delta_x[6] + nom_x[6];
|
||||
out_4436222918597397805[7] = delta_x[7] + nom_x[7];
|
||||
out_4436222918597397805[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_1869575881362213489) {
|
||||
out_1869575881362213489[0] = -nom_x[0] + true_x[0];
|
||||
out_1869575881362213489[1] = -nom_x[1] + true_x[1];
|
||||
out_1869575881362213489[2] = -nom_x[2] + true_x[2];
|
||||
out_1869575881362213489[3] = -nom_x[3] + true_x[3];
|
||||
out_1869575881362213489[4] = -nom_x[4] + true_x[4];
|
||||
out_1869575881362213489[5] = -nom_x[5] + true_x[5];
|
||||
out_1869575881362213489[6] = -nom_x[6] + true_x[6];
|
||||
out_1869575881362213489[7] = -nom_x[7] + true_x[7];
|
||||
out_1869575881362213489[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894) {
|
||||
out_5455437002668972894[0] = -nom_x[0] + true_x[0];
|
||||
out_5455437002668972894[1] = -nom_x[1] + true_x[1];
|
||||
out_5455437002668972894[2] = -nom_x[2] + true_x[2];
|
||||
out_5455437002668972894[3] = -nom_x[3] + true_x[3];
|
||||
out_5455437002668972894[4] = -nom_x[4] + true_x[4];
|
||||
out_5455437002668972894[5] = -nom_x[5] + true_x[5];
|
||||
out_5455437002668972894[6] = -nom_x[6] + true_x[6];
|
||||
out_5455437002668972894[7] = -nom_x[7] + true_x[7];
|
||||
out_5455437002668972894[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_4134445094065349854) {
|
||||
out_4134445094065349854[0] = 1.0;
|
||||
out_4134445094065349854[1] = 0.0;
|
||||
out_4134445094065349854[2] = 0.0;
|
||||
out_4134445094065349854[3] = 0.0;
|
||||
out_4134445094065349854[4] = 0.0;
|
||||
out_4134445094065349854[5] = 0.0;
|
||||
out_4134445094065349854[6] = 0.0;
|
||||
out_4134445094065349854[7] = 0.0;
|
||||
out_4134445094065349854[8] = 0.0;
|
||||
out_4134445094065349854[9] = 0.0;
|
||||
out_4134445094065349854[10] = 1.0;
|
||||
out_4134445094065349854[11] = 0.0;
|
||||
out_4134445094065349854[12] = 0.0;
|
||||
out_4134445094065349854[13] = 0.0;
|
||||
out_4134445094065349854[14] = 0.0;
|
||||
out_4134445094065349854[15] = 0.0;
|
||||
out_4134445094065349854[16] = 0.0;
|
||||
out_4134445094065349854[17] = 0.0;
|
||||
out_4134445094065349854[18] = 0.0;
|
||||
out_4134445094065349854[19] = 0.0;
|
||||
out_4134445094065349854[20] = 1.0;
|
||||
out_4134445094065349854[21] = 0.0;
|
||||
out_4134445094065349854[22] = 0.0;
|
||||
out_4134445094065349854[23] = 0.0;
|
||||
out_4134445094065349854[24] = 0.0;
|
||||
out_4134445094065349854[25] = 0.0;
|
||||
out_4134445094065349854[26] = 0.0;
|
||||
out_4134445094065349854[27] = 0.0;
|
||||
out_4134445094065349854[28] = 0.0;
|
||||
out_4134445094065349854[29] = 0.0;
|
||||
out_4134445094065349854[30] = 1.0;
|
||||
out_4134445094065349854[31] = 0.0;
|
||||
out_4134445094065349854[32] = 0.0;
|
||||
out_4134445094065349854[33] = 0.0;
|
||||
out_4134445094065349854[34] = 0.0;
|
||||
out_4134445094065349854[35] = 0.0;
|
||||
out_4134445094065349854[36] = 0.0;
|
||||
out_4134445094065349854[37] = 0.0;
|
||||
out_4134445094065349854[38] = 0.0;
|
||||
out_4134445094065349854[39] = 0.0;
|
||||
out_4134445094065349854[40] = 1.0;
|
||||
out_4134445094065349854[41] = 0.0;
|
||||
out_4134445094065349854[42] = 0.0;
|
||||
out_4134445094065349854[43] = 0.0;
|
||||
out_4134445094065349854[44] = 0.0;
|
||||
out_4134445094065349854[45] = 0.0;
|
||||
out_4134445094065349854[46] = 0.0;
|
||||
out_4134445094065349854[47] = 0.0;
|
||||
out_4134445094065349854[48] = 0.0;
|
||||
out_4134445094065349854[49] = 0.0;
|
||||
out_4134445094065349854[50] = 1.0;
|
||||
out_4134445094065349854[51] = 0.0;
|
||||
out_4134445094065349854[52] = 0.0;
|
||||
out_4134445094065349854[53] = 0.0;
|
||||
out_4134445094065349854[54] = 0.0;
|
||||
out_4134445094065349854[55] = 0.0;
|
||||
out_4134445094065349854[56] = 0.0;
|
||||
out_4134445094065349854[57] = 0.0;
|
||||
out_4134445094065349854[58] = 0.0;
|
||||
out_4134445094065349854[59] = 0.0;
|
||||
out_4134445094065349854[60] = 1.0;
|
||||
out_4134445094065349854[61] = 0.0;
|
||||
out_4134445094065349854[62] = 0.0;
|
||||
out_4134445094065349854[63] = 0.0;
|
||||
out_4134445094065349854[64] = 0.0;
|
||||
out_4134445094065349854[65] = 0.0;
|
||||
out_4134445094065349854[66] = 0.0;
|
||||
out_4134445094065349854[67] = 0.0;
|
||||
out_4134445094065349854[68] = 0.0;
|
||||
out_4134445094065349854[69] = 0.0;
|
||||
out_4134445094065349854[70] = 1.0;
|
||||
out_4134445094065349854[71] = 0.0;
|
||||
out_4134445094065349854[72] = 0.0;
|
||||
out_4134445094065349854[73] = 0.0;
|
||||
out_4134445094065349854[74] = 0.0;
|
||||
out_4134445094065349854[75] = 0.0;
|
||||
out_4134445094065349854[76] = 0.0;
|
||||
out_4134445094065349854[77] = 0.0;
|
||||
out_4134445094065349854[78] = 0.0;
|
||||
out_4134445094065349854[79] = 0.0;
|
||||
out_4134445094065349854[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_210375879520347438) {
|
||||
out_210375879520347438[0] = 1.0;
|
||||
out_210375879520347438[1] = 0.0;
|
||||
out_210375879520347438[2] = 0.0;
|
||||
out_210375879520347438[3] = 0.0;
|
||||
out_210375879520347438[4] = 0.0;
|
||||
out_210375879520347438[5] = 0.0;
|
||||
out_210375879520347438[6] = 0.0;
|
||||
out_210375879520347438[7] = 0.0;
|
||||
out_210375879520347438[8] = 0.0;
|
||||
out_210375879520347438[9] = 0.0;
|
||||
out_210375879520347438[10] = 1.0;
|
||||
out_210375879520347438[11] = 0.0;
|
||||
out_210375879520347438[12] = 0.0;
|
||||
out_210375879520347438[13] = 0.0;
|
||||
out_210375879520347438[14] = 0.0;
|
||||
out_210375879520347438[15] = 0.0;
|
||||
out_210375879520347438[16] = 0.0;
|
||||
out_210375879520347438[17] = 0.0;
|
||||
out_210375879520347438[18] = 0.0;
|
||||
out_210375879520347438[19] = 0.0;
|
||||
out_210375879520347438[20] = 1.0;
|
||||
out_210375879520347438[21] = 0.0;
|
||||
out_210375879520347438[22] = 0.0;
|
||||
out_210375879520347438[23] = 0.0;
|
||||
out_210375879520347438[24] = 0.0;
|
||||
out_210375879520347438[25] = 0.0;
|
||||
out_210375879520347438[26] = 0.0;
|
||||
out_210375879520347438[27] = 0.0;
|
||||
out_210375879520347438[28] = 0.0;
|
||||
out_210375879520347438[29] = 0.0;
|
||||
out_210375879520347438[30] = 1.0;
|
||||
out_210375879520347438[31] = 0.0;
|
||||
out_210375879520347438[32] = 0.0;
|
||||
out_210375879520347438[33] = 0.0;
|
||||
out_210375879520347438[34] = 0.0;
|
||||
out_210375879520347438[35] = 0.0;
|
||||
out_210375879520347438[36] = 0.0;
|
||||
out_210375879520347438[37] = 0.0;
|
||||
out_210375879520347438[38] = 0.0;
|
||||
out_210375879520347438[39] = 0.0;
|
||||
out_210375879520347438[40] = 1.0;
|
||||
out_210375879520347438[41] = 0.0;
|
||||
out_210375879520347438[42] = 0.0;
|
||||
out_210375879520347438[43] = 0.0;
|
||||
out_210375879520347438[44] = 0.0;
|
||||
out_210375879520347438[45] = 0.0;
|
||||
out_210375879520347438[46] = 0.0;
|
||||
out_210375879520347438[47] = 0.0;
|
||||
out_210375879520347438[48] = 0.0;
|
||||
out_210375879520347438[49] = 0.0;
|
||||
out_210375879520347438[50] = 1.0;
|
||||
out_210375879520347438[51] = 0.0;
|
||||
out_210375879520347438[52] = 0.0;
|
||||
out_210375879520347438[53] = 0.0;
|
||||
out_210375879520347438[54] = 0.0;
|
||||
out_210375879520347438[55] = 0.0;
|
||||
out_210375879520347438[56] = 0.0;
|
||||
out_210375879520347438[57] = 0.0;
|
||||
out_210375879520347438[58] = 0.0;
|
||||
out_210375879520347438[59] = 0.0;
|
||||
out_210375879520347438[60] = 1.0;
|
||||
out_210375879520347438[61] = 0.0;
|
||||
out_210375879520347438[62] = 0.0;
|
||||
out_210375879520347438[63] = 0.0;
|
||||
out_210375879520347438[64] = 0.0;
|
||||
out_210375879520347438[65] = 0.0;
|
||||
out_210375879520347438[66] = 0.0;
|
||||
out_210375879520347438[67] = 0.0;
|
||||
out_210375879520347438[68] = 0.0;
|
||||
out_210375879520347438[69] = 0.0;
|
||||
out_210375879520347438[70] = 1.0;
|
||||
out_210375879520347438[71] = 0.0;
|
||||
out_210375879520347438[72] = 0.0;
|
||||
out_210375879520347438[73] = 0.0;
|
||||
out_210375879520347438[74] = 0.0;
|
||||
out_210375879520347438[75] = 0.0;
|
||||
out_210375879520347438[76] = 0.0;
|
||||
out_210375879520347438[77] = 0.0;
|
||||
out_210375879520347438[78] = 0.0;
|
||||
out_210375879520347438[79] = 0.0;
|
||||
out_210375879520347438[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_1187519494066628558) {
|
||||
out_1187519494066628558[0] = state[0];
|
||||
out_1187519494066628558[1] = state[1];
|
||||
out_1187519494066628558[2] = state[2];
|
||||
out_1187519494066628558[3] = state[3];
|
||||
out_1187519494066628558[4] = state[4];
|
||||
out_1187519494066628558[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_1187519494066628558[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_1187519494066628558[7] = state[7];
|
||||
out_1187519494066628558[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_467846126367206141) {
|
||||
out_467846126367206141[0] = state[0];
|
||||
out_467846126367206141[1] = state[1];
|
||||
out_467846126367206141[2] = state[2];
|
||||
out_467846126367206141[3] = state[3];
|
||||
out_467846126367206141[4] = state[4];
|
||||
out_467846126367206141[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_467846126367206141[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_467846126367206141[7] = state[7];
|
||||
out_467846126367206141[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_638221139616797346) {
|
||||
out_638221139616797346[0] = 1;
|
||||
out_638221139616797346[1] = 0;
|
||||
out_638221139616797346[2] = 0;
|
||||
out_638221139616797346[3] = 0;
|
||||
out_638221139616797346[4] = 0;
|
||||
out_638221139616797346[5] = 0;
|
||||
out_638221139616797346[6] = 0;
|
||||
out_638221139616797346[7] = 0;
|
||||
out_638221139616797346[8] = 0;
|
||||
out_638221139616797346[9] = 0;
|
||||
out_638221139616797346[10] = 1;
|
||||
out_638221139616797346[11] = 0;
|
||||
out_638221139616797346[12] = 0;
|
||||
out_638221139616797346[13] = 0;
|
||||
out_638221139616797346[14] = 0;
|
||||
out_638221139616797346[15] = 0;
|
||||
out_638221139616797346[16] = 0;
|
||||
out_638221139616797346[17] = 0;
|
||||
out_638221139616797346[18] = 0;
|
||||
out_638221139616797346[19] = 0;
|
||||
out_638221139616797346[20] = 1;
|
||||
out_638221139616797346[21] = 0;
|
||||
out_638221139616797346[22] = 0;
|
||||
out_638221139616797346[23] = 0;
|
||||
out_638221139616797346[24] = 0;
|
||||
out_638221139616797346[25] = 0;
|
||||
out_638221139616797346[26] = 0;
|
||||
out_638221139616797346[27] = 0;
|
||||
out_638221139616797346[28] = 0;
|
||||
out_638221139616797346[29] = 0;
|
||||
out_638221139616797346[30] = 1;
|
||||
out_638221139616797346[31] = 0;
|
||||
out_638221139616797346[32] = 0;
|
||||
out_638221139616797346[33] = 0;
|
||||
out_638221139616797346[34] = 0;
|
||||
out_638221139616797346[35] = 0;
|
||||
out_638221139616797346[36] = 0;
|
||||
out_638221139616797346[37] = 0;
|
||||
out_638221139616797346[38] = 0;
|
||||
out_638221139616797346[39] = 0;
|
||||
out_638221139616797346[40] = 1;
|
||||
out_638221139616797346[41] = 0;
|
||||
out_638221139616797346[42] = 0;
|
||||
out_638221139616797346[43] = 0;
|
||||
out_638221139616797346[44] = 0;
|
||||
out_638221139616797346[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_638221139616797346[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_638221139616797346[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_638221139616797346[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_638221139616797346[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_638221139616797346[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_638221139616797346[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_638221139616797346[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_638221139616797346[53] = -9.8100000000000005*dt;
|
||||
out_638221139616797346[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_638221139616797346[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_638221139616797346[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_638221139616797346[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_638221139616797346[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_638221139616797346[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_638221139616797346[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_638221139616797346[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_638221139616797346[62] = 0;
|
||||
out_638221139616797346[63] = 0;
|
||||
out_638221139616797346[64] = 0;
|
||||
out_638221139616797346[65] = 0;
|
||||
out_638221139616797346[66] = 0;
|
||||
out_638221139616797346[67] = 0;
|
||||
out_638221139616797346[68] = 0;
|
||||
out_638221139616797346[69] = 0;
|
||||
out_638221139616797346[70] = 1;
|
||||
out_638221139616797346[71] = 0;
|
||||
out_638221139616797346[72] = 0;
|
||||
out_638221139616797346[73] = 0;
|
||||
out_638221139616797346[74] = 0;
|
||||
out_638221139616797346[75] = 0;
|
||||
out_638221139616797346[76] = 0;
|
||||
out_638221139616797346[77] = 0;
|
||||
out_638221139616797346[78] = 0;
|
||||
out_638221139616797346[79] = 0;
|
||||
out_638221139616797346[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_3146439140780538523) {
|
||||
out_3146439140780538523[0] = 1;
|
||||
out_3146439140780538523[1] = 0;
|
||||
out_3146439140780538523[2] = 0;
|
||||
out_3146439140780538523[3] = 0;
|
||||
out_3146439140780538523[4] = 0;
|
||||
out_3146439140780538523[5] = 0;
|
||||
out_3146439140780538523[6] = 0;
|
||||
out_3146439140780538523[7] = 0;
|
||||
out_3146439140780538523[8] = 0;
|
||||
out_3146439140780538523[9] = 0;
|
||||
out_3146439140780538523[10] = 1;
|
||||
out_3146439140780538523[11] = 0;
|
||||
out_3146439140780538523[12] = 0;
|
||||
out_3146439140780538523[13] = 0;
|
||||
out_3146439140780538523[14] = 0;
|
||||
out_3146439140780538523[15] = 0;
|
||||
out_3146439140780538523[16] = 0;
|
||||
out_3146439140780538523[17] = 0;
|
||||
out_3146439140780538523[18] = 0;
|
||||
out_3146439140780538523[19] = 0;
|
||||
out_3146439140780538523[20] = 1;
|
||||
out_3146439140780538523[21] = 0;
|
||||
out_3146439140780538523[22] = 0;
|
||||
out_3146439140780538523[23] = 0;
|
||||
out_3146439140780538523[24] = 0;
|
||||
out_3146439140780538523[25] = 0;
|
||||
out_3146439140780538523[26] = 0;
|
||||
out_3146439140780538523[27] = 0;
|
||||
out_3146439140780538523[28] = 0;
|
||||
out_3146439140780538523[29] = 0;
|
||||
out_3146439140780538523[30] = 1;
|
||||
out_3146439140780538523[31] = 0;
|
||||
out_3146439140780538523[32] = 0;
|
||||
out_3146439140780538523[33] = 0;
|
||||
out_3146439140780538523[34] = 0;
|
||||
out_3146439140780538523[35] = 0;
|
||||
out_3146439140780538523[36] = 0;
|
||||
out_3146439140780538523[37] = 0;
|
||||
out_3146439140780538523[38] = 0;
|
||||
out_3146439140780538523[39] = 0;
|
||||
out_3146439140780538523[40] = 1;
|
||||
out_3146439140780538523[41] = 0;
|
||||
out_3146439140780538523[42] = 0;
|
||||
out_3146439140780538523[43] = 0;
|
||||
out_3146439140780538523[44] = 0;
|
||||
out_3146439140780538523[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_3146439140780538523[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_3146439140780538523[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3146439140780538523[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3146439140780538523[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_3146439140780538523[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_3146439140780538523[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_3146439140780538523[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3146439140780538523[53] = -9.8100000000000005*dt;
|
||||
out_3146439140780538523[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_3146439140780538523[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_3146439140780538523[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3146439140780538523[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3146439140780538523[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_3146439140780538523[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_3146439140780538523[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_3146439140780538523[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3146439140780538523[62] = 0;
|
||||
out_3146439140780538523[63] = 0;
|
||||
out_3146439140780538523[64] = 0;
|
||||
out_3146439140780538523[65] = 0;
|
||||
out_3146439140780538523[66] = 0;
|
||||
out_3146439140780538523[67] = 0;
|
||||
out_3146439140780538523[68] = 0;
|
||||
out_3146439140780538523[69] = 0;
|
||||
out_3146439140780538523[70] = 1;
|
||||
out_3146439140780538523[71] = 0;
|
||||
out_3146439140780538523[72] = 0;
|
||||
out_3146439140780538523[73] = 0;
|
||||
out_3146439140780538523[74] = 0;
|
||||
out_3146439140780538523[75] = 0;
|
||||
out_3146439140780538523[76] = 0;
|
||||
out_3146439140780538523[77] = 0;
|
||||
out_3146439140780538523[78] = 0;
|
||||
out_3146439140780538523[79] = 0;
|
||||
out_3146439140780538523[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_6007362445110034756) {
|
||||
out_6007362445110034756[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_3524127915259881655) {
|
||||
out_3524127915259881655[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_5119110753731964525) {
|
||||
out_5119110753731964525[0] = 0;
|
||||
out_5119110753731964525[1] = 0;
|
||||
out_5119110753731964525[2] = 0;
|
||||
out_5119110753731964525[3] = 0;
|
||||
out_5119110753731964525[4] = 0;
|
||||
out_5119110753731964525[5] = 0;
|
||||
out_5119110753731964525[6] = 1;
|
||||
out_5119110753731964525[7] = 0;
|
||||
out_5119110753731964525[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_3733424518165712586) {
|
||||
out_3733424518165712586[0] = 0;
|
||||
out_3733424518165712586[1] = 0;
|
||||
out_3733424518165712586[2] = 0;
|
||||
out_3733424518165712586[3] = 0;
|
||||
out_3733424518165712586[4] = 0;
|
||||
out_3733424518165712586[5] = 0;
|
||||
out_3733424518165712586[6] = 1;
|
||||
out_3733424518165712586[7] = 0;
|
||||
out_3733424518165712586[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_5449620096162801389) {
|
||||
out_5449620096162801389[0] = state[4];
|
||||
out_5449620096162801389[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_5258226883159248845) {
|
||||
out_5258226883159248845[0] = state[4];
|
||||
out_5258226883159248845[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_2946461154726464959) {
|
||||
out_2946461154726464959[0] = 0;
|
||||
out_2946461154726464959[1] = 0;
|
||||
out_2946461154726464959[2] = 0;
|
||||
out_2946461154726464959[3] = 0;
|
||||
out_2946461154726464959[4] = 1;
|
||||
out_2946461154726464959[5] = 0;
|
||||
out_2946461154726464959[6] = 0;
|
||||
out_2946461154726464959[7] = 0;
|
||||
out_2946461154726464959[8] = 0;
|
||||
out_2946461154726464959[9] = 0;
|
||||
out_2946461154726464959[10] = 0;
|
||||
out_2946461154726464959[11] = 0;
|
||||
out_2946461154726464959[12] = 0;
|
||||
out_2946461154726464959[13] = 0;
|
||||
out_2946461154726464959[14] = 1;
|
||||
out_2946461154726464959[15] = 0;
|
||||
out_2946461154726464959[16] = 0;
|
||||
out_2946461154726464959[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_2842147288425805515) {
|
||||
out_2842147288425805515[0] = 0;
|
||||
out_2842147288425805515[1] = 0;
|
||||
out_2842147288425805515[2] = 0;
|
||||
out_2842147288425805515[3] = 0;
|
||||
out_2842147288425805515[4] = 1;
|
||||
out_2842147288425805515[5] = 0;
|
||||
out_2842147288425805515[6] = 0;
|
||||
out_2842147288425805515[7] = 0;
|
||||
out_2842147288425805515[8] = 0;
|
||||
out_2842147288425805515[9] = 0;
|
||||
out_2842147288425805515[10] = 0;
|
||||
out_2842147288425805515[11] = 0;
|
||||
out_2842147288425805515[12] = 0;
|
||||
out_2842147288425805515[13] = 0;
|
||||
out_2842147288425805515[14] = 1;
|
||||
out_2842147288425805515[15] = 0;
|
||||
out_2842147288425805515[16] = 0;
|
||||
out_2842147288425805515[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_2369912916856012406) {
|
||||
out_2369912916856012406[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_2922709757646518101) {
|
||||
out_2922709757646518101[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_7637443712239213152) {
|
||||
out_7637443712239213152[0] = 0;
|
||||
out_7637443712239213152[1] = 0;
|
||||
out_7637443712239213152[2] = 0;
|
||||
out_7637443712239213152[3] = 0;
|
||||
out_7637443712239213152[4] = 1;
|
||||
out_7637443712239213152[5] = 0;
|
||||
out_7637443712239213152[6] = 0;
|
||||
out_7637443712239213152[7] = 0;
|
||||
out_7637443712239213152[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_3183265823325904169) {
|
||||
out_3183265823325904169[0] = 0;
|
||||
out_3183265823325904169[1] = 0;
|
||||
out_3183265823325904169[2] = 0;
|
||||
out_3183265823325904169[3] = 0;
|
||||
out_3183265823325904169[4] = 1;
|
||||
out_3183265823325904169[5] = 0;
|
||||
out_3183265823325904169[6] = 0;
|
||||
out_3183265823325904169[7] = 0;
|
||||
out_3183265823325904169[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_6836290203325307661) {
|
||||
out_6836290203325307661[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_2078963510014327544) {
|
||||
out_2078963510014327544[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_1377607434857908301) {
|
||||
out_1377607434857908301[0] = 0;
|
||||
out_1377607434857908301[1] = 0;
|
||||
out_1377607434857908301[2] = 0;
|
||||
out_1377607434857908301[3] = 0;
|
||||
out_1377607434857908301[4] = 0;
|
||||
out_1377607434857908301[5] = 0;
|
||||
out_1377607434857908301[6] = 0;
|
||||
out_1377607434857908301[7] = 1;
|
||||
out_1377607434857908301[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_7474927837039768810) {
|
||||
out_7474927837039768810[0] = 0;
|
||||
out_7474927837039768810[1] = 0;
|
||||
out_7474927837039768810[2] = 0;
|
||||
out_7474927837039768810[3] = 0;
|
||||
out_7474927837039768810[4] = 0;
|
||||
out_7474927837039768810[5] = 0;
|
||||
out_7474927837039768810[6] = 0;
|
||||
out_7474927837039768810[7] = 1;
|
||||
out_7474927837039768810[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_2710474474758657570) {
|
||||
out_2710474474758657570[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_9006985404715807625) {
|
||||
out_9006985404715807625[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_8585706290286395247) {
|
||||
out_8585706290286395247[0] = 0;
|
||||
out_8585706290286395247[1] = 0;
|
||||
out_8585706290286395247[2] = 0;
|
||||
out_8585706290286395247[3] = 1;
|
||||
out_8585706290286395247[4] = 0;
|
||||
out_8585706290286395247[5] = 0;
|
||||
out_8585706290286395247[6] = 0;
|
||||
out_8585706290286395247[7] = 0;
|
||||
out_8585706290286395247[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_1008502511525479258) {
|
||||
out_1008502511525479258[0] = 0;
|
||||
out_1008502511525479258[1] = 0;
|
||||
out_1008502511525479258[2] = 0;
|
||||
out_1008502511525479258[3] = 1;
|
||||
out_1008502511525479258[4] = 0;
|
||||
out_1008502511525479258[5] = 0;
|
||||
out_1008502511525479258[6] = 0;
|
||||
out_1008502511525479258[7] = 0;
|
||||
out_1008502511525479258[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_1542363084005055587) {
|
||||
out_1542363084005055587[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_1979199904744855666) {
|
||||
out_1979199904744855666[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_8147675056553605336) {
|
||||
out_8147675056553605336[0] = 0;
|
||||
out_8147675056553605336[1] = 1;
|
||||
out_8147675056553605336[2] = 0;
|
||||
out_8147675056553605336[3] = 0;
|
||||
out_8147675056553605336[4] = 0;
|
||||
out_8147675056553605336[5] = 0;
|
||||
out_8147675056553605336[6] = 0;
|
||||
out_8147675056553605336[7] = 0;
|
||||
out_8147675056553605336[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_704860215344071775) {
|
||||
out_704860215344071775[0] = 0;
|
||||
out_704860215344071775[1] = 1;
|
||||
out_704860215344071775[2] = 0;
|
||||
out_704860215344071775[3] = 0;
|
||||
out_704860215344071775[4] = 0;
|
||||
out_704860215344071775[5] = 0;
|
||||
out_704860215344071775[6] = 0;
|
||||
out_704860215344071775[7] = 0;
|
||||
out_704860215344071775[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_7712147742419503215) {
|
||||
out_7712147742419503215[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_6835353348185046340) {
|
||||
out_6835353348185046340[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_3065276039484074762) {
|
||||
out_3065276039484074762[0] = 1;
|
||||
out_3065276039484074762[1] = 0;
|
||||
out_3065276039484074762[2] = 0;
|
||||
out_3065276039484074762[3] = 0;
|
||||
out_3065276039484074762[4] = 0;
|
||||
out_3065276039484074762[5] = 0;
|
||||
out_3065276039484074762[6] = 0;
|
||||
out_3065276039484074762[7] = 0;
|
||||
out_3065276039484074762[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_5787259232413602349) {
|
||||
out_5787259232413602349[0] = 1;
|
||||
out_5787259232413602349[1] = 0;
|
||||
out_5787259232413602349[2] = 0;
|
||||
out_5787259232413602349[3] = 0;
|
||||
out_5787259232413602349[4] = 0;
|
||||
out_5787259232413602349[5] = 0;
|
||||
out_5787259232413602349[6] = 0;
|
||||
out_5787259232413602349[7] = 0;
|
||||
out_5787259232413602349[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_6282556507394540645) {
|
||||
out_6282556507394540645[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_4974759307888764822) {
|
||||
out_4974759307888764822[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_5149756715608924953) {
|
||||
out_5149756715608924953[0] = 0;
|
||||
out_5149756715608924953[1] = 0;
|
||||
out_5149756715608924953[2] = 0;
|
||||
out_5149756715608924953[3] = 0;
|
||||
out_5149756715608924953[4] = 0;
|
||||
out_5149756715608924953[5] = 0;
|
||||
out_5149756715608924953[6] = 0;
|
||||
out_5149756715608924953[7] = 0;
|
||||
out_5149756715608924953[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_3702778556288752158) {
|
||||
out_3702778556288752158[0] = 0;
|
||||
out_3702778556288752158[1] = 0;
|
||||
out_3702778556288752158[2] = 0;
|
||||
out_3702778556288752158[3] = 0;
|
||||
out_3702778556288752158[4] = 0;
|
||||
out_3702778556288752158[5] = 0;
|
||||
out_3702778556288752158[6] = 0;
|
||||
out_3702778556288752158[7] = 0;
|
||||
out_3702778556288752158[8] = 1;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
|
||||
}
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_8339538066892951453) {
|
||||
err_fun(nom_x, delta_x, out_8339538066892951453);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_4436222918597397805) {
|
||||
err_fun(nom_x, delta_x, out_4436222918597397805);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1869575881362213489) {
|
||||
inv_err_fun(nom_x, true_x, out_1869575881362213489);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894) {
|
||||
inv_err_fun(nom_x, true_x, out_5455437002668972894);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_4134445094065349854) {
|
||||
H_mod_fun(state, out_4134445094065349854);
|
||||
void car_H_mod_fun(double *state, double *out_210375879520347438) {
|
||||
H_mod_fun(state, out_210375879520347438);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_1187519494066628558) {
|
||||
f_fun(state, dt, out_1187519494066628558);
|
||||
void car_f_fun(double *state, double dt, double *out_467846126367206141) {
|
||||
f_fun(state, dt, out_467846126367206141);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_638221139616797346) {
|
||||
F_fun(state, dt, out_638221139616797346);
|
||||
void car_F_fun(double *state, double dt, double *out_3146439140780538523) {
|
||||
F_fun(state, dt, out_3146439140780538523);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_6007362445110034756) {
|
||||
h_25(state, unused, out_6007362445110034756);
|
||||
void car_h_25(double *state, double *unused, double *out_3524127915259881655) {
|
||||
h_25(state, unused, out_3524127915259881655);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_5119110753731964525) {
|
||||
H_25(state, unused, out_5119110753731964525);
|
||||
void car_H_25(double *state, double *unused, double *out_3733424518165712586) {
|
||||
H_25(state, unused, out_3733424518165712586);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_5449620096162801389) {
|
||||
h_24(state, unused, out_5449620096162801389);
|
||||
void car_h_24(double *state, double *unused, double *out_5258226883159248845) {
|
||||
h_24(state, unused, out_5258226883159248845);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_2946461154726464959) {
|
||||
H_24(state, unused, out_2946461154726464959);
|
||||
void car_H_24(double *state, double *unused, double *out_2842147288425805515) {
|
||||
H_24(state, unused, out_2842147288425805515);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_2369912916856012406) {
|
||||
h_30(state, unused, out_2369912916856012406);
|
||||
void car_h_30(double *state, double *unused, double *out_2922709757646518101) {
|
||||
h_30(state, unused, out_2922709757646518101);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_7637443712239213152) {
|
||||
H_30(state, unused, out_7637443712239213152);
|
||||
void car_H_30(double *state, double *unused, double *out_3183265823325904169) {
|
||||
H_30(state, unused, out_3183265823325904169);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_6836290203325307661) {
|
||||
h_26(state, unused, out_6836290203325307661);
|
||||
void car_h_26(double *state, double *unused, double *out_2078963510014327544) {
|
||||
h_26(state, unused, out_2078963510014327544);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_1377607434857908301) {
|
||||
H_26(state, unused, out_1377607434857908301);
|
||||
void car_H_26(double *state, double *unused, double *out_7474927837039768810) {
|
||||
H_26(state, unused, out_7474927837039768810);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_2710474474758657570) {
|
||||
h_27(state, unused, out_2710474474758657570);
|
||||
void car_h_27(double *state, double *unused, double *out_9006985404715807625) {
|
||||
h_27(state, unused, out_9006985404715807625);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_8585706290286395247) {
|
||||
H_27(state, unused, out_8585706290286395247);
|
||||
void car_H_27(double *state, double *unused, double *out_1008502511525479258) {
|
||||
H_27(state, unused, out_1008502511525479258);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_1542363084005055587) {
|
||||
h_29(state, unused, out_1542363084005055587);
|
||||
void car_h_29(double *state, double *unused, double *out_1979199904744855666) {
|
||||
h_29(state, unused, out_1979199904744855666);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_8147675056553605336) {
|
||||
H_29(state, unused, out_8147675056553605336);
|
||||
void car_H_29(double *state, double *unused, double *out_704860215344071775) {
|
||||
H_29(state, unused, out_704860215344071775);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_7712147742419503215) {
|
||||
h_28(state, unused, out_7712147742419503215);
|
||||
void car_h_28(double *state, double *unused, double *out_6835353348185046340) {
|
||||
h_28(state, unused, out_6835353348185046340);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_3065276039484074762) {
|
||||
H_28(state, unused, out_3065276039484074762);
|
||||
void car_H_28(double *state, double *unused, double *out_5787259232413602349) {
|
||||
H_28(state, unused, out_5787259232413602349);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_6282556507394540645) {
|
||||
h_31(state, unused, out_6282556507394540645);
|
||||
void car_h_31(double *state, double *unused, double *out_4974759307888764822) {
|
||||
h_31(state, unused, out_4974759307888764822);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_5149756715608924953) {
|
||||
H_31(state, unused, out_5149756715608924953);
|
||||
void car_H_31(double *state, double *unused, double *out_3702778556288752158) {
|
||||
H_31(state, unused, out_3702778556288752158);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_8339538066892951453);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1869575881362213489);
|
||||
void car_H_mod_fun(double *state, double *out_4134445094065349854);
|
||||
void car_f_fun(double *state, double dt, double *out_1187519494066628558);
|
||||
void car_F_fun(double *state, double dt, double *out_638221139616797346);
|
||||
void car_h_25(double *state, double *unused, double *out_6007362445110034756);
|
||||
void car_H_25(double *state, double *unused, double *out_5119110753731964525);
|
||||
void car_h_24(double *state, double *unused, double *out_5449620096162801389);
|
||||
void car_H_24(double *state, double *unused, double *out_2946461154726464959);
|
||||
void car_h_30(double *state, double *unused, double *out_2369912916856012406);
|
||||
void car_H_30(double *state, double *unused, double *out_7637443712239213152);
|
||||
void car_h_26(double *state, double *unused, double *out_6836290203325307661);
|
||||
void car_H_26(double *state, double *unused, double *out_1377607434857908301);
|
||||
void car_h_27(double *state, double *unused, double *out_2710474474758657570);
|
||||
void car_H_27(double *state, double *unused, double *out_8585706290286395247);
|
||||
void car_h_29(double *state, double *unused, double *out_1542363084005055587);
|
||||
void car_H_29(double *state, double *unused, double *out_8147675056553605336);
|
||||
void car_h_28(double *state, double *unused, double *out_7712147742419503215);
|
||||
void car_H_28(double *state, double *unused, double *out_3065276039484074762);
|
||||
void car_h_31(double *state, double *unused, double *out_6282556507394540645);
|
||||
void car_H_31(double *state, double *unused, double *out_5149756715608924953);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_4436222918597397805);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5455437002668972894);
|
||||
void car_H_mod_fun(double *state, double *out_210375879520347438);
|
||||
void car_f_fun(double *state, double dt, double *out_467846126367206141);
|
||||
void car_F_fun(double *state, double dt, double *out_3146439140780538523);
|
||||
void car_h_25(double *state, double *unused, double *out_3524127915259881655);
|
||||
void car_H_25(double *state, double *unused, double *out_3733424518165712586);
|
||||
void car_h_24(double *state, double *unused, double *out_5258226883159248845);
|
||||
void car_H_24(double *state, double *unused, double *out_2842147288425805515);
|
||||
void car_h_30(double *state, double *unused, double *out_2922709757646518101);
|
||||
void car_H_30(double *state, double *unused, double *out_3183265823325904169);
|
||||
void car_h_26(double *state, double *unused, double *out_2078963510014327544);
|
||||
void car_H_26(double *state, double *unused, double *out_7474927837039768810);
|
||||
void car_h_27(double *state, double *unused, double *out_9006985404715807625);
|
||||
void car_H_27(double *state, double *unused, double *out_1008502511525479258);
|
||||
void car_h_29(double *state, double *unused, double *out_1979199904744855666);
|
||||
void car_H_29(double *state, double *unused, double *out_704860215344071775);
|
||||
void car_h_28(double *state, double *unused, double *out_6835353348185046340);
|
||||
void car_H_28(double *state, double *unused, double *out_5787259232413602349);
|
||||
void car_h_31(double *state, double *unused, double *out_4974759307888764822);
|
||||
void car_H_31(double *state, double *unused, double *out_3702778556288752158);
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
void car_set_mass(double x);
|
||||
void car_set_rotational_inertia(double x);
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user