Compare commits
25 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 235c1cab79 | |||
| eb507d5ea8 | |||
| 159ca85be2 | |||
| 40d8eb8a4e | |||
| ebf77baf33 | |||
| 7c2fa00aec | |||
| 9bd0463c64 | |||
| e247249033 | |||
| ea48457d72 | |||
| 8a4e817f6b | |||
| a8767be7b1 | |||
| 0c249d253e | |||
| 9bb9d6ed2c | |||
| ceac092b67 | |||
| 2d23f740cc | |||
| 80c20399f1 | |||
| 4f65f90567 | |||
| 0a217baf18 | |||
| b81d6fb1eb | |||
| 944fc51f8a | |||
| 086570b29f | |||
| 8face05df0 | |||
| 7231aeb5ee | |||
| d01cd48bee | |||
| 4eefdc3268 |
@@ -1 +1 @@
|
||||
/data/themes/theme_packs/duck/colors
|
||||
/data/openpilot/openpilot/frogpilot/assets/holiday_themes/easter_week/colors
|
||||
@@ -1 +1 @@
|
||||
/data/themes/theme_packs/frog-animated/distance_icons
|
||||
/data/openpilot/openpilot/frogpilot/assets/stock_theme/distance_icons
|
||||
@@ -1 +1 @@
|
||||
/data/themes/theme_packs/duck/icons
|
||||
/data/openpilot/openpilot/frogpilot/assets/holiday_themes/easter_week/icons
|
||||
@@ -1 +1 @@
|
||||
/data/themes/theme_packs/duck/signals
|
||||
/data/openpilot/openpilot/frogpilot/assets/holiday_themes/easter_week/signals
|
||||
@@ -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
|
||||
|
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
|
||||
}
|
||||
}
|
||||
|
||||
|
After Width: | Height: | Size: 88 KiB |
|
Before Width: | Height: | Size: 8.5 KiB |
|
After Width: | Height: | Size: 54 KiB |
|
Before Width: | Height: | Size: 13 KiB |
|
After Width: | Height: | Size: 64 KiB |
|
Before Width: | Height: | Size: 11 KiB |
|
After Width: | Height: | Size: 29 KiB |
|
Before Width: | Height: | Size: 13 KiB |
|
Before Width: | Height: | Size: 26 KiB After Width: | Height: | Size: 32 KiB |
|
Before Width: | Height: | Size: 40 KiB After Width: | Height: | Size: 37 KiB |
|
Before Width: | Height: | Size: 31 KiB After Width: | Height: | Size: 2.7 KiB |
|
Before Width: | Height: | Size: 41 KiB After Width: | Height: | Size: 44 KiB |
|
Before Width: | Height: | Size: 51 KiB After Width: | Height: | Size: 53 KiB |
|
Before Width: | Height: | Size: 54 KiB After Width: | Height: | Size: 58 KiB |
|
Before Width: | Height: | Size: 56 KiB After Width: | Height: | Size: 60 KiB |
|
Before Width: | Height: | Size: 41 KiB After Width: | Height: | Size: 44 KiB |
|
Before Width: | Height: | Size: 54 KiB After Width: | Height: | Size: 53 KiB |
|
Before Width: | Height: | Size: 35 KiB After Width: | Height: | Size: 37 KiB |
|
Before Width: | Height: | Size: 42 KiB After Width: | Height: | Size: 63 KiB |
|
After Width: | Height: | Size: 371 KiB |
|
Before Width: | Height: | Size: 568 KiB After Width: | Height: | Size: 503 KiB |
@@ -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"))
|
||||
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
|
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,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
|
||||
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
|
||||
|
Before Width: | Height: | Size: 108 KiB After Width: | Height: | Size: 418 KiB |
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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]]] = {
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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})
|
||||
|
||||
@@ -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']
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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"""
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
Before Width: | Height: | Size: 131 B After Width: | Height: | Size: 402 KiB |
@@ -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
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -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
|
||||
|
||||