mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-24 01:33:46 +08:00
Compare commits
7 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Michael Tawata
|
26bb817231 | ||
|
Michael Tawata
|
3118ff5c52 | ||
|
firestar5683
|
d0598334e4 | ||
|
firestar5683
|
2f13d3846c | ||
|
firestar5683
|
7558054914 | ||
|
firestar5683
|
71260b52d1 | ||
|
firestarsdog
|
f55046f6e9 |
@@ -1,7 +1,7 @@
|
||||
# StarPilot
|
||||
|
||||
[](https://deepwiki.com/firestar5683/StarPilot)
|
||||
[](https://firestar.link/discord)
|
||||
[](https://firestar.link/discord)
|
||||
[](https://github.com/firestar5683/StarPilot)
|
||||
[](https://wiki.firestar.link)
|
||||
|
||||
@@ -15,11 +15,11 @@ Openpilot provides
|
||||
* Lane Change Assist
|
||||
* Driver Monitoring *without wheel nags*
|
||||
|
||||
StarPilot adds support for many GM vehicles along with improved tuning,
|
||||
especially for radar-less (camera only) vehicles.
|
||||
StarPilot was formerly a GM targeted fork,
|
||||
but [has expanded to offer Quality-Of-Life improvements for all](#features)!
|
||||
|
||||
StarPilot is built off of [StarPilot](https://github.com/FrogAi/StarPilot)
|
||||
and supports the major features StarPilot offers.
|
||||
StarPilot is built off of [FrogPilot](https://github.com/FrogAi/FrogPilot)
|
||||
and supports the major features FrogPilot offers.
|
||||
|
||||
StarPilot has a vibrant, welcoming community [discord](https://firestar.link/discord).
|
||||
Stop by to chat or ask questions!
|
||||
@@ -32,9 +32,9 @@ installation guides, and software configuration.
|
||||
## Features
|
||||
|
||||
* Full support for Comma C3, C3X, and C4
|
||||
* C4 is currently in release testing. Join our fleet of C4 testers!
|
||||
* Model switcher with all of comma's tinygrad driving models
|
||||
* Special longitudinal planner tuning for VoACC (visual only, radar-less) vehicles
|
||||
* Custom-tuned torque controllers for an expanding list of cars.
|
||||
* Galaxy: StarPilot's portal to configure your comma device using your phone from anywhere.
|
||||
Download models, change settings, update software, visualize live model outputs for tuning.
|
||||
* Always On Lateral (full time steering assist)*
|
||||
@@ -49,8 +49,9 @@ Download models, change settings, update software, visualize live model outputs
|
||||
* ZSS support*
|
||||
* High quality dashcam recordings*
|
||||
* Enhanced tuning for CEM (dynamic experimental mode switching)
|
||||
* And more!
|
||||
|
||||
\* [Inherited from StarPilot](https://github.com/FrogAi/StarPilot#openpilot-vs-starpilot)
|
||||
\* [Inherited from FrogPilot](https://github.com/FrogAi/FrogPilot#openpilot-vs-frogpilot)
|
||||
|
||||
## GM-only Features
|
||||
|
||||
|
||||
@@ -199,7 +199,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"CEStopLights", {PERSISTENT, BOOL, "1", "0", 1}},
|
||||
{"CEStoppedLead", {PERSISTENT, BOOL, "0", "0", 1}},
|
||||
{"ClusterOffset", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
|
||||
{"ColorScheme", {PERSISTENT, STRING, "frog", "stock", 0}},
|
||||
{"ColorScheme", {PERSISTENT, STRING, "stock", "stock", 0}},
|
||||
{"ColorToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"BootLogoToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"Compass", {PERSISTENT, BOOL, "0", "0", 1}},
|
||||
@@ -227,7 +227,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"AggressivePersonalityProfile", {PERSISTENT, BOOL, "1", "1", 2}},
|
||||
{"StandardPersonalityProfile", {PERSISTENT, BOOL, "1", "1", 2}},
|
||||
{"RelaxedPersonalityProfile", {PERSISTENT, BOOL, "1", "1", 2}},
|
||||
{"CustomThemes", {PERSISTENT, BOOL, "1", "0", 0}},
|
||||
{"CustomThemes", {PERSISTENT, BOOL, "0", "0", 0}},
|
||||
{"CustomUI", {PERSISTENT, BOOL, "1", "0", 1}},
|
||||
{"DebugMode", {CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0", "0"}},
|
||||
{"DecelerationProfile", {PERSISTENT, INT, "1", "0", 2}},
|
||||
@@ -325,7 +325,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"HumanAcceleration", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"CoastUpToLeads", {PERSISTENT, BOOL, "1", "1", 2}},
|
||||
{"HumanLaneChanges", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"IconPack", {PERSISTENT, STRING, "frog-animated", "stock", 0}},
|
||||
{"IconPack", {PERSISTENT, STRING, "stock", "stock", 0}},
|
||||
{"IconToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"IncreasedStoppedDistance", {PERSISTENT, FLOAT, "0.0", "0.0", 1}},
|
||||
{"IncreasedStoppedDistanceLowVisibility", {PERSISTENT, FLOAT, "0.0", "0.0", 2}},
|
||||
@@ -501,7 +501,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ShowStorageUsed", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"SidebarMetrics", {PERSISTENT, BOOL, "1", "0", 3}},
|
||||
{"SidebarOpen", {PERSISTENT, BOOL, "0", "0", 0}},
|
||||
{"SignalAnimation", {PERSISTENT, STRING, "frog", "stock", 0}},
|
||||
{"SignalAnimation", {PERSISTENT, STRING, "stock", "stock", 0}},
|
||||
{"SignalMetrics", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"SignalToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"SimpleMode", {PERSISTENT, BOOL, "0", "0", 0}},
|
||||
@@ -519,7 +519,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SLCPriority1", {PERSISTENT, STRING, "Map Data", "Map Data", 2}},
|
||||
{"SLCPriority2", {PERSISTENT, STRING, "Dashboard", "Dashboard", 2}},
|
||||
{"SNGHack", {PERSISTENT, BOOL, "1", "0", 2}},
|
||||
{"SoundPack", {PERSISTENT, STRING, "frog", "stock", 0}},
|
||||
{"SoundPack", {PERSISTENT, STRING, "stock", "stock", 0}},
|
||||
{"SoundToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"SLCAdoptSpeedLimit", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}},
|
||||
{"SLCForceCruiseSpeed", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}},
|
||||
@@ -544,8 +544,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"StandbyMode", {PERSISTENT, BOOL, "0", "0", 1}},
|
||||
{"StartAccel", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
{"StartAccelStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
{"StartupMessageBottom", {PERSISTENT, STRING, "Human-tested, frog-approved 🐸", "Always keep hands on wheel and eyes on road", 0}},
|
||||
{"StartupMessageTop", {PERSISTENT, STRING, "Hop in and buckle up!", "Be ready to take over at any time", 0}},
|
||||
{"StartupMessageBottom", {PERSISTENT, STRING, "Always keep hands on wheel and eyes on road", "Always keep hands on wheel and eyes on road", 0}},
|
||||
{"StartupMessageTop", {PERSISTENT, STRING, "Be ready to take over at any time", "Be ready to take over at any time", 0}},
|
||||
{"StaticPedalsOnUI", {PERSISTENT, BOOL, "0", "0", 1}},
|
||||
{"SteerDelay", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
{"SteerDelayStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
|
||||
@@ -614,7 +614,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"WeatherPresets", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"WeatherToken", {PERSISTENT | DONT_LOG, STRING, "", "", 2}},
|
||||
{"WheelControls", {PERSISTENT, STRING, "", "", 2}},
|
||||
{"WheelIcon", {PERSISTENT, STRING, "frog", "stock", 0}},
|
||||
{"WheelIcon", {PERSISTENT, STRING, "stock", "stock", 0}},
|
||||
{"WheelSpeed", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"WheelToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
};
|
||||
|
||||
@@ -2,7 +2,8 @@ from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
|
||||
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs
|
||||
from opendbc.car.common.filter_simple import FirstOrderFilter
|
||||
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.hyundai import hyundaicanfd, hyundaican
|
||||
@@ -61,6 +62,7 @@ REDNECK_BUTTON_COPIES = 2
|
||||
REDNECK_BUTTON_COPIES_TIME = 7
|
||||
REDNECK_BUTTON_COPIES_TIME_IMPERIAL = [REDNECK_BUTTON_COPIES_TIME + 3, 70]
|
||||
REDNECK_BUTTON_COPIES_TIME_METRIC = [REDNECK_BUTTON_COPIES_TIME, 40]
|
||||
ANGLE_SAFETY_BASELINE_MODEL = str(CAR.KIA_SPORTAGE_HEV_2026)
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -195,6 +197,28 @@ def update_genesis_g90_longitudinal_tuning(state: GenesisG90LongitudinalTuningSt
|
||||
return state
|
||||
|
||||
|
||||
def get_baseline_safety_cp():
|
||||
from opendbc.car.hyundai.interface import CarInterface
|
||||
return CarInterface.get_non_essential_params(ANGLE_SAFETY_BASELINE_MODEL)
|
||||
|
||||
|
||||
def compute_torque_reduction_gain(steering_torque, v_ego, lat_active, last_gain):
|
||||
if lat_active:
|
||||
ceiling = np.interp(v_ego, [0.5, 1.5], [1.0, 0.85])
|
||||
shelf = np.interp(v_ego, [2.0, 11.0], [0.45, 0.6])
|
||||
floor = np.interp(v_ego, [2.0, 22.0], [0.1, 0.3])
|
||||
bp1 = np.interp(v_ego, [2.0, 11.0], [75.0, 125.0])
|
||||
bp2 = np.interp(v_ego, [2.0, 11.0], [125.0, 150.0])
|
||||
bp3 = np.interp(v_ego, [2.0, 11.0], [175.0, 275.0])
|
||||
bp4 = np.interp(v_ego, [2.0, 22.0], [400.0, 700.0])
|
||||
target = np.interp(abs(steering_torque), [bp1, bp2, bp3, bp4], [ceiling, shelf, shelf, floor])
|
||||
else:
|
||||
target = 0.0
|
||||
|
||||
gain = rate_limit(target, last_gain, -0.014, 0.004)
|
||||
return round(gain / 0.004) * 0.004
|
||||
|
||||
|
||||
def process_hud_alert(enabled, fingerprint, hud_control):
|
||||
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
|
||||
|
||||
@@ -227,6 +251,8 @@ class CarController(CarControllerBase):
|
||||
self.packer = CANPacker(dbc_names[Bus.pt])
|
||||
self.angle_limit_counter = 0
|
||||
self.VM = VehicleModel(CP)
|
||||
self.BASELINE_VM = VehicleModel(get_baseline_safety_cp()) if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else self.VM
|
||||
self.angle_filter = FirstOrderFilter(0.0, 0.2, DT_CTRL)
|
||||
|
||||
self.accel_last = 0
|
||||
self.apply_torque_last = 0
|
||||
@@ -328,32 +354,41 @@ class CarController(CarControllerBase):
|
||||
hud_control = CC.hudControl
|
||||
lka_icon, lfa_icon = self._update_dash_icon_state(CC)
|
||||
|
||||
self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
|
||||
if not self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
|
||||
self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
|
||||
apply_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
|
||||
v_ego_raw = CS.out.vEgoRaw
|
||||
desired_angle = float(np.clip(actuators.steeringAngleDeg,
|
||||
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
|
||||
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
|
||||
apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, CS.out.vEgoRaw,
|
||||
|
||||
self.angle_filter.update_alpha(float(np.interp(CS.out.vEgo, [5.0, 10.0, 20.0], [0.2, 0.1, 0.0])))
|
||||
desired_angle = self.angle_filter.update(desired_angle)
|
||||
|
||||
apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, v_ego_raw,
|
||||
CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM)
|
||||
|
||||
if CS.out.steeringPressed and abs(CS.out.steeringTorque) > self.params.STEER_THRESHOLD:
|
||||
apply_torque = self.params.ANGLE_MIN_TORQUE_REDUCTION_GAIN
|
||||
elif CC.latActive and CS.out.vEgoRaw < 0.3:
|
||||
apply_torque = self.params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN
|
||||
else:
|
||||
apply_torque = self.params.ANGLE_MAX_TORQUE_REDUCTION_GAIN if CC.latActive else 0.0
|
||||
if str(self.CP.carFingerprint) != ANGLE_SAFETY_BASELINE_MODEL:
|
||||
apply_angle = apply_steer_angle_limits_vm(apply_angle or desired_angle, self.apply_angle_last, v_ego_raw,
|
||||
CS.out.steeringAngleDeg, CC.latActive, self.params, self.BASELINE_VM)
|
||||
|
||||
apply_steer_req = CC.latActive and apply_torque > 0.0
|
||||
apply_torque = compute_torque_reduction_gain(CS.out.steeringTorque, v_ego_raw, CC.latActive, self.apply_torque_last)
|
||||
apply_steer_req = CC.latActive and apply_torque != 0.0
|
||||
torque_fault = False
|
||||
|
||||
if apply_angle is None:
|
||||
apply_torque = 0.0
|
||||
apply_torque = 0
|
||||
apply_angle = CS.out.steeringAngleDeg
|
||||
apply_steer_req = False
|
||||
|
||||
self.apply_angle_last = apply_angle
|
||||
if not CC.latActive:
|
||||
self.apply_angle_last = float(np.clip(CS.out.steeringAngleDeg,
|
||||
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
|
||||
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
|
||||
self.angle_filter.x = self.apply_angle_last
|
||||
else:
|
||||
# steering torque
|
||||
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
|
||||
|
||||
@@ -1167,11 +1167,11 @@ class TestHyundaiFingerprint:
|
||||
assert parser.vl["FCA12"]["FCA_DrvSetState"] == 2
|
||||
assert parser.vl["FCA12"]["FCA_USM"] == 2
|
||||
|
||||
def test_sportage_angle_steering_uses_lfa_and_adas_cmd_with_send_lfa(self):
|
||||
def test_angle_steering_uses_lfa_and_adas_cmd_with_send_lfa(self):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
cam_can = CanBus(None, fingerprint).CAM
|
||||
fingerprint[cam_can][0xCB] = 24
|
||||
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, None)
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_SANTA_FE_HEV_5TH_GEN, fingerprint, [], False, False, False, None)
|
||||
|
||||
assert CP.flags & HyundaiFlags.SEND_LFA
|
||||
assert CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING
|
||||
@@ -1474,34 +1474,6 @@ class TestHyundaiFingerprint:
|
||||
]
|
||||
assert hyundaicanfd.create_ioniq_6_cluster_lane_change_messages(can_bus, 5, "none") == []
|
||||
|
||||
def test_sportage_angle_jerk_override_is_scoped(self):
|
||||
sportage = CarParams.new_message()
|
||||
sportage.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
|
||||
sportage.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)
|
||||
|
||||
comparison_angle = CarParams.new_message()
|
||||
comparison_angle.carFingerprint = CAR.KIA_EV6
|
||||
comparison_angle.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)
|
||||
|
||||
ioniq6 = CarParams.new_message()
|
||||
ioniq6.carFingerprint = CAR.HYUNDAI_IONIQ_6
|
||||
ioniq6.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
|
||||
|
||||
sportage_params = CarControllerParams(sportage)
|
||||
sportage_low_speed_params = CarControllerParams(sportage, vEgoRaw=5.0)
|
||||
sportage_high_speed_params = CarControllerParams(sportage, vEgoRaw=20.0)
|
||||
comparison_params = CarControllerParams(comparison_angle)
|
||||
ioniq6_params = CarControllerParams(ioniq6)
|
||||
|
||||
assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
assert sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK == sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK > sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
assert sportage_params.ANGLE_LIMITS.STEER_ANGLE_MAX > comparison_params.ANGLE_LIMITS.STEER_ANGLE_MAX
|
||||
assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_ACCEL > comparison_params.ANGLE_LIMITS.MAX_LATERAL_ACCEL
|
||||
assert sportage_params.ANGLE_LIMITS.MAX_ANGLE_RATE > comparison_params.ANGLE_LIMITS.MAX_ANGLE_RATE
|
||||
assert comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK == ioniq6_params.ANGLE_LIMITS.MAX_LATERAL_JERK
|
||||
|
||||
def test_ioniq_5_canfd_aux_messages_are_optional(self):
|
||||
toggles = get_test_toggles()
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
import re
|
||||
from dataclasses import dataclass, field, replace
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntFlag
|
||||
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, ISO_LATERAL_JERK
|
||||
from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, SupportType
|
||||
@@ -11,13 +11,6 @@ from opendbc.car.fw_query_definitions import FwQueryConfig, Request, p16
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
AVERAGE_ROAD_ROLL = 0.06 # conservative roll margin used by Hyundai CAN-FD angle steering safety
|
||||
SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL = 3.6
|
||||
SPORTAGE_HEV_2026_BASE_LATERAL_JERK = 3.25
|
||||
SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST = 0.55
|
||||
SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED = 11.0
|
||||
SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH = 5.0
|
||||
SPORTAGE_HEV_2026_MAX_ANGLE_RATE = 6.5
|
||||
SPORTAGE_HEV_2026_STEER_ANGLE_MAX = 220.0
|
||||
HYUNDAI_MANDO_FRONT_RADAR_DBC = "hyundai_kia_mando_front_radar_generated"
|
||||
HYUNDAI_MRREVO14F_RADAR_DBC = "hyundai_mrrevo14f_radar_generated"
|
||||
HYUNDAI_MRR30_RADAR_DBC = "hyundai_mrr30_radar_generated"
|
||||
@@ -28,16 +21,13 @@ class CarControllerParams:
|
||||
ACCEL_MIN = -3.5 # m/s
|
||||
ACCEL_MAX = 3.5 # m/s
|
||||
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
|
||||
180,
|
||||
360,
|
||||
([], []),
|
||||
([], []),
|
||||
MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
MAX_LATERAL_JERK=ISO_LATERAL_JERK + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
MAX_ANGLE_RATE=5,
|
||||
)
|
||||
ANGLE_MAX_TORQUE_REDUCTION_GAIN = 1.0
|
||||
ANGLE_MIN_TORQUE_REDUCTION_GAIN = 0.6
|
||||
ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN = 0.6
|
||||
|
||||
def __init__(self, CP, vEgoRaw=100.):
|
||||
self.ANGLE_LIMITS = self.ANGLE_LIMITS
|
||||
@@ -64,21 +54,6 @@ class CarControllerParams:
|
||||
if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
|
||||
self.STEER_THRESHOLD = 175
|
||||
|
||||
# The Sportage angle port still needs more authority in real turns than the
|
||||
# fully calmed branch-wide ceiling allows, but the old low-speed jerk boost
|
||||
# made the 3-20 degree band angry and ping-pongy as the car slowed down.
|
||||
# Split the difference:
|
||||
# - keep a calmer low-speed boost that fades out earlier
|
||||
# - give the car a little more true turn headroom through accel/rate limits
|
||||
if CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026:
|
||||
sportage_low_speed_weight = min(max((SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED - vEgoRaw) / SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH, 0.0), 1.0)
|
||||
sportage_lateral_jerk = SPORTAGE_HEV_2026_BASE_LATERAL_JERK + (SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST * sportage_low_speed_weight)
|
||||
self.ANGLE_LIMITS = replace(self.ANGLE_LIMITS,
|
||||
STEER_ANGLE_MAX=SPORTAGE_HEV_2026_STEER_ANGLE_MAX,
|
||||
MAX_LATERAL_ACCEL=SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
MAX_LATERAL_JERK=sportage_lateral_jerk + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
MAX_ANGLE_RATE=SPORTAGE_HEV_2026_MAX_ANGLE_RATE)
|
||||
|
||||
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
|
||||
# If the max stock LKAS request is <384, add your car to this list.
|
||||
elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ,
|
||||
|
||||
@@ -190,7 +190,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
|
||||
.has_steer_req_tolerance = true,
|
||||
};
|
||||
const AngleSteeringLimits HYUNDAI_CANFD_ANGLE_STEERING_LIMITS = {
|
||||
.max_angle = 1800,
|
||||
.max_angle = 3600,
|
||||
.angle_deg_to_can = 10,
|
||||
.frequency = 100U,
|
||||
};
|
||||
@@ -230,9 +230,16 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
|
||||
int desired_angle = (msg->data[11] << 6U) | (msg->data[10] >> 2U);
|
||||
desired_angle = to_signed(desired_angle, 14);
|
||||
|
||||
// ADAS_ACIAnglTqRedcGainVal: bit 96, 8 bits, unsigned. Raw 0-250 valid, 251-255 reserved.
|
||||
const uint8_t gain_raw = msg->data[12];
|
||||
bool gain_violation = gain_raw > 250U;
|
||||
if (!steer_angle_req && (gain_raw != 0U)) {
|
||||
gain_violation = true;
|
||||
}
|
||||
|
||||
if (steer_angle_cmd_checks_vm(desired_angle, steer_angle_req,
|
||||
HYUNDAI_CANFD_ANGLE_STEERING_LIMITS,
|
||||
HYUNDAI_CANFD_ANGLE_STEERING_PARAMS)) {
|
||||
HYUNDAI_CANFD_ANGLE_STEERING_PARAMS) || gain_violation) {
|
||||
tx = false;
|
||||
}
|
||||
} else {
|
||||
|
||||
@@ -174,7 +174,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
|
||||
BUTTONS_TX_BUS = 2
|
||||
LATERAL_FREQUENCY = 100
|
||||
STANDSTILL_THRESHOLD = 12
|
||||
STEER_ANGLE_MAX = 180
|
||||
STEER_ANGLE_MAX = 360
|
||||
DEG_TO_CAN = 10
|
||||
GAS_MSG = ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL")
|
||||
SAFETY_PARAM = HyundaiSafetyFlags.CANFD_ANGLE_STEERING | HyundaiSafetyFlags.CAMERA_SCC | HyundaiSafetyFlags.HYBRID_GAS
|
||||
@@ -248,7 +248,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
|
||||
checksum = sig_checksum.calc_checksum(addr, sig_checksum, dat)
|
||||
_set_value(dat, sig_checksum, checksum)
|
||||
|
||||
def _angle_cmd_msg(self, angle, enabled, increment_timer=True):
|
||||
def _angle_cmd_msg(self, angle, enabled, increment_timer=True, gain_raw=250):
|
||||
if increment_timer:
|
||||
self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY))
|
||||
self.angle_cmd_cnt += 1
|
||||
@@ -272,7 +272,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
|
||||
dat[9] = (dat[9] & ~0x30) | (((2 if enabled else 1) & 0x3) << 4)
|
||||
dat[10] = (dat[10] & 0x03) | ((desired_angle & 0x3F) << 2)
|
||||
dat[11] = (desired_angle >> 6) & 0xFF
|
||||
dat[12] = 250 if enabled else 0
|
||||
dat[12] = gain_raw if enabled or gain_raw != 250 else 0
|
||||
self._update_checksum(addr, dat)
|
||||
return libsafety_py.make_CANPacket(addr, 0, bytes(dat))
|
||||
|
||||
@@ -335,6 +335,19 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
|
||||
self.assertFalse(self._tx(self._angle_cmd_msg(0, True, increment_timer=False)))
|
||||
self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False)))
|
||||
|
||||
def test_angle_torque_reduction_gain_limits(self):
|
||||
if self.__class__.__name__ != "TestHyundaiCanfdAngleSteering":
|
||||
return
|
||||
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._reset_speed_measurement(1)
|
||||
self._set_prev_desired_angle(0)
|
||||
self.assertTrue(self._tx(self._angle_cmd_msg(0, True, gain_raw=250)))
|
||||
self._set_prev_desired_angle(0)
|
||||
self.assertFalse(self._tx(self._angle_cmd_msg(0, True, gain_raw=251)))
|
||||
self._set_prev_desired_angle(0)
|
||||
self.assertFalse(self._tx(self._angle_cmd_msg(0, False, gain_raw=1)))
|
||||
|
||||
|
||||
class TestHyundaiCanfdAngleSteeringLfaAlt(TestHyundaiCanfdAngleSteering):
|
||||
|
||||
|
||||
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-78e9b513-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-71260b52-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.
@@ -1 +1 @@
|
||||
DEV-78e9b513-DEBUG
|
||||
DEV-71260b52-DEBUG
|
||||
@@ -480,11 +480,11 @@ IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04
|
||||
IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60
|
||||
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.62
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 17.0
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 2.0
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.45
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.14
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.72
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 9.0
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.4
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.18
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.10
|
||||
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82
|
||||
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12
|
||||
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10
|
||||
|
||||
@@ -433,6 +433,8 @@ class TestLatControl:
|
||||
assert get_ioniq_6_directional_taper_scale(-1.2, -0.40, 8.0) > get_ioniq_6_directional_taper_scale(-1.2, -0.40, 25.0)
|
||||
assert get_ioniq_6_directional_taper_scale(1.2, 0.40, 8.0) > get_ioniq_6_directional_taper_scale(1.2, 0.40, 25.0)
|
||||
assert get_ioniq_6_directional_taper_scale(-1.2, 0.7, 8.0) == pytest.approx(get_ioniq_6_directional_taper_scale(-1.2, 0.7, 25.0), abs=0.02)
|
||||
assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0)
|
||||
assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 20.0)
|
||||
|
||||
def test_ioniq_6_output_taper_curve(self):
|
||||
assert get_ioniq_6_output_taper_scale(0.0, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.0, 0.0, 8.0) <= 1.0
|
||||
|
||||
@@ -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_6333242559483084265) {
|
||||
out_6333242559483084265[0] = delta_x[0] + nom_x[0];
|
||||
out_6333242559483084265[1] = delta_x[1] + nom_x[1];
|
||||
out_6333242559483084265[2] = delta_x[2] + nom_x[2];
|
||||
out_6333242559483084265[3] = delta_x[3] + nom_x[3];
|
||||
out_6333242559483084265[4] = delta_x[4] + nom_x[4];
|
||||
out_6333242559483084265[5] = delta_x[5] + nom_x[5];
|
||||
out_6333242559483084265[6] = delta_x[6] + nom_x[6];
|
||||
out_6333242559483084265[7] = delta_x[7] + nom_x[7];
|
||||
out_6333242559483084265[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_4080197981179722536) {
|
||||
out_4080197981179722536[0] = delta_x[0] + nom_x[0];
|
||||
out_4080197981179722536[1] = delta_x[1] + nom_x[1];
|
||||
out_4080197981179722536[2] = delta_x[2] + nom_x[2];
|
||||
out_4080197981179722536[3] = delta_x[3] + nom_x[3];
|
||||
out_4080197981179722536[4] = delta_x[4] + nom_x[4];
|
||||
out_4080197981179722536[5] = delta_x[5] + nom_x[5];
|
||||
out_4080197981179722536[6] = delta_x[6] + nom_x[6];
|
||||
out_4080197981179722536[7] = delta_x[7] + nom_x[7];
|
||||
out_4080197981179722536[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_829248915283082072) {
|
||||
out_829248915283082072[0] = -nom_x[0] + true_x[0];
|
||||
out_829248915283082072[1] = -nom_x[1] + true_x[1];
|
||||
out_829248915283082072[2] = -nom_x[2] + true_x[2];
|
||||
out_829248915283082072[3] = -nom_x[3] + true_x[3];
|
||||
out_829248915283082072[4] = -nom_x[4] + true_x[4];
|
||||
out_829248915283082072[5] = -nom_x[5] + true_x[5];
|
||||
out_829248915283082072[6] = -nom_x[6] + true_x[6];
|
||||
out_829248915283082072[7] = -nom_x[7] + true_x[7];
|
||||
out_829248915283082072[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_3164357961811210311) {
|
||||
out_3164357961811210311[0] = -nom_x[0] + true_x[0];
|
||||
out_3164357961811210311[1] = -nom_x[1] + true_x[1];
|
||||
out_3164357961811210311[2] = -nom_x[2] + true_x[2];
|
||||
out_3164357961811210311[3] = -nom_x[3] + true_x[3];
|
||||
out_3164357961811210311[4] = -nom_x[4] + true_x[4];
|
||||
out_3164357961811210311[5] = -nom_x[5] + true_x[5];
|
||||
out_3164357961811210311[6] = -nom_x[6] + true_x[6];
|
||||
out_3164357961811210311[7] = -nom_x[7] + true_x[7];
|
||||
out_3164357961811210311[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_8283696179185346057) {
|
||||
out_8283696179185346057[0] = 1.0;
|
||||
out_8283696179185346057[1] = 0.0;
|
||||
out_8283696179185346057[2] = 0.0;
|
||||
out_8283696179185346057[3] = 0.0;
|
||||
out_8283696179185346057[4] = 0.0;
|
||||
out_8283696179185346057[5] = 0.0;
|
||||
out_8283696179185346057[6] = 0.0;
|
||||
out_8283696179185346057[7] = 0.0;
|
||||
out_8283696179185346057[8] = 0.0;
|
||||
out_8283696179185346057[9] = 0.0;
|
||||
out_8283696179185346057[10] = 1.0;
|
||||
out_8283696179185346057[11] = 0.0;
|
||||
out_8283696179185346057[12] = 0.0;
|
||||
out_8283696179185346057[13] = 0.0;
|
||||
out_8283696179185346057[14] = 0.0;
|
||||
out_8283696179185346057[15] = 0.0;
|
||||
out_8283696179185346057[16] = 0.0;
|
||||
out_8283696179185346057[17] = 0.0;
|
||||
out_8283696179185346057[18] = 0.0;
|
||||
out_8283696179185346057[19] = 0.0;
|
||||
out_8283696179185346057[20] = 1.0;
|
||||
out_8283696179185346057[21] = 0.0;
|
||||
out_8283696179185346057[22] = 0.0;
|
||||
out_8283696179185346057[23] = 0.0;
|
||||
out_8283696179185346057[24] = 0.0;
|
||||
out_8283696179185346057[25] = 0.0;
|
||||
out_8283696179185346057[26] = 0.0;
|
||||
out_8283696179185346057[27] = 0.0;
|
||||
out_8283696179185346057[28] = 0.0;
|
||||
out_8283696179185346057[29] = 0.0;
|
||||
out_8283696179185346057[30] = 1.0;
|
||||
out_8283696179185346057[31] = 0.0;
|
||||
out_8283696179185346057[32] = 0.0;
|
||||
out_8283696179185346057[33] = 0.0;
|
||||
out_8283696179185346057[34] = 0.0;
|
||||
out_8283696179185346057[35] = 0.0;
|
||||
out_8283696179185346057[36] = 0.0;
|
||||
out_8283696179185346057[37] = 0.0;
|
||||
out_8283696179185346057[38] = 0.0;
|
||||
out_8283696179185346057[39] = 0.0;
|
||||
out_8283696179185346057[40] = 1.0;
|
||||
out_8283696179185346057[41] = 0.0;
|
||||
out_8283696179185346057[42] = 0.0;
|
||||
out_8283696179185346057[43] = 0.0;
|
||||
out_8283696179185346057[44] = 0.0;
|
||||
out_8283696179185346057[45] = 0.0;
|
||||
out_8283696179185346057[46] = 0.0;
|
||||
out_8283696179185346057[47] = 0.0;
|
||||
out_8283696179185346057[48] = 0.0;
|
||||
out_8283696179185346057[49] = 0.0;
|
||||
out_8283696179185346057[50] = 1.0;
|
||||
out_8283696179185346057[51] = 0.0;
|
||||
out_8283696179185346057[52] = 0.0;
|
||||
out_8283696179185346057[53] = 0.0;
|
||||
out_8283696179185346057[54] = 0.0;
|
||||
out_8283696179185346057[55] = 0.0;
|
||||
out_8283696179185346057[56] = 0.0;
|
||||
out_8283696179185346057[57] = 0.0;
|
||||
out_8283696179185346057[58] = 0.0;
|
||||
out_8283696179185346057[59] = 0.0;
|
||||
out_8283696179185346057[60] = 1.0;
|
||||
out_8283696179185346057[61] = 0.0;
|
||||
out_8283696179185346057[62] = 0.0;
|
||||
out_8283696179185346057[63] = 0.0;
|
||||
out_8283696179185346057[64] = 0.0;
|
||||
out_8283696179185346057[65] = 0.0;
|
||||
out_8283696179185346057[66] = 0.0;
|
||||
out_8283696179185346057[67] = 0.0;
|
||||
out_8283696179185346057[68] = 0.0;
|
||||
out_8283696179185346057[69] = 0.0;
|
||||
out_8283696179185346057[70] = 1.0;
|
||||
out_8283696179185346057[71] = 0.0;
|
||||
out_8283696179185346057[72] = 0.0;
|
||||
out_8283696179185346057[73] = 0.0;
|
||||
out_8283696179185346057[74] = 0.0;
|
||||
out_8283696179185346057[75] = 0.0;
|
||||
out_8283696179185346057[76] = 0.0;
|
||||
out_8283696179185346057[77] = 0.0;
|
||||
out_8283696179185346057[78] = 0.0;
|
||||
out_8283696179185346057[79] = 0.0;
|
||||
out_8283696179185346057[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_4408837966520463957) {
|
||||
out_4408837966520463957[0] = 1.0;
|
||||
out_4408837966520463957[1] = 0.0;
|
||||
out_4408837966520463957[2] = 0.0;
|
||||
out_4408837966520463957[3] = 0.0;
|
||||
out_4408837966520463957[4] = 0.0;
|
||||
out_4408837966520463957[5] = 0.0;
|
||||
out_4408837966520463957[6] = 0.0;
|
||||
out_4408837966520463957[7] = 0.0;
|
||||
out_4408837966520463957[8] = 0.0;
|
||||
out_4408837966520463957[9] = 0.0;
|
||||
out_4408837966520463957[10] = 1.0;
|
||||
out_4408837966520463957[11] = 0.0;
|
||||
out_4408837966520463957[12] = 0.0;
|
||||
out_4408837966520463957[13] = 0.0;
|
||||
out_4408837966520463957[14] = 0.0;
|
||||
out_4408837966520463957[15] = 0.0;
|
||||
out_4408837966520463957[16] = 0.0;
|
||||
out_4408837966520463957[17] = 0.0;
|
||||
out_4408837966520463957[18] = 0.0;
|
||||
out_4408837966520463957[19] = 0.0;
|
||||
out_4408837966520463957[20] = 1.0;
|
||||
out_4408837966520463957[21] = 0.0;
|
||||
out_4408837966520463957[22] = 0.0;
|
||||
out_4408837966520463957[23] = 0.0;
|
||||
out_4408837966520463957[24] = 0.0;
|
||||
out_4408837966520463957[25] = 0.0;
|
||||
out_4408837966520463957[26] = 0.0;
|
||||
out_4408837966520463957[27] = 0.0;
|
||||
out_4408837966520463957[28] = 0.0;
|
||||
out_4408837966520463957[29] = 0.0;
|
||||
out_4408837966520463957[30] = 1.0;
|
||||
out_4408837966520463957[31] = 0.0;
|
||||
out_4408837966520463957[32] = 0.0;
|
||||
out_4408837966520463957[33] = 0.0;
|
||||
out_4408837966520463957[34] = 0.0;
|
||||
out_4408837966520463957[35] = 0.0;
|
||||
out_4408837966520463957[36] = 0.0;
|
||||
out_4408837966520463957[37] = 0.0;
|
||||
out_4408837966520463957[38] = 0.0;
|
||||
out_4408837966520463957[39] = 0.0;
|
||||
out_4408837966520463957[40] = 1.0;
|
||||
out_4408837966520463957[41] = 0.0;
|
||||
out_4408837966520463957[42] = 0.0;
|
||||
out_4408837966520463957[43] = 0.0;
|
||||
out_4408837966520463957[44] = 0.0;
|
||||
out_4408837966520463957[45] = 0.0;
|
||||
out_4408837966520463957[46] = 0.0;
|
||||
out_4408837966520463957[47] = 0.0;
|
||||
out_4408837966520463957[48] = 0.0;
|
||||
out_4408837966520463957[49] = 0.0;
|
||||
out_4408837966520463957[50] = 1.0;
|
||||
out_4408837966520463957[51] = 0.0;
|
||||
out_4408837966520463957[52] = 0.0;
|
||||
out_4408837966520463957[53] = 0.0;
|
||||
out_4408837966520463957[54] = 0.0;
|
||||
out_4408837966520463957[55] = 0.0;
|
||||
out_4408837966520463957[56] = 0.0;
|
||||
out_4408837966520463957[57] = 0.0;
|
||||
out_4408837966520463957[58] = 0.0;
|
||||
out_4408837966520463957[59] = 0.0;
|
||||
out_4408837966520463957[60] = 1.0;
|
||||
out_4408837966520463957[61] = 0.0;
|
||||
out_4408837966520463957[62] = 0.0;
|
||||
out_4408837966520463957[63] = 0.0;
|
||||
out_4408837966520463957[64] = 0.0;
|
||||
out_4408837966520463957[65] = 0.0;
|
||||
out_4408837966520463957[66] = 0.0;
|
||||
out_4408837966520463957[67] = 0.0;
|
||||
out_4408837966520463957[68] = 0.0;
|
||||
out_4408837966520463957[69] = 0.0;
|
||||
out_4408837966520463957[70] = 1.0;
|
||||
out_4408837966520463957[71] = 0.0;
|
||||
out_4408837966520463957[72] = 0.0;
|
||||
out_4408837966520463957[73] = 0.0;
|
||||
out_4408837966520463957[74] = 0.0;
|
||||
out_4408837966520463957[75] = 0.0;
|
||||
out_4408837966520463957[76] = 0.0;
|
||||
out_4408837966520463957[77] = 0.0;
|
||||
out_4408837966520463957[78] = 0.0;
|
||||
out_4408837966520463957[79] = 0.0;
|
||||
out_4408837966520463957[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_3453347444055346076) {
|
||||
out_3453347444055346076[0] = state[0];
|
||||
out_3453347444055346076[1] = state[1];
|
||||
out_3453347444055346076[2] = state[2];
|
||||
out_3453347444055346076[3] = state[3];
|
||||
out_3453347444055346076[4] = state[4];
|
||||
out_3453347444055346076[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_3453347444055346076[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_3453347444055346076[7] = state[7];
|
||||
out_3453347444055346076[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_5836050664863580465) {
|
||||
out_5836050664863580465[0] = state[0];
|
||||
out_5836050664863580465[1] = state[1];
|
||||
out_5836050664863580465[2] = state[2];
|
||||
out_5836050664863580465[3] = state[3];
|
||||
out_5836050664863580465[4] = state[4];
|
||||
out_5836050664863580465[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_5836050664863580465[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_5836050664863580465[7] = state[7];
|
||||
out_5836050664863580465[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_1825222330008905672) {
|
||||
out_1825222330008905672[0] = 1;
|
||||
out_1825222330008905672[1] = 0;
|
||||
out_1825222330008905672[2] = 0;
|
||||
out_1825222330008905672[3] = 0;
|
||||
out_1825222330008905672[4] = 0;
|
||||
out_1825222330008905672[5] = 0;
|
||||
out_1825222330008905672[6] = 0;
|
||||
out_1825222330008905672[7] = 0;
|
||||
out_1825222330008905672[8] = 0;
|
||||
out_1825222330008905672[9] = 0;
|
||||
out_1825222330008905672[10] = 1;
|
||||
out_1825222330008905672[11] = 0;
|
||||
out_1825222330008905672[12] = 0;
|
||||
out_1825222330008905672[13] = 0;
|
||||
out_1825222330008905672[14] = 0;
|
||||
out_1825222330008905672[15] = 0;
|
||||
out_1825222330008905672[16] = 0;
|
||||
out_1825222330008905672[17] = 0;
|
||||
out_1825222330008905672[18] = 0;
|
||||
out_1825222330008905672[19] = 0;
|
||||
out_1825222330008905672[20] = 1;
|
||||
out_1825222330008905672[21] = 0;
|
||||
out_1825222330008905672[22] = 0;
|
||||
out_1825222330008905672[23] = 0;
|
||||
out_1825222330008905672[24] = 0;
|
||||
out_1825222330008905672[25] = 0;
|
||||
out_1825222330008905672[26] = 0;
|
||||
out_1825222330008905672[27] = 0;
|
||||
out_1825222330008905672[28] = 0;
|
||||
out_1825222330008905672[29] = 0;
|
||||
out_1825222330008905672[30] = 1;
|
||||
out_1825222330008905672[31] = 0;
|
||||
out_1825222330008905672[32] = 0;
|
||||
out_1825222330008905672[33] = 0;
|
||||
out_1825222330008905672[34] = 0;
|
||||
out_1825222330008905672[35] = 0;
|
||||
out_1825222330008905672[36] = 0;
|
||||
out_1825222330008905672[37] = 0;
|
||||
out_1825222330008905672[38] = 0;
|
||||
out_1825222330008905672[39] = 0;
|
||||
out_1825222330008905672[40] = 1;
|
||||
out_1825222330008905672[41] = 0;
|
||||
out_1825222330008905672[42] = 0;
|
||||
out_1825222330008905672[43] = 0;
|
||||
out_1825222330008905672[44] = 0;
|
||||
out_1825222330008905672[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_1825222330008905672[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1825222330008905672[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1825222330008905672[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1825222330008905672[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_1825222330008905672[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1825222330008905672[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1825222330008905672[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1825222330008905672[53] = -9.8100000000000005*dt;
|
||||
out_1825222330008905672[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_1825222330008905672[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1825222330008905672[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1825222330008905672[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1825222330008905672[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_1825222330008905672[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1825222330008905672[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_1825222330008905672[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1825222330008905672[62] = 0;
|
||||
out_1825222330008905672[63] = 0;
|
||||
out_1825222330008905672[64] = 0;
|
||||
out_1825222330008905672[65] = 0;
|
||||
out_1825222330008905672[66] = 0;
|
||||
out_1825222330008905672[67] = 0;
|
||||
out_1825222330008905672[68] = 0;
|
||||
out_1825222330008905672[69] = 0;
|
||||
out_1825222330008905672[70] = 1;
|
||||
out_1825222330008905672[71] = 0;
|
||||
out_1825222330008905672[72] = 0;
|
||||
out_1825222330008905672[73] = 0;
|
||||
out_1825222330008905672[74] = 0;
|
||||
out_1825222330008905672[75] = 0;
|
||||
out_1825222330008905672[76] = 0;
|
||||
out_1825222330008905672[77] = 0;
|
||||
out_1825222330008905672[78] = 0;
|
||||
out_1825222330008905672[79] = 0;
|
||||
out_1825222330008905672[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_584824642242628871) {
|
||||
out_584824642242628871[0] = 1;
|
||||
out_584824642242628871[1] = 0;
|
||||
out_584824642242628871[2] = 0;
|
||||
out_584824642242628871[3] = 0;
|
||||
out_584824642242628871[4] = 0;
|
||||
out_584824642242628871[5] = 0;
|
||||
out_584824642242628871[6] = 0;
|
||||
out_584824642242628871[7] = 0;
|
||||
out_584824642242628871[8] = 0;
|
||||
out_584824642242628871[9] = 0;
|
||||
out_584824642242628871[10] = 1;
|
||||
out_584824642242628871[11] = 0;
|
||||
out_584824642242628871[12] = 0;
|
||||
out_584824642242628871[13] = 0;
|
||||
out_584824642242628871[14] = 0;
|
||||
out_584824642242628871[15] = 0;
|
||||
out_584824642242628871[16] = 0;
|
||||
out_584824642242628871[17] = 0;
|
||||
out_584824642242628871[18] = 0;
|
||||
out_584824642242628871[19] = 0;
|
||||
out_584824642242628871[20] = 1;
|
||||
out_584824642242628871[21] = 0;
|
||||
out_584824642242628871[22] = 0;
|
||||
out_584824642242628871[23] = 0;
|
||||
out_584824642242628871[24] = 0;
|
||||
out_584824642242628871[25] = 0;
|
||||
out_584824642242628871[26] = 0;
|
||||
out_584824642242628871[27] = 0;
|
||||
out_584824642242628871[28] = 0;
|
||||
out_584824642242628871[29] = 0;
|
||||
out_584824642242628871[30] = 1;
|
||||
out_584824642242628871[31] = 0;
|
||||
out_584824642242628871[32] = 0;
|
||||
out_584824642242628871[33] = 0;
|
||||
out_584824642242628871[34] = 0;
|
||||
out_584824642242628871[35] = 0;
|
||||
out_584824642242628871[36] = 0;
|
||||
out_584824642242628871[37] = 0;
|
||||
out_584824642242628871[38] = 0;
|
||||
out_584824642242628871[39] = 0;
|
||||
out_584824642242628871[40] = 1;
|
||||
out_584824642242628871[41] = 0;
|
||||
out_584824642242628871[42] = 0;
|
||||
out_584824642242628871[43] = 0;
|
||||
out_584824642242628871[44] = 0;
|
||||
out_584824642242628871[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_584824642242628871[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_584824642242628871[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_584824642242628871[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_584824642242628871[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_584824642242628871[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_584824642242628871[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_584824642242628871[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_584824642242628871[53] = -9.8100000000000005*dt;
|
||||
out_584824642242628871[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_584824642242628871[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_584824642242628871[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_584824642242628871[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_584824642242628871[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_584824642242628871[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_584824642242628871[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_584824642242628871[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_584824642242628871[62] = 0;
|
||||
out_584824642242628871[63] = 0;
|
||||
out_584824642242628871[64] = 0;
|
||||
out_584824642242628871[65] = 0;
|
||||
out_584824642242628871[66] = 0;
|
||||
out_584824642242628871[67] = 0;
|
||||
out_584824642242628871[68] = 0;
|
||||
out_584824642242628871[69] = 0;
|
||||
out_584824642242628871[70] = 1;
|
||||
out_584824642242628871[71] = 0;
|
||||
out_584824642242628871[72] = 0;
|
||||
out_584824642242628871[73] = 0;
|
||||
out_584824642242628871[74] = 0;
|
||||
out_584824642242628871[75] = 0;
|
||||
out_584824642242628871[76] = 0;
|
||||
out_584824642242628871[77] = 0;
|
||||
out_584824642242628871[78] = 0;
|
||||
out_584824642242628871[79] = 0;
|
||||
out_584824642242628871[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_7364871104219513908) {
|
||||
out_7364871104219513908[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_3846550558690327557) {
|
||||
out_3846550558690327557[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_5259356069339540719) {
|
||||
out_5259356069339540719[0] = 0;
|
||||
out_5259356069339540719[1] = 0;
|
||||
out_5259356069339540719[2] = 0;
|
||||
out_5259356069339540719[3] = 0;
|
||||
out_5259356069339540719[4] = 0;
|
||||
out_5259356069339540719[5] = 0;
|
||||
out_5259356069339540719[6] = 1;
|
||||
out_5259356069339540719[7] = 0;
|
||||
out_5259356069339540719[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_3533529222181460977) {
|
||||
out_3533529222181460977[0] = 0;
|
||||
out_3533529222181460977[1] = 0;
|
||||
out_3533529222181460977[2] = 0;
|
||||
out_3533529222181460977[3] = 0;
|
||||
out_3533529222181460977[4] = 0;
|
||||
out_3533529222181460977[5] = 0;
|
||||
out_3533529222181460977[6] = 1;
|
||||
out_3533529222181460977[7] = 0;
|
||||
out_3533529222181460977[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_6181950591427252357) {
|
||||
out_6181950591427252357[0] = state[4];
|
||||
out_6181950591427252357[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_5575782566501311775) {
|
||||
out_5575782566501311775[0] = state[4];
|
||||
out_5575782566501311775[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_1316215737251977382) {
|
||||
out_1316215737251977382[0] = 0;
|
||||
out_1316215737251977382[1] = 0;
|
||||
out_1316215737251977382[2] = 0;
|
||||
out_1316215737251977382[3] = 0;
|
||||
out_1316215737251977382[4] = 1;
|
||||
out_1316215737251977382[5] = 0;
|
||||
out_1316215737251977382[6] = 0;
|
||||
out_1316215737251977382[7] = 0;
|
||||
out_1316215737251977382[8] = 0;
|
||||
out_1316215737251977382[9] = 0;
|
||||
out_1316215737251977382[10] = 0;
|
||||
out_1316215737251977382[11] = 0;
|
||||
out_1316215737251977382[12] = 0;
|
||||
out_1316215737251977382[13] = 0;
|
||||
out_1316215737251977382[14] = 1;
|
||||
out_1316215737251977382[15] = 0;
|
||||
out_1316215737251977382[16] = 0;
|
||||
out_1316215737251977382[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_24396376620862159) {
|
||||
out_24396376620862159[0] = 0;
|
||||
out_24396376620862159[1] = 0;
|
||||
out_24396376620862159[2] = 0;
|
||||
out_24396376620862159[3] = 0;
|
||||
out_24396376620862159[4] = 1;
|
||||
out_24396376620862159[5] = 0;
|
||||
out_24396376620862159[6] = 0;
|
||||
out_24396376620862159[7] = 0;
|
||||
out_24396376620862159[8] = 0;
|
||||
out_24396376620862159[9] = 0;
|
||||
out_24396376620862159[10] = 0;
|
||||
out_24396376620862159[11] = 0;
|
||||
out_24396376620862159[12] = 0;
|
||||
out_24396376620862159[13] = 0;
|
||||
out_24396376620862159[14] = 1;
|
||||
out_24396376620862159[15] = 0;
|
||||
out_24396376620862159[16] = 0;
|
||||
out_24396376620862159[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_4311037743139686772) {
|
||||
out_4311037743139686772[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_6794276549045812518) {
|
||||
out_6794276549045812518[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_1657334272152076036) {
|
||||
out_1657334272152076036[0] = 0;
|
||||
out_1657334272152076036[1] = 0;
|
||||
out_1657334272152076036[2] = 0;
|
||||
out_1657334272152076036[3] = 0;
|
||||
out_1657334272152076036[4] = 1;
|
||||
out_1657334272152076036[5] = 0;
|
||||
out_1657334272152076036[6] = 0;
|
||||
out_1657334272152076036[7] = 0;
|
||||
out_1657334272152076036[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_1015196263674212350) {
|
||||
out_1015196263674212350[0] = 0;
|
||||
out_1015196263674212350[1] = 0;
|
||||
out_1015196263674212350[2] = 0;
|
||||
out_1015196263674212350[3] = 0;
|
||||
out_1015196263674212350[4] = 1;
|
||||
out_1015196263674212350[5] = 0;
|
||||
out_1015196263674212350[6] = 0;
|
||||
out_1015196263674212350[7] = 0;
|
||||
out_1015196263674212350[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_8569605146509354979) {
|
||||
out_8569605146509354979[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_9048062203855041447) {
|
||||
out_9048062203855041447[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_9000859388213596943) {
|
||||
out_9000859388213596943[0] = 0;
|
||||
out_9000859388213596943[1] = 0;
|
||||
out_9000859388213596943[2] = 0;
|
||||
out_9000859388213596943[3] = 0;
|
||||
out_9000859388213596943[4] = 0;
|
||||
out_9000859388213596943[5] = 0;
|
||||
out_9000859388213596943[6] = 0;
|
||||
out_9000859388213596943[7] = 1;
|
||||
out_9000859388213596943[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_229003252420660376) {
|
||||
out_229003252420660376[0] = 0;
|
||||
out_229003252420660376[1] = 0;
|
||||
out_229003252420660376[2] = 0;
|
||||
out_229003252420660376[3] = 0;
|
||||
out_229003252420660376[4] = 0;
|
||||
out_229003252420660376[5] = 0;
|
||||
out_229003252420660376[6] = 0;
|
||||
out_229003252420660376[7] = 1;
|
||||
out_229003252420660376[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_2363197836558668263) {
|
||||
out_2363197836558668263[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_5135418613316513208) {
|
||||
out_5135418613316513208[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_517429039648348875) {
|
||||
out_517429039648348875[0] = 0;
|
||||
out_517429039648348875[1] = 0;
|
||||
out_517429039648348875[2] = 0;
|
||||
out_517429039648348875[3] = 1;
|
||||
out_517429039648348875[4] = 0;
|
||||
out_517429039648348875[5] = 0;
|
||||
out_517429039648348875[6] = 0;
|
||||
out_517429039648348875[7] = 0;
|
||||
out_517429039648348875[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_3189959575474637261) {
|
||||
out_3189959575474637261[0] = 0;
|
||||
out_3189959575474637261[1] = 0;
|
||||
out_3189959575474637261[2] = 0;
|
||||
out_3189959575474637261[3] = 1;
|
||||
out_3189959575474637261[4] = 0;
|
||||
out_3189959575474637261[5] = 0;
|
||||
out_3189959575474637261[6] = 0;
|
||||
out_3189959575474637261[7] = 0;
|
||||
out_3189959575474637261[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_1498485639869833389) {
|
||||
out_1498485639869833389[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_5850766696144150083) {
|
||||
out_5850766696144150083[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_2230791766517899908) {
|
||||
out_2230791766517899908[0] = 0;
|
||||
out_2230791766517899908[1] = 1;
|
||||
out_2230791766517899908[2] = 0;
|
||||
out_2230791766517899908[3] = 0;
|
||||
out_2230791766517899908[4] = 0;
|
||||
out_2230791766517899908[5] = 0;
|
||||
out_2230791766517899908[6] = 0;
|
||||
out_2230791766517899908[7] = 0;
|
||||
out_2230791766517899908[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_504964919359820166) {
|
||||
out_504964919359820166[0] = 0;
|
||||
out_504964919359820166[1] = 1;
|
||||
out_504964919359820166[2] = 0;
|
||||
out_504964919359820166[3] = 0;
|
||||
out_504964919359820166[4] = 0;
|
||||
out_504964919359820166[5] = 0;
|
||||
out_504964919359820166[6] = 0;
|
||||
out_504964919359820166[7] = 0;
|
||||
out_504964919359820166[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_6657345575881808179) {
|
||||
out_6657345575881808179[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_133745345655667563) {
|
||||
out_133745345655667563[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_7313190783587430482) {
|
||||
out_7313190783587430482[0] = 1;
|
||||
out_7313190783587430482[1] = 0;
|
||||
out_7313190783587430482[2] = 0;
|
||||
out_7313190783587430482[3] = 0;
|
||||
out_7313190783587430482[4] = 0;
|
||||
out_7313190783587430482[5] = 0;
|
||||
out_7313190783587430482[6] = 0;
|
||||
out_7313190783587430482[7] = 0;
|
||||
out_7313190783587430482[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_5587363936429350740) {
|
||||
out_5587363936429350740[0] = 1;
|
||||
out_5587363936429350740[1] = 0;
|
||||
out_5587363936429350740[2] = 0;
|
||||
out_5587363936429350740[3] = 0;
|
||||
out_5587363936429350740[4] = 0;
|
||||
out_5587363936429350740[5] = 0;
|
||||
out_5587363936429350740[6] = 0;
|
||||
out_5587363936429350740[7] = 0;
|
||||
out_5587363936429350740[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_398394152601158533) {
|
||||
out_398394152601158533[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_4672202664478170413) {
|
||||
out_4672202664478170413[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_5228710107462580291) {
|
||||
out_5228710107462580291[0] = 0;
|
||||
out_5228710107462580291[1] = 0;
|
||||
out_5228710107462580291[2] = 0;
|
||||
out_5228710107462580291[3] = 0;
|
||||
out_5228710107462580291[4] = 0;
|
||||
out_5228710107462580291[5] = 0;
|
||||
out_5228710107462580291[6] = 0;
|
||||
out_5228710107462580291[7] = 0;
|
||||
out_5228710107462580291[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_7901240643288868677) {
|
||||
out_7901240643288868677[0] = 0;
|
||||
out_7901240643288868677[1] = 0;
|
||||
out_7901240643288868677[2] = 0;
|
||||
out_7901240643288868677[3] = 0;
|
||||
out_7901240643288868677[4] = 0;
|
||||
out_7901240643288868677[5] = 0;
|
||||
out_7901240643288868677[6] = 0;
|
||||
out_7901240643288868677[7] = 0;
|
||||
out_7901240643288868677[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_6333242559483084265) {
|
||||
err_fun(nom_x, delta_x, out_6333242559483084265);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_4080197981179722536) {
|
||||
err_fun(nom_x, delta_x, out_4080197981179722536);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_829248915283082072) {
|
||||
inv_err_fun(nom_x, true_x, out_829248915283082072);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3164357961811210311) {
|
||||
inv_err_fun(nom_x, true_x, out_3164357961811210311);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_8283696179185346057) {
|
||||
H_mod_fun(state, out_8283696179185346057);
|
||||
void car_H_mod_fun(double *state, double *out_4408837966520463957) {
|
||||
H_mod_fun(state, out_4408837966520463957);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_3453347444055346076) {
|
||||
f_fun(state, dt, out_3453347444055346076);
|
||||
void car_f_fun(double *state, double dt, double *out_5836050664863580465) {
|
||||
f_fun(state, dt, out_5836050664863580465);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_1825222330008905672) {
|
||||
F_fun(state, dt, out_1825222330008905672);
|
||||
void car_F_fun(double *state, double dt, double *out_584824642242628871) {
|
||||
F_fun(state, dt, out_584824642242628871);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_7364871104219513908) {
|
||||
h_25(state, unused, out_7364871104219513908);
|
||||
void car_h_25(double *state, double *unused, double *out_3846550558690327557) {
|
||||
h_25(state, unused, out_3846550558690327557);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_5259356069339540719) {
|
||||
H_25(state, unused, out_5259356069339540719);
|
||||
void car_H_25(double *state, double *unused, double *out_3533529222181460977) {
|
||||
H_25(state, unused, out_3533529222181460977);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_6181950591427252357) {
|
||||
h_24(state, unused, out_6181950591427252357);
|
||||
void car_h_24(double *state, double *unused, double *out_5575782566501311775) {
|
||||
h_24(state, unused, out_5575782566501311775);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_1316215737251977382) {
|
||||
H_24(state, unused, out_1316215737251977382);
|
||||
void car_H_24(double *state, double *unused, double *out_24396376620862159) {
|
||||
H_24(state, unused, out_24396376620862159);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_4311037743139686772) {
|
||||
h_30(state, unused, out_4311037743139686772);
|
||||
void car_h_30(double *state, double *unused, double *out_6794276549045812518) {
|
||||
h_30(state, unused, out_6794276549045812518);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_1657334272152076036) {
|
||||
H_30(state, unused, out_1657334272152076036);
|
||||
void car_H_30(double *state, double *unused, double *out_1015196263674212350) {
|
||||
H_30(state, unused, out_1015196263674212350);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_8569605146509354979) {
|
||||
h_26(state, unused, out_8569605146509354979);
|
||||
void car_h_26(double *state, double *unused, double *out_9048062203855041447) {
|
||||
h_26(state, unused, out_9048062203855041447);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_9000859388213596943) {
|
||||
H_26(state, unused, out_9000859388213596943);
|
||||
void car_H_26(double *state, double *unused, double *out_229003252420660376) {
|
||||
H_26(state, unused, out_229003252420660376);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_2363197836558668263) {
|
||||
h_27(state, unused, out_2363197836558668263);
|
||||
void car_h_27(double *state, double *unused, double *out_5135418613316513208) {
|
||||
h_27(state, unused, out_5135418613316513208);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_517429039648348875) {
|
||||
H_27(state, unused, out_517429039648348875);
|
||||
void car_H_27(double *state, double *unused, double *out_3189959575474637261) {
|
||||
H_27(state, unused, out_3189959575474637261);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_1498485639869833389) {
|
||||
h_29(state, unused, out_1498485639869833389);
|
||||
void car_h_29(double *state, double *unused, double *out_5850766696144150083) {
|
||||
h_29(state, unused, out_5850766696144150083);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_2230791766517899908) {
|
||||
H_29(state, unused, out_2230791766517899908);
|
||||
void car_H_29(double *state, double *unused, double *out_504964919359820166) {
|
||||
H_29(state, unused, out_504964919359820166);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_6657345575881808179) {
|
||||
h_28(state, unused, out_6657345575881808179);
|
||||
void car_h_28(double *state, double *unused, double *out_133745345655667563) {
|
||||
h_28(state, unused, out_133745345655667563);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_7313190783587430482) {
|
||||
H_28(state, unused, out_7313190783587430482);
|
||||
void car_H_28(double *state, double *unused, double *out_5587363936429350740) {
|
||||
H_28(state, unused, out_5587363936429350740);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_398394152601158533) {
|
||||
h_31(state, unused, out_398394152601158533);
|
||||
void car_h_31(double *state, double *unused, double *out_4672202664478170413) {
|
||||
h_31(state, unused, out_4672202664478170413);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_5228710107462580291) {
|
||||
H_31(state, unused, out_5228710107462580291);
|
||||
void car_H_31(double *state, double *unused, double *out_7901240643288868677) {
|
||||
H_31(state, unused, out_7901240643288868677);
|
||||
}
|
||||
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_6333242559483084265);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_829248915283082072);
|
||||
void car_H_mod_fun(double *state, double *out_8283696179185346057);
|
||||
void car_f_fun(double *state, double dt, double *out_3453347444055346076);
|
||||
void car_F_fun(double *state, double dt, double *out_1825222330008905672);
|
||||
void car_h_25(double *state, double *unused, double *out_7364871104219513908);
|
||||
void car_H_25(double *state, double *unused, double *out_5259356069339540719);
|
||||
void car_h_24(double *state, double *unused, double *out_6181950591427252357);
|
||||
void car_H_24(double *state, double *unused, double *out_1316215737251977382);
|
||||
void car_h_30(double *state, double *unused, double *out_4311037743139686772);
|
||||
void car_H_30(double *state, double *unused, double *out_1657334272152076036);
|
||||
void car_h_26(double *state, double *unused, double *out_8569605146509354979);
|
||||
void car_H_26(double *state, double *unused, double *out_9000859388213596943);
|
||||
void car_h_27(double *state, double *unused, double *out_2363197836558668263);
|
||||
void car_H_27(double *state, double *unused, double *out_517429039648348875);
|
||||
void car_h_29(double *state, double *unused, double *out_1498485639869833389);
|
||||
void car_H_29(double *state, double *unused, double *out_2230791766517899908);
|
||||
void car_h_28(double *state, double *unused, double *out_6657345575881808179);
|
||||
void car_H_28(double *state, double *unused, double *out_7313190783587430482);
|
||||
void car_h_31(double *state, double *unused, double *out_398394152601158533);
|
||||
void car_H_31(double *state, double *unused, double *out_5228710107462580291);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_4080197981179722536);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3164357961811210311);
|
||||
void car_H_mod_fun(double *state, double *out_4408837966520463957);
|
||||
void car_f_fun(double *state, double dt, double *out_5836050664863580465);
|
||||
void car_F_fun(double *state, double dt, double *out_584824642242628871);
|
||||
void car_h_25(double *state, double *unused, double *out_3846550558690327557);
|
||||
void car_H_25(double *state, double *unused, double *out_3533529222181460977);
|
||||
void car_h_24(double *state, double *unused, double *out_5575782566501311775);
|
||||
void car_H_24(double *state, double *unused, double *out_24396376620862159);
|
||||
void car_h_30(double *state, double *unused, double *out_6794276549045812518);
|
||||
void car_H_30(double *state, double *unused, double *out_1015196263674212350);
|
||||
void car_h_26(double *state, double *unused, double *out_9048062203855041447);
|
||||
void car_H_26(double *state, double *unused, double *out_229003252420660376);
|
||||
void car_h_27(double *state, double *unused, double *out_5135418613316513208);
|
||||
void car_H_27(double *state, double *unused, double *out_3189959575474637261);
|
||||
void car_h_29(double *state, double *unused, double *out_5850766696144150083);
|
||||
void car_H_29(double *state, double *unused, double *out_504964919359820166);
|
||||
void car_h_28(double *state, double *unused, double *out_133745345655667563);
|
||||
void car_H_28(double *state, double *unused, double *out_5587363936429350740);
|
||||
void car_h_31(double *state, double *unused, double *out_4672202664478170413);
|
||||
void car_H_31(double *state, double *unused, double *out_7901240643288868677);
|
||||
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);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_3574067617578089570);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1515328263168530495);
|
||||
void pose_H_mod_fun(double *state, double *out_207229337745545750);
|
||||
void pose_f_fun(double *state, double dt, double *out_3493541226789963130);
|
||||
void pose_F_fun(double *state, double dt, double *out_5154051058888586616);
|
||||
void pose_h_4(double *state, double *unused, double *out_5970382648807117116);
|
||||
void pose_H_4(double *state, double *unused, double *out_4945289047293534985);
|
||||
void pose_h_10(double *state, double *unused, double *out_520088425664385209);
|
||||
void pose_H_10(double *state, double *unused, double *out_4050335309714945768);
|
||||
void pose_h_13(double *state, double *unused, double *out_1434579794425570866);
|
||||
void pose_H_13(double *state, double *unused, double *out_1111533583991010961);
|
||||
void pose_h_14(double *state, double *unused, double *out_5370135948753942539);
|
||||
void pose_H_14(double *state, double *unused, double *out_1862500614998162689);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_616508134330162473);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3534488348241865019);
|
||||
void pose_H_mod_fun(double *state, double *out_5384152065241129084);
|
||||
void pose_f_fun(double *state, double dt, double *out_1966290028419981018);
|
||||
void pose_F_fun(double *state, double dt, double *out_8500986371107107872);
|
||||
void pose_h_4(double *state, double *unused, double *out_5400718070291809963);
|
||||
void pose_H_4(double *state, double *unused, double *out_9155959275085036656);
|
||||
void pose_h_10(double *state, double *unused, double *out_2963274022918519597);
|
||||
void pose_H_10(double *state, double *unused, double *out_5627581855619872909);
|
||||
void pose_h_13(double *state, double *unused, double *out_6005618121794927988);
|
||||
void pose_H_13(double *state, double *unused, double *out_1680153590307814031);
|
||||
void pose_h_14(double *state, double *unused, double *out_5652518326669022365);
|
||||
void pose_H_14(double *state, double *unused, double *out_6073170842789664360);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -99,10 +99,14 @@ def _with_alpha(color: rl.Color, a: int) -> rl.Color:
|
||||
def _pulse(freq: float) -> float:
|
||||
return 0.5 + 0.5 * math.sin(rl.get_time() * freq)
|
||||
|
||||
def _get_road_height(data: 'AetherGaugeData | None') -> float:
|
||||
is_curve = data.indicator_type == IndicatorType.ROAD_CURVE if data else False
|
||||
return ROAD_HEIGHT if is_curve else (45.0 * SCALE)
|
||||
|
||||
def _road_xy(distance: float, icx: float, bottom: float, data: 'AetherGaugeData') -> tuple[float, float, float]:
|
||||
t = max(0.0, min(1.0, _get_t_from_x(distance)))
|
||||
offset = _get_perspective_offset(t, data)
|
||||
return t, icx + offset, bottom - t * ROAD_HEIGHT
|
||||
return t, icx + offset, bottom - t * _get_road_height(data)
|
||||
|
||||
def _get_t_from_x(x: float) -> float:
|
||||
x_clamped = max(X_MIN, min(X_MAX, x))
|
||||
@@ -110,11 +114,8 @@ def _get_t_from_x(x: float) -> float:
|
||||
|
||||
def _get_perspective_offset(t: float, data: 'AetherGaugeData | None') -> float:
|
||||
curvature = 0.0
|
||||
if data:
|
||||
if data.indicator_type == IndicatorType.ROAD_CURVE:
|
||||
curvature = data.indicator_value
|
||||
elif _sm_valid("starpilotPlan"):
|
||||
curvature = ui_state.sm["starpilotPlan"].roadCurvature
|
||||
if data and data.indicator_type == IndicatorType.ROAD_CURVE:
|
||||
curvature = data.indicator_value
|
||||
|
||||
path_y_far = PATH_Y_FAR_SCALE * curvature
|
||||
max_offset_top = math.tanh(path_y_far * PERSPECTIVE_GAIN) * PERSPECTIVE_MAX_OFFSET
|
||||
@@ -311,7 +312,8 @@ class AetherGauge:
|
||||
]
|
||||
if TEST_CYCLE:
|
||||
self._sources.insert(0, (_test_cycle_active, _test_cycle_data))
|
||||
self._stop_smooth_s = 0.0
|
||||
self._chevron_accum = 0.0
|
||||
self._lead_chevron_accum = 0.0
|
||||
|
||||
def get_active_data(self) -> AetherGaugeData | None:
|
||||
for is_active, get_data in self._sources:
|
||||
@@ -319,42 +321,42 @@ class AetherGauge:
|
||||
return get_data()
|
||||
return None
|
||||
|
||||
def render(self, rect: rl.Rectangle, font_bold: rl.Font, font_medium: rl.Font, current_speed: float):
|
||||
def render(self, rect: rl.Rectangle, font_bold: rl.Font, font_medium: rl.Font, current_speed: float, cx: float | None = None, bottom: float | None = None):
|
||||
data = self.get_active_data()
|
||||
if not data:
|
||||
return
|
||||
|
||||
cx = rect.x + rect.width / 2
|
||||
cy_speed = rect.y + 180 * SCALE
|
||||
|
||||
speed_text = str(round(current_speed))
|
||||
speed_text_size = measure_text_cached(font_bold, speed_text, int(176 * SCALE))
|
||||
icon_cx = cx - speed_text_size.x / 2 - 70.0 * SCALE
|
||||
if cx is None or bottom is None:
|
||||
base_cx = rect.x + rect.width / 2
|
||||
cy_speed = rect.y + 180 * SCALE
|
||||
speed_text = str(round(current_speed))
|
||||
speed_text_size = measure_text_cached(font_bold, speed_text, int(176 * SCALE))
|
||||
icx = base_cx - speed_text_size.x / 2 - 70.0 * SCALE
|
||||
icy = cy_speed - 39.5 * SCALE
|
||||
else:
|
||||
icx = cx
|
||||
icy = bottom - ROAD_HALF_SIZE
|
||||
|
||||
if data.indicator_type in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.LEAD, IndicatorType.STOP_LIGHT):
|
||||
self._render_unified_road(rect, icon_cx, cy_speed - 39.5 * SCALE, data, font_bold, font_medium)
|
||||
self._render_unified_road(rect, icx, icy, data, font_bold, font_medium)
|
||||
|
||||
def _render_unified_road(self, rect, icx, icy, data, font_bold, font_medium):
|
||||
bottom = icy + ROAD_HALF_SIZE
|
||||
|
||||
if data.indicator_type == IndicatorType.FORCE_STOP:
|
||||
raw_s = max(0.0, min(1.0, (STOP_DISTANCE_MAX - data.indicator_value) / STOP_DISTANCE_MAX))
|
||||
if abs(raw_s - self._stop_smooth_s) > STOP_SNAP_THRESHOLD:
|
||||
self._stop_smooth_s = raw_s
|
||||
else:
|
||||
self._stop_smooth_s += (raw_s - self._stop_smooth_s) * STOP_LERP_RATE
|
||||
distance = STOP_DISTANCE_MAX - self._stop_smooth_s * STOP_DISTANCE_MAX
|
||||
if data.indicator_type in (IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
|
||||
distance = 15.0
|
||||
else:
|
||||
distance = data.indicator_value
|
||||
|
||||
points_left = []
|
||||
points_right = []
|
||||
road_h = _get_road_height(data)
|
||||
|
||||
for i in range(ROAD_SEGMENTS + 1):
|
||||
t = i / ROAD_SEGMENTS
|
||||
offset = _get_perspective_offset(t, data)
|
||||
cx_t = icx + offset
|
||||
y_t = bottom - t * ROAD_HEIGHT
|
||||
y_t = bottom - t * road_h
|
||||
w_t = ROAD_W_BOTTOM - t * (ROAD_W_BOTTOM - ROAD_W_TOP)
|
||||
|
||||
points_left.append(rl.Vector2(cx_t - w_t, y_t))
|
||||
@@ -402,7 +404,7 @@ class AetherGauge:
|
||||
rl.draw_line_ex(p_left, p_right, max(1.5 * SCALE, 3.5 * SCALE * fade), COLOR_STOP_LINE_CORE)
|
||||
|
||||
def _draw_lead_car(self, icx, bottom, data):
|
||||
t_lead, cx_lead, cy_lead = _road_xy(data.indicator_value, icx, bottom, data)
|
||||
t_lead, cx_lead, cy_lead = _road_xy(6.5, icx, bottom, data)
|
||||
t_lead = max(0.15, min(0.85, t_lead))
|
||||
|
||||
car_scale = 0.45 + (1.0 - t_lead) * 0.55
|
||||
@@ -415,7 +417,7 @@ class AetherGauge:
|
||||
if is_stopped:
|
||||
border_color = COLOR_LEAD_STOPPED
|
||||
elif is_slower:
|
||||
border_color = _with_alpha(data.color, int(160 + 95 * _pulse(3.0)))
|
||||
border_color = _with_alpha(data.color, int(160 + 95 * _pulse(1.2)))
|
||||
else:
|
||||
border_color = data.color
|
||||
|
||||
@@ -436,12 +438,12 @@ class AetherGauge:
|
||||
for tl_x, glow_x in [(int(cx_lead - W * 0.45), int(cx_lead - W * 0.38)),
|
||||
(int(cx_lead + W * 0.3), int(cx_lead + W * 0.38))]:
|
||||
if is_stopped:
|
||||
pulse = _pulse(8.0)
|
||||
pulse = _pulse(2.5)
|
||||
r_glow = int(W * 0.08 * (1.0 + 0.4 * pulse))
|
||||
rl.draw_circle(glow_x, glow_y, r_glow, rl.Color(255, 30, 60, int(150 + 105 * pulse)))
|
||||
c_tl = rl.Color(255, 220, 220, 255)
|
||||
elif is_slower:
|
||||
pulse = _pulse(3.0)
|
||||
pulse = _pulse(1.2)
|
||||
r_glow = int(W * 0.06 * (1.0 + 0.2 * pulse))
|
||||
rl.draw_circle(glow_x, glow_y, r_glow, rl.Color(255, 140, 30, int(100 + 80 * pulse)))
|
||||
c_tl = rl.Color(255, 160, 60, int(180 + 75 * pulse))
|
||||
@@ -458,12 +460,17 @@ class AetherGauge:
|
||||
rl.draw_rectangle(int(cx_lead + W * 0.28), wheel_y, wheel_w, wheel_h, wheel_c)
|
||||
|
||||
# Following chevrons (from lead car back toward viewer)
|
||||
progress = (rl.get_time() * 1.5) % 1.0
|
||||
dt = rl.get_frame_time()
|
||||
v_ego = _get_val("carState", "vEgo", 0.0)
|
||||
flow_speed_factor = min(1.8, v_ego * 0.08)
|
||||
self._lead_chevron_accum = (self._lead_chevron_accum + dt * flow_speed_factor * 1.8) % 1.0
|
||||
progress = self._lead_chevron_accum
|
||||
for i in range(2):
|
||||
t = t_lead * (1.0 - ((i + progress) / 2) % 1.0)
|
||||
|
||||
cx_t = icx + _get_perspective_offset(t, data)
|
||||
cy_t = bottom - t * ROAD_HEIGHT
|
||||
road_h = _get_road_height(data)
|
||||
cy_t = bottom - t * road_h
|
||||
|
||||
chevron_w = 12.0 * SCALE - t * 5.0 * SCALE
|
||||
chevron_thick = max(1.5 * SCALE, 3.5 * SCALE - t * 1.5 * SCALE)
|
||||
@@ -473,12 +480,16 @@ class AetherGauge:
|
||||
alpha = max(0, min(255, int(data.color.a * (1.0 - t / t_lead) * math.sin(t / t_lead * math.pi))))
|
||||
c_color = _with_alpha(data.color, alpha)
|
||||
|
||||
rl.draw_line_ex(rl.Vector2(lx, cy_t + chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
|
||||
rl.draw_line_ex(rl.Vector2(rx, cy_t + chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
|
||||
rl.draw_line_ex(rl.Vector2(lx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
|
||||
rl.draw_line_ex(rl.Vector2(rx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
|
||||
|
||||
def _draw_standard_chevrons(self, icx, bottom, distance, data):
|
||||
is_stop = data.indicator_type in (IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT)
|
||||
progress = (rl.get_time() * 1.2) % 1.0
|
||||
dt = rl.get_frame_time()
|
||||
v_ego = _get_val("carState", "vEgo", 0.0)
|
||||
flow_speed_factor = min(1.5, v_ego * 0.08)
|
||||
self._chevron_accum = (self._chevron_accum + dt * flow_speed_factor * 1.5) % 1.0
|
||||
progress = self._chevron_accum
|
||||
t_stop = max(0.05, min(1.0, distance / STOP_DISTANCE_MAX)) if is_stop else 1.0
|
||||
max_t = t_stop if is_stop else 1.0
|
||||
|
||||
@@ -491,8 +502,9 @@ class AetherGauge:
|
||||
cx_t = icx + _get_perspective_offset(t, data)
|
||||
cx_next = icx + _get_perspective_offset(t_next, data)
|
||||
|
||||
cy_t = bottom - t * ROAD_HEIGHT
|
||||
cy_next = bottom - t_next * ROAD_HEIGHT
|
||||
road_h = _get_road_height(data)
|
||||
cy_t = bottom - t * road_h
|
||||
cy_next = bottom - t_next * road_h
|
||||
|
||||
dx = cx_next - cx_t
|
||||
dy = cy_next - cy_t
|
||||
@@ -506,10 +518,10 @@ class AetherGauge:
|
||||
chevron_h = chevron_w * 0.6
|
||||
chevron_thick = max(2.0 * SCALE, 4.0 * SCALE - t * 2.0 * SCALE)
|
||||
|
||||
lx = cx_t - dir_right_x * chevron_w - dir_up_x * chevron_h
|
||||
ly = cy_t - dir_right_y * chevron_w - dir_up_y * chevron_h
|
||||
rx = cx_t + dir_right_x * chevron_w - dir_up_x * chevron_h
|
||||
ry = cy_t + dir_right_y * chevron_w - dir_up_y * chevron_h
|
||||
lx = cx_t - dir_right_x * chevron_w + dir_up_x * chevron_h
|
||||
ly = cy_t - dir_right_y * chevron_w + dir_up_y * chevron_h
|
||||
rx = cx_t + dir_right_x * chevron_w + dir_up_x * chevron_h
|
||||
ry = cy_t + dir_right_y * chevron_w + dir_up_y * chevron_h
|
||||
|
||||
alpha_factor = math.sin((t / max_t) * math.pi) if max_t > 0.01 else 0.0
|
||||
alpha = max(0, min(255, int(data.color.a * alpha_factor)))
|
||||
@@ -545,7 +557,7 @@ class AetherGauge:
|
||||
]
|
||||
for y, name, active_c, inactive_c in bulbs:
|
||||
if name == "red" and active_light == "red":
|
||||
glow_pulse = _pulse(5.0)
|
||||
glow_pulse = _pulse(1.5)
|
||||
rl.draw_circle_v(rl.Vector2(cx_light, y), r_bulb + 3.0 * SCALE * glow_pulse, rl.Color(255, 30, 60, 45))
|
||||
rl.draw_circle_v(rl.Vector2(cx_light, y), r_bulb + 6.0 * SCALE * glow_pulse, rl.Color(255, 30, 60, 15))
|
||||
c = active_c if active_light == name else inactive_c
|
||||
@@ -558,7 +570,8 @@ class AetherGauge:
|
||||
offset_stop = _get_perspective_offset(t_stop_gauge, data) if data else 0.0
|
||||
cx_stop = cx + offset_stop
|
||||
|
||||
cy_road = bottom - t_stop_gauge * ROAD_HEIGHT
|
||||
road_h = _get_road_height(data)
|
||||
cy_road = bottom - t_stop_gauge * road_h
|
||||
y_sign = cy_road - 25.0 * SCALE * smoothed_s
|
||||
|
||||
r_min = 6.0 * SCALE
|
||||
|
||||
@@ -59,7 +59,16 @@ class StarPilotOnroadView(AugmentedRoadView):
|
||||
self._personality_button.render()
|
||||
self._render_standstill_timer()
|
||||
self._render_developer_metrics()
|
||||
self._aethergauge.render(self._content_rect, self._font_bold, self._font_medium, current_speed=self._hud_renderer.speed)
|
||||
dm = self.driver_state_renderer
|
||||
if dm and dm.is_visible and dm.position_x != 0.0:
|
||||
dm_top = dm.position_y - 96 # top of DM icon
|
||||
self._aethergauge.render(
|
||||
self._content_rect, self._font_bold, self._font_medium,
|
||||
current_speed=self._hud_renderer.speed,
|
||||
cx=dm.position_x, bottom=dm_top - 105
|
||||
)
|
||||
else:
|
||||
self._aethergauge.render(self._content_rect, self._font_bold, self._font_medium, current_speed=self._hud_renderer.speed)
|
||||
self._render_bottom_row_widgets()
|
||||
self._render_pedals()
|
||||
|
||||
|
||||
Binary file not shown.
@@ -52,7 +52,7 @@ class Joystick:
|
||||
self.flip_map = {'ABS_RZ': accel_axis}
|
||||
else:
|
||||
accel_axis = 'ABS_RX'
|
||||
steer_axis = 'ABS_Z'
|
||||
steer_axis = 'ABS_X'
|
||||
self.flip_map = {'ABS_RY': accel_axis}
|
||||
|
||||
self.min_axis_value = {accel_axis: 0., steer_axis: 0.}
|
||||
|
||||
Reference in New Issue
Block a user