Compare commits

..

25 Commits

Author SHA1 Message Date
FrogAi 235c1cab79 agnos 10.1.1 2025-07-05 12:59:50 -05:00
firestar5683 eb507d5ea8 Update interface.py 2025-07-05 11:35:11 -05:00
firestar5683 159ca85be2 Update tinygrad_modeld.py 2025-05-31 12:59:15 -05:00
FrogAi 40d8eb8a4e agnos 10.1.1 2025-05-26 20:47:53 -05:00
firestar5683 ebf77baf33 Vikander Model 2025-05-24 14:59:24 -05:00
firestar5683 7c2fa00aec Restore Sanity
AHHHHHHH
2025-05-18 13:19:38 -05:00
firestar5683 9bd0463c64 Update interface.py 2025-05-15 17:51:42 -05:00
firestar5683 e247249033 Update interface.py 2025-05-12 15:09:54 -05:00
firestar5683 ea48457d72 Update interface.py 2025-05-12 14:01:05 -05:00
firestar5683 8a4e817f6b Reapply "panda fix"
This reverts commit a8767be7b1.
2025-05-12 13:16:08 -05:00
firestar5683 a8767be7b1 Revert "panda fix"
This reverts commit 2d23f740cc.
2025-05-12 13:15:49 -05:00
firestar5683 0c249d253e Update interface.py 2025-05-12 01:53:59 -05:00
firestar5683 9bb9d6ed2c Panda fix 2025-05-12 00:19:48 -05:00
firestar5683 ceac092b67 Revert "panda fix"
This reverts commit 2d23f740cc.
2025-05-12 00:11:13 -05:00
firestar5683 2d23f740cc panda fix 2025-05-11 23:08:48 -05:00
firestar5683 80c20399f1 Panda Fix? 2025-05-11 18:08:54 -05:00
firestar5683 4f65f90567 Update interface.py 2025-05-11 01:33:49 -05:00
firestar5683 0a217baf18 Revert "Reapply "Tomb Raider 6""
This reverts commit f3de8169b9.
2025-05-11 01:33:48 -05:00
firestar5683 b81d6fb1eb StarPilot 2025-05-11 01:33:48 -05:00
firestar5683 944fc51f8a Reapply "Tomb Raider 6" 2025-05-11 01:33:48 -05:00
firestar5683 086570b29f Update carcontroller.py 2025-05-11 01:33:47 -05:00
firestar5683 8face05df0 Logging 2025-05-11 01:33:46 -05:00
firestar5683 7231aeb5ee TorqueTune/Paddle
panda
2025-05-11 01:33:46 -05:00
FrogAi d01cd48bee Compile FrogPilot 2025-04-20 15:12:33 -07:00
FrogAi 4eefdc3268 Update 2025-04-20 22:00:46 +00:00
97 changed files with 2311 additions and 3352 deletions
Binary file not shown.
+1 -1
View File
@@ -1 +1 @@
/data/themes/theme_packs/duck/colors
/data/openpilot/openpilot/frogpilot/assets/holiday_themes/easter_week/colors
+1 -1
View File
@@ -1 +1 @@
/data/themes/theme_packs/frog-animated/distance_icons
/data/openpilot/openpilot/frogpilot/assets/stock_theme/distance_icons
+1 -1
View File
@@ -1 +1 @@
/data/themes/theme_packs/duck/icons
/data/openpilot/openpilot/frogpilot/assets/holiday_themes/easter_week/icons
+1 -1
View File
@@ -1 +1 @@
/data/themes/theme_packs/duck/signals
/data/openpilot/openpilot/frogpilot/assets/holiday_themes/easter_week/signals
+1 -1
View File
@@ -1 +1 @@
/data/themes/theme_packs/duck/sounds
/data/openpilot/openpilot/frogpilot/assets/holiday_themes/easter_week/sounds
@@ -1 +0,0 @@
/data/themes/steering_wheels/pedro.gif
@@ -0,0 +1 @@
/data/openpilot/openpilot/frogpilot/assets/holiday_themes/easter_week/steering_wheel/wheel.png
Binary file not shown.

Before

Width:  |  Height:  |  Size: 55 KiB

After

Width:  |  Height:  |  Size: 61 KiB

@@ -1,44 +1,44 @@
{
"LaneLines": {
"red": 76,
"green": 150,
"blue": 255,
"alpha": 255
"red": 23,
"green": 134,
"blue": 68,
"alpha": 242
},
"LeadMarker": {
"red": 76,
"green": 150,
"blue": 255,
"alpha": 255
"red": 23,
"green": 134,
"blue": 68,
"alpha": 242
},
"Path": {
"red": 76,
"green": 150,
"blue": 255,
"alpha": 255
"red": 23,
"green": 134,
"blue": 68,
"alpha": 242
},
"PathEdge": {
"red": 255,
"green": 32,
"blue": 32,
"alpha": 255
"red": 18,
"green": 107,
"blue": 54,
"alpha": 242
},
"Sidebar1": {
"red": 76,
"green": 150,
"blue": 255,
"alpha": 255
"red": 23,
"green": 134,
"blue": 68,
"alpha": 242
},
"Sidebar2": {
"red": 0,
"green": 255,
"blue": 102,
"alpha": 255
"red": 23,
"green": 134,
"blue": 68,
"alpha": 242
},
"Sidebar3": {
"red": 255,
"green": 32,
"blue": 32,
"alpha": 255
"red": 23,
"green": 134,
"blue": 68,
"alpha": 242
}
}
Binary file not shown.

After

Width:  |  Height:  |  Size: 88 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 8.5 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 54 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 13 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 64 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 11 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 29 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 13 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 26 KiB

After

Width:  |  Height:  |  Size: 32 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 40 KiB

After

Width:  |  Height:  |  Size: 37 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 31 KiB

After

Width:  |  Height:  |  Size: 2.7 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 41 KiB

After

Width:  |  Height:  |  Size: 44 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 51 KiB

After

Width:  |  Height:  |  Size: 53 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 54 KiB

After

Width:  |  Height:  |  Size: 58 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 56 KiB

After

Width:  |  Height:  |  Size: 60 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 41 KiB

After

Width:  |  Height:  |  Size: 44 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 54 KiB

After

Width:  |  Height:  |  Size: 53 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 35 KiB

After

Width:  |  Height:  |  Size: 37 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 42 KiB

After

Width:  |  Height:  |  Size: 63 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 371 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 568 KiB

After

Width:  |  Height:  |  Size: 503 KiB

-1
View File
@@ -139,7 +139,6 @@ def frogpilot_boot_functions(build_metadata, params_cache):
params.put("DongleId", params.get("KonikDongleId", encoding="utf8"))
else:
params.put("KonikDongleId", register(show_spinner=True, register_konik=True))
HARDWARE.reboot()
elif params.get("StockDongleId", encoding="utf8") != None:
params.put("DongleId", params.get("StockDongleId", encoding="utf8"))
+1 -2
View File
@@ -4,7 +4,6 @@ import math
import numpy as np
import requests
import shutil
import signal
import subprocess
import threading
import time
@@ -174,7 +173,7 @@ def restart_processes(sm):
if not any(ps.ignitionLine or ps.ignitionCan for ps in sm["pandaStates"] if ps.pandaType != log.PandaState.PandaType.unknown):
for name in ["mapd", "ui"]:
get_managed_processes()[name].stop(block=False, retry=False, sig=signal.SIGKILL)
get_managed_processes()[name].stop(block=False, retry=False)
def run_cmd(cmd, success_message, fail_message, report=True):
try:
+7 -6
View File
@@ -64,7 +64,7 @@ DEFAULT_TINYGRAD_MODEL_VERSION = "v8"
EXCLUDED_KEYS = {
"AvailableModels", "AvailableModelNames", "CarParamsPersistent", "ExperimentalLongitudinalEnabled",
"ExperimentalModels", "KonikMinutes", "ModelDrivesAndScores", "ModelVersions", "openpilotMinutes",
"ExperimentalModels", "ModelDrivesAndScores", "ModelVersions", "openpilotMinutes",
"SpeedLimits", "UpdaterAvailableBranches"
}
@@ -187,7 +187,6 @@ frogpilot_default_params: list[tuple[str, str | bytes, int]] = [
("IsMetric", "0", 0),
("JerkInfo", "0", 3),
("KonikDongleId", "", 3),
("KonikMinutes", "0", 0),
("LaneChangeCustomizations", "0", 0),
("LaneChangeTime", "2.0", 0),
("LaneDetectionWidth", "0", 2),
@@ -391,10 +390,12 @@ class FrogPilotVariables:
self.vetting_branch = self.short_branch == "FrogPilot-Vetting"
self.frogpilot_toggles.frogs_go_moo = Path("/persist/frogsgomoo.py").is_file()
self.frogpilot_toggles.block_user = False
self.frogpilot_toggles.block_user = self.development_branch and not self.frogpilot_toggles.frogs_go_moo
self.not_vetted = False
self.frogpilot_toggles.use_konik_server = params.get_bool("UseKonikServer")
self.frogpilot_toggles.use_konik_server |= Path("/data/openpilot/not_vetted").is_file()
self.frogpilot_toggles.use_konik_server |= self.not_vetted
if not KONIK_PATH.is_file() and self.frogpilot_toggles.use_konik_server:
KONIK_PATH.touch()
@@ -632,8 +633,8 @@ class FrogPilotVariables:
toggle.device_shutdown_time = (device_shutdown_setting - 3) * 3600 if device_shutdown_setting >= 4 else device_shutdown_setting * (60 * 15)
toggle.increase_thermal_limits = device_management and (params.get_bool("IncreaseThermalLimits") if tuning_level >= level["IncreaseThermalLimits"] else default.get_bool("IncreaseThermalLimits"))
toggle.low_voltage_shutdown = np.clip(params.get_float("LowVoltageShutdown"), VBATT_PAUSE_CHARGING, 12.5) if device_management and tuning_level >= level["LowVoltageShutdown"] else default.get_float("LowVoltageShutdown")
toggle.no_logging = (device_management and (params.get_bool("NoLogging") if tuning_level >= level["NoLogging"] else default.get_bool("NoLogging")) or self.development_branch) and not self.vetting_branch
toggle.no_uploads = (device_management and (params.get_bool("NoUploads") if tuning_level >= level["NoUploads"] else default.get_bool("NoUploads")) or self.development_branch) and not self.vetting_branch
toggle.no_logging = (device_management and (params.get_bool("NoLogging") if tuning_level >= level["NoLogging"] else default.get_bool("NoLogging")) or self.development_branch or self.not_vetted) and not self.vetting_branch
toggle.no_uploads = (device_management and (params.get_bool("NoUploads") if tuning_level >= level["NoUploads"] else default.get_bool("NoUploads")) or self.development_branch or self.not_vetted) and not self.vetting_branch
toggle.no_onroad_uploads = toggle.no_uploads and (params.get_bool("DisableOnroadUploads") if tuning_level >= level["DisableOnroadUploads"] else default.get_bool("DisableOnroadUploads"))
toggle.offline_mode = device_management and (params.get_bool("OfflineMode") if tuning_level >= level["OfflineMode"] else default.get_bool("OfflineMode"))
@@ -77,8 +77,8 @@ class FrogPilotFollowing:
distance_factor = max(lead_distance - (v_ego * self.t_follow), 1)
standstill_offset = max(STOP_DISTANCE - v_ego, 1)
acceleration_offset = float(np.clip((v_lead - v_ego) * standstill_offset - COMFORT_BRAKE, 1, distance_factor))
self.acceleration_jerk /= standstill_offset
self.speed_jerk /= standstill_offset
self.acceleration_jerk /= acceleration_offset
self.speed_jerk /= acceleration_offset
self.t_follow /= acceleration_offset
# Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
Binary file not shown.

Before

Width:  |  Height:  |  Size: 106 KiB

After

Width:  |  Height:  |  Size: 402 KiB

@@ -40,7 +40,7 @@
text-align: center;
}
</style>
<title>FrogPilot: {% block title %}{% endblock %}</title>
<title>StarPilot: {% block title %}{% endblock %}</title>
</head>
<body>
<nav class="navbar navbar-fixed-top navbar-expand-sm navbar-dark bg-dark">
+1 -22
View File
@@ -1,5 +1,6 @@
#!/usr/bin/bash
if [ -z "$BASEDIR" ]; then
BASEDIR="/data/openpilot"
fi
@@ -7,7 +8,6 @@ fi
source "$BASEDIR/launch_env.sh"
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
ONCE_FLAG_FILE="/data/openpilot/.setup_done"
function agnos_init {
# TODO: move this to agnos
@@ -32,24 +32,6 @@ function agnos_init {
fi
}
function one_time_setup {
if [ ! -f "$ONCE_FLAG_FILE" ]; then
echo "Performing one-time setup tasks..."
# Run once:
echo "Wiping old params..."
rm /data/params/d/DongleId
rm /data/params/d/StockDongleId
rm /cache/params/d/DongleId
rm /cache/params/d/StockDongleId
echo "Old params wiped."
touch "$ONCE_FLAG_FILE"
else
echo "One-time setup already completed. Skipping."
fi
}
function launch {
# Remove orphaned git lock if it exists on boot
[ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock
@@ -97,9 +79,6 @@ function launch {
agnos_init
fi
# Perform one-time setup tasks
one_time_setup
# write tmux scrollback to a file
tmux capture-pane -pq -S-1000 > /tmp/launch_log
View File
Binary file not shown.
+15 -4
View File
@@ -82,6 +82,12 @@ VAL_TABLE_ HandsOffSWDetectionMode 2 "Failed" 1 "Enabled" 0 "Disabled" ;
BO_ 189 EBCMRegenPaddle: 7 K17_EBCM
SG_ RegenPaddle : 7|4@0+ (1,0) [0|0] "" NEO
SG_ Byte1 : 8|8@1+ (1,0) [0|255] "" NEO
SG_ Byte2 : 16|8@1+ (1,0) [0|255] "" NEO
SG_ Byte3 : 24|8@1+ (1,0) [0|255] "" NEO
SG_ Byte4 : 32|8@1+ (1,0) [0|255] "" NEO
SG_ Byte5 : 40|8@1+ (1,0) [0|255] "" NEO
SG_ Byte6 : 48|8@1+ (1,0) [0|255] "" NEO
BO_ 190 ECMAcceleratorPos: 6 K20_ECM
SG_ BrakePedalPos : 15|8@0+ (1,0) [0|0] "sticky" NEO
@@ -165,7 +171,6 @@ BO_ 481 ASCMSteeringButton: 7 K124_ASCM
SG_ DistanceButton : 22|1@0+ (1,0) [0|0] "" NEO
SG_ LKAButton : 23|1@0+ (1,0) [0|0] "" NEO
SG_ ACCAlwaysOne : 24|1@0+ (1,0) [0|1] "" XXX
SG_ ACCByFive : 26|2@0+ (1,0) [0|3] "" XXX
SG_ ACCButtons : 46|3@0+ (1,0) [0|0] "" NEO
SG_ DriveModeButton : 39|1@0+ (1,0) [0|1] "" XXX
SG_ RollingCounter : 33|2@0+ (1,0) [0|3] "" NEO
@@ -193,10 +198,15 @@ BO_ 500 SportMode: 6 XXX
SG_ SportMode : 15|1@0+ (1,0) [0|1] "" XXX
BO_ 501 ECMPRDNL2: 8 K20_ECM
SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO
SG_ Byte0 : 0|8@1+ (1,0) [0|255] "" NEO
SG_ Byte1 : 8|8@1+ (1,0) [0|255] "" NEO
SG_ Byte2 : 16|8@1+ (1,0) [0|255] "" NEO
SG_ PRNDL2 : 27|4@0+ (1,0) [0|255] "" NEO
SG_ Byte4 : 32|8@1+ (1,0) [0|255] "" NEO
SG_ ManualMode : 41|1@0+ (1,0) [0|1] "" NEO
SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO
SG_ Byte7 : 56|8@1+ (1,0) [0|255] "" NEO
BO_ 532 BRAKE_RELATED: 6 XXX
SG_ UserBrakePressure : 0|9@0+ (1,0) [0|511] "" XXX
@@ -222,6 +232,7 @@ BO_ 715 ASCMGasRegenCmd: 8 K124_ASCM
SG_ GasRegenFullStopActive : 13|1@0+ (1,0) [0|0] "" NEO
SG_ GasRegenCmdActive : 0|1@0+ (1,0) [0|0] "" NEO
SG_ RollingCounter : 7|2@0+ (1,0) [0|0] "" NEO
SG_ GasRegenAlwaysOne3 : 23|1@0+ (1,0) [0|1] "" NEO
SG_ GasRegenCmd : 8|14@0+ (1,0) [0|0] "" NEO
BO_ 717 ASCM_2CD: 5 K124_ASCM
@@ -370,6 +381,6 @@ VAL_ 715 GasRegenCmdActive 1 "Active" 0 "Inactive" ;
VAL_ 320 Intellibeam 1 "Active" 0 "Inactive" ;
VAL_ 320 HighBeamsActive 1 "Active" 0 "Inactive" ;
VAL_ 320 HighBeamsTemporary 1 "Active" 0 "Inactive" ;
VAL_ 501 PRNDL2 6 "L" 4 "D" 3 "N" 2 "R" 1 "P" 0 "Shifting";
VAL_ 501 PRNDL2 7 "L2" 6 "L" 5 "L3" 4 "D" 3 "N" 2 "R" 1 "P" 0 "Shifting";
VAL_ 501 TransmissionState 11 "Shifting" 10 "Reverse" 9 "Forward" 8 "Disengaged";
VAL_ 501 ManualMode 1 "Active" 0 "Inactive"
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+7 -7
View File
@@ -231,13 +231,13 @@ class Panda:
FLAG_GM_HW_CAM = 1
FLAG_GM_HW_CAM_LONG = 2
FLAG_GM_CC_LONG = 4
FLAG_GM_HW_ASCM_LONG = 8
FLAG_GM_NO_CAMERA = 16
FLAG_GM_NO_ACC = 32
FLAG_GM_PEDAL_LONG = 64 # TODO: This can be inferred
FLAG_GM_GAS_INTERCEPTOR = 128
FLAG_GM_ASCM_INT = 256
FLAG_GM_HW_SDGM = 4
FLAG_GM_CC_LONG = 8
FLAG_GM_HW_ASCM_LONG = 16
FLAG_GM_NO_CAMERA = 32
FLAG_GM_NO_ACC = 64
FLAG_GM_PEDAL_LONG = 128 # TODO: This can be inferred
FLAG_GM_GAS_INTERCEPTOR = 256
FLAG_FORD_LONG_CONTROL = 1
FLAG_FORD_CANFD = 2
Binary file not shown.

Before

Width:  |  Height:  |  Size: 108 KiB

After

Width:  |  Height:  |  Size: 418 KiB

+12 -12
View File
@@ -57,17 +57,16 @@ class CarController(CarControllerBase):
self.accel_g = 0.0
@staticmethod
def calc_pedal_command(accel: float, long_active: bool) -> float:
def calc_pedal_command(accel: float, long_active: bool, car_velocity) -> float:
if not long_active: return 0.
zero = 0.15625 # 40/256
if accel > 0.:
# Scales the accel from 0-1 to 0.156-1
pedal_gas = clip(((1 - zero) * accel + zero), 0., 1.)
if accel < -0.5:
pedal_gas = 0
else:
# if accel is negative, -0.1 -> 0.015625
pedal_gas = clip(zero + accel, 0., zero) # Make brake the same size as gas, but clip to regen
pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240])
pedal_gas = clip((pedaloffset + accel * 0.6), 0.0, 1.0)
return pedal_gas
@@ -160,7 +159,7 @@ class CarController(CarControllerBase):
self.apply_gas = self.params.INACTIVE_REGEN
if self.CP.carFingerprint in CC_ONLY_CAR:
# gas interceptor only used for full long control on cars without ACC
interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive)
interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo)
if self.CP.enableGasInterceptor and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill:
# "Tap" the accelerator pedal to re-engage ACC
@@ -183,8 +182,6 @@ class CarController(CarControllerBase):
if self.CP.networkLocation == NetworkLocation.fwdCamera and self.CP.carFingerprint not in CC_ONLY_CAR:
at_full_stop = at_full_stop and stopping
friction_brake_bus = CanBus.POWERTRAIN
if self.CP.carFingerprint in SDGM_CAR:
friction_brake_bus = CanBus.CAMERA
if self.CP.autoResumeSng:
resume = actuators.longControlState != LongCtrlState.starting or CC.cruiseControl.resume
@@ -210,7 +207,7 @@ class CarController(CarControllerBase):
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
if not self.CP.radarUnavailable and self.CP.networkLocation != NetworkLocation.fwdCamera:
if not self.CP.radarUnavailable:
tt = self.frame * DT_CTRL
time_and_headlights_step = 10
if self.frame % time_and_headlights_step == 0:
@@ -245,7 +242,10 @@ class CarController(CarControllerBase):
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
self.last_button_frame = self.frame
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
if self.CP.carFingerprint in SDGM_CAR:
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, CS.buttons_counter, CruiseButtons.CANCEL))
else:
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
if self.CP.networkLocation == NetworkLocation.fwdCamera:
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
+92 -45
View File
@@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import mean
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR, CAMERA_ACC_CAR, SDGM_CAR, ASCM_INT
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR, CAMERA_ACC_CAR, SDGM_CAR
TransmissionType = car.CarParams.TransmissionType
NetworkLocation = car.CarParams.NetworkLocation
@@ -39,9 +39,14 @@ class CarState(CarStateBase):
self.prev_cruise_buttons = self.cruise_buttons
self.prev_distance_button = self.distance_button
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
if self.CP.carFingerprint not in SDGM_CAR:
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
else:
self.cruise_buttons = cam_cp.vl["ASCMSteeringButton"]["ACCButtons"]
self.distance_button = cam_cp.vl["ASCMSteeringButton"]["DistanceButton"]
self.buttons_counter = cam_cp.vl["ASCMSteeringButton"]["RollingCounter"]
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
# This is to avoid a fault where you engage while still moving backwards after shifting to D.
# An Equinox has been seen with an unsupported status (3), so only check if either wheel is in reverse (2)
@@ -71,26 +76,32 @@ class CarState(CarStateBase):
else:
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None))
if self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.FORCE_BRAKE_C9.value: ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0
if self.CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value:
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
else:
ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"]
if self.CP.networkLocation == NetworkLocation.fwdCamera:
ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0
else:
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
# that the brake is being intermittently pressed without user interaction.
# To avoid a cruise fault we need to use a conservative brake position threshold
# https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
if self.CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value:
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
else:
ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"]
ret.brakePressed = ret.brake >= 8
# Regen braking is braking
if self.CP.transmissionType == TransmissionType.direct:
ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
self.single_pedal_mode = ret.gearShifter == GearShifter.low or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1 or (ret.regenBraking and GearShifter.manumatic)
self.single_pedal_mode = (
ret.gearShifter == GearShifter.low
or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1
or pt_cp.vl["ECMPRDNL2"]["TransmissionState"] == 21
or (ret.regenBraking and GearShifter.manumatic)
)
if self.CP.enableGasInterceptor:
ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
threshold = 10 if self.CP.carFingerprint in (CAMERA_ACC_CAR | SDGM_CAR) else 4 # Panda 515 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting.
threshold = 12 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 515 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting.
ret.gasPressed = ret.gas > threshold
else:
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
@@ -107,19 +118,32 @@ class CarState(CarStateBase):
ret.steerFaultTemporary = self.lkas_status == 2
ret.steerFaultPermanent = self.lkas_status == 3
if self.CP.carFingerprint not in SDGM_CAR:
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
else:
# 1 - open, 0 - closed
ret.doorOpen = (cam_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
cam_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
cam_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
cam_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
# 1 - latched
ret.seatbeltUnlatched = cam_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
ret.leftBlinker = cam_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
ret.rightBlinker = cam_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
ret.parkingBrake = cam_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
@@ -130,11 +154,12 @@ class CarState(CarStateBase):
if self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.NO_CAMERA.value:
if self.CP.carFingerprint not in CC_ONLY_CAR:
ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
if self.CP.carFingerprint not in (SDGM_CAR|ASCM_INT):
if self.CP.carFingerprint not in SDGM_CAR:
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
else:
ret.stockAeb = False
# openpilot controls nonAdaptive when not pcmCruise
# 2016-2018 Volt won't identify non-adaptive cruise state since switchable cruise state was not introduced till 2019 model year / SDGM Global AAdd commentMore actions
if self.CP.pcmCruise and self.CP.carFingerprint not in ASCM_INT:
if self.CP.pcmCruise:
ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3)
if self.CP.carFingerprint in CC_ONLY_CAR:
ret.accFaulted = False
@@ -142,11 +167,19 @@ class CarState(CarStateBase):
ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
if self.CP.enableBsm:
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
if self.CP.carFingerprint not in SDGM_CAR:
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
else:
ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
# FrogPilot CarState functions
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
if self.CP.carFingerprint in SDGM_CAR:
self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"]
else:
self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
@@ -161,7 +194,16 @@ class CarState(CarStateBase):
messages += [
("ASCMLKASteeringCmd", 10),
]
if CP.carFingerprint not in (SDGM_CAR|ASCM_INT):
if CP.carFingerprint in SDGM_CAR:
messages += [
("BCMTurnSignals", 1),
("BCMDoorBeltStatus", 10),
("BCMGeneralPlatformStatus", 10),
("ASCMSteeringButton", 33),
]
if CP.enableBsm:
messages.append(("BCMBlindSpotMonitor", 10))
else:
messages += [
("AEBCmd", 10),
]
@@ -175,30 +217,43 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parser(CP, FPCP):
messages = [
("BCMTurnSignals", 1),
("ECMPRDNL2", 10),
("PSCMStatus", 10),
("ESPStatus", 10),
("BCMDoorBeltStatus", 10),
("BCMGeneralPlatformStatus", 10),
("EBCMWheelSpdFront", 20),
("EBCMWheelSpdRear", 20),
("EBCMFrictionBrakeStatus", 20),
("AcceleratorPedal2", 33),
("ASCMSteeringButton", 33),
("ECMEngineStatus", 100),
("PSCMSteeringAngle", 100),
("ECMAcceleratorPos", 80),
("SportMode", 0),
]
if CP.enableBsm:
messages.append(("BCMBlindSpotMonitor", 10))
if CP.carFingerprint in SDGM_CAR:
messages += [
("ECMPRDNL2", 40),
("AcceleratorPedal2", 40),
("ECMEngineStatus", 80),
]
else:
messages += [
("ECMPRDNL2", 10),
("AcceleratorPedal2", 33),
("ECMEngineStatus", 100),
("BCMTurnSignals", 1),
("BCMDoorBeltStatus", 10),
("BCMGeneralPlatformStatus", 10),
("ASCMSteeringButton", 33),
]
if CP.enableBsm:
messages.append(("BCMBlindSpotMonitor", 10))
# Used to read back last counter sent to PT by camera
if CP.networkLocation == NetworkLocation.fwdCamera:
messages += [
("ASCMLKASteeringCmd", 0),
]
if CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value:
messages.remove(("ECMAcceleratorPos", 80))
messages.append(("EBCMBrakePedalPosition", 100))
if CP.transmissionType == TransmissionType.direct:
messages += [
@@ -216,14 +271,6 @@ class CarState(CarStateBase):
("GAS_SENSOR", 50),
]
if CP.networkLocation != NetworkLocation.fwdCamera:
messages += [
("ECMAcceleratorPos", 80),
]
if CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value:
messages.remove(("ECMAcceleratorPos", 80))
messages.append(("EBCMBrakePedalPosition", 100))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.POWERTRAIN)
@staticmethod
+4 -14
View File
@@ -26,12 +26,6 @@ FINGERPRINTS = {
{
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2017: 8, 2020: 8, 2025: 8, 2028: 8
}],
CAR.CHEVROLET_VOLT_ASCM: [
# Causes errors with normal OBD install
# {
# 189: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 497: 8, 500: 6, 501: 8, 528: 4, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 767: 4, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1922: 7, 1930: 7Add commentMore actions
# }
],
CAR.BUICK_LACROSSE: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 5, 707: 8, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 872: 1, 882: 8, 890: 1, 892: 2, 893: 1, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1916: 7, 1918: 7, 1919: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
}],
@@ -184,7 +178,8 @@ FINGERPRINTS = {
CAR.CADILLAC_XT4: [
# Cadillac XT4 w/ ACC 2023
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 767: 4, 806: 1, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1517: 8, 1601: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 8, 1924: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2024: 8, 2026: 8 }],
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 806: 1, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1517: 8, 1601: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 8, 1924: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2024: 8, 2026: 8
}],
CAR.CADILLAC_XT5_CC: [
# TRain's 2017 XT5
{
@@ -193,12 +188,12 @@ FINGERPRINTS = {
CAR.CHEVROLET_TRAVERSE: [
# Chevy Traverse w/ ACC 2023
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 513: 6, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 603: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 723: 4, 730: 4, 753: 5, 761: 7, 767: 4, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1346: 8, 1347: 8, 1355: 8, 1362: 8, 1417: 8, 1512: 8, 1514: 8, 1601: 8, 1602: 8, 1603: 7, 1609: 8, 1611: 8, 1613: 8, 1618: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 7, 1927: 8, 1930: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2004: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2024: 8, 2026: 8
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 603: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 723: 4, 730: 4, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1346: 8, 1347: 8, 1355: 8, 1362: 8, 1417: 8, 1512: 8, 1514: 8, 1601: 8, 1602: 8, 1603: 7, 1609: 8, 1611: 8, 1613: 8, 1618: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 7, 1927: 8, 1930: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2004: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2024: 8, 2026: 8
}],
CAR.BUICK_BABYENCLAVE: [
# Buick Baby Enclave w/ ACC 2020-23
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 394: 7, 398: 8, 401: 8, 405: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 450: 4, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 456: 8, 457: 6, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 723: 4, 730: 4, 761: 7, 767: 4, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 872: 1, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1201: 3, 1217: 8, 1218: 3, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1514: 8, 1517: 8, 1601: 8, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1927: 7, 1930: 7, 2018: 8, 2020: 8, 2021: 8, 2028: 8
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 394: 7, 398: 8, 401: 8, 405: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 450: 4, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 456: 8, 457: 6, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 723: 4, 730: 4, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 872: 1, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1201: 3, 1217: 8, 1218: 3, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1514: 8, 1517: 8, 1601: 8, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1927: 7, 1930: 7, 2018: 8, 2020: 8, 2021: 8, 2028: 8
}],
CAR.CHEVROLET_MALIBU_CC: [
{
@@ -208,11 +203,6 @@ FINGERPRINTS = {
{
190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1930: 7
}],
CAR.CHEVROLET_VOLT_2019: [
# Chevy Volt w/ ACC 2019
{
170: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 573: 1, 577: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 767: 4, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1268: 2, 1273: 3, 1275: 3, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1513: 8, 1516: 8, 1517: 8, 1601: 8, 1609: 8, 1611: 8, 1618: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1920: 8, 1922: 7, 1927: 7, 1930: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2004: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2023: 8, 2025: 8, 2028: 8, 2031: 8
}],
}
FW_VERSIONS: dict[str, dict[tuple, list[bytes]]] = {
+48 -58
View File
@@ -8,12 +8,12 @@ from openpilot.common.basedir import BASEDIR
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus, GMFlags, CC_ONLY_CAR, SDGM_CAR, ASCM_INT
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus, GMFlags, CC_ONLY_CAR, SDGM_CAR
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
from openpilot.common.params import Params
params = Params()
from openpilot.frogpilot.common.frogpilot_variables import params
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
@@ -29,8 +29,8 @@ CAM_MSG = 0x320 # AEBCmd
ACCELERATOR_POS_MSG = 0xbe
NON_LINEAR_TORQUE_PARAMS = {
CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
CAR.CHEVROLET_BOLT_CC: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
CAR.CHEVROLET_BOLT_EUV: [1.8, 1.1, 0.290, -0.045],
CAR.CHEVROLET_BOLT_CC: [1.8, 1.1, 0.290, -0.045],
CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
}
@@ -74,8 +74,8 @@ class CarInterface(CarInterfaceBase):
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, _ = non_linear_torque_params
steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c)
a, b, c, d = non_linear_torque_params
steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c) + d
return float(steer_torque) + friction
def torque_from_lateral_accel_neural(self, latcontrol_inputs: LatControlInputs, torque_params: car.CarParams.LateralTorqueTuning, lateral_accel_error: float,
@@ -87,13 +87,7 @@ class CarInterface(CarInterfaceBase):
return float(self.neural_ff_model.predict(inputs)) + friction
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
if self.CP.carFingerprint in (CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_BOLT_CC):
self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, self.CP.carFingerprint)
return self.torque_from_lateral_accel_neural
elif self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
return self.torque_from_lateral_accel_siglin
else:
return self.torque_from_lateral_accel_linear
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, frogpilot_toggles):
@@ -112,35 +106,41 @@ class CarInterface(CarInterfaceBase):
else:
ret.transmissionType = TransmissionType.automatic
ret.longitudinalTuning.kiBP = [5., 35.]
ret.longitudinalTuning.kiBP = [5., 35., 60.]
if candidate in (CAMERA_ACC_CAR | SDGM_CAR | ASCM_INT):
ret.experimentalLongitudinalAvailable = candidate not in (CC_ONLY_CAR | SDGM_CAR | ASCM_INT) or 0x2FF in fingerprint[CanBus.POWERTRAIN]
if candidate in CAMERA_ACC_CAR:
ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR
ret.networkLocation = NetworkLocation.fwdCamera
ret.radarUnavailable = 0x460 not in fingerprint[CanBus.OBSTACLE]
ret.radarUnavailable = True # no radar
ret.pcmCruise = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
ret.minEnableSpeed = 5 * CV.KPH_TO_MS
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
if candidate in ASCM_INT:
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_ASCM_INT
# Tuning for experimental long
ret.longitudinalTuning.kiV = [1.0, 1.0]
ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5]
ret.vEgoStopping = 0.1
ret.vEgoStarting = 0.1
ret.stoppingDecelRate = 1.0 # reach brake quickly after enabling
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stopAccel = -0.20
ret.stopAccel = -0.25
if experimental_long:
ret.pcmCruise = False
ret.openpilotLongitudinalControl = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
if candidate in SDGM_CAR:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_FORCE_BRAKE_C9
ret.flags |= GMFlags.FORCE_BRAKE_C9.value
elif candidate in SDGM_CAR:
ret.longitudinalTuning.kiV = [0., 0., 0.] # TODO: tuning
ret.experimentalLongitudinalAvailable = False
ret.networkLocation = NetworkLocation.fwdCamera
ret.pcmCruise = True
ret.radarUnavailable = True
ret.minEnableSpeed = -1. # engage speed is decided by ASCM
ret.minSteerSpeed = 30 * CV.MPH_TO_MS
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM
else: # ASCM, OBD-II harness
ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
@@ -152,11 +152,7 @@ class CarInterface(CarInterfaceBase):
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
# Tuning
ret.longitudinalTuning.kiV = [0.5, 0.5]
ret.stoppingDecelRate = 3
ret.vEgoStopping = 0.75
ret.vEgoStarting = 0.75
ret.stopAccel = -1.5
ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5]
if ret.enableGasInterceptor:
# Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits
@@ -207,6 +203,7 @@ class CarInterface(CarInterfaceBase):
elif candidate in (CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_BOLT_CC):
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
ret.lateralTuning.torque.kp = 0.6
if ret.enableGasInterceptor:
# ACC Bolts use pedal for full longitudinal control, not just sng
@@ -237,8 +234,6 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.CADILLAC_XT4:
ret.steerActuatorDelay = 0.2
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.minSteerSpeed = 30 * CV.MPH_TO_MS
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CADILLAC_XT5_CC:
@@ -247,12 +242,12 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.CHEVROLET_TRAVERSE:
ret.steerActuatorDelay = 0.2
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_BABYENCLAVE:
ret.steerActuatorDelay = 0.2
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CADILLAC_CT6_CC:
@@ -265,29 +260,28 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.CHEVROLET_TRAX:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_VOLT_2019:
ret.steerActuatorDelay = 0.2
if not ret.openpilotLongitudinalControl:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if ret.enableGasInterceptor:
ret.networkLocation = NetworkLocation.fwdCamera
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
ret.minEnableSpeed = -1
ret.pcmCruise = False
ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
ret.stoppingControl = True
ret.autoResumeSng = True
if candidate in CC_ONLY_CAR:
if candidate in CC_ONLY_CAR: #pedal interceptor tuning
ret.flags |= GMFlags.PEDAL_LONG.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG
# Note: Low speed, stop and go not tested. Should be fairly smooth on highway
ret.longitudinalTuning.kiBP = [0.0, 5., 35.]
ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5]
ret.longitudinalTuning.kf = 0.15
ret.longitudinalTuning.kiBP = [0., 3., 6., 35.]
ret.longitudinalTuning.kiV = [0.125, 0.175, 0.225, 0.33]
ret.longitudinalTuning.kf = 0.25
ret.stoppingDecelRate = 0.8
ret.minEnableSpeed = -1
ret.pcmCruise = False
ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
else: # Pedal used for SNG, ACC for longitudinal control otherwise
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
ret.startingState = True
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
elif candidate in CC_ONLY_CAR:
ret.flags |= GMFlags.CC_LONG.value
@@ -297,27 +291,23 @@ class CarInterface(CarInterfaceBase):
ret.minEnableSpeed = 24 * CV.MPH_TO_MS
ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
ret.pcmCruise = False
experimental_long = False
ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate)
if candidate != CAR.CHEVROLET_BOLT_CC:
ret.longitudinalTuning.kiBP = [10.7, 10.8, 28.]
ret.longitudinalTuning.kiV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
else:
if not ret.enableGasInterceptor and candidate in CC_ONLY_CAR: #redneck tuning
ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph
ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s
ret.longitudinalActuatorDelay = 1. # TODO: measure this
ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph
ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.1]
ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate)
if candidate in CC_ONLY_CAR:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC
# Exception for flashed cars, or cars whose camera was removed
if (ret.networkLocation == NetworkLocation.fwdCamera or candidate in CC_ONLY_CAR) and CAM_MSG not in fingerprint[CanBus.CAMERA] and not candidate in (SDGM_CAR | ASCM_INT):
if (ret.networkLocation == NetworkLocation.fwdCamera or candidate in CC_ONLY_CAR) and CAM_MSG not in fingerprint[CanBus.CAMERA] and not candidate in SDGM_CAR:
ret.flags |= GMFlags.NO_CAMERA.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_CAMERA
@@ -353,7 +343,7 @@ class CarInterface(CarInterfaceBase):
# TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
below_min_enable_speed = ret.vEgo < self.CP.minEnableSpeed or self.CS.moving_backward
if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and
self.CP.networkLocation == NetworkLocation.fwdCamera):
(self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.carFingerprint in SDGM_CAR)):
events.add(EventName.belowEngageSpeed)
if ret.cruiseState.standstill and not (self.CP.autoResumeSng or self.disable_resumeRequired):
events.add(EventName.resumeRequired)
+14 -19
View File
@@ -41,7 +41,7 @@ class CarControllerParams:
self.ZERO_GAS = 6144 # Coasting
self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
if CP.carFingerprint in (CAMERA_ACC_CAR | SDGM_CAR) and CP.carFingerprint not in CC_ONLY_CAR and CP.carFingerprint != CAR.CHEVROLET_BOLT_EUV:
if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR:
self.MAX_GAS = 7496
self.MAX_GAS_PLUS = 8848
self.MAX_ACC_REGEN = 5610
@@ -50,6 +50,13 @@ class CarControllerParams:
# Camera transitions to MAX_ACC_REGEN from ZERO_GAS and uses friction brakes instantly
self.max_regen_acceleration = 0.
elif CP.carFingerprint in SDGM_CAR:
self.MAX_GAS = 7496
self.MAX_GAS_PLUS = 7496
self.MAX_ACC_REGEN = 5610
self.INACTIVE_REGEN = 5650
self.max_regen_acceleration = 0.
else:
self.MAX_GAS = 7168 # Safety limit, not ACC max. Stock ACC >8192 from standstill.
self.MAX_GAS_PLUS = 8191 # 8292 uses new bit, possible but not tested. Matches Twilsonco tw-main max
@@ -118,11 +125,6 @@ class CAR(Platforms):
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=0.469, minEnableSpeed=-1),
dbc_dict=dbc_dict('gm_global_a_powertrain_volt', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')
)
CHEVROLET_VOLT_ASCM = GMPlatformConfig(
[GMCarDocs("Chevrolet Volt 2017-18 w/ ASCM Harness", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ")],
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=0.469, minEnableSpeed=-1),
dbc_dict=dbc_dict('gm_global_a_powertrain_volt', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')
)
CADILLAC_ATS = GMASCMPlatformConfig(
[GMCarDocs("Cadillac ATS Premium Performance 2018")],
GMCarSpecs(mass=1601, wheelbase=2.78, steerRatio=15.3),
@@ -203,15 +205,15 @@ class CAR(Platforms):
CHEVROLET_SUBURBAN.specs,
)
GMC_YUKON_CC = GMPlatformConfig(
[GMCarDocs("GMC Yukon - No-ACC")],
[GMCarDocs("GMC Yukon No ACC")],
CarSpecs(mass=2541, wheelbase=2.95, steerRatio=16.3, centerToFrontRatio=0.4),
)
CADILLAC_CT6_CC = GMPlatformConfig(
[GMCarDocs("Cadillac CT6 - No-ACC")],
[GMCarDocs("Cadillac CT6 No ACC")],
CarSpecs(mass=2358, wheelbase=3.11, steerRatio=17.7, centerToFrontRatio=0.4),
)
CHEVROLET_TRAILBLAZER_CC = GMPlatformConfig(
[GMCarDocs("Chevrolet Trailblazer 2021-22 - No-ACC")],
[GMCarDocs("Chevrolet Trailblazer 2021-22")],
CHEVROLET_TRAILBLAZER.specs,
)
CADILLAC_XT4 = GMPlatformConfig(
@@ -219,7 +221,7 @@ class CAR(Platforms):
CarSpecs(mass=1660, wheelbase=2.78, steerRatio=14.4, centerToFrontRatio=0.4),
)
CADILLAC_XT5_CC = GMPlatformConfig(
[GMCarDocs("Cadillac XT5 - No-ACC")],
[GMCarDocs("Cadillac XT5 No ACC")],
CarSpecs(mass=1810, wheelbase=2.86, steerRatio=16.34, centerToFrontRatio=0.5),
)
CHEVROLET_TRAVERSE = GMPlatformConfig(
@@ -231,17 +233,13 @@ class CAR(Platforms):
CarSpecs(mass=2050, wheelbase=2.86, steerRatio=16.0, centerToFrontRatio=0.5),
)
CHEVROLET_MALIBU_CC = GMPlatformConfig(
[GMCarDocs("Chevrolet Malibu 2023 - No-ACC")],
[GMCarDocs("Chevrolet Malibu 2023 No ACC")],
CarSpecs(mass=1450, wheelbase=2.8, steerRatio=15.8, centerToFrontRatio=0.4),
)
CHEVROLET_TRAX = GMPlatformConfig(
[GMCarDocs("Chevrolet TRAX 2024")],
CarSpecs(mass=1365, wheelbase=2.7, steerRatio=16.4, centerToFrontRatio=0.4),
)
CHEVROLET_VOLT_2019 = GMPlatformConfig(
[GMCarDocs("Chevrolet Volt 2019")],
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=15.7, centerToFrontRatio=0.45),
)
class CruiseButtons:
@@ -271,7 +269,6 @@ class GMFlags(IntFlag):
CC_LONG = 2
NO_CAMERA = 4
NO_ACCELERATOR_POS_MSG = 8
FORCE_BRAKE_C9 = 16
# In a Data Module, an identifier is a string used to recognize an object,
@@ -323,15 +320,13 @@ FW_QUERY_CONFIG = FwQueryConfig(
extra_ecus=[(Ecu.fwdCamera, 0x24b, None)],
)
EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_VOLT_ASCM}
EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC}
CC_ONLY_CAR = {CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_EQUINOX_CC, CAR.CHEVROLET_SUBURBAN_CC, CAR.GMC_YUKON_CC, CAR.CADILLAC_CT6_CC, CAR.CHEVROLET_TRAILBLAZER_CC, CAR.CADILLAC_XT5_CC, CAR.CHEVROLET_MALIBU_CC}
# CC_ONLY_CAR = set(c for c in CAR if str(c).endswith('_CC'))
# We're integrated at the Safety Data Gateway Module on these cars
SDGM_CAR = {CAR.CADILLAC_XT4, CAR.CHEVROLET_TRAVERSE, CAR.BUICK_BABYENCLAVE}
ASCM_INT = {CAR.CHEVROLET_VOLT_ASCM}
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
CAMERA_ACC_CAR = {CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_EQUINOX, CAR.CHEVROLET_TRAILBLAZER, CAR.CHEVROLET_TRAX}
CAMERA_ACC_CAR.update({CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_EQUINOX_CC, CAR.GMC_YUKON_CC, CAR.CADILLAC_CT6_CC, CAR.CHEVROLET_TRAILBLAZER_CC, CAR.CADILLAC_XT5_CC, CAR.CHEVROLET_MALIBU_CC})
+1 -1
View File
@@ -412,7 +412,7 @@ class CarInterfaceBase(ABC):
tune.init('torque')
tune.torque.useSteeringAngle = use_steering_angle
tune.torque.kp = 1.0
tune.torque.kp = 0.6
tune.torque.kf = 1.0
tune.torque.ki = 0.1
tune.torque.friction = params['FRICTION']
+1 -1
View File
@@ -5,7 +5,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase
# mocked car interface for dashcam mode
class CarInterface(CarInterfaceBase):
def __init__(self, CP, FPCP, CarController, CarState):
def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState)
self.speed = 0.
-1
View File
@@ -49,7 +49,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16]
"CHEVROLET_TRAVERSE" = [1.33, 1.33, 0.18]
"CHEVROLET_EQUINOX" = [2.5, 2.5, 0.05]
"CHEVROLET_VOLT_2019" = [1.45, 1.6, 0.2]
"VOLKSWAGEN_CADDY_MK3" = [1.2, 1.2, 0.1]
"VOLKSWAGEN_PASSAT_NMS" = [2.5, 2.5, 0.1]
"VOLKSWAGEN_SHARAN_MK2" = [2.5, 2.5, 0.1]
@@ -56,7 +56,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"CADILLAC_ESCALADE_ESV" = "CHEVROLET_VOLT"
"CADILLAC_ATS" = "CHEVROLET_VOLT"
"CHEVROLET_MALIBU" = "CHEVROLET_VOLT"
"CHEVROLET_VOLT_ASCM" = "CHEVROLET_VOLT"
"HOLDEN_ASTRA" = "CHEVROLET_VOLT"
"CHEVROLET_VOLT_CC" = "CHEVROLET_VOLT"
"CHEVROLET_BOLT_CC" = "CHEVROLET_BOLT_EUV"
+1 -1
View File
@@ -170,7 +170,7 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.cruise_decreased, self.CS.cruise_decreased_previously, {1: ButtonType.decelCruise}),
*create_button_events(self.CS.cruise_increased, self.CS.cruise_increased_previously, {1: ButtonType.accelCruise}),
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled != self.CS.lkas_previously_enabled, False, {1: FrogPilotButtonType.lkas}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
# events
+9 -1
View File
@@ -32,7 +32,7 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
from openpilot.system.hardware import HARDWARE
from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles, params_memory
from openpilot.frogpilot.common.frogpilot_variables import ERROR_LOGS_PATH, get_frogpilot_toggles, params_memory
from openpilot.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel
SOFT_DISABLE_TIME = 3 # seconds
@@ -199,6 +199,8 @@ class Controls:
self.has_menu = self.CP.carName == "gm" and not (self.CP.flags & GMFlags.NO_CAMERA.value or self.CP.carFingerprint in CC_ONLY_CAR)
self.error_log = ERROR_LOGS_PATH / "error.txt"
def set_initial_state(self):
if REPLAY:
controls_state = self.params.get("ReplayControlsState")
@@ -429,6 +431,12 @@ class Controls:
if self.frogpilot_toggles.block_user:
self.events.add(EventName.blockUser, static=True)
if self.error_log.is_file():
if self.frogpilot_toggles.random_events:
self.events.add(EventName.openpilotCrashedRandomEvent)
else:
self.events.add(EventName.openpilotCrashed)
def data_sample(self):
"""Receive data from sockets"""
+2 -2
View File
@@ -87,7 +87,7 @@ class DesireHelper:
if torque_applied:
self.lane_change_wait_timer = frogpilot_toggles.lane_change_delay
torque_applied |= frogpilot_toggles.nudgeless and lane_available and self.lane_change_wait_timer >= frogpilot_toggles.lane_change_delay
torque_applied |= frogpilot_toggles.nudgeless
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
@@ -95,7 +95,7 @@ class DesireHelper:
if not one_blinker or below_lane_change_speed or self.lane_change_completed:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
elif torque_applied and not blindspot_detected:
elif torque_applied and not blindspot_detected and lane_available and self.lane_change_wait_timer >= frogpilot_toggles.lane_change_delay:
self.lane_change_state = LaneChangeState.laneChangeStarting
self.lane_change_completed = frogpilot_toggles.one_lane_change
self.lane_change_wait_timer = 0.0
-1
View File
@@ -353,7 +353,6 @@ def holiday_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster,
"world_frog_day": "Happy World Frog Day! 🐸",
"april_fools": "Happy April Fool's Day! 🤡",
"easter_week": "Happy Easter! 🐰",
"may_the_fourth": "May the 4th be with you! 🚀",
"cinco_de_mayo": "¡Feliz Cinco de Mayo! 🌮",
"fourth_of_july": "Happy Fourth of July! 🎆",
"halloween_week": "Happy Halloween! 🎃",
@@ -1,538 +0,0 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [
2,
3
],
"idxbx_0": [
0,
1,
2,
3
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2,
3
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [
-1.5707963267948966,
-0.8726646259971648
],
"lbx_0": [
0.0,
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [
1.5707963267948966,
0.8726646259971648
],
"ubx_0": [
0.0,
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [],
"Vu_0": [],
"Vx": [],
"Vx_0": [],
"Vx_e": [],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"Zl": [],
"Zl_e": [],
"Zu": [],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0,
0.0
],
"zl": [],
"zl_e": [],
"zu": [],
"zu_e": []
},
"cython_include_dirs": [
"/usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include",
"/usr/local/pyenv/versions/3.11.4/include/python3.11"
],
"dims": {
"N": 32,
"nbu": 0,
"nbx": 2,
"nbx_0": 4,
"nbx_e": 0,
"nbxe_0": 4,
"ng": 0,
"ng_e": 0,
"nh": 0,
"nh_e": 0,
"np": 2,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 0,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 0,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 4,
"ny": 5,
"ny_0": 5,
"ny_e": 3,
"nz": 0
},
"json_file": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json",
"model": {
"con_h_expr": null,
"con_h_expr_e": null,
"con_phi_expr": null,
"con_phi_expr_e": null,
"con_r_expr": null,
"con_r_expr_e": null,
"con_r_in_phi": null,
"con_r_in_phi_e": null,
"cost_conl_custom_outer_hess": null,
"cost_conl_custom_outer_hess_0": null,
"cost_conl_custom_outer_hess_e": null,
"cost_expr_ext_cost": null,
"cost_expr_ext_cost_0": null,
"cost_expr_ext_cost_custom_hess": null,
"cost_expr_ext_cost_custom_hess_0": null,
"cost_expr_ext_cost_custom_hess_e": null,
"cost_expr_ext_cost_e": null,
"cost_psi_expr": null,
"cost_psi_expr_0": null,
"cost_psi_expr_e": null,
"cost_r_in_psi_expr": null,
"cost_r_in_psi_expr_0": null,
"cost_r_in_psi_expr_e": null,
"cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpgegeaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjlpd",
"cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpgegeaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjlpd",
"cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpg",
"disc_dyn_expr": null,
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_generic_source": null,
"f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdhegnaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegnaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachfaaaaaaaaaaaaaaaegoaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdhegnaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaajhpffghgpgpfegpgehegbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegnaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachgaaaaaaaaaaaaaaaegoaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaalaaaaaaaahdhjgpffghgpgpfegpgehchjaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaaabaaaaaaahdhjgpfchbgehfgpffghgpgpfegpgehegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "lat",
"p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeggaaaaaaaaaaaaaaacaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdh",
"u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpg",
"xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaajhpffghgpgpfegpgehegpcaaaaaaaaaaaaaalaaaaaaaahdhjgpffghgpgpfegpgehegpcaaaaaaaaaaaaaaabaaaaaaahdhjgpfchbgehfgpffghgpgpfegpgeh",
"z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
},
"parameter_values": [
0.0,
0.0
],
"problem_class": "OCP",
"shared_lib_ext": ".so",
"solver_options": {
"Tsim": 0.009765625,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"custom_templates": [],
"custom_update_copy": true,
"custom_update_filename": "",
"custom_update_header_filename": "",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"ext_fun_compile_flags": "-O2",
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_ext_qp_res": 0,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_cond_ric_alg": 1,
"qp_solver_iter_max": 1,
"qp_solver_ric_alg": 1,
"qp_solver_tol_comp": null,
"qp_solver_tol_eq": null,
"qp_solver_tol_ineq": null,
"qp_solver_tol_stat": null,
"qp_solver_warm_start": 0,
"regularize_method": null,
"shooting_nodes": [
0.0,
0.009765625,
0.0390625,
0.087890625,
0.15625,
0.244140625,
0.3515625,
0.478515625,
0.625,
0.791015625,
0.9765625,
1.181640625,
1.40625,
1.650390625,
1.9140625,
2.197265625,
2.5,
2.822265625,
3.1640625,
3.525390625,
3.90625,
4.306640625,
4.7265625,
5.166015625,
5.625,
6.103515625,
6.6015625,
7.119140625,
7.65625,
8.212890625,
8.7890625,
9.384765625,
10.0
],
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_newton_tol": 0.0,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 10.0,
"time_steps": [
0.009765625,
0.029296875,
0.048828125,
0.068359375,
0.087890625,
0.107421875,
0.126953125,
0.146484375,
0.166015625,
0.185546875,
0.205078125,
0.224609375,
0.244140625,
0.263671875,
0.283203125,
0.302734375,
0.322265625,
0.341796875,
0.361328125,
0.380859375,
0.400390625,
0.419921875,
0.439453125,
0.458984375,
0.478515625,
0.498046875,
0.517578125,
0.537109375,
0.556640625,
0.576171875,
0.595703125,
0.615234375
]
}
}
+1 -2
View File
@@ -92,11 +92,10 @@ class LongControl:
self.CP = CP
self.long_control_state = LongCtrlState.off
self.experimental_mode = False
pos_p_limit = 0.0
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL,
pos_p_limit=pos_p_limit)
pos_p_limit=None)
self.v_pid = 0.0
self._mode_setup()
self.last_output_accel = 0.0
@@ -1,524 +0,0 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [],
"idxbx_0": [
0,
1,
2
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [
0,
1,
2,
3
],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [],
"lbx_0": [
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [
0.0,
0.0,
0.0,
0.0
],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [
0.0,
0.0,
0.0,
0.0
],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [],
"ubx_0": [
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [
10000.0,
10000.0,
10000.0,
10000.0
],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [
0.0,
0.0,
0.0,
0.0
],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [],
"Vu_0": [],
"Vx": [],
"Vx_0": [],
"Vx_e": [],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"Zl": [
0.0,
0.0,
0.0,
0.0
],
"Zl_e": [],
"Zu": [
0.0,
0.0,
0.0,
0.0
],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0,
0.0,
0.0,
0.0
],
"zl": [
0.0,
0.0,
0.0,
0.0
],
"zl_e": [],
"zu": [
0.0,
0.0,
0.0,
0.0
],
"zu_e": []
},
"cython_include_dirs": [
"/usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include",
"/usr/local/pyenv/versions/3.11.4/include/python3.11"
],
"dims": {
"N": 12,
"nbu": 0,
"nbx": 0,
"nbx_0": 3,
"nbx_e": 0,
"nbxe_0": 3,
"ng": 0,
"ng_e": 0,
"nh": 4,
"nh_e": 0,
"np": 6,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 4,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 4,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 3,
"ny": 6,
"ny_0": 6,
"ny_e": 5,
"nz": 0
},
"json_file": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
"model": {
"con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
"con_h_expr_e": null,
"con_phi_expr": null,
"con_phi_expr_e": null,
"con_r_expr": null,
"con_r_expr_e": null,
"con_r_in_phi": null,
"con_r_in_phi_e": null,
"cost_conl_custom_outer_hess": null,
"cost_conl_custom_outer_hess_0": null,
"cost_conl_custom_outer_hess_e": null,
"cost_expr_ext_cost": null,
"cost_expr_ext_cost_0": null,
"cost_expr_ext_cost_custom_hess": null,
"cost_expr_ext_cost_custom_hess_0": null,
"cost_expr_ext_cost_custom_hess_e": null,
"cost_expr_ext_cost_e": null,
"cost_psi_expr": null,
"cost_psi_expr_0": null,
"cost_psi_expr_e": null,
"cost_r_in_psi_expr": null,
"cost_r_in_psi_expr_0": null,
"cost_r_in_psi_expr_e": null,
"cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbg",
"disc_dyn_expr": null,
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_generic_source": null,
"f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "long",
"p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgch",
"u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
"xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgeh",
"z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
},
"parameter_values": [
-1.2,
1.2,
0.0,
0.0,
1.45,
0.75
],
"problem_class": "OCP",
"shared_lib_ext": ".so",
"solver_options": {
"Tsim": 0.06944444444444445,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"custom_templates": [],
"custom_update_copy": true,
"custom_update_filename": "",
"custom_update_header_filename": "",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"ext_fun_compile_flags": "-O2",
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_ext_qp_res": 0,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_cond_ric_alg": 1,
"qp_solver_iter_max": 10,
"qp_solver_ric_alg": 1,
"qp_solver_tol_comp": 0.001,
"qp_solver_tol_eq": 0.001,
"qp_solver_tol_ineq": 0.001,
"qp_solver_tol_stat": 0.001,
"qp_solver_warm_start": 0,
"regularize_method": null,
"shooting_nodes": [
0.0,
0.06944444444444445,
0.2777777777777778,
0.625,
1.1111111111111112,
1.7361111111111114,
2.5,
3.4027777777777786,
4.444444444444445,
5.625,
6.9444444444444455,
8.402777777777777,
10.0
],
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_newton_tol": 0.0,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 10.0,
"time_steps": [
0.06944444444444445,
0.20833333333333334,
0.3472222222222222,
0.48611111111111116,
0.6250000000000002,
0.7638888888888886,
0.9027777777777786,
1.041666666666666,
1.1805555555555554,
1.3194444444444455,
1.4583333333333313,
1.5972222222222232
]
}
}
+1 -1
View File
@@ -79,7 +79,7 @@ class Track:
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau.x = min(max(self.aLeadTau.x, 1e-2) * 1.1, _LEAD_ACCEL_TAU)
self.aLeadTau.x = min(max(self.aLeadTau, 1e-2) * 1.1, _LEAD_ACCEL_TAU)
else:
self.aLeadTau.update(0.0)
Binary file not shown.
+342 -342
View File
@@ -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_5662147043452870344) {
out_5662147043452870344[0] = delta_x[0] + nom_x[0];
out_5662147043452870344[1] = delta_x[1] + nom_x[1];
out_5662147043452870344[2] = delta_x[2] + nom_x[2];
out_5662147043452870344[3] = delta_x[3] + nom_x[3];
out_5662147043452870344[4] = delta_x[4] + nom_x[4];
out_5662147043452870344[5] = delta_x[5] + nom_x[5];
out_5662147043452870344[6] = delta_x[6] + nom_x[6];
out_5662147043452870344[7] = delta_x[7] + nom_x[7];
out_5662147043452870344[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_2366717911037962719) {
out_2366717911037962719[0] = delta_x[0] + nom_x[0];
out_2366717911037962719[1] = delta_x[1] + nom_x[1];
out_2366717911037962719[2] = delta_x[2] + nom_x[2];
out_2366717911037962719[3] = delta_x[3] + nom_x[3];
out_2366717911037962719[4] = delta_x[4] + nom_x[4];
out_2366717911037962719[5] = delta_x[5] + nom_x[5];
out_2366717911037962719[6] = delta_x[6] + nom_x[6];
out_2366717911037962719[7] = delta_x[7] + nom_x[7];
out_2366717911037962719[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_2016113788005242335) {
out_2016113788005242335[0] = -nom_x[0] + true_x[0];
out_2016113788005242335[1] = -nom_x[1] + true_x[1];
out_2016113788005242335[2] = -nom_x[2] + true_x[2];
out_2016113788005242335[3] = -nom_x[3] + true_x[3];
out_2016113788005242335[4] = -nom_x[4] + true_x[4];
out_2016113788005242335[5] = -nom_x[5] + true_x[5];
out_2016113788005242335[6] = -nom_x[6] + true_x[6];
out_2016113788005242335[7] = -nom_x[7] + true_x[7];
out_2016113788005242335[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_70336108065318295) {
out_70336108065318295[0] = -nom_x[0] + true_x[0];
out_70336108065318295[1] = -nom_x[1] + true_x[1];
out_70336108065318295[2] = -nom_x[2] + true_x[2];
out_70336108065318295[3] = -nom_x[3] + true_x[3];
out_70336108065318295[4] = -nom_x[4] + true_x[4];
out_70336108065318295[5] = -nom_x[5] + true_x[5];
out_70336108065318295[6] = -nom_x[6] + true_x[6];
out_70336108065318295[7] = -nom_x[7] + true_x[7];
out_70336108065318295[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_8997301641043644665) {
out_8997301641043644665[0] = 1.0;
out_8997301641043644665[1] = 0;
out_8997301641043644665[2] = 0;
out_8997301641043644665[3] = 0;
out_8997301641043644665[4] = 0;
out_8997301641043644665[5] = 0;
out_8997301641043644665[6] = 0;
out_8997301641043644665[7] = 0;
out_8997301641043644665[8] = 0;
out_8997301641043644665[9] = 0;
out_8997301641043644665[10] = 1.0;
out_8997301641043644665[11] = 0;
out_8997301641043644665[12] = 0;
out_8997301641043644665[13] = 0;
out_8997301641043644665[14] = 0;
out_8997301641043644665[15] = 0;
out_8997301641043644665[16] = 0;
out_8997301641043644665[17] = 0;
out_8997301641043644665[18] = 0;
out_8997301641043644665[19] = 0;
out_8997301641043644665[20] = 1.0;
out_8997301641043644665[21] = 0;
out_8997301641043644665[22] = 0;
out_8997301641043644665[23] = 0;
out_8997301641043644665[24] = 0;
out_8997301641043644665[25] = 0;
out_8997301641043644665[26] = 0;
out_8997301641043644665[27] = 0;
out_8997301641043644665[28] = 0;
out_8997301641043644665[29] = 0;
out_8997301641043644665[30] = 1.0;
out_8997301641043644665[31] = 0;
out_8997301641043644665[32] = 0;
out_8997301641043644665[33] = 0;
out_8997301641043644665[34] = 0;
out_8997301641043644665[35] = 0;
out_8997301641043644665[36] = 0;
out_8997301641043644665[37] = 0;
out_8997301641043644665[38] = 0;
out_8997301641043644665[39] = 0;
out_8997301641043644665[40] = 1.0;
out_8997301641043644665[41] = 0;
out_8997301641043644665[42] = 0;
out_8997301641043644665[43] = 0;
out_8997301641043644665[44] = 0;
out_8997301641043644665[45] = 0;
out_8997301641043644665[46] = 0;
out_8997301641043644665[47] = 0;
out_8997301641043644665[48] = 0;
out_8997301641043644665[49] = 0;
out_8997301641043644665[50] = 1.0;
out_8997301641043644665[51] = 0;
out_8997301641043644665[52] = 0;
out_8997301641043644665[53] = 0;
out_8997301641043644665[54] = 0;
out_8997301641043644665[55] = 0;
out_8997301641043644665[56] = 0;
out_8997301641043644665[57] = 0;
out_8997301641043644665[58] = 0;
out_8997301641043644665[59] = 0;
out_8997301641043644665[60] = 1.0;
out_8997301641043644665[61] = 0;
out_8997301641043644665[62] = 0;
out_8997301641043644665[63] = 0;
out_8997301641043644665[64] = 0;
out_8997301641043644665[65] = 0;
out_8997301641043644665[66] = 0;
out_8997301641043644665[67] = 0;
out_8997301641043644665[68] = 0;
out_8997301641043644665[69] = 0;
out_8997301641043644665[70] = 1.0;
out_8997301641043644665[71] = 0;
out_8997301641043644665[72] = 0;
out_8997301641043644665[73] = 0;
out_8997301641043644665[74] = 0;
out_8997301641043644665[75] = 0;
out_8997301641043644665[76] = 0;
out_8997301641043644665[77] = 0;
out_8997301641043644665[78] = 0;
out_8997301641043644665[79] = 0;
out_8997301641043644665[80] = 1.0;
void H_mod_fun(double *state, double *out_281700328218384173) {
out_281700328218384173[0] = 1.0;
out_281700328218384173[1] = 0;
out_281700328218384173[2] = 0;
out_281700328218384173[3] = 0;
out_281700328218384173[4] = 0;
out_281700328218384173[5] = 0;
out_281700328218384173[6] = 0;
out_281700328218384173[7] = 0;
out_281700328218384173[8] = 0;
out_281700328218384173[9] = 0;
out_281700328218384173[10] = 1.0;
out_281700328218384173[11] = 0;
out_281700328218384173[12] = 0;
out_281700328218384173[13] = 0;
out_281700328218384173[14] = 0;
out_281700328218384173[15] = 0;
out_281700328218384173[16] = 0;
out_281700328218384173[17] = 0;
out_281700328218384173[18] = 0;
out_281700328218384173[19] = 0;
out_281700328218384173[20] = 1.0;
out_281700328218384173[21] = 0;
out_281700328218384173[22] = 0;
out_281700328218384173[23] = 0;
out_281700328218384173[24] = 0;
out_281700328218384173[25] = 0;
out_281700328218384173[26] = 0;
out_281700328218384173[27] = 0;
out_281700328218384173[28] = 0;
out_281700328218384173[29] = 0;
out_281700328218384173[30] = 1.0;
out_281700328218384173[31] = 0;
out_281700328218384173[32] = 0;
out_281700328218384173[33] = 0;
out_281700328218384173[34] = 0;
out_281700328218384173[35] = 0;
out_281700328218384173[36] = 0;
out_281700328218384173[37] = 0;
out_281700328218384173[38] = 0;
out_281700328218384173[39] = 0;
out_281700328218384173[40] = 1.0;
out_281700328218384173[41] = 0;
out_281700328218384173[42] = 0;
out_281700328218384173[43] = 0;
out_281700328218384173[44] = 0;
out_281700328218384173[45] = 0;
out_281700328218384173[46] = 0;
out_281700328218384173[47] = 0;
out_281700328218384173[48] = 0;
out_281700328218384173[49] = 0;
out_281700328218384173[50] = 1.0;
out_281700328218384173[51] = 0;
out_281700328218384173[52] = 0;
out_281700328218384173[53] = 0;
out_281700328218384173[54] = 0;
out_281700328218384173[55] = 0;
out_281700328218384173[56] = 0;
out_281700328218384173[57] = 0;
out_281700328218384173[58] = 0;
out_281700328218384173[59] = 0;
out_281700328218384173[60] = 1.0;
out_281700328218384173[61] = 0;
out_281700328218384173[62] = 0;
out_281700328218384173[63] = 0;
out_281700328218384173[64] = 0;
out_281700328218384173[65] = 0;
out_281700328218384173[66] = 0;
out_281700328218384173[67] = 0;
out_281700328218384173[68] = 0;
out_281700328218384173[69] = 0;
out_281700328218384173[70] = 1.0;
out_281700328218384173[71] = 0;
out_281700328218384173[72] = 0;
out_281700328218384173[73] = 0;
out_281700328218384173[74] = 0;
out_281700328218384173[75] = 0;
out_281700328218384173[76] = 0;
out_281700328218384173[77] = 0;
out_281700328218384173[78] = 0;
out_281700328218384173[79] = 0;
out_281700328218384173[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_2973469614408186678) {
out_2973469614408186678[0] = state[0];
out_2973469614408186678[1] = state[1];
out_2973469614408186678[2] = state[2];
out_2973469614408186678[3] = state[3];
out_2973469614408186678[4] = state[4];
out_2973469614408186678[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_2973469614408186678[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_2973469614408186678[7] = state[7];
out_2973469614408186678[8] = state[8];
void f_fun(double *state, double dt, double *out_3945758947465398650) {
out_3945758947465398650[0] = state[0];
out_3945758947465398650[1] = state[1];
out_3945758947465398650[2] = state[2];
out_3945758947465398650[3] = state[3];
out_3945758947465398650[4] = state[4];
out_3945758947465398650[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_3945758947465398650[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_3945758947465398650[7] = state[7];
out_3945758947465398650[8] = state[8];
}
void F_fun(double *state, double dt, double *out_4040592134712196686) {
out_4040592134712196686[0] = 1;
out_4040592134712196686[1] = 0;
out_4040592134712196686[2] = 0;
out_4040592134712196686[3] = 0;
out_4040592134712196686[4] = 0;
out_4040592134712196686[5] = 0;
out_4040592134712196686[6] = 0;
out_4040592134712196686[7] = 0;
out_4040592134712196686[8] = 0;
out_4040592134712196686[9] = 0;
out_4040592134712196686[10] = 1;
out_4040592134712196686[11] = 0;
out_4040592134712196686[12] = 0;
out_4040592134712196686[13] = 0;
out_4040592134712196686[14] = 0;
out_4040592134712196686[15] = 0;
out_4040592134712196686[16] = 0;
out_4040592134712196686[17] = 0;
out_4040592134712196686[18] = 0;
out_4040592134712196686[19] = 0;
out_4040592134712196686[20] = 1;
out_4040592134712196686[21] = 0;
out_4040592134712196686[22] = 0;
out_4040592134712196686[23] = 0;
out_4040592134712196686[24] = 0;
out_4040592134712196686[25] = 0;
out_4040592134712196686[26] = 0;
out_4040592134712196686[27] = 0;
out_4040592134712196686[28] = 0;
out_4040592134712196686[29] = 0;
out_4040592134712196686[30] = 1;
out_4040592134712196686[31] = 0;
out_4040592134712196686[32] = 0;
out_4040592134712196686[33] = 0;
out_4040592134712196686[34] = 0;
out_4040592134712196686[35] = 0;
out_4040592134712196686[36] = 0;
out_4040592134712196686[37] = 0;
out_4040592134712196686[38] = 0;
out_4040592134712196686[39] = 0;
out_4040592134712196686[40] = 1;
out_4040592134712196686[41] = 0;
out_4040592134712196686[42] = 0;
out_4040592134712196686[43] = 0;
out_4040592134712196686[44] = 0;
out_4040592134712196686[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_4040592134712196686[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_4040592134712196686[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_4040592134712196686[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_4040592134712196686[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_4040592134712196686[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_4040592134712196686[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_4040592134712196686[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_4040592134712196686[53] = -9.8000000000000007*dt;
out_4040592134712196686[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_4040592134712196686[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_4040592134712196686[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_4040592134712196686[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_4040592134712196686[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_4040592134712196686[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_4040592134712196686[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_4040592134712196686[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_4040592134712196686[62] = 0;
out_4040592134712196686[63] = 0;
out_4040592134712196686[64] = 0;
out_4040592134712196686[65] = 0;
out_4040592134712196686[66] = 0;
out_4040592134712196686[67] = 0;
out_4040592134712196686[68] = 0;
out_4040592134712196686[69] = 0;
out_4040592134712196686[70] = 1;
out_4040592134712196686[71] = 0;
out_4040592134712196686[72] = 0;
out_4040592134712196686[73] = 0;
out_4040592134712196686[74] = 0;
out_4040592134712196686[75] = 0;
out_4040592134712196686[76] = 0;
out_4040592134712196686[77] = 0;
out_4040592134712196686[78] = 0;
out_4040592134712196686[79] = 0;
out_4040592134712196686[80] = 1;
void F_fun(double *state, double dt, double *out_7239966002367196486) {
out_7239966002367196486[0] = 1;
out_7239966002367196486[1] = 0;
out_7239966002367196486[2] = 0;
out_7239966002367196486[3] = 0;
out_7239966002367196486[4] = 0;
out_7239966002367196486[5] = 0;
out_7239966002367196486[6] = 0;
out_7239966002367196486[7] = 0;
out_7239966002367196486[8] = 0;
out_7239966002367196486[9] = 0;
out_7239966002367196486[10] = 1;
out_7239966002367196486[11] = 0;
out_7239966002367196486[12] = 0;
out_7239966002367196486[13] = 0;
out_7239966002367196486[14] = 0;
out_7239966002367196486[15] = 0;
out_7239966002367196486[16] = 0;
out_7239966002367196486[17] = 0;
out_7239966002367196486[18] = 0;
out_7239966002367196486[19] = 0;
out_7239966002367196486[20] = 1;
out_7239966002367196486[21] = 0;
out_7239966002367196486[22] = 0;
out_7239966002367196486[23] = 0;
out_7239966002367196486[24] = 0;
out_7239966002367196486[25] = 0;
out_7239966002367196486[26] = 0;
out_7239966002367196486[27] = 0;
out_7239966002367196486[28] = 0;
out_7239966002367196486[29] = 0;
out_7239966002367196486[30] = 1;
out_7239966002367196486[31] = 0;
out_7239966002367196486[32] = 0;
out_7239966002367196486[33] = 0;
out_7239966002367196486[34] = 0;
out_7239966002367196486[35] = 0;
out_7239966002367196486[36] = 0;
out_7239966002367196486[37] = 0;
out_7239966002367196486[38] = 0;
out_7239966002367196486[39] = 0;
out_7239966002367196486[40] = 1;
out_7239966002367196486[41] = 0;
out_7239966002367196486[42] = 0;
out_7239966002367196486[43] = 0;
out_7239966002367196486[44] = 0;
out_7239966002367196486[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_7239966002367196486[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_7239966002367196486[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7239966002367196486[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7239966002367196486[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_7239966002367196486[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_7239966002367196486[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_7239966002367196486[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_7239966002367196486[53] = -9.8000000000000007*dt;
out_7239966002367196486[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_7239966002367196486[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_7239966002367196486[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7239966002367196486[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7239966002367196486[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_7239966002367196486[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_7239966002367196486[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_7239966002367196486[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7239966002367196486[62] = 0;
out_7239966002367196486[63] = 0;
out_7239966002367196486[64] = 0;
out_7239966002367196486[65] = 0;
out_7239966002367196486[66] = 0;
out_7239966002367196486[67] = 0;
out_7239966002367196486[68] = 0;
out_7239966002367196486[69] = 0;
out_7239966002367196486[70] = 1;
out_7239966002367196486[71] = 0;
out_7239966002367196486[72] = 0;
out_7239966002367196486[73] = 0;
out_7239966002367196486[74] = 0;
out_7239966002367196486[75] = 0;
out_7239966002367196486[76] = 0;
out_7239966002367196486[77] = 0;
out_7239966002367196486[78] = 0;
out_7239966002367196486[79] = 0;
out_7239966002367196486[80] = 1;
}
void h_25(double *state, double *unused, double *out_1319061140791389331) {
out_1319061140791389331[0] = state[6];
void h_25(double *state, double *unused, double *out_4414103654617947245) {
out_4414103654617947245[0] = state[6];
}
void H_25(double *state, double *unused, double *out_7793576592585487069) {
out_7793576592585487069[0] = 0;
out_7793576592585487069[1] = 0;
out_7793576592585487069[2] = 0;
out_7793576592585487069[3] = 0;
out_7793576592585487069[4] = 0;
out_7793576592585487069[5] = 0;
out_7793576592585487069[6] = 1;
out_7793576592585487069[7] = 0;
out_7793576592585487069[8] = 0;
void H_25(double *state, double *unused, double *out_1365985619853648806) {
out_1365985619853648806[0] = 0;
out_1365985619853648806[1] = 0;
out_1365985619853648806[2] = 0;
out_1365985619853648806[3] = 0;
out_1365985619853648806[4] = 0;
out_1365985619853648806[5] = 0;
out_1365985619853648806[6] = 1;
out_1365985619853648806[7] = 0;
out_1365985619853648806[8] = 0;
}
void h_24(double *state, double *unused, double *out_3718149663781647640) {
out_3718149663781647640[0] = state[4];
out_3718149663781647640[1] = state[5];
void h_24(double *state, double *unused, double *out_218399050948173848) {
out_218399050948173848[0] = state[4];
out_218399050948173848[1] = state[5];
}
void H_24(double *state, double *unused, double *out_7095241882321741411) {
out_7095241882321741411[0] = 0;
out_7095241882321741411[1] = 0;
out_7095241882321741411[2] = 0;
out_7095241882321741411[3] = 0;
out_7095241882321741411[4] = 1;
out_7095241882321741411[5] = 0;
out_7095241882321741411[6] = 0;
out_7095241882321741411[7] = 0;
out_7095241882321741411[8] = 0;
out_7095241882321741411[9] = 0;
out_7095241882321741411[10] = 0;
out_7095241882321741411[11] = 0;
out_7095241882321741411[12] = 0;
out_7095241882321741411[13] = 0;
out_7095241882321741411[14] = 1;
out_7095241882321741411[15] = 0;
out_7095241882321741411[16] = 0;
out_7095241882321741411[17] = 0;
void H_24(double *state, double *unused, double *out_8020191077961834563) {
out_8020191077961834563[0] = 0;
out_8020191077961834563[1] = 0;
out_8020191077961834563[2] = 0;
out_8020191077961834563[3] = 0;
out_8020191077961834563[4] = 1;
out_8020191077961834563[5] = 0;
out_8020191077961834563[6] = 0;
out_8020191077961834563[7] = 0;
out_8020191077961834563[8] = 0;
out_8020191077961834563[9] = 0;
out_8020191077961834563[10] = 0;
out_8020191077961834563[11] = 0;
out_8020191077961834563[12] = 0;
out_8020191077961834563[13] = 0;
out_8020191077961834563[14] = 1;
out_8020191077961834563[15] = 0;
out_8020191077961834563[16] = 0;
out_8020191077961834563[17] = 0;
}
void h_30(double *state, double *unused, double *out_1043867078506883442) {
out_1043867078506883442[0] = state[4];
void h_30(double *state, double *unused, double *out_7444812492615309208) {
out_7444812492615309208[0] = state[4];
}
void H_30(double *state, double *unused, double *out_3736477139632447792) {
out_3736477139632447792[0] = 0;
out_3736477139632447792[1] = 0;
out_3736477139632447792[2] = 0;
out_3736477139632447792[3] = 0;
out_3736477139632447792[4] = 1;
out_3736477139632447792[5] = 0;
out_3736477139632447792[6] = 0;
out_3736477139632447792[7] = 0;
out_3736477139632447792[8] = 0;
void H_30(double *state, double *unused, double *out_1236646672710408736) {
out_1236646672710408736[0] = 0;
out_1236646672710408736[1] = 0;
out_1236646672710408736[2] = 0;
out_1236646672710408736[3] = 0;
out_1236646672710408736[4] = 1;
out_1236646672710408736[5] = 0;
out_1236646672710408736[6] = 0;
out_1236646672710408736[7] = 0;
out_1236646672710408736[8] = 0;
}
void h_26(double *state, double *unused, double *out_6922152566065598530) {
out_6922152566065598530[0] = state[7];
void h_26(double *state, double *unused, double *out_442049788822243429) {
out_442049788822243429[0] = state[7];
}
void H_26(double *state, double *unused, double *out_4052073273711430845) {
out_4052073273711430845[0] = 0;
out_4052073273711430845[1] = 0;
out_4052073273711430845[2] = 0;
out_4052073273711430845[3] = 0;
out_4052073273711430845[4] = 0;
out_4052073273711430845[5] = 0;
out_4052073273711430845[6] = 0;
out_4052073273711430845[7] = 1;
out_4052073273711430845[8] = 0;
void H_26(double *state, double *unused, double *out_2375517699020407418) {
out_2375517699020407418[0] = 0;
out_2375517699020407418[1] = 0;
out_2375517699020407418[2] = 0;
out_2375517699020407418[3] = 0;
out_2375517699020407418[4] = 0;
out_2375517699020407418[5] = 0;
out_2375517699020407418[6] = 0;
out_2375517699020407418[7] = 1;
out_2375517699020407418[8] = 0;
}
void h_27(double *state, double *unused, double *out_7701482271649087111) {
out_7701482271649087111[0] = state[3];
void h_27(double *state, double *unused, double *out_3470593801716284810) {
out_3470593801716284810[0] = state[3];
}
void H_27(double *state, double *unused, double *out_5911240451432872703) {
out_5911240451432872703[0] = 0;
out_5911240451432872703[1] = 0;
out_5911240451432872703[2] = 0;
out_5911240451432872703[3] = 1;
out_5911240451432872703[4] = 0;
out_5911240451432872703[5] = 0;
out_5911240451432872703[6] = 0;
out_5911240451432872703[7] = 0;
out_5911240451432872703[8] = 0;
void H_27(double *state, double *unused, double *out_938116639090016175) {
out_938116639090016175[0] = 0;
out_938116639090016175[1] = 0;
out_938116639090016175[2] = 0;
out_938116639090016175[3] = 1;
out_938116639090016175[4] = 0;
out_938116639090016175[5] = 0;
out_938116639090016175[6] = 0;
out_938116639090016175[7] = 0;
out_938116639090016175[8] = 0;
}
void h_29(double *state, double *unused, double *out_7544295603297957966) {
out_7544295603297957966[0] = state[1];
void h_29(double *state, double *unused, double *out_7965145529649909723) {
out_7965145529649909723[0] = state[1];
}
void H_29(double *state, double *unused, double *out_3226245795318055608) {
out_3226245795318055608[0] = 0;
out_3226245795318055608[1] = 1;
out_3226245795318055608[2] = 0;
out_3226245795318055608[3] = 0;
out_3226245795318055608[4] = 0;
out_3226245795318055608[5] = 0;
out_3226245795318055608[6] = 0;
out_3226245795318055608[7] = 0;
out_3226245795318055608[8] = 0;
void H_29(double *state, double *unused, double *out_1746878017024800920) {
out_1746878017024800920[0] = 0;
out_1746878017024800920[1] = 1;
out_1746878017024800920[2] = 0;
out_1746878017024800920[3] = 0;
out_1746878017024800920[4] = 0;
out_1746878017024800920[5] = 0;
out_1746878017024800920[6] = 0;
out_1746878017024800920[7] = 0;
out_1746878017024800920[8] = 0;
}
void h_28(double *state, double *unused, double *out_3095916628749130163) {
out_3095916628749130163[0] = state[0];
void h_28(double *state, double *unused, double *out_5501663824664413576) {
out_5501663824664413576[0] = state[0];
}
void H_28(double *state, double *unused, double *out_8308644812387586182) {
out_8308644812387586182[0] = 1;
out_8308644812387586182[1] = 0;
out_8308644812387586182[2] = 0;
out_8308644812387586182[3] = 0;
out_8308644812387586182[4] = 0;
out_8308644812387586182[5] = 0;
out_8308644812387586182[6] = 0;
out_8308644812387586182[7] = 0;
out_8308644812387586182[8] = 0;
void H_28(double *state, double *unused, double *out_3335521000044729654) {
out_3335521000044729654[0] = 1;
out_3335521000044729654[1] = 0;
out_3335521000044729654[2] = 0;
out_3335521000044729654[3] = 0;
out_3335521000044729654[4] = 0;
out_3335521000044729654[5] = 0;
out_3335521000044729654[6] = 0;
out_3335521000044729654[7] = 0;
out_3335521000044729654[8] = 0;
}
void h_31(double *state, double *unused, double *out_4956510669045411681) {
out_4956510669045411681[0] = state[8];
void h_31(double *state, double *unused, double *out_5392762942373062487) {
out_5392762942373062487[0] = state[8];
}
void H_31(double *state, double *unused, double *out_7824222554462447497) {
out_7824222554462447497[0] = 0;
out_7824222554462447497[1] = 0;
out_7824222554462447497[2] = 0;
out_7824222554462447497[3] = 0;
out_7824222554462447497[4] = 0;
out_7824222554462447497[5] = 0;
out_7824222554462447497[6] = 0;
out_7824222554462447497[7] = 0;
out_7824222554462447497[8] = 1;
void H_31(double *state, double *unused, double *out_1396631581730609234) {
out_1396631581730609234[0] = 0;
out_1396631581730609234[1] = 0;
out_1396631581730609234[2] = 0;
out_1396631581730609234[3] = 0;
out_1396631581730609234[4] = 0;
out_1396631581730609234[5] = 0;
out_1396631581730609234[6] = 0;
out_1396631581730609234[7] = 0;
out_1396631581730609234[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_5662147043452870344) {
err_fun(nom_x, delta_x, out_5662147043452870344);
void car_err_fun(double *nom_x, double *delta_x, double *out_2366717911037962719) {
err_fun(nom_x, delta_x, out_2366717911037962719);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_2016113788005242335) {
inv_err_fun(nom_x, true_x, out_2016113788005242335);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_70336108065318295) {
inv_err_fun(nom_x, true_x, out_70336108065318295);
}
void car_H_mod_fun(double *state, double *out_8997301641043644665) {
H_mod_fun(state, out_8997301641043644665);
void car_H_mod_fun(double *state, double *out_281700328218384173) {
H_mod_fun(state, out_281700328218384173);
}
void car_f_fun(double *state, double dt, double *out_2973469614408186678) {
f_fun(state, dt, out_2973469614408186678);
void car_f_fun(double *state, double dt, double *out_3945758947465398650) {
f_fun(state, dt, out_3945758947465398650);
}
void car_F_fun(double *state, double dt, double *out_4040592134712196686) {
F_fun(state, dt, out_4040592134712196686);
void car_F_fun(double *state, double dt, double *out_7239966002367196486) {
F_fun(state, dt, out_7239966002367196486);
}
void car_h_25(double *state, double *unused, double *out_1319061140791389331) {
h_25(state, unused, out_1319061140791389331);
void car_h_25(double *state, double *unused, double *out_4414103654617947245) {
h_25(state, unused, out_4414103654617947245);
}
void car_H_25(double *state, double *unused, double *out_7793576592585487069) {
H_25(state, unused, out_7793576592585487069);
void car_H_25(double *state, double *unused, double *out_1365985619853648806) {
H_25(state, unused, out_1365985619853648806);
}
void car_h_24(double *state, double *unused, double *out_3718149663781647640) {
h_24(state, unused, out_3718149663781647640);
void car_h_24(double *state, double *unused, double *out_218399050948173848) {
h_24(state, unused, out_218399050948173848);
}
void car_H_24(double *state, double *unused, double *out_7095241882321741411) {
H_24(state, unused, out_7095241882321741411);
void car_H_24(double *state, double *unused, double *out_8020191077961834563) {
H_24(state, unused, out_8020191077961834563);
}
void car_h_30(double *state, double *unused, double *out_1043867078506883442) {
h_30(state, unused, out_1043867078506883442);
void car_h_30(double *state, double *unused, double *out_7444812492615309208) {
h_30(state, unused, out_7444812492615309208);
}
void car_H_30(double *state, double *unused, double *out_3736477139632447792) {
H_30(state, unused, out_3736477139632447792);
void car_H_30(double *state, double *unused, double *out_1236646672710408736) {
H_30(state, unused, out_1236646672710408736);
}
void car_h_26(double *state, double *unused, double *out_6922152566065598530) {
h_26(state, unused, out_6922152566065598530);
void car_h_26(double *state, double *unused, double *out_442049788822243429) {
h_26(state, unused, out_442049788822243429);
}
void car_H_26(double *state, double *unused, double *out_4052073273711430845) {
H_26(state, unused, out_4052073273711430845);
void car_H_26(double *state, double *unused, double *out_2375517699020407418) {
H_26(state, unused, out_2375517699020407418);
}
void car_h_27(double *state, double *unused, double *out_7701482271649087111) {
h_27(state, unused, out_7701482271649087111);
void car_h_27(double *state, double *unused, double *out_3470593801716284810) {
h_27(state, unused, out_3470593801716284810);
}
void car_H_27(double *state, double *unused, double *out_5911240451432872703) {
H_27(state, unused, out_5911240451432872703);
void car_H_27(double *state, double *unused, double *out_938116639090016175) {
H_27(state, unused, out_938116639090016175);
}
void car_h_29(double *state, double *unused, double *out_7544295603297957966) {
h_29(state, unused, out_7544295603297957966);
void car_h_29(double *state, double *unused, double *out_7965145529649909723) {
h_29(state, unused, out_7965145529649909723);
}
void car_H_29(double *state, double *unused, double *out_3226245795318055608) {
H_29(state, unused, out_3226245795318055608);
void car_H_29(double *state, double *unused, double *out_1746878017024800920) {
H_29(state, unused, out_1746878017024800920);
}
void car_h_28(double *state, double *unused, double *out_3095916628749130163) {
h_28(state, unused, out_3095916628749130163);
void car_h_28(double *state, double *unused, double *out_5501663824664413576) {
h_28(state, unused, out_5501663824664413576);
}
void car_H_28(double *state, double *unused, double *out_8308644812387586182) {
H_28(state, unused, out_8308644812387586182);
void car_H_28(double *state, double *unused, double *out_3335521000044729654) {
H_28(state, unused, out_3335521000044729654);
}
void car_h_31(double *state, double *unused, double *out_4956510669045411681) {
h_31(state, unused, out_4956510669045411681);
void car_h_31(double *state, double *unused, double *out_5392762942373062487) {
h_31(state, unused, out_5392762942373062487);
}
void car_H_31(double *state, double *unused, double *out_7824222554462447497) {
H_31(state, unused, out_7824222554462447497);
void car_H_31(double *state, double *unused, double *out_1396631581730609234) {
H_31(state, unused, out_1396631581730609234);
}
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large Load Diff
Binary file not shown.
Binary file not shown.
+1 -1
View File
@@ -93,7 +93,7 @@ def register(show_spinner=False, register_konik=False) -> str | None:
if show_spinner:
spinner.close()
if not register_konik and dongle_id != params.get("KonikDongleId", encoding="utf8"):
if dongle_id and not register_konik:
params.put("DongleId", dongle_id)
params.put("StockDongleId", dongle_id)
set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
Binary file not shown.
Binary file not shown.

Before

Width:  |  Height:  |  Size: 131 B

After

Width:  |  Height:  |  Size: 402 KiB

+11 -6
View File
@@ -1,13 +1,18 @@
[
{
"name": "boot",
"url": "https://boot.frogpilot.download",
"url": "https://www.dropbox.com/scl/fi/z8gcamb7n78xqb515kfgq/boot.img.xz?rlkey=r2zxothb3pz0q9rtqysr1zhwv&st=f0acze3w&dl=1",
"hash": "b997aae3f1c93de82449ef7f23f30ff482b0978f3d0ac08219366f9ce362ad7a",
"hash_raw": "b997aae3f1c93de82449ef7f23f30ff482b0978f3d0ac08219366f9ce362ad7a",
"size": 16029696,
"sparse": false,
"full_check": true,
"has_ab": true
"has_ab": true,
"alt": {
"hash": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f",
"url": "https://commadist.azureedge.net/agnosupdate/boot-5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f.img.xz",
"size": 16029696
}
},
{
"name": "abl",
@@ -61,7 +66,7 @@
},
{
"name": "system",
"url": "https://system.frogpilot.download",
"url": "https://www.dropbox.com/scl/fi/n22f3eex1z52dbrhhxqry/system.img.xz?rlkey=yw4ult7s3sdm6b7d31hrm3zx8&st=of6m7zis&dl=1",
"hash": "be1c6bb9ee5e06779087b1b81e09b6df61d942566b0f8d4539c452179c661782",
"hash_raw": "a5f84e68d199466fda5c9aead760b90a4cd2d2ef9a418708b9794d95bb03ec5b",
"size": 10737418240,
@@ -69,9 +74,9 @@
"full_check": false,
"has_ab": true,
"alt": {
"hash": "bc11d2148f29862ee1326aca2af1cf6bbf5fed831e3f8f6b8f7a0f110dfe8d26",
"url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz",
"size": 4548070000
"hash": "328e90c62068222dfd98f71dd3f6251fcb962f082b49c6be66ab2699f5db6f4f",
"url": "https://commadist.azureedge.net/agnosupdate/system-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz",
"size": 10737418240
}
}
]
Binary file not shown.
Binary file not shown.
Binary file not shown.
+1
View File
@@ -7,6 +7,7 @@ from pathlib import Path
from sentry_sdk.integrations.threading import ThreadingIntegration
from openpilot.common.params import Params, ParamKeyType
from openpilot.system.athena.registration import is_registered_device
from openpilot.system.hardware import HARDWARE, PC
from openpilot.common.swaglog import cloudlog
from openpilot.system.version import get_build_metadata, get_version