mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-09-30 03:13:41 +08:00
dragonpilot 2022-09-15T06:50:26 for EON/C2
version: dragonpilot v0.8.17 beta for EON/C2 date: 2022-09-15T06:50:26 dp-dev(priv2) master commit: 4d0dc8f4278758e1c000fbbef41ad124187335e2
This commit is contained in:
Dragonpilot Team
+40
-8
@@ -1,13 +1,14 @@
|
||||
dragonpilot [latest] - EON/C2 Release
|
||||
========================
|
||||
* NEW: Synced to openpilot master 2022.08.29 commits.
|
||||
* Nuclear Grade driving model.
|
||||
* TWEAK: Used fused location service on mapd.
|
||||
* TWEAK: Used fused location service on gpxd.
|
||||
* NEW: Added back auto boot on C2.
|
||||
* NEW: TOYOTA - Door auto lock/unlock. (Thanks to @AlexandreSato and @cydia2020)
|
||||
* NEW: Added Camera based Driver Monitor back. (This may overheat your device).
|
||||
* NEW: Added a toggle to turn on/off Camera based Driver Monitor.
|
||||
* Synced with openpilot master 2022.09.01 commits.
|
||||
* e2e longitudinal toggle. (Extremely alpha feature)
|
||||
* Deprecated SNPE lib and switched to tinygrad.
|
||||
* NEW: Added back nav support on EON/C2.
|
||||
* WARNING: DUE TO THE INTRODUCTION OF END TO END LONGITUDINAL FEATURE, FOLLOWING FEATURES HAS BEEN CHANGED:
|
||||
* WIP: Dynamic Follow has been temporary removed for refactoring.
|
||||
* WIP: Vision Turn Control has been temporary removed for refactoring.
|
||||
* REMOVED: Camera offset has been removed permanently.
|
||||
* EON/C2 WILL NO LONGER RECEIVE ANY DRIVING/DRIVER MONITORING MODEL UPDATES.
|
||||
|
||||
dragonpilot 2022.08.29 - EON/C2 Release
|
||||
========================
|
||||
@@ -40,6 +41,37 @@ dragonpilot 2022.07.29 - EON/C2 Release
|
||||
|
||||
dragonpilot 2022.07.20 - EON/C2 Release
|
||||
========================
|
||||
* Synced with openpilot master 2022.09.09 commits.
|
||||
* ADDED: us-west coast can now use mapd without data. See toggle description for more detail.
|
||||
* READD: Temp check toggl e, max speed toggle and couple other UI toggles.
|
||||
* bug fix and improvement
|
||||
|
||||
dragonpilot [2022.09.04]
|
||||
* Synced with openpilot master 2022.09.01 commits.
|
||||
* Refactor: Dynamic Follow has been removerd temporary. Needs a refactor. (wip)
|
||||
* ADDED: Auto Toyota door lock/unlock **only works on some car**
|
||||
* READDED: Camera offset toggle.
|
||||
* Improvement: gpxd now uses liveLocationKalman better performance.
|
||||
* bug fix and improvement.
|
||||
|
||||
dragonpilot [2022.08.07]
|
||||
========================
|
||||
Synced to openpilot master 2022.08.07 commits.
|
||||
NEW: Added back auto shutdown toggle.
|
||||
NEW: Added back LQR tune toggle.
|
||||
NEW: MAZDA - Ability to enable/disable below steer speed alert. (Thanks to @TheCrowd)
|
||||
TWEAKED: Panda Recovery should kill boardd process first.
|
||||
FIXED: Fixed stop working issue when mapd crashed.
|
||||
|
||||
dragonpilot 2022.07.27
|
||||
========================
|
||||
NEW: Synced to openpilot 2022.07.27 commits.
|
||||
NEW: Included Mazda vehicles.
|
||||
TWEAK: Only show road name bar when it's not empty.
|
||||
TWEAK: Enlarged lead car distance text.
|
||||
TWEAK: TOYOTA - Updated lowest speed override feature.
|
||||
FIXED: Fixed Panda Flash/Recovery buttons.
|
||||
FIXED: Included dp_patcher.py execution.
|
||||
* FIXED: Included correct camera/path offset values for EON/C2.
|
||||
* FIXED: Param bug in HKG. (Thanks to @sunnyhaibin).
|
||||
* FIXED: Included fan controller for UNO (C2).
|
||||
|
||||
@@ -50,7 +50,7 @@ We have detailed instructions for [how to mount the device in a car](https://com
|
||||
Running on PC
|
||||
------
|
||||
|
||||
All of openpilot's services can run as normal on a PC, even without special hardware or a car. To develop or experiment with openpilot you can run openpilot on recorded or simulated data.
|
||||
All openpilot services can run as usual on a PC without requiring special hardware or a car. You can also run openpilot on recorded or simulated data to develop or experiment with openpilot.
|
||||
|
||||
With openpilot's tools, you can plot logs, replay drives, and watch the full-res camera streams. See [the tools README](tools/README.md) for more information.
|
||||
|
||||
@@ -119,7 +119,7 @@ Directory Structure
|
||||
├── debug # Tools to help you debug and do car ports
|
||||
├── locationd # Precise localization and vehicle parameter estimation
|
||||
├── loggerd # Logger and uploader of car data
|
||||
├── manager # Deamon that starts/stops all other daemons as needed
|
||||
├── manager # Daemon that starts/stops all other daemons as needed
|
||||
├── modeld # Driving and monitoring model runners
|
||||
├── monitoring # Daemon to determine driver attention
|
||||
├── navd # Turn-by-turn navigation
|
||||
|
||||
+3
-3
@@ -78,7 +78,7 @@ Version 0.8.14 (2022-06-01)
|
||||
Version 0.8.13 (2022-02-18)
|
||||
========================
|
||||
* Improved driver monitoring
|
||||
* Retuned driver pose learner for relaxed driving positions
|
||||
* Re-tuned driver pose learner for relaxed driving positions
|
||||
* Added reliance on driving model to be more scene adaptive
|
||||
* Matched strictness between comma two and comma three
|
||||
* Improved performance in turns by compensating for the road bank angle
|
||||
@@ -224,7 +224,7 @@ Version 0.8.4 (2021-05-17)
|
||||
* Delay controls start until system is ready
|
||||
* Fuzzy car identification, enabled with Community Features toggle
|
||||
* Localizer optimized for increased precision and less CPU usage
|
||||
* Retuned lateral control to be more aggressive when model is confident
|
||||
* Re-tuned lateral control to be more aggressive when model is confident
|
||||
* Toyota Mirai 2021 support
|
||||
* Lexus NX 300 2020 support thanks to goesreallyfast!
|
||||
* Volkswagen Atlas 2018-19 support thanks to jyoung8607!
|
||||
@@ -389,7 +389,7 @@ Version 0.7 (2019-12-13)
|
||||
* Improve GM longitudinal control: proper computations for 15Hz radar
|
||||
* Move GM port, Toyota with DSU removed, comma pedal in community features; toggle switch required
|
||||
* Remove upload over cellular toggle: only upload qlog and qcamera files if not on wifi
|
||||
* Refactor Panda code towards ISO26262 and SIL2 compliancy
|
||||
* Refactor Panda code towards ISO26262 and SIL2 compliance
|
||||
* Forward stock FCW for Honda Nidec
|
||||
* Volkswagen port now standard: comma Harness intercepts stock camera
|
||||
|
||||
|
||||
+19
-13
@@ -350,8 +350,7 @@ struct CarControl {
|
||||
off @0;
|
||||
pid @1;
|
||||
stopping @2;
|
||||
|
||||
startingDEPRECATED @3;
|
||||
starting @3;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -427,20 +426,17 @@ struct CarParams {
|
||||
enableApgs @6 :Bool; # advanced parking guidance system
|
||||
enableBsm @56 :Bool; # blind spot monitoring
|
||||
flags @64 :UInt32; # flags for car specific quirks
|
||||
experimentalLongitudinalAvailable @71 :Bool;
|
||||
|
||||
minEnableSpeed @7 :Float32;
|
||||
minSteerSpeed @8 :Float32;
|
||||
maxSteeringAngleDeg @54 :Float32;
|
||||
safetyConfigs @62 :List(SafetyConfig);
|
||||
alternativeExperience @65 :Int16; # panda flag for features like no disengage on gas
|
||||
maxLateralAccel @68 :Float32;
|
||||
|
||||
steerMaxBPDEPRECATED @11 :List(Float32);
|
||||
steerMaxVDEPRECATED @12 :List(Float32);
|
||||
gasMaxBPDEPRECATED @13 :List(Float32);
|
||||
gasMaxVDEPRECATED @14 :List(Float32);
|
||||
brakeMaxBPDEPRECATED @15 :List(Float32);
|
||||
brakeMaxVDEPRECATED @16 :List(Float32);
|
||||
# Car docs fields
|
||||
maxLateralAccel @68 :Float32;
|
||||
autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically
|
||||
|
||||
# things about the car in the manual
|
||||
mass @17 :Float32; # [kg] curb weight: all fluids no cargo
|
||||
@@ -469,11 +465,13 @@ struct CarParams {
|
||||
vEgoStopping @29 :Float32; # Speed at which the car goes into stopping state
|
||||
vEgoStarting @59 :Float32; # Speed at which the car goes into starting state
|
||||
directAccelControl @30 :Bool; # Does the car have direct accel control or just gas/brake
|
||||
stoppingControl @31 :Bool; # Does the car allows full control even at lows speeds when stopping
|
||||
stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary
|
||||
stoppingControl @31 :Bool; # Does the car allow full control even at lows speeds when stopping
|
||||
steerControlType @34 :SteerControlType;
|
||||
radarOffCan @35 :Bool; # True when radar objects aren't visible on CAN
|
||||
stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary
|
||||
stoppingDecelRate @52 :Float32; # m/s^2/s while trying to stop
|
||||
startAccel @32 :Float32; # Required acceleration to get car moving
|
||||
startingState @70 :Bool; # Does this car make use of special starting state
|
||||
|
||||
steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
|
||||
longitudinalActuatorDelayLowerBound @61 :Float32; # Gas/Brake actuator delay in seconds, lower bound
|
||||
@@ -617,7 +615,7 @@ struct CarParams {
|
||||
|
||||
enum Ecu {
|
||||
eps @0;
|
||||
esp @1;
|
||||
abs @1;
|
||||
fwdRadar @2;
|
||||
fwdCamera @3;
|
||||
engine @4;
|
||||
@@ -638,6 +636,9 @@ struct CarParams {
|
||||
electricBrakeBooster @15;
|
||||
shiftByWire @16;
|
||||
|
||||
# Chrysler only
|
||||
hcp @18; # Hybrid Control Processor
|
||||
|
||||
debug @17;
|
||||
}
|
||||
|
||||
@@ -660,7 +661,12 @@ struct CarParams {
|
||||
safetyModelDEPRECATED @9 :SafetyModel;
|
||||
safetyModelPassiveDEPRECATED @42 :SafetyModel = silent;
|
||||
minSpeedCanDEPRECATED @51 :Float32;
|
||||
startAccelDEPRECATED @32 :Float32;
|
||||
communityFeatureDEPRECATED @46: Bool;
|
||||
startingAccelRateDEPRECATED @53 :Float32;
|
||||
steerMaxBPDEPRECATED @11 :List(Float32);
|
||||
steerMaxVDEPRECATED @12 :List(Float32);
|
||||
gasMaxBPDEPRECATED @13 :List(Float32);
|
||||
gasMaxVDEPRECATED @14 :List(Float32);
|
||||
brakeMaxBPDEPRECATED @15 :List(Float32);
|
||||
brakeMaxVDEPRECATED @16 :List(Float32);
|
||||
}
|
||||
|
||||
+3
-3
@@ -27,9 +27,9 @@ struct DragonConf {
|
||||
dpToyotaCruiseOverrideSpeed @19 :UInt8;
|
||||
dpToyotaAutoLock @20 :Bool;
|
||||
dpToyotaAutoUnlock @21 :Bool;
|
||||
dpUseLanelines @22 :Bool;
|
||||
dpMapd @23 :Bool;
|
||||
dpMapd @22 :Bool;
|
||||
dpLocalDb @23 :Bool;
|
||||
dpDashcamd @24 :Bool;
|
||||
dpMazdaSteerAlert @25 :Bool;
|
||||
dpCameraOffset @26 :Int8;
|
||||
dpSpeedCheck @26 :Bool;
|
||||
}
|
||||
|
||||
Binary file not shown.
+10
-10
@@ -291,7 +291,6 @@ struct CanData {
|
||||
}
|
||||
|
||||
struct DeviceState @0xa4d8b5af2aa492eb {
|
||||
usbOnline @12 :Bool;
|
||||
networkType @22 :NetworkType;
|
||||
networkInfo @31 :NetworkInfo;
|
||||
networkStrength @24 :NetworkStrength;
|
||||
@@ -309,10 +308,6 @@ struct DeviceState @0xa4d8b5af2aa492eb {
|
||||
cpuUsagePercent @34 :List(Int8); # per-core cpu usage
|
||||
|
||||
# power
|
||||
batteryPercent @8 :Int16;
|
||||
batteryCurrent @15 :Int32;
|
||||
chargingError @17 :Bool;
|
||||
chargingDisabled @18 :Bool;
|
||||
offroadPowerUsageUwh @23 :UInt32;
|
||||
carBatteryCapacityUwh @25 :UInt32;
|
||||
powerDrawW @40 :Float32;
|
||||
@@ -389,6 +384,11 @@ struct DeviceState @0xa4d8b5af2aa492eb {
|
||||
batteryStatusDEPRECATED @9 :Text;
|
||||
batteryVoltageDEPRECATED @16 :Int32;
|
||||
batteryTempCDEPRECATED @29 :Float32;
|
||||
batteryPercent @8 :Int16;
|
||||
batteryCurrent @15 :Int32;
|
||||
chargingError @17 :Bool;
|
||||
chargingDisabled @18 :Bool;
|
||||
usbOnline @12 :Bool;
|
||||
}
|
||||
|
||||
struct PandaState @0xa7649e2575e4591e {
|
||||
@@ -479,8 +479,8 @@ struct PeripheralState {
|
||||
voltage @1 :UInt32;
|
||||
current @2 :UInt32;
|
||||
fanSpeedRpm @3 :UInt16;
|
||||
usbPowerMode @4 :UsbPowerMode;
|
||||
|
||||
usbPowerMode @4 :UsbPowerMode;
|
||||
enum UsbPowerMode @0xa8883583b32c9877 {
|
||||
none @0;
|
||||
client @1;
|
||||
@@ -953,11 +953,11 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
|
||||
|
||||
struct LateralPlan @0xe1e9318e2ae8b51e {
|
||||
modelMonoTime @31 :UInt64;
|
||||
laneWidth @0 :Float32;
|
||||
lProb @5 :Float32;
|
||||
rProb @7 :Float32;
|
||||
laneWidthDEPRECATED @0 :Float32;
|
||||
lProbDEPRECATED @5 :Float32;
|
||||
rProbDEPRECATED @7 :Float32;
|
||||
dPathPoints @20 :List(Float32);
|
||||
dProb @21 :Float32;
|
||||
dProbDEPRECATED @21 :Float32;
|
||||
|
||||
#dp
|
||||
dPathWLinesX @32 :List(Float32);
|
||||
|
||||
Binary file not shown.
+9
-8
@@ -5,7 +5,6 @@ import sys
|
||||
import json
|
||||
import time
|
||||
from math import floor
|
||||
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
|
||||
|
||||
'''
|
||||
* type: Bool, Int8, UInt8, UInt16, Float32
|
||||
@@ -49,7 +48,7 @@ confs = [
|
||||
# lateral - alc
|
||||
{'name': 'dp_lateral_mode', 'default': 1, 'type': 'UInt8', 'min': 0, 'max': 2, 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_signal_off_delay', 'default': 3., 'type': 'Float32', 'min': 0., 'max': 10., 'depends': [{'name': 'dp_lateral_mode', 'vals': [0]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_lc_min_mph', 'default': 30, 'type': 'UInt8', 'min': 0, 'max': 255, 'depends': [{'name': 'dp_lateral_mode', 'vals': [1, 2]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_lc_min_mph', 'default': 15, 'type': 'UInt8', 'min': 0, 'max': 255, 'depends': [{'name': 'dp_lateral_mode', 'vals': [1, 2]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_lc_auto_min_mph', 'default': 40, 'type': 'UInt8', 'min': 0, 'max': 255, 'depends': [{'name': 'dp_lateral_mode', 'vals': [2]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_lc_auto_delay', 'default': 3., 'type': 'Float32', 'min': 0., 'max': 10., 'depends': [{'name': 'dp_lateral_mode', 'vals': [2]}], 'conf_type': ['param', 'struct']},
|
||||
|
||||
@@ -75,8 +74,9 @@ confs = [
|
||||
{'name': 'dp_toyota_auto_lock', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_toyota_auto_unlock', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
|
||||
{'name': 'dp_use_lanelines', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_mapd', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_local_db', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
|
||||
|
||||
{'name': 'dp_lateral_lqr', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
|
||||
@@ -94,6 +94,11 @@ confs = [
|
||||
# {'name': 'dp_uploader', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct'], 'update_once': True},
|
||||
# # {'name': 'dp_gpxd', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_hotspot_on_boot', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_honda_eps_mod', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_dm', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_speed_check', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_temp_check', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_vag_resume_fix', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# # lat ctrl
|
||||
# {'name': 'dp_lane_less_mode_ctrl', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_lane_less_mode', 'default': 2, 'type': 'UInt8', 'min': 0, 'max': 2, 'depends': [{'name': 'dp_lane_less_mode_ctrl', 'vals': [True]}], 'conf_type': ['param', 'struct']},
|
||||
@@ -105,8 +110,6 @@ confs = [
|
||||
# {'name': 'dp_accel_profile', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 2, 'depends': [{'name': 'dp_accel_profile_ctrl', 'vals': [True]}], 'conf_type': ['param', 'struct']},
|
||||
# # safety
|
||||
# {'name': 'dp_gear_check', 'default': True, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_speed_check', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_temp_monitor', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# # UIs
|
||||
# {'name': 'dp_ui_display_mode', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 1, 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_ui_speed', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
@@ -134,7 +137,6 @@ confs = [
|
||||
# # hyundai
|
||||
# {'name': 'dp_hkg_smart_mdps', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# # honda
|
||||
{'name': 'dp_honda_eps_mod', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# {'name': 'dp_honda_kmh_display', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# # volkswagen
|
||||
# # {'name': 'dp_vw_panda', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
@@ -142,7 +144,7 @@ confs = [
|
||||
# {'name': 'dp_ip_addr', 'default': '', 'type': 'Text', 'conf_type': ['struct']},
|
||||
# {'name': 'dp_fan_mode', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 2, 'conf_type': ['param']},
|
||||
# {'name': 'dp_last_modified', 'default': str(floor(time.time())), 'type': 'Text', 'conf_type': ['param']},
|
||||
{'name': 'dp_camera_offset', 'default': CAMERA_OFFSET, 'type': 'Int8', 'min': -100, 'max': 100, 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_camera_offset', 'default': CAMERA_OFFSET, 'type': 'Int8', 'min': -100, 'max': 100, 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_path_offset', 'default': 0 if EON else -4 if TICI else 0, 'type': 'Int8', 'min': -100, 'max': 100, 'conf_type': ['param', 'struct']},
|
||||
#
|
||||
# {'name': 'dp_reg', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
@@ -165,7 +167,6 @@ confs = [
|
||||
#
|
||||
# {'name': 'dp_no_offroad_fix', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# {'name': 'dp_ftpd', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_dm', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
]
|
||||
|
||||
def get_definition(name):
|
||||
|
||||
+12
-1
@@ -1,3 +1,5 @@
|
||||
from typing import Optional
|
||||
|
||||
def gpio_init(pin: int, output: bool) -> None:
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f:
|
||||
@@ -5,10 +7,19 @@ def gpio_init(pin: int, output: bool) -> None:
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} direction: {e}")
|
||||
|
||||
|
||||
def gpio_set(pin: int, high: bool) -> None:
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f:
|
||||
f.write(b"1" if high else b"0")
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} value: {e}")
|
||||
|
||||
def gpio_read(pin: int) -> Optional[bool]:
|
||||
val = None
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f:
|
||||
val = bool(int(f.read().strip()))
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} value: {e}")
|
||||
|
||||
return val
|
||||
|
||||
+7
-3
@@ -4,6 +4,7 @@ from common.params import Params
|
||||
locale_dir = "/data/openpilot/selfdrive/assets/locales"
|
||||
# supported_language = ["en-US", "zh-TW", "zh-CN", "ja-JP", "ko-KR"]
|
||||
supported_languages = {
|
||||
"main_en": "en-US",
|
||||
"main_zh-CHT": "zh-TW",
|
||||
"main_zh-CHS": "zh-CN",
|
||||
"main_ko": "ko-KR",
|
||||
@@ -11,9 +12,12 @@ supported_languages = {
|
||||
|
||||
def events():
|
||||
locale = Params().get("LanguageSetting", encoding='utf8')
|
||||
if locale is not None:
|
||||
locale = supported_languages[locale.strip()]
|
||||
else:
|
||||
try:
|
||||
if locale is not None:
|
||||
locale = supported_languages[locale.strip()]
|
||||
else:
|
||||
locale = "en-US"
|
||||
except KeyError:
|
||||
locale = "en-US"
|
||||
i18n = gettext.translation("events", localedir=locale_dir, fallback=True, languages=[locale])
|
||||
i18n.install()
|
||||
|
||||
Binary file not shown.
+1
-1
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "2022.09.07"
|
||||
#define COMMA_VERSION "2022.09.15"
|
||||
|
||||
+207
-207
@@ -6,213 +6,213 @@ A supported vehicle is one that just works when you install a comma three. All s
|
||||
|
||||
# 205 Supported Cars
|
||||
|
||||
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Harness|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|
|
||||
|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[](##)|Honda Nidec|
|
||||
|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Acura|RDX 2019-22|All|Stock|0 mph|3 mph|[](##)|Honda Bosch A|
|
||||
|Audi|A3 2014-19|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Audi|Q2 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Audi|Q3 2020-21|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Audi|RS3 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Audi|S3 2015-17|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Cadillac|Escalade ESV 2016[<sup>1</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[](##)|OBD-II|
|
||||
|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|Stock|0 mph|7 mph|[](##)|GM|
|
||||
|Chevrolet|Silverado 1500 2020-21|Safety Package II|Stock|0 mph|7 mph|[](##)|GM|
|
||||
|Chevrolet|Volt 2017-18[<sup>1</sup>](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|7 mph|[](##)|OBD-II|
|
||||
|Chrysler|Pacifica 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[](##)|FCA|
|
||||
|Chrysler|Pacifica 2019-20|Adaptive Cruise Control|Stock|0 mph|39 mph|[](##)|FCA|
|
||||
|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[](##)|FCA|
|
||||
|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[](##)|FCA|
|
||||
|Chrysler|Pacifica Hybrid 2019-22|Adaptive Cruise Control|Stock|0 mph|39 mph|[](##)|FCA|
|
||||
|comma|body|All|Stock|0 mph|0 mph|[](##)|None|
|
||||
|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[](##)|Hyundai F|
|
||||
|Genesis|G70 2020|All|openpilot|0 mph|0 mph|[](##)|Hyundai F|
|
||||
|Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Genesis|G90 2017-18|All|Stock|0 mph|0 mph|[](##)|Hyundai C|
|
||||
|GMC|Acadia 2018[<sup>1</sup>](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|7 mph|[](##)|OBD-II|
|
||||
|GMC|Sierra 1500 2020-21|Driver Alert Package II|Stock|0 mph|7 mph|[](##)|GM|
|
||||
|Honda|Accord 2018-22|All|Stock|0 mph|3 mph|[](##)|Honda Bosch A|
|
||||
|Honda|Accord Hybrid 2018-22|All|Stock|0 mph|3 mph|[](##)|Honda Bosch A|
|
||||
|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Honda|Civic 2019-21|All|Stock|0 mph|2 mph[<sup>2</sup>](#footnotes)|[](##)|Honda Bosch A|
|
||||
|Honda|Civic 2022|All|Stock|0 mph|0 mph|[](##)|Honda Bosch B|
|
||||
|Honda|Civic Hatchback 2017-21|Honda Sensing|Stock|0 mph|12 mph|[](##)|Honda Bosch A|
|
||||
|Honda|Civic Hatchback 2022|All|Stock|0 mph|0 mph|[](##)|Honda Bosch B|
|
||||
|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Honda|CR-V 2017-22|Honda Sensing|Stock|0 mph|12 mph|[](##)|Honda Bosch A|
|
||||
|Honda|CR-V Hybrid 2017-19|Honda Sensing|Stock|0 mph|12 mph|[](##)|Honda Bosch A|
|
||||
|Honda|e 2020|All|Stock|0 mph|3 mph|[](##)|Honda Bosch A|
|
||||
|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Honda|Insight 2019-22|All|Stock|0 mph|3 mph|[](##)|Honda Bosch A|
|
||||
|Honda|Inspire 2018|All|Stock|0 mph|3 mph|[](##)|Honda Bosch A|
|
||||
|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[](##)|Honda Nidec|
|
||||
|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Honda|Ridgeline 2017-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|Honda Nidec|
|
||||
|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|32 mph|[](##)|Hyundai B|
|
||||
|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai K|
|
||||
|Hyundai|Elantra Hybrid 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|Hyundai K|
|
||||
|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|37 mph|[](##)|Hyundai J|
|
||||
|Hyundai|Ioniq 5 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[](##)|Hyundai Q|
|
||||
|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Electric 2020|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC) & LFA|Stock|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2020-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|Hyundai B|
|
||||
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai G|
|
||||
|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|Hyundai O|
|
||||
|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai I|
|
||||
|Hyundai|Palisade 2020-22|All|openpilot|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Hyundai|Santa Fe 2019-20|All|openpilot|0 mph|0 mph|[](##)|Hyundai D|
|
||||
|Hyundai|Santa Fe 2021-22|All|Stock|0 mph|0 mph|[](##)|Hyundai L|
|
||||
|Hyundai|Santa Fe Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|Hyundai L|
|
||||
|Hyundai|Santa Fe Plug-in Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|Hyundai L|
|
||||
|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai E|
|
||||
|Hyundai|Sonata 2020-22|All|openpilot|0 mph|0 mph|[](##)|Hyundai A|
|
||||
|Hyundai|Sonata Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|Hyundai A|
|
||||
|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|Stock|19 mph|0 mph|[](##)|Hyundai L|
|
||||
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|Hyundai L|
|
||||
|Hyundai|Tucson Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|Hyundai N|
|
||||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[](##)|Hyundai E|
|
||||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[](##)|FCA|
|
||||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control|Stock|0 mph|39 mph|[](##)|FCA|
|
||||
|Kia|Ceed 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai E|
|
||||
|Kia|EV6 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[](##)|Hyundai P|
|
||||
|Kia|Forte 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai B|
|
||||
|Kia|Forte 2019-21|All|Stock|0 mph|0 mph|[](##)|Hyundai G|
|
||||
|Kia|K5 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|Hyundai A|
|
||||
|Kia|Niro Electric 2019|All|openpilot|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Kia|Niro Electric 2020|All|openpilot|0 mph|0 mph|[](##)|Hyundai F|
|
||||
|Kia|Niro Electric 2021|All|openpilot|0 mph|0 mph|[](##)|Hyundai C|
|
||||
|Kia|Niro Electric 2022|All|openpilot|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai F|
|
||||
|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Kia|Niro Plug-in Hybrid 2018-19|Smart Cruise Control (SCC) & LKAS|openpilot|10 mph|32 mph|[](##)|Hyundai C|
|
||||
|Kia|Optima 2017|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[](##)|Hyundai B|
|
||||
|Kia|Optima 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai G|
|
||||
|Kia|Seltos 2021|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|Hyundai A|
|
||||
|Kia|Sorento 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai C|
|
||||
|Kia|Sorento 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai E|
|
||||
|Kia|Stinger 2018-20|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|Hyundai C|
|
||||
|Kia|Telluride 2020|All|openpilot|0 mph|0 mph|[](##)|Hyundai H|
|
||||
|Lexus|CT Hybrid 2017-18|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|ES Hybrid 2017-18|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|ES Hybrid 2019-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|NX 2018-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|NX Hybrid 2018-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|RX 2016-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|RX Hybrid 2016-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Mazda|CX-5 2022|All|Stock|0 mph|0 mph|[](##)|Mazda|
|
||||
|Mazda|CX-9 2021-22|All|Stock|0 mph|28 mph|[](##)|Mazda|
|
||||
|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|Nissan B|
|
||||
|Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|Nissan A|
|
||||
|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|Nissan A|
|
||||
|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|Nissan A|
|
||||
|Ram|1500 2019-22|Adaptive Cruise Control|Stock|0 mph|32 mph|[](##)|Ram|
|
||||
|SEAT|Ateca 2018|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|SEAT|Leon 2014-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[](##)|Subaru A|
|
||||
|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|Subaru A|
|
||||
|Subaru|Crosstrek 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|Subaru A|
|
||||
|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[](##)|Subaru A|
|
||||
|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|Subaru A|
|
||||
|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|Subaru A|
|
||||
|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[](##)|Subaru B|
|
||||
|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[](##)|Subaru B|
|
||||
|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|Subaru A|
|
||||
|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|Subaru A|
|
||||
|Škoda|Kamiq 2021[<sup>5</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Škoda|Karoq 2019-21[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Škoda|Kodiaq 2018-19|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Škoda|Octavia 2015, 2018-19|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Škoda|Octavia RS 2016|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Škoda|Scala 2020|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Škoda|Superb 2015-18|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Avalon 2016|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Avalon 2017-18|All|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Avalon 2019-21|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Avalon Hybrid 2019-21|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|C-HR 2017-21|All|Stock|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Camry 2018-20|All|Stock|0 mph[<sup>4</sup>](#footnotes)|0 mph|[](##)|Toyota|
|
||||
|Toyota|Camry 2021-22|All|openpilot|0 mph[<sup>4</sup>](#footnotes)|0 mph|[](##)|Toyota|
|
||||
|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Camry Hybrid 2021-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Corolla 2017-19|All|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Corolla Cross (Non-US only) 2020-21|All|openpilot|17 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Highlander 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Highlander 2020-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Highlander Hybrid 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Highlander Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Prius 2016|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Prius 2017-20|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Prius Prime 2017-20|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Prius v 2017|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|RAV4 2016|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|RAV4 2017-18|All|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2017-18|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|Toyota|
|
||||
|Toyota|Sienna 2018-20|All|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|Toyota|
|
||||
|Volkswagen|Arteon 2018-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Arteon eHybrid 2020-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Arteon R 2020-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Atlas 2018-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Atlas Cross Sport 2021-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|California 2021[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[](##)|J533|
|
||||
|Volkswagen|Caravelle 2020[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[](##)|J533|
|
||||
|Volkswagen|CC 2018-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|e-Golf 2014-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Volkswagen|Golf 2015-20[<sup>8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Volkswagen|Golf Alltrack 2015-19|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Volkswagen|Golf GTD 2015-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Volkswagen|Golf GTE 2015-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Volkswagen|Golf GTI 2015-21|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Volkswagen|Golf R 2015-19[<sup>8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Volkswagen|Golf SportsVan 2015-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Volkswagen|Jetta 2018-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Jetta GLI 2021-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Passat 2015-22[<sup>6,7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Passat Alltrack 2015-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Passat GTE 2015-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Polo 2020-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Polo GTI 2020-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|T-Cross 2021[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|T-Roc 2021[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Taos 2022[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Teramont 2018-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Teramont Cross Sport 2021-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Teramont X 2021-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Tiguan 2019-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|J533|
|
||||
|Volkswagen|Touran 2017|Driver Assistance|Stock|0 mph|0 mph|[](##)|VW|
|
||||
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|
|
||||
|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Acura|RDX 2019-22|All|openpilot|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Audi|A3 2014-19|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|Q2 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|Q3 2020-21|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|RS3 2018|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|S3 2015-17|ACC + Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Cadillac|Escalade ESV 2016[<sup>1</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 mph|[](##)|[](##)|OBD-II|
|
||||
|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|Stock|0 mph|6 mph|[](##)|[](##)|GM|
|
||||
|Chevrolet|Silverado 1500 2020-21|Safety Package II|Stock|0 mph|6 mph|[](##)|[](##)|GM|
|
||||
|Chevrolet|Volt 2017-18[<sup>1</sup>](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|6 mph|[](##)|[](##)|OBD-II|
|
||||
|Chrysler|Pacifica 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[](##)|[](##)|FCA|
|
||||
|Chrysler|Pacifica 2019-20|Adaptive Cruise Control|Stock|0 mph|39 mph|[](##)|[](##)|FCA|
|
||||
|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[](##)|[](##)|FCA|
|
||||
|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[](##)|[](##)|FCA|
|
||||
|Chrysler|Pacifica Hybrid 2019-22|Adaptive Cruise Control|Stock|0 mph|39 mph|[](##)|[](##)|FCA|
|
||||
|comma|body|All|openpilot|0 mph|0 mph|[](##)|[](##)|None|
|
||||
|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
||||
|Genesis|G70 2020|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
||||
|Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Genesis|G90 2017-18|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
||||
|GMC|Acadia 2018[<sup>1</sup>](#footnotes)|Adaptive Cruise Control|openpilot|0 mph|6 mph|[](##)|[](##)|OBD-II|
|
||||
|GMC|Sierra 1500 2020-21|Driver Alert Package II|Stock|0 mph|6 mph|[](##)|[](##)|GM|
|
||||
|Honda|Accord 2018-22|All|openpilot|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Accord Hybrid 2018-22|All|openpilot|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Honda|Civic 2019-21|All|openpilot|0 mph|2 mph[<sup>2</sup>](#footnotes)|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Civic 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|Honda Bosch B|
|
||||
|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot|0 mph|12 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Civic Hatchback 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|Honda Bosch B|
|
||||
|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Honda|CR-V 2017-22|Honda Sensing|openpilot|0 mph|12 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot|0 mph|12 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|e 2020|All|openpilot|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Honda|Insight 2019-22|All|openpilot|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Inspire 2018|All|openpilot|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Honda|Ridgeline 2017-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
||||
|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|32 mph|[](##)|[](##)|Hyundai B|
|
||||
|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Hyundai|Elantra Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC) & LKAS|Stock|19 mph|37 mph|[](##)|[](##)|Hyundai J|
|
||||
|Hyundai|Ioniq 5 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai Q|
|
||||
|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Electric 2020|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC) & LFA|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2020-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai B|
|
||||
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai G|
|
||||
|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai O|
|
||||
|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai I|
|
||||
|Hyundai|Palisade 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Hyundai|Santa Fe 2019-20|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai D|
|
||||
|Hyundai|Santa Fe 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Santa Fe Hybrid 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai E|
|
||||
|Hyundai|Sonata 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Hyundai|Sonata Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot|19 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Tucson Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai N|
|
||||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[](##)|[](##)|Hyundai E|
|
||||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control|Stock|0 mph|9 mph|[](##)|[](##)|FCA|
|
||||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control|Stock|0 mph|39 mph|[](##)|[](##)|FCA|
|
||||
|Kia|Ceed 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai E|
|
||||
|Kia|EV6 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai P|
|
||||
|Kia|Forte 2018|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai B|
|
||||
|Kia|Forte 2019-21|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai G|
|
||||
|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Kia|Niro Electric 2019|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro Electric 2020|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
||||
|Kia|Niro Electric 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|Niro Electric 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
||||
|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC) & LKAS|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro Plug-in Hybrid 2018-19|Smart Cruise Control (SCC) & LKAS|openpilot|10 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|Optima 2017|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai B|
|
||||
|Kia|Optima 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai G|
|
||||
|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Kia|Sorento 2018|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|Sorento 2019|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai E|
|
||||
|Kia|Stinger 2018-20|Smart Cruise Control (SCC) & LKAS|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|Telluride 2020|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Lexus|CT Hybrid 2017-18|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|ES Hybrid 2017-18|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|ES Hybrid 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|NX 2018-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|NX Hybrid 2018-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX 2016-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX Hybrid 2016-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Mazda|CX-5 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|Mazda|
|
||||
|Mazda|CX-9 2021-22|All|Stock|0 mph|28 mph|[](##)|[](##)|Mazda|
|
||||
|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|Nissan B|
|
||||
|Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|Nissan A|
|
||||
|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|Nissan A|
|
||||
|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|Nissan A|
|
||||
|Ram|1500 2019-22|Adaptive Cruise Control|Stock|0 mph|32 mph|[](##)|[](##)|Ram|
|
||||
|SEAT|Ateca 2018|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|SEAT|Leon 2014-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
||||
|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
||||
|Subaru|Crosstrek 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
||||
|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
||||
|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
||||
|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
||||
|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|Subaru B|
|
||||
|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|Subaru B|
|
||||
|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
||||
|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
||||
|Škoda|Kamiq 2021[<sup>5</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Karoq 2019-21[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Kodiaq 2018-19|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Octavia 2015, 2018-19|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Octavia RS 2016|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Scala 2020|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Superb 2015-18|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Avalon 2016|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Avalon 2017-18|All|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Avalon 2019-21|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Avalon Hybrid 2019-21|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|C-HR 2017-21|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Camry 2018-20|All|Stock|0 mph[<sup>4</sup>](#footnotes)|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Camry 2021-22|All|openpilot|0 mph[<sup>4</sup>](#footnotes)|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Camry Hybrid 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Corolla 2017-19|All|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Corolla Cross (Non-US only) 2020-21|All|openpilot|17 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Highlander 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Highlander 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Highlander Hybrid 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Highlander Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Prius 2016|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Prius 2017-20|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Prius Prime 2017-20|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Prius v 2017|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 2016|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 2017-18|All|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2017-18|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Sienna 2018-20|All|Stock[<sup>3</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Volkswagen|Arteon 2018-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Arteon eHybrid 2020-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Arteon R 2020-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Atlas 2018-23[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Atlas Cross Sport 2021-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|California 2021[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Caravelle 2020[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|31 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|CC 2018-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|e-Golf 2014-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf 2015-20[<sup>8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf Alltrack 2015-19|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf GTD 2015-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf GTE 2015-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf GTI 2015-21|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf R 2015-19[<sup>8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf SportsVan 2015-20|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Jetta 2018-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Jetta GLI 2021-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat 2015-22[<sup>6,7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat Alltrack 2015-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat GTE 2015-22[<sup>7,8</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Polo 2020-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Polo GTI 2020-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|T-Cross 2021[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|T-Roc 2021[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Taos 2022[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont 2018-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont Cross Sport 2021-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont X 2021-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Tiguan 2019-22[<sup>7</sup>](#footnotes)|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Touran 2017|Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|
||||
<a id="footnotes"></a>
|
||||
<sup>1</sup>Requires a <a href="https://github.com/commaai/openpilot/wiki/GM#hardware">community built ASCM harness</a>. <b><i>NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).</i></b> <br />
|
||||
|
||||
Executable
+31
@@ -0,0 +1,31 @@
|
||||
# OSM Installation instructions:
|
||||
# https://wiki.openstreetmap.org/wiki/Overpass_API/Installation
|
||||
|
||||
# Install expat. All other needed libraries are already installed.
|
||||
# g++ make expat libexpat1-dev zlib1g-dev
|
||||
#sudo apt-get update
|
||||
#sudo apt-get install expat
|
||||
|
||||
# Add required path variables to environment
|
||||
export EXEC_DIR=/data/media/0/osm/v0.7.57/
|
||||
export SOURCE_FILE_ROOT=osm-3s_v0.7.57
|
||||
export GZ_FILE=${SOURCE_FILE_ROOT}.tar.xz
|
||||
# export DB_DIR=/data/osm/db/
|
||||
|
||||
# Download and extract overpass library
|
||||
cd /data/media/0/
|
||||
mkdir osm
|
||||
cd /data/media/0/osm
|
||||
#wget http://dev.overpass-api.de/releases/$GZ_FILE
|
||||
cp -f /data/openpilot/selfdrive/mapd/assets/$GZ_FILE .
|
||||
tar -vxf $GZ_FILE
|
||||
|
||||
# Configure and install overpass
|
||||
#cd $(ls | grep $SOURCE_FILE_ROOT\.[0-9]*)
|
||||
#cd osm-3s_v0.7.56
|
||||
#./configure CXXFLAGS="-O2" --prefix=$EXEC_DIR
|
||||
#make install
|
||||
|
||||
# Remove source files after installation
|
||||
#cd ..
|
||||
mv osm-3s_v0.7.57 v0.7.57
|
||||
Executable
+27
@@ -0,0 +1,27 @@
|
||||
export OSM_DIR=/data/media/0/osm
|
||||
export DB_DIR=${OSM_DIR}/db
|
||||
export XZ_MAP_FILE_NAME=us-west.tar.xz
|
||||
export XZ_MAP_FILE=${OSM_DIR}/${XZ_MAP_FILE_NAME}
|
||||
|
||||
# Remove legacy compressed map file if existing
|
||||
rm -rf $XZ_MAP_FILE
|
||||
|
||||
# WD
|
||||
cd $OSM_DIR
|
||||
|
||||
# Download map file
|
||||
wget -O ${XZ_MAP_FILE_NAME} "https://thecorgirosie-my.sharepoint.com/:u:/p/jason_wen/EYWdXcNpOhJCuOLymUHSwH4BwdRddfiYEbZAK1ltd-hZJA?e=TW5ia4&download=1"
|
||||
|
||||
if [[ "$?" != 0 ]]; then
|
||||
echo "Error downloading map file"
|
||||
else
|
||||
echo "Successfully downloaded map file"
|
||||
# Remove current db dir if existing
|
||||
rm -rf $DB_DIR
|
||||
# Decompressing
|
||||
tar -vxf ${XZ_MAP_FILE_NAME}
|
||||
#mv us-west db
|
||||
|
||||
# Remove compressed map files after expanding
|
||||
# rm -rf $GZ_MAP_FILE_NAME
|
||||
fi
|
||||
Binary file not shown.
@@ -1126,7 +1126,8 @@ BO_ 984 IPMA_Data: 8 IPMA_ADAS
|
||||
SG_ DasStats_D_Dsply : 47|2@0+ (1,0) [0|3] "SED" GWM
|
||||
SG_ DasWarn_D_Dsply : 44|2@0+ (1,0) [0|3] "SED" GWM
|
||||
SG_ AhbHiBeam_D_Rq : 59|2@0+ (1,0) [0|3] "SED" GWM
|
||||
SG_ Set_Me_X1 : 63|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ Passthru_63 : 63|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ Passthru_48 : 48|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 985 IPMA_Data2: 8 IPMA_ADAS
|
||||
SG_ LdwChime_B_Rq : 34|1@0+ (1,0) [0|1] "SED" GWM
|
||||
@@ -3369,7 +3370,7 @@ BO_ 979 LateralMotionControl: 8 IPMA_ADAS
|
||||
SG_ LatCtlRampType_D_Rq : 53|2@0+ (1,0) [0|3] "SED" GWM
|
||||
SG_ LatCtlPrecision_D_Rq : 33|2@0+ (1,0) [0|3] "SED" GWM
|
||||
SG_ LatCtlPathOffst_L_Actl : 47|10@0+ (0.01,-5.12) [-5.12|5.11] "meter" GWM
|
||||
SG_ LatCtlPath_An_Actl : 31|11@0+ (0.0005,-0.5) [-0.5|0.5235] "radians" GWM
|
||||
SG_ LatCtlPath_An_Actl : 31|11@0+ (0.0005,-0.4995) [-0.4995|0.5240] "radians" GWM
|
||||
SG_ LatCtlCurv_NoRate_Actl : 12|13@0+ (2.5E-007,-0.001024) [-0.001024|0.00102375] "1/meter" GWM
|
||||
SG_ LatCtlCurv_No_Actl : 7|11@0+ (2E-005,-0.02) [-0.02|0.02094] "1/meter" GWM
|
||||
|
||||
|
||||
@@ -204,7 +204,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
|
||||
BO_ 1009 PCM_CRUISE_ALT: 8 XXX
|
||||
SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
|
||||
SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
|
||||
|
||||
BO_ 1020 SOLAR_SENSOR: 8 XXX
|
||||
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
|
||||
@@ -328,6 +328,7 @@ BO_ 1552 BODY_CONTROL_STATE_2: 8 XXX
|
||||
SG_ METER_SLIDER_BRIGHTNESS_PCT : 30|7@0+ (1,0) [12|100] "%" XXX
|
||||
SG_ METER_SLIDER_LOW_BRIGHTNESS : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ METER_SLIDER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ UNITS : 63|3@0+ (1,0) [1|4] "" XXX
|
||||
|
||||
BO_ 1553 UI_SETTING: 8 XXX
|
||||
SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX
|
||||
@@ -394,7 +395,7 @@ CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT.";
|
||||
CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit";
|
||||
CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
|
||||
CM_ SG_ 956 GEAR "on 6MT, only R shows.";
|
||||
CM_ SG_ 1009 SET_SPEED "units seem to be whatever the car is set to";
|
||||
CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to";
|
||||
CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
|
||||
CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
|
||||
CM_ SG_ 1042 LDA_SA_TOGGLE "LDA Steering Assist Toggle";
|
||||
@@ -427,7 +428,7 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds";
|
||||
CM_ SG_ 1163 TSRSPU "always 1";
|
||||
CM_ SG_ 1552 UI_SPEED "Vehicle's speedometer's display speed after Toyota's offset was applied, uses vehicle's unit";
|
||||
CM_ SG_ 1552 UI_SPEED "Does not appear to match dash";
|
||||
CM_ SG_ 1552 METER_SLIDER_BRIGHTNESS_PCT "Combination display brightness setting, scales from 12 per cent to 100 per cent, reflects combination meter settings only, not linked with headlight state";
|
||||
CM_ SG_ 1552 METER_SLIDER_LOW_BRIGHTNESS "Combination display low brightness mode, also controls footwell lighting";
|
||||
CM_ SG_ 1552 METER_SLIDER_DIMMED "Combination display slider not at max, reflects combination meter settings only, not linked with headlight state";
|
||||
@@ -479,6 +480,7 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
|
||||
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
|
||||
VAL_ 1552 METER_SLIDER_LOW_BRIGHTNESS 1 "Low brightness mode, footwell lights off" 0 "Normal mode, footwell lights on";
|
||||
VAL_ 1552 METER_SLIDER_DIMMED 1 "Dimmed" 0 "Not Dimmed";
|
||||
VAL_ 1552 UNITS 1 "km (km/L)" 2 "km (L/100km)" 3 "miles (MPG US)" 4 "miles (MPG Imperial)";
|
||||
VAL_ 1553 UNITS 1 "km" 2 "miles";
|
||||
VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
|
||||
VAL_ 1592 LOCK_STATUS 0 "locked" 1 "unlocked";
|
||||
|
||||
@@ -62,11 +62,14 @@ BO_ 80 LKAS: 16 XXX
|
||||
SG_ NEW_SIGNAL_2 : 70|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ SET_ME_1 : 80|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NEW_SIGNAL_3 : 111|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ FCA_SYSWARN : 40|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 96 ESP_STATUS: 32 XXX
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ ESP_DISABLED : 42|3@1+ (1,0) [0|63] "" XXX
|
||||
SG_ BRAKE_POSITION : 128|10@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ BRAKE_PRESSED : 148|1@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 101 BRAKE: 32 XXX
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
|
||||
@@ -1645,7 +1645,7 @@ CM_ "BO_ E_EMS11: All (plug-in) hybrids use this gas signal: CR_Vcu_AccPedDep_Po
|
||||
CM_ SG_ 1348 SpeedLim_Nav_Clu "Speed limit displayed on Nav, Cluster and HUD";
|
||||
|
||||
VAL_ 274 CUR_GR 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D" 7 "D" 8 "D" 14 "R" 0 "P";
|
||||
VAL_ 871 CF_Lvr_Gear 5 "D" 8 "S" 6 "N" 7 "R" 0 "P";
|
||||
VAL_ 871 CF_Lvr_Gear 12 "T" 5 "D" 8 "S" 6 "N" 7 "R" 0 "P";
|
||||
VAL_ 882 Elect_Gear_Shifter 5 "D" 8 "S" 6 "N" 7 "R" 0 "P";
|
||||
VAL_ 905 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault" 4 "cancelled";
|
||||
VAL_ 909 CF_VSM_Warn 2 "FCW" 3 "AEB";
|
||||
|
||||
@@ -249,7 +249,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
|
||||
BO_ 1009 PCM_CRUISE_ALT: 8 XXX
|
||||
SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
|
||||
SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
|
||||
|
||||
BO_ 1020 SOLAR_SENSOR: 8 XXX
|
||||
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
|
||||
@@ -373,6 +373,7 @@ BO_ 1552 BODY_CONTROL_STATE_2: 8 XXX
|
||||
SG_ METER_SLIDER_BRIGHTNESS_PCT : 30|7@0+ (1,0) [12|100] "%" XXX
|
||||
SG_ METER_SLIDER_LOW_BRIGHTNESS : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ METER_SLIDER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ UNITS : 63|3@0+ (1,0) [1|4] "" XXX
|
||||
|
||||
BO_ 1553 UI_SETTING: 8 XXX
|
||||
SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX
|
||||
@@ -439,7 +440,7 @@ CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT.";
|
||||
CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit";
|
||||
CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
|
||||
CM_ SG_ 956 GEAR "on 6MT, only R shows.";
|
||||
CM_ SG_ 1009 SET_SPEED "units seem to be whatever the car is set to";
|
||||
CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to";
|
||||
CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
|
||||
CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
|
||||
CM_ SG_ 1042 LDA_SA_TOGGLE "LDA Steering Assist Toggle";
|
||||
@@ -472,7 +473,7 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds";
|
||||
CM_ SG_ 1163 TSRSPU "always 1";
|
||||
CM_ SG_ 1552 UI_SPEED "Vehicle's speedometer's display speed after Toyota's offset was applied, uses vehicle's unit";
|
||||
CM_ SG_ 1552 UI_SPEED "Does not appear to match dash";
|
||||
CM_ SG_ 1552 METER_SLIDER_BRIGHTNESS_PCT "Combination display brightness setting, scales from 12 per cent to 100 per cent, reflects combination meter settings only, not linked with headlight state";
|
||||
CM_ SG_ 1552 METER_SLIDER_LOW_BRIGHTNESS "Combination display low brightness mode, also controls footwell lighting";
|
||||
CM_ SG_ 1552 METER_SLIDER_DIMMED "Combination display slider not at max, reflects combination meter settings only, not linked with headlight state";
|
||||
@@ -524,6 +525,7 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
|
||||
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
|
||||
VAL_ 1552 METER_SLIDER_LOW_BRIGHTNESS 1 "Low brightness mode, footwell lights off" 0 "Normal mode, footwell lights on";
|
||||
VAL_ 1552 METER_SLIDER_DIMMED 1 "Dimmed" 0 "Not Dimmed";
|
||||
VAL_ 1552 UNITS 1 "km (km/L)" 2 "km (L/100km)" 3 "miles (MPG US)" 4 "miles (MPG Imperial)";
|
||||
VAL_ 1553 UNITS 1 "km" 2 "miles";
|
||||
VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
|
||||
VAL_ 1592 LOCK_STATUS 0 "locked" 1 "unlocked";
|
||||
|
||||
@@ -250,7 +250,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
|
||||
BO_ 1009 PCM_CRUISE_ALT: 8 XXX
|
||||
SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
|
||||
SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
|
||||
|
||||
BO_ 1020 SOLAR_SENSOR: 8 XXX
|
||||
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
|
||||
@@ -374,6 +374,7 @@ BO_ 1552 BODY_CONTROL_STATE_2: 8 XXX
|
||||
SG_ METER_SLIDER_BRIGHTNESS_PCT : 30|7@0+ (1,0) [12|100] "%" XXX
|
||||
SG_ METER_SLIDER_LOW_BRIGHTNESS : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ METER_SLIDER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ UNITS : 63|3@0+ (1,0) [1|4] "" XXX
|
||||
|
||||
BO_ 1553 UI_SETTING: 8 XXX
|
||||
SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX
|
||||
@@ -440,7 +441,7 @@ CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT.";
|
||||
CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit";
|
||||
CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
|
||||
CM_ SG_ 956 GEAR "on 6MT, only R shows.";
|
||||
CM_ SG_ 1009 SET_SPEED "units seem to be whatever the car is set to";
|
||||
CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to";
|
||||
CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
|
||||
CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
|
||||
CM_ SG_ 1042 LDA_SA_TOGGLE "LDA Steering Assist Toggle";
|
||||
@@ -473,7 +474,7 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds";
|
||||
CM_ SG_ 1163 TSRSPU "always 1";
|
||||
CM_ SG_ 1552 UI_SPEED "Vehicle's speedometer's display speed after Toyota's offset was applied, uses vehicle's unit";
|
||||
CM_ SG_ 1552 UI_SPEED "Does not appear to match dash";
|
||||
CM_ SG_ 1552 METER_SLIDER_BRIGHTNESS_PCT "Combination display brightness setting, scales from 12 per cent to 100 per cent, reflects combination meter settings only, not linked with headlight state";
|
||||
CM_ SG_ 1552 METER_SLIDER_LOW_BRIGHTNESS "Combination display low brightness mode, also controls footwell lighting";
|
||||
CM_ SG_ 1552 METER_SLIDER_DIMMED "Combination display slider not at max, reflects combination meter settings only, not linked with headlight state";
|
||||
@@ -526,6 +527,7 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
|
||||
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
|
||||
VAL_ 1552 METER_SLIDER_LOW_BRIGHTNESS 1 "Low brightness mode, footwell lights off" 0 "Normal mode, footwell lights on";
|
||||
VAL_ 1552 METER_SLIDER_DIMMED 1 "Dimmed" 0 "Not Dimmed";
|
||||
VAL_ 1552 UNITS 1 "km (km/L)" 2 "km (L/100km)" 3 "miles (MPG US)" 4 "miles (MPG Imperial)";
|
||||
VAL_ 1553 UNITS 1 "km" 2 "miles";
|
||||
VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
|
||||
VAL_ 1592 LOCK_STATUS 0 "locked" 1 "unlocked";
|
||||
|
||||
@@ -249,7 +249,7 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
|
||||
BO_ 1009 PCM_CRUISE_ALT: 8 XXX
|
||||
SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
|
||||
SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
|
||||
|
||||
BO_ 1020 SOLAR_SENSOR: 8 XXX
|
||||
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
|
||||
@@ -373,6 +373,7 @@ BO_ 1552 BODY_CONTROL_STATE_2: 8 XXX
|
||||
SG_ METER_SLIDER_BRIGHTNESS_PCT : 30|7@0+ (1,0) [12|100] "%" XXX
|
||||
SG_ METER_SLIDER_LOW_BRIGHTNESS : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ METER_SLIDER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ UNITS : 63|3@0+ (1,0) [1|4] "" XXX
|
||||
|
||||
BO_ 1553 UI_SETTING: 8 XXX
|
||||
SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX
|
||||
@@ -439,7 +440,7 @@ CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT.";
|
||||
CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit";
|
||||
CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
|
||||
CM_ SG_ 956 GEAR "on 6MT, only R shows.";
|
||||
CM_ SG_ 1009 SET_SPEED "units seem to be whatever the car is set to";
|
||||
CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to";
|
||||
CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
|
||||
CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
|
||||
CM_ SG_ 1042 LDA_SA_TOGGLE "LDA Steering Assist Toggle";
|
||||
@@ -472,7 +473,7 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds";
|
||||
CM_ SG_ 1163 TSRSPU "always 1";
|
||||
CM_ SG_ 1552 UI_SPEED "Vehicle's speedometer's display speed after Toyota's offset was applied, uses vehicle's unit";
|
||||
CM_ SG_ 1552 UI_SPEED "Does not appear to match dash";
|
||||
CM_ SG_ 1552 METER_SLIDER_BRIGHTNESS_PCT "Combination display brightness setting, scales from 12 per cent to 100 per cent, reflects combination meter settings only, not linked with headlight state";
|
||||
CM_ SG_ 1552 METER_SLIDER_LOW_BRIGHTNESS "Combination display low brightness mode, also controls footwell lighting";
|
||||
CM_ SG_ 1552 METER_SLIDER_DIMMED "Combination display slider not at max, reflects combination meter settings only, not linked with headlight state";
|
||||
@@ -524,6 +525,7 @@ VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highwa
|
||||
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
|
||||
VAL_ 1552 METER_SLIDER_LOW_BRIGHTNESS 1 "Low brightness mode, footwell lights off" 0 "Normal mode, footwell lights on";
|
||||
VAL_ 1552 METER_SLIDER_DIMMED 1 "Dimmed" 0 "Not Dimmed";
|
||||
VAL_ 1552 UNITS 1 "km (km/L)" 2 "km (L/100km)" 3 "miles (MPG US)" 4 "miles (MPG Imperial)";
|
||||
VAL_ 1553 UNITS 1 "km" 2 "miles";
|
||||
VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
|
||||
VAL_ 1592 LOCK_STATUS 0 "locked" 1 "unlocked";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -181,6 +181,9 @@ class Panda:
|
||||
F4_DEVICES = (HW_TYPE_WHITE_PANDA, HW_TYPE_GREY_PANDA, HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS)
|
||||
H7_DEVICES = (HW_TYPE_RED_PANDA, )
|
||||
|
||||
INTERNAL_DEVICES = (HW_TYPE_UNO, HW_TYPE_DOS)
|
||||
HAS_OBD = (HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS, HW_TYPE_RED_PANDA)
|
||||
|
||||
CLOCK_SOURCE_MODE_DISABLED = 0
|
||||
CLOCK_SOURCE_MODE_FREE_RUNNING = 1
|
||||
|
||||
@@ -498,27 +501,6 @@ class Panda:
|
||||
else:
|
||||
return (0, 0)
|
||||
|
||||
def is_white(self):
|
||||
return self.get_type() == Panda.HW_TYPE_WHITE_PANDA
|
||||
|
||||
def is_grey(self):
|
||||
return self.get_type() == Panda.HW_TYPE_GREY_PANDA
|
||||
|
||||
def is_black(self):
|
||||
return self.get_type() == Panda.HW_TYPE_BLACK_PANDA
|
||||
|
||||
def is_pedal(self):
|
||||
return self.get_type() == Panda.HW_TYPE_PEDAL
|
||||
|
||||
def is_uno(self):
|
||||
return self.get_type() == Panda.HW_TYPE_UNO
|
||||
|
||||
def is_dos(self):
|
||||
return self.get_type() == Panda.HW_TYPE_DOS
|
||||
|
||||
def is_red(self):
|
||||
return self.get_type() == Panda.HW_TYPE_RED_PANDA
|
||||
|
||||
def get_mcu_type(self):
|
||||
hw_type = self.get_type()
|
||||
if hw_type in Panda.F2_DEVICES:
|
||||
@@ -530,13 +512,10 @@ class Panda:
|
||||
return None
|
||||
|
||||
def has_obd(self):
|
||||
return (self.is_uno() or self.is_dos() or self.is_black() or self.is_red())
|
||||
return self.get_type() in Panda.HAS_OBD
|
||||
|
||||
def has_canfd(self) -> bool:
|
||||
return self.get_type() in Panda.H7_DEVICES
|
||||
|
||||
def is_internal(self) -> bool:
|
||||
return self.get_type() in (Panda.HW_TYPE_UNO, Panda.HW_TYPE_DOS)
|
||||
def is_internal(self):
|
||||
return self.get_type() in Panda.INTERNAL_DEVICES
|
||||
|
||||
def get_serial(self):
|
||||
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20)
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -8,7 +8,7 @@ from selfdrive.car.body.values import SPEED_FROM_RPM
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=None, car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=None, car_fw=None, experimental_long=False):
|
||||
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
|
||||
@@ -91,7 +91,7 @@ def fingerprint(logcan, sendcan):
|
||||
fixed_fingerprint = car_selected
|
||||
|
||||
if not fixed_fingerprint and not skip_fw_query:
|
||||
# Vin query only reliably works thorugh OBDII
|
||||
# Vin query only reliably works through OBDII
|
||||
bus = 1
|
||||
|
||||
cached_params = Params().get("CarParamsCache")
|
||||
@@ -213,11 +213,11 @@ def get_car(logcan, sendcan):
|
||||
x = threading.Thread(target=crash_log, args=(candidate,))
|
||||
x.start()
|
||||
|
||||
disable_radar = Params().get_bool("DisableRadar")
|
||||
experimental_long = Params().get_bool("ExperimentalLongitudinalEnabled")
|
||||
|
||||
try:
|
||||
CarInterface, CarController, CarState = interfaces[candidate]
|
||||
CP = CarInterface.get_params(candidate, fingerprints, car_fw, disable_radar)
|
||||
CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long)
|
||||
CP.carVin = vin
|
||||
CP.carFw = car_fw
|
||||
CP.fingerprintSource = source
|
||||
|
||||
@@ -58,8 +58,8 @@ class CarState(CarStateBase):
|
||||
)
|
||||
|
||||
# button presses
|
||||
ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1
|
||||
ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1,
|
||||
cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2)
|
||||
ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1
|
||||
|
||||
# steering wheel
|
||||
|
||||
@@ -8,7 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "chrysler"
|
||||
|
||||
@@ -60,7 +60,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.minSteerSpeed = 14.5
|
||||
if car_fw is not None:
|
||||
for fw in car_fw:
|
||||
if fw.ecu == 'eps' and fw.fwVersion in (b"68312176AE", b"68312176AG", b"68273275AG"):
|
||||
if fw.ecu == 'eps' and fw.fwVersion[:8] in (b"68312176", b"68273275"):
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
elif candidate == CAR.RAM_HD:
|
||||
|
||||
@@ -130,6 +130,11 @@ FINGERPRINTS = {
|
||||
}
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.PACIFICA_2019_HYBRID: {
|
||||
(Ecu.hcp, 0x7e2, None): [],
|
||||
(Ecu.abs, 0x7e4, None): [],
|
||||
},
|
||||
|
||||
CAR.RAM_1500: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68294063AH',
|
||||
@@ -144,7 +149,7 @@ FW_VERSIONS = {
|
||||
b'68428609AB',
|
||||
b'68500728AA',
|
||||
],
|
||||
(Ecu.esp, 0x747, None): [
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68432418AD',
|
||||
b'68432418AB',
|
||||
b'68436004AE',
|
||||
@@ -200,7 +205,7 @@ FW_VERSIONS = {
|
||||
b'68428503AA',
|
||||
b'68428505AA',
|
||||
],
|
||||
(Ecu.esp, 0x747, None): [
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68334977AH',
|
||||
b'68504022AB',
|
||||
b'68530686AB',
|
||||
|
||||
@@ -10,10 +10,6 @@ from common.conversions import Conversions as CV
|
||||
GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2
|
||||
MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)"
|
||||
|
||||
# Makes that lack auto-resume with stock long, and auto resume in any configuration
|
||||
NO_AUTO_RESUME_STOCK_LONG = {"toyota", "gm"}
|
||||
NO_AUTO_RESUME = NO_AUTO_RESUME_STOCK_LONG | {"nissan", "subaru"}
|
||||
|
||||
|
||||
class Column(Enum):
|
||||
MAKE = "Make"
|
||||
@@ -23,6 +19,7 @@ class Column(Enum):
|
||||
FSR_LONGITUDINAL = "No ACC accel below"
|
||||
FSR_STEERING = "No ALC below"
|
||||
STEERING_TORQUE = "Steering Torque"
|
||||
AUTO_RESUME = "Resume from stop"
|
||||
HARNESS = "Harness"
|
||||
|
||||
|
||||
@@ -135,10 +132,11 @@ class CarInfo:
|
||||
Column.MAKE: self.make,
|
||||
Column.MODEL: self.model,
|
||||
Column.PACKAGE: self.package,
|
||||
Column.LONGITUDINAL: "openpilot" if CP.openpilotLongitudinalControl and not CP.radarOffCan else "Stock",
|
||||
Column.LONGITUDINAL: "openpilot" if CP.openpilotLongitudinalControl or CP.experimentalLongitudinalAvailable else "Stock",
|
||||
Column.FSR_LONGITUDINAL: f"{max(self.min_enable_speed * CV.MS_TO_MPH, 0):.0f} mph",
|
||||
Column.FSR_STEERING: f"{max(self.min_steer_speed * CV.MS_TO_MPH, 0):.0f} mph",
|
||||
Column.STEERING_TORQUE: Star.EMPTY,
|
||||
Column.AUTO_RESUME: Star.FULL if CP.autoResumeSng else Star.EMPTY,
|
||||
Column.HARNESS: self.harness.value,
|
||||
}
|
||||
|
||||
@@ -166,7 +164,7 @@ class CarInfo:
|
||||
acc = ""
|
||||
if self.min_enable_speed > 0:
|
||||
acc = f" <strong>while driving above {self.min_enable_speed * CV.MS_TO_MPH:.0f} mph</strong>"
|
||||
elif CP.carName not in NO_AUTO_RESUME or (CP.carName in NO_AUTO_RESUME_STOCK_LONG and CP.openpilotLongitudinalControl):
|
||||
elif CP.autoResumeSng:
|
||||
acc = " <strong>that automatically resumes from a stop</strong>"
|
||||
|
||||
if self.row[Column.STEERING_TORQUE] != Star.FULL:
|
||||
@@ -193,4 +191,3 @@ class CarInfo:
|
||||
item += footnote_tag.format(f'{",".join(map(str, sups))}')
|
||||
|
||||
return item
|
||||
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
import math
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from opendbc.can.packer import CANPacker
|
||||
@@ -7,14 +8,19 @@ from selfdrive.car.ford.values import CarControllerParams
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo):
|
||||
def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo):
|
||||
# rate limit
|
||||
steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last)
|
||||
rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN
|
||||
steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last)
|
||||
rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN
|
||||
max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
|
||||
apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff))
|
||||
apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff))
|
||||
|
||||
return apply_steer
|
||||
# absolute limit (LatCtlPath_An_Actl)
|
||||
apply_path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO
|
||||
apply_path_angle = clip(apply_path_angle, -0.4995, 0.5240)
|
||||
apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO
|
||||
|
||||
return apply_angle
|
||||
|
||||
|
||||
class CarController:
|
||||
@@ -24,7 +30,7 @@ class CarController:
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.frame = 0
|
||||
|
||||
self.apply_steer_last = 0
|
||||
self.apply_angle_last = 0
|
||||
self.main_on_last = False
|
||||
self.lkas_enabled_last = False
|
||||
self.steer_alert_last = False
|
||||
@@ -38,52 +44,62 @@ class CarController:
|
||||
main_on = CS.out.cruiseState.available
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
|
||||
### acc buttons ###
|
||||
if CC.cruiseControl.cancel:
|
||||
# cancel stock ACC
|
||||
can_sends.append(fordcan.spam_cancel_button(self.packer))
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True))
|
||||
elif CC.cruiseControl.resume:
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True))
|
||||
|
||||
# apply rate limits
|
||||
new_steer = actuators.steeringAngleDeg
|
||||
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
|
||||
# if stock lane centering is active or in standby, toggle it off
|
||||
# the stock system checks for steering pressed, and eventually disengages cruise control
|
||||
if (self.frame % 200) == 0 and CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0:
|
||||
can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True))
|
||||
|
||||
|
||||
### lateral control ###
|
||||
if CC.latActive:
|
||||
apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo)
|
||||
else:
|
||||
apply_angle = CS.out.steeringAngleDeg
|
||||
|
||||
# send steering commands at 20Hz
|
||||
if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
|
||||
lca_rq = 1 if CC.latActive else 0
|
||||
|
||||
# use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented
|
||||
path_angle = apply_steer
|
||||
# use LatCtlPath_An_Actl to actuate steering
|
||||
# path angle is the car wheel angle, not the steering wheel angle
|
||||
path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO
|
||||
|
||||
# convert steer angle to curvature
|
||||
curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0)
|
||||
# ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
|
||||
# TODO: try slower ramp speed when driver torque detected
|
||||
ramp_type = 3
|
||||
precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable)
|
||||
|
||||
# TODO: get other actuators
|
||||
curvature_rate = 0
|
||||
path_offset = 0
|
||||
offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094)
|
||||
|
||||
ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
|
||||
precision = 0 # 0=Comfortable, 1=Precise
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature))
|
||||
self.apply_angle_last = apply_angle
|
||||
can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0))
|
||||
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
|
||||
path_offset, path_angle, curvature_rate, curvature))
|
||||
0, path_angle, 0, offset_roll_compensation_curvature))
|
||||
|
||||
|
||||
### ui ###
|
||||
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
|
||||
|
||||
# send lkas ui command at 1Hz or if ui state changes
|
||||
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values))
|
||||
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
|
||||
|
||||
# send acc ui command at 20Hz or if ui state changes
|
||||
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values))
|
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values))
|
||||
|
||||
self.main_on_last = main_on
|
||||
self.lkas_enabled_last = CC.latActive
|
||||
self.steer_alert_last = steer_alert
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steeringAngleDeg = apply_steer
|
||||
new_actuators.steeringAngleDeg = apply_angle
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -3,7 +3,7 @@ from common.conversions import Conversions as CV
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.ford.values import CANBUS, DBC
|
||||
from selfdrive.car.ford.values import CANBUS, DBC, CarControllerParams
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
@@ -22,7 +22,7 @@ class CarState(CarStateBase):
|
||||
# car speed
|
||||
ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG
|
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
|
||||
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
|
||||
|
||||
# gas pedal
|
||||
@@ -37,7 +37,7 @@ class CarState(CarStateBase):
|
||||
# steering wheel
|
||||
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
|
||||
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
|
||||
ret.steeringPressed = cp.vl["Lane_Assist_Data3_FD1"]["LaHandsOff_B_Actl"] == 0
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
|
||||
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
|
||||
# ret.espDisabled = False # TODO: find traction control signal
|
||||
@@ -46,6 +46,8 @@ class CarState(CarStateBase):
|
||||
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS
|
||||
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
|
||||
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
|
||||
ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
|
||||
ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
|
||||
|
||||
# gear
|
||||
if self.CP.transmissionType == TransmissionType.automatic:
|
||||
@@ -65,6 +67,7 @@ class CarState(CarStateBase):
|
||||
# button presses
|
||||
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
|
||||
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
|
||||
# TODO: block this going to the camera otherwise it will enable stock TJA
|
||||
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
||||
|
||||
# lock info
|
||||
@@ -77,9 +80,13 @@ class CarState(CarStateBase):
|
||||
ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
|
||||
ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
|
||||
|
||||
# Stock steering buttons so that we can passthru blinkers etc.
|
||||
self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
|
||||
# Stock values from IPMA so that we can retain some stock functionality
|
||||
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
|
||||
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
|
||||
# Use stock sensor values
|
||||
self.yaw_data = cp.vl["Yaw_Data_FD1"]
|
||||
|
||||
return ret
|
||||
|
||||
@@ -97,6 +104,8 @@ class CarState(CarStateBase):
|
||||
("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph)
|
||||
# The units might change with IPC settings?
|
||||
("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status
|
||||
("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill
|
||||
("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable
|
||||
("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg)
|
||||
# Calculates steering angle (and offset) from pinion
|
||||
# angle and driving measurements.
|
||||
@@ -104,7 +113,6 @@ class CarState(CarStateBase):
|
||||
# to zero at the beginning of the drive.
|
||||
("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm)
|
||||
("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status
|
||||
("LaHandsOff_B_Actl", "Lane_Assist_Data3_FD1"), # PSCM LKAS hands off wheel
|
||||
("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch
|
||||
("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle
|
||||
("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver
|
||||
@@ -112,6 +120,38 @@ class CarState(CarStateBase):
|
||||
("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left
|
||||
("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right
|
||||
("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver
|
||||
("HeadLghtHiFlash_D_Stat", "Steering_Data_FD1"), # SCCM Passthru the remaining buttons
|
||||
("WiprFront_D_Stat", "Steering_Data_FD1"),
|
||||
("LghtAmb_D_Sns", "Steering_Data_FD1"),
|
||||
("AccButtnGapDecPress", "Steering_Data_FD1"),
|
||||
("AccButtnGapIncPress", "Steering_Data_FD1"),
|
||||
("AslButtnOnOffCnclPress", "Steering_Data_FD1"),
|
||||
("AslButtnOnOffPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnCnclPress", "Steering_Data_FD1"),
|
||||
("LaSwtchPos_D_Stat", "Steering_Data_FD1"),
|
||||
("CcAslButtnCnclResPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnDeny_B_Actl", "Steering_Data_FD1"),
|
||||
("CcAslButtnIndxDecPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnIndxIncPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnOffCnclPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnOnOffCncl", "Steering_Data_FD1"),
|
||||
("CcAslButtnOnPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnResDecPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnResIncPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnSetDecPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnSetIncPress", "Steering_Data_FD1"),
|
||||
("CcAslButtnSetPress", "Steering_Data_FD1"),
|
||||
("CcAsllButtnResPress", "Steering_Data_FD1"),
|
||||
("CcButtnOffPress", "Steering_Data_FD1"),
|
||||
("CcButtnOnOffCnclPress", "Steering_Data_FD1"),
|
||||
("CcButtnOnOffPress", "Steering_Data_FD1"),
|
||||
("CcButtnOnPress", "Steering_Data_FD1"),
|
||||
("HeadLghtHiFlash_D_Actl", "Steering_Data_FD1"),
|
||||
("HeadLghtHiOn_B_StatAhb", "Steering_Data_FD1"),
|
||||
("AhbStat_B_Dsply", "Steering_Data_FD1"),
|
||||
("AccButtnGapTogglePress", "Steering_Data_FD1"),
|
||||
("WiprFrontSwtch_D_Stat", "Steering_Data_FD1"),
|
||||
("HeadLghtHiCtrl_D_RqAhb", "Steering_Data_FD1"),
|
||||
]
|
||||
|
||||
checks = [
|
||||
@@ -122,6 +162,7 @@ class CarState(CarStateBase):
|
||||
("EngVehicleSpThrottle", 100),
|
||||
("BrakeSnData_4", 50),
|
||||
("EngBrakeData", 10),
|
||||
("Cluster_Info1_FD1", 10),
|
||||
("SteeringPinion_Data", 100),
|
||||
("EPAS_INFO", 50),
|
||||
("Lane_Assist_Data3_FD1", 33),
|
||||
@@ -203,7 +244,8 @@ class CarState(CarStateBase):
|
||||
("DasStats_D_Dsply", "IPMA_Data"), # DAS status
|
||||
("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning
|
||||
("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status
|
||||
("Set_Me_X1", "IPMA_Data"),
|
||||
("Passthru_63", "IPMA_Data"),
|
||||
("Passthru_48", "IPMA_Data"),
|
||||
]
|
||||
|
||||
checks = [
|
||||
|
||||
@@ -1,7 +1,10 @@
|
||||
from common.numpy_fast import clip
|
||||
from cereal import car
|
||||
from selfdrive.car.ford.values import CANBUS
|
||||
|
||||
HUDControl = car.CarControl.HUDControl
|
||||
|
||||
|
||||
def create_lkas_command(packer, angle_deg: float, curvature: float):
|
||||
def create_lka_command(packer, angle_deg: float, curvature: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford LKAS Command.
|
||||
|
||||
@@ -20,7 +23,7 @@ def create_lkas_command(packer, angle_deg: float, curvature: float):
|
||||
"LdwActvStats_D_Req": 0, # LDW status [0|7]
|
||||
"LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength
|
||||
}
|
||||
return packer.make_can_msg("Lane_Assist_Data1", 0, values)
|
||||
return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float):
|
||||
@@ -38,20 +41,20 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
|
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
|
||||
"LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
"LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
"LatCtlPathOffst_L_Actl": clip(path_offset, -5.12, 5.11), # Path offset [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235), # Path angle [-0.5|0.5235] radians
|
||||
"LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375), # Curvature rate [-0.001024|0.00102375] 1/meter^2
|
||||
"LatCtlCurv_No_Actl": clip(curvature, -0.02, 0.02094), # Curvature [-0.02|0.02094] 1/meter
|
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
|
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
|
||||
"LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
"LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.4995|0.5240] radians
|
||||
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
|
||||
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
|
||||
}
|
||||
return packer.make_can_msg("LateralMotionControl", 0, values)
|
||||
return packer.make_can_msg("LateralMotionControl", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values):
|
||||
def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC IPMA/LKAS status.
|
||||
|
||||
@@ -63,34 +66,47 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo
|
||||
"""
|
||||
|
||||
# LaActvStats_D_Dsply
|
||||
# TODO: get LDW state from OP
|
||||
# R Intvn Warn Supprs Avail No
|
||||
# L
|
||||
# Intvn 24 19 14 9 4
|
||||
# Warn 23 18 13 8 3
|
||||
# Supprs 22 17 12 7 2
|
||||
# Avail 21 16 11 6 1
|
||||
# No 20 15 10 5 0
|
||||
#
|
||||
# TODO: test suppress state
|
||||
if enabled:
|
||||
lines = 6
|
||||
lines = 0 # NoLeft_NoRight
|
||||
if hud_control.leftLaneDepart:
|
||||
lines += 4
|
||||
elif hud_control.leftLaneVisible:
|
||||
lines += 1
|
||||
if hud_control.rightLaneDepart:
|
||||
lines += 20
|
||||
elif hud_control.rightLaneVisible:
|
||||
lines += 5
|
||||
elif main_on:
|
||||
lines = 0
|
||||
else:
|
||||
lines = 30
|
||||
if hud_control.leftLaneDepart:
|
||||
lines = 3 # WarnLeft_NoRight
|
||||
elif hud_control.rightLaneDepart:
|
||||
lines = 15 # NoLeft_WarnRight
|
||||
else:
|
||||
lines = 30 # LA_Off
|
||||
|
||||
# TODO: use level 1 for no sound when less severe?
|
||||
hands_on_wheel_dsply = 2 if steer_alert else 0
|
||||
|
||||
values = {
|
||||
"FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535]
|
||||
"FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535]
|
||||
"PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7]
|
||||
"AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3]
|
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
|
||||
"LaDenyStats_B_Dsply": stock_values["LaDenyStats_B_Dsply"], # LKAS error [0|1]
|
||||
"LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
|
||||
"CamraDefog_B_Req": stock_values["CamraDefog_B_Req"], # Windshield heater? [0|1]
|
||||
"CamraStats_D_Dsply": stock_values["CamraStats_D_Dsply"], # Camera status [0|3]
|
||||
"DasAlrtLvl_D_Dsply": stock_values["DasAlrtLvl_D_Dsply"], # DAS alert level [0|7]
|
||||
"DasStats_D_Dsply": stock_values["DasStats_D_Dsply"], # DAS status [0|3]
|
||||
"DasWarn_D_Dsply": stock_values["DasWarn_D_Dsply"], # DAS warning [0|3]
|
||||
"AhbHiBeam_D_Rq": stock_values["AhbHiBeam_D_Rq"], # AHB status [0|3]
|
||||
"Set_Me_X1": stock_values["Set_Me_X1"], # [0|15]
|
||||
**stock_values,
|
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
|
||||
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
|
||||
}
|
||||
return packer.make_can_msg("IPMA_Data", 0, values)
|
||||
return packer.make_can_msg("IPMA_Data", CANBUS.main, values)
|
||||
|
||||
|
||||
def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values):
|
||||
def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision
|
||||
warning and traffic jam assist status.
|
||||
@@ -100,38 +116,32 @@ def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values):
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
# Tja_D_Stat
|
||||
if enabled:
|
||||
if hud_control.leftLaneDepart:
|
||||
status = 3 # ActiveInterventionLeft
|
||||
elif hud_control.rightLaneDepart:
|
||||
status = 4 # ActiveInterventionRight
|
||||
else:
|
||||
status = 2 # Active
|
||||
elif main_on:
|
||||
if hud_control.leftLaneDepart:
|
||||
status = 5 # ActiveWarningLeft
|
||||
elif hud_control.rightLaneDepart:
|
||||
status = 6 # ActiveWarningRight
|
||||
else:
|
||||
status = 1 # Standby
|
||||
else:
|
||||
status = 0 # Off
|
||||
|
||||
values = {
|
||||
"HaDsply_No_Cs": stock_values["HaDsply_No_Cs"], # [0|255]
|
||||
"HaDsply_No_Cnt": stock_values["HaDsply_No_Cnt"], # [0|15]
|
||||
"AccStopStat_D_Dsply": stock_values["AccStopStat_D_Dsply"], # ACC stopped status message: 0=NoDisplay, 1=ResumeReady, 2=Stopped, 3=PressResume [0|3]
|
||||
"AccTrgDist2_D_Dsply": stock_values["AccTrgDist2_D_Dsply"], # ACC target distance [0|15]
|
||||
"AccStopRes_B_Dsply": stock_values["AccStopRes_B_Dsply"], # [0|1]
|
||||
"TjaWarn_D_Rq": stock_values["TjaWarn_D_Rq"], # TJA warning: 0=NoWarning, 1=Cancel, 2=HardTakeOverLevel1, 3=HardTakeOverLevel2 [0|7]
|
||||
"Tja_D_Stat": 2 if enabled else (1 if main_on else 0), # TJA status: 0=Off, 1=Standby, 2=Active, 3=InterventionLeft, 4=InterventionRight, 5=WarningLeft, 6=WarningRight, 7=NotUsed [0|7]
|
||||
"TjaMsgTxt_D_Dsply": stock_values["TjaMsgTxt_D_Dsply"], # TJA text [0|7]
|
||||
"IaccLamp_D_Rq": stock_values["IaccLamp_D_Rq"], # iACC status icon [0|3]
|
||||
"AccMsgTxt_D2_Rq": stock_values["AccMsgTxt_D2_Rq"], # ACC text [0|15]
|
||||
"FcwDeny_B_Dsply": stock_values["FcwDeny_B_Dsply"], # FCW disabled [0|1]
|
||||
"FcwMemStat_B_Actl": stock_values["FcwMemStat_B_Actl"], # FCW enabled setting [0|1]
|
||||
"AccTGap_B_Dsply": stock_values["AccTGap_B_Dsply"], # ACC time gap display setting [0|1]
|
||||
"CadsAlignIncplt_B_Actl": stock_values["CadsAlignIncplt_B_Actl"], # Radar alignment? [0|1]
|
||||
"AccFllwMde_B_Dsply": stock_values["AccFllwMde_B_Dsply"], # ACC follow mode display setting [0|1]
|
||||
"CadsRadrBlck_B_Actl": stock_values["CadsRadrBlck_B_Actl"], # Radar blocked? [0|1]
|
||||
"CmbbPostEvnt_B_Dsply": stock_values["CmbbPostEvnt_B_Dsply"], # AEB event status [0|1]
|
||||
"AccStopMde_B_Dsply": stock_values["AccStopMde_B_Dsply"], # ACC stop mode display setting [0|1]
|
||||
"FcwMemSens_D_Actl": stock_values["FcwMemSens_D_Actl"], # FCW sensitivity setting [0|3]
|
||||
"FcwMsgTxt_D_Rq": stock_values["FcwMsgTxt_D_Rq"], # FCW text [0|7]
|
||||
"AccWarn_D_Dsply": stock_values["AccWarn_D_Dsply"], # ACC warning [0|3]
|
||||
"FcwVisblWarn_B_Rq": stock_values["FcwVisblWarn_B_Rq"], # FCW alert: 0=Off, 1=On [0|1]
|
||||
"FcwAudioWarn_B_Rq": stock_values["FcwAudioWarn_B_Rq"], # FCW audio: 0=Off, 1=On [0|1]
|
||||
"AccTGap_D_Dsply": stock_values["AccTGap_D_Dsply"], # ACC time gap: 1=Time_Gap_1, 2=Time_Gap_2, ..., 5=Time_Gap_5 [0|7]
|
||||
"AccMemEnbl_B_RqDrv": stock_values["AccMemEnbl_B_RqDrv"], # ACC setting: 0=NormalCruise, 1=AdaptiveCruise [0|1]
|
||||
"FdaMem_B_Stat": stock_values["FdaMem_B_Stat"], # FDA enabled setting [0|1]
|
||||
**stock_values,
|
||||
"Tja_D_Stat": status,
|
||||
}
|
||||
return packer.make_can_msg("ACCDATA_3", 0, values)
|
||||
return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)
|
||||
|
||||
|
||||
def spam_cancel_button(packer, cancel=1):
|
||||
def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus = CANBUS.camera):
|
||||
"""
|
||||
Creates a CAN message for the Ford SCCM buttons/switches.
|
||||
|
||||
@@ -139,6 +149,9 @@ def spam_cancel_button(packer, cancel=1):
|
||||
"""
|
||||
|
||||
values = {
|
||||
"CcAslButtnCnclPress": cancel, # CC cancel button
|
||||
**stock_values,
|
||||
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
|
||||
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
|
||||
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # TJA toggle button
|
||||
}
|
||||
return packer.make_can_msg("Steering_Data_FD1", 0, values)
|
||||
return packer.make_can_msg("Steering_Data_FD1", bus, values)
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
|
||||
from selfdrive.car.ford.values import CAR, TransmissionType, GearShifter
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
||||
@@ -11,60 +11,60 @@ EventName = car.CarEvent.EventName
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
if car_fw is None:
|
||||
car_fw = []
|
||||
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "ford"
|
||||
#ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
|
||||
ret.dashcamOnly = True
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
|
||||
|
||||
# Angle-based steering
|
||||
# TODO: use curvature control when ready
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerActuatorDelay = 0.4
|
||||
ret.steerLimitTimer = 1.0
|
||||
|
||||
# TODO: detect stop-and-go vehicles
|
||||
stop_and_go = False
|
||||
tire_stiffness_factor = 1.0
|
||||
|
||||
if candidate == CAR.ESCAPE_MK4:
|
||||
ret.wheelbase = 2.71
|
||||
ret.steerRatio = 14.3 # Copied from Focus
|
||||
tire_stiffness_factor = 0.5328 # Copied from Focus
|
||||
ret.mass = 1750 + STD_CARGO_KG
|
||||
|
||||
elif candidate == CAR.EXPLORER_MK6:
|
||||
ret.wheelbase = 3.025
|
||||
ret.steerRatio = 16.8 # learned
|
||||
ret.mass = 2050 + STD_CARGO_KG
|
||||
|
||||
elif candidate == CAR.FOCUS_MK4:
|
||||
ret.wheelbase = 2.7
|
||||
ret.steerRatio = 14.3
|
||||
tire_stiffness_factor = 0.5328
|
||||
ret.steerRatio = 13.8 # learned
|
||||
ret.mass = 1350 + STD_CARGO_KG
|
||||
|
||||
else:
|
||||
raise ValueError(f"Unsupported car: ${candidate}")
|
||||
|
||||
# Auto Transmission: Gear_Shift_by_Wire_FD1
|
||||
# TODO: detect transmission in car_fw?
|
||||
if 0x5A in fingerprint[0]:
|
||||
# Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
|
||||
found_ecus = [fw.ecu for fw in car_fw]
|
||||
if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[0]:
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.manual
|
||||
ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS
|
||||
|
||||
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
|
||||
# TODO: detect bsm in car_fw?
|
||||
ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0]
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter.
|
||||
ret.minEnableSpeed = -1. if (stop_and_go) else 20. * CV.MPH_TO_MS
|
||||
# LCA can steer down to zero
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
|
||||
ret.autoResumeSng = ret.minEnableSpeed == -1.
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
return ret
|
||||
|
||||
def _update(self, c):
|
||||
|
||||
@@ -1,9 +1,11 @@
|
||||
from collections import namedtuple
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from typing import Dict, List, Union
|
||||
|
||||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
from selfdrive.car.docs_definitions import CarInfo
|
||||
from selfdrive.car.docs_definitions import CarInfo, Harness
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
@@ -20,8 +22,11 @@ class CarControllerParams:
|
||||
# Message: ACCDATA_3
|
||||
ACC_UI_STEP = 5
|
||||
|
||||
STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
|
||||
STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
|
||||
STEER_RATIO = 2.75
|
||||
STEER_DRIVER_ALLOWANCE = 0.8
|
||||
|
||||
RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
|
||||
RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
|
||||
|
||||
|
||||
class RADAR:
|
||||
@@ -37,12 +42,23 @@ class CANBUS:
|
||||
|
||||
class CAR:
|
||||
ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
|
||||
EXPLORER_MK6 = "FORD EXPLORER 6TH GEN"
|
||||
FOCUS_MK4 = "FORD FOCUS 4TH GEN"
|
||||
|
||||
|
||||
@dataclass
|
||||
class FordCarInfo(CarInfo):
|
||||
package: str = "Co-Pilot360 Assist+"
|
||||
harness: Enum = Harness.ford_q3
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.ESCAPE_MK4: CarInfo("Ford Escape", "NA"),
|
||||
CAR.FOCUS_MK4: CarInfo("Ford Focus", "NA"),
|
||||
CAR.ESCAPE_MK4: [
|
||||
FordCarInfo("Ford Escape 2020"),
|
||||
FordCarInfo("Ford Kuga EU", "Driver Assistance Pack"),
|
||||
],
|
||||
CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-21"),
|
||||
CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"),
|
||||
}
|
||||
|
||||
|
||||
@@ -51,7 +67,7 @@ FW_VERSIONS = {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x760, None): [
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
@@ -63,12 +79,39 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7E0, None): [
|
||||
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.shiftByWire, 0x732, None): [
|
||||
],
|
||||
},
|
||||
CAR.EXPLORER_MK6: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7E0, None): [
|
||||
b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.shiftByWire, 0x732, None): [
|
||||
b'L1MP-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MP-14G395-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FOCUS_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x760, None): [
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
@@ -80,11 +123,14 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7E0, None): [
|
||||
b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.shiftByWire, 0x732, None): [
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
DBC = {
|
||||
CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
|
||||
CAR.EXPLORER_MK6: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
|
||||
CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
|
||||
}
|
||||
|
||||
@@ -16,7 +16,7 @@ from selfdrive.car.toyota.values import CAR as TOYOTA
|
||||
from system.swaglog import cloudlog
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
|
||||
ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
|
||||
|
||||
|
||||
def p16(val):
|
||||
@@ -213,14 +213,14 @@ REQUESTS: List[Request] = [
|
||||
"chrysler",
|
||||
[CHRYSLER_VERSION_REQUEST],
|
||||
[CHRYSLER_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.esp, Ecu.eps, Ecu.srs, Ecu.gateway, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.combinationMeter],
|
||||
whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.srs, Ecu.gateway, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.combinationMeter],
|
||||
rx_offset=CHRYSLER_RX_OFFSET,
|
||||
),
|
||||
Request(
|
||||
"chrysler",
|
||||
[CHRYSLER_VERSION_REQUEST],
|
||||
[CHRYSLER_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.engine, Ecu.transmission],
|
||||
whitelist_ecus=[Ecu.abs, Ecu.hcp, Ecu.engine, Ecu.transmission],
|
||||
),
|
||||
Request(
|
||||
"chrysler",
|
||||
@@ -240,7 +240,7 @@ REQUESTS: List[Request] = [
|
||||
[TESTER_PRESENT_REQUEST, FORD_VERSION_REQUEST],
|
||||
[TESTER_PRESENT_RESPONSE, FORD_VERSION_RESPONSE],
|
||||
bus=0,
|
||||
whitelist_ecus=[Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera],
|
||||
whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
|
||||
),
|
||||
]
|
||||
|
||||
@@ -328,7 +328,7 @@ def match_fw_to_car_exact(fw_versions_dict):
|
||||
ecu_type = ecu[0]
|
||||
addr = ecu[1:]
|
||||
found_versions = fw_versions_dict.get(addr, set())
|
||||
if ecu_type == Ecu.esp and candidate in (TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER, TOYOTA.SIENNA, TOYOTA.LEXUS_IS) and not len(found_versions):
|
||||
if ecu_type == Ecu.abs and candidate in (TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER, TOYOTA.SIENNA, TOYOTA.LEXUS_IS) and not len(found_versions):
|
||||
continue
|
||||
|
||||
# On some Toyota models, the engine can show on two different addresses
|
||||
|
||||
@@ -45,10 +45,11 @@ class CarInterface(CarInterfaceBase):
|
||||
return CarInterfaceBase.get_steer_feedforward_default
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "gm"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
|
||||
ret.autoResumeSng = False
|
||||
|
||||
if candidate in EV_CAR:
|
||||
ret.transmissionType = TransmissionType.direct
|
||||
@@ -73,7 +74,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL}
|
||||
|
||||
# Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below.
|
||||
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
|
||||
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
|
||||
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
|
||||
|
||||
@@ -30,7 +30,7 @@ class CarInterface(CarInterfaceBase):
|
||||
return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False): # pylint: disable=dangerous-default-value
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "honda"
|
||||
|
||||
@@ -41,7 +41,8 @@ class CarInterface(CarInterfaceBase):
|
||||
if candidate not in HONDA_BOSCH_RADARLESS:
|
||||
# Disable the radar and let openpilot control longitudinal
|
||||
# WARNING: THIS DISABLES AEB!
|
||||
ret.openpilotLongitudinalControl = disable_radar
|
||||
ret.experimentalLongitudinalAvailable = True
|
||||
ret.openpilotLongitudinalControl = experimental_long
|
||||
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
else:
|
||||
|
||||
@@ -144,6 +144,7 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
|
||||
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.ODYSSEY_CHN: {(Ecu.vsa, 0xfff, None): [b'\x00']},
|
||||
CAR.ACCORD: {
|
||||
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
|
||||
b'37805-6A0-8720\x00\x00',
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from common.numpy_fast import clip, interp
|
||||
from common.numpy_fast import clip
|
||||
from common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
@@ -15,6 +15,7 @@ def process_hud_alert(enabled, fingerprint, hud_control):
|
||||
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
|
||||
|
||||
# initialize to no line visible
|
||||
# TODO: this is not accurate for all cars
|
||||
sys_state = 1
|
||||
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
|
||||
sys_state = 3 if enabled or sys_warning else 4
|
||||
@@ -122,12 +123,9 @@ class CarController:
|
||||
|
||||
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
accel = actuators.accel
|
||||
jerk = 0
|
||||
|
||||
if CC.longActive:
|
||||
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
|
||||
if accel < 0:
|
||||
accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
|
||||
#TODO unclear if this is needed
|
||||
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
|
||||
|
||||
accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
|
||||
|
||||
|
||||
@@ -37,12 +37,26 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
|
||||
# Note: the warning is hidden while the blinkers are on
|
||||
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
|
||||
|
||||
# Likely cars lacking the ability to show individual lane lines in the dash
|
||||
elif car_fingerprint in (CAR.KIA_OPTIMA,):
|
||||
# SysWarning 4 = keep hands on wheel + beep
|
||||
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
|
||||
|
||||
# SysState 0 = no icons
|
||||
# SysState 1-2 = white car + lanes
|
||||
# SysState 3 = green car + lanes, green steering wheel
|
||||
# SysState 4 = green car + lanes
|
||||
values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1
|
||||
values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition
|
||||
|
||||
# these have no effect
|
||||
values["CF_Lkas_LdwsActivemode"] = 0
|
||||
values["CF_Lkas_FcwOpt_USM"] = 0
|
||||
|
||||
elif car_fingerprint == CAR.HYUNDAI_GENESIS:
|
||||
# This field is actually LdwsActivemode
|
||||
# Genesis and Optima fault when forwarding while engaged
|
||||
values["CF_Lkas_LdwsActivemode"] = 2
|
||||
elif car_fingerprint == CAR.KIA_OPTIMA:
|
||||
values["CF_Lkas_LdwsActivemode"] = 0
|
||||
|
||||
dat = packer.make_can_msg("LKAS11", 0, values)[2]
|
||||
|
||||
@@ -80,7 +94,7 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
|
||||
}
|
||||
return packer.make_can_msg("LFAHDA_MFC", 0, values)
|
||||
|
||||
def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_speed, stopping, gas_pressed):
|
||||
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, gas_pressed):
|
||||
commands = []
|
||||
|
||||
scc11_values = {
|
||||
@@ -88,12 +102,12 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
|
||||
"TauGapSet": 4,
|
||||
"VSetDis": set_speed if enabled else 0,
|
||||
"AliveCounterACC": idx % 0x10,
|
||||
"ObjValid": 0, # TODO: these two bits may allow for better longitudinal control
|
||||
"ACC_ObjStatus": 0,
|
||||
"ObjValid": 1, # close lead makes controls tighter
|
||||
"ACC_ObjStatus": 1, # close lead makes controls tighter
|
||||
"ACC_ObjLatPos": 0,
|
||||
"ACC_ObjRelSpd": 0,
|
||||
"ACC_ObjDist": 0,
|
||||
}
|
||||
"ACC_ObjDist": 1, # close lead makes controls tighter
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
|
||||
|
||||
scc12_values = {
|
||||
@@ -111,8 +125,8 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
|
||||
scc14_values = {
|
||||
"ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
|
||||
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
|
||||
"JerkUpperLimit": max(jerk, 1.0) if not stopping else 0, # stock usually is 1.0 but sometimes uses higher values
|
||||
"JerkLowerLimit": max(-jerk, 1.0), # stock usually is 0.5 but sometimes uses higher values
|
||||
"JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
|
||||
"JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
|
||||
"ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
||||
"ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
|
||||
}
|
||||
|
||||
@@ -22,14 +22,15 @@ class CarInterface(CarInterfaceBase):
|
||||
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False): # pylint: disable=dangerous-default-value
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "hyundai"
|
||||
ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None
|
||||
|
||||
# WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB)
|
||||
ret.openpilotLongitudinalControl = disable_radar and (candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR))
|
||||
ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR | CANFD_CAR)
|
||||
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
|
||||
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
|
||||
@@ -43,13 +44,15 @@ class CarInterface(CarInterfaceBase):
|
||||
tire_stiffness_factor = 1.
|
||||
|
||||
ret.stoppingControl = True
|
||||
ret.vEgoStopping = 1.0
|
||||
ret.startingState = True
|
||||
ret.vEgoStarting = 0.1
|
||||
ret.startAccel = 2.0
|
||||
|
||||
ret.longitudinalTuning.kpV = [0.1]
|
||||
ret.longitudinalTuning.kpV = [0.5]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.stopAccel = 0.0
|
||||
|
||||
ret.longitudinalActuatorDelayUpperBound = 1.0 # s
|
||||
ret.longitudinalActuatorDelayLowerBound = 0.5 # s
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5 # s
|
||||
if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
@@ -300,6 +303,7 @@ class CarInterface(CarInterfaceBase):
|
||||
# non-HDA2
|
||||
if 0x1cf not in fingerprint[4]:
|
||||
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
|
||||
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
|
||||
else:
|
||||
ret.enableBsm = 0x58b in fingerprint[0]
|
||||
|
||||
|
||||
@@ -140,7 +140,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
|
||||
# Kia
|
||||
CAR.KIA_FORTE: [
|
||||
HyundaiCarInfo("Kia Forte 2018", harness=Harness.hyundai_b),
|
||||
HyundaiCarInfo("Kia Forte 2019-21", "All", harness=Harness.hyundai_g),
|
||||
HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g),
|
||||
],
|
||||
CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", "Smart Cruise Control (SCC)", harness=Harness.hyundai_a),
|
||||
CAR.KIA_NIRO_EV: [
|
||||
@@ -398,7 +398,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x8799110L0000\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ',
|
||||
b'\xf1\x8799110L0000\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100',
|
||||
b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300',
|
||||
b'\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300',
|
||||
@@ -518,7 +518,7 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00LF__ SCC F-CUP 1.00 1.00 96401-C2200 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00LF ESC \f 11 \x17\x01\x13 58920-C2610',
|
||||
b'\xf1\x00LF ESC \t 11 \x17\x01\x13 58920-C2610',
|
||||
],
|
||||
@@ -565,7 +565,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00TM__ SCC F-CUP 1.00 1.02 99110-S2000 ',
|
||||
b'\xf1\x00TM__ SCC F-CUP 1.00 1.03 99110-S2000 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00TM ESC \r 100\x18\x031 58910-S2650',
|
||||
b'\xf1\x00TM ESC \r 103\x18\x11\x08 58910-S2650',
|
||||
b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650',
|
||||
@@ -623,7 +623,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x8799110S1500\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ',
|
||||
b'\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
|
||||
b'\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
|
||||
b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
|
||||
@@ -744,7 +744,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 ',
|
||||
b'\xf1\x00ON__ FCA FHCUP 1.00 1.02 99110-S9100 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360',
|
||||
b'\xf1\x00LX ESC \x01 1031\t\x10 58910-S8360',
|
||||
b'\xf1\x00LX ESC \x0b 101\x19\x03\x17 58910-S8330',
|
||||
@@ -834,7 +834,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00JS__ SCC H-CUP 1.00 1.02 95650-J3200 ',
|
||||
b'\xf1\x00JS__ SCC HNCUP 1.00 1.02 95650-J3100 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\xf1\x816V8RAC00121.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -895,7 +895,7 @@ FW_VERSIONS = {
|
||||
},
|
||||
CAR.KONA: {
|
||||
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00OS__ SCC F-CUP 1.00 1.00 95655-J9200 ', ],
|
||||
(Ecu.esp, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ],
|
||||
(Ecu.abs, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ],
|
||||
(Ecu.engine, 0x7e0, None): [b'"\x01TOS-0NU06F301J02', ],
|
||||
(Ecu.eps, 0x7d4, None): [b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105', ],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ],
|
||||
@@ -910,7 +910,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x816U2V7051\000\000\xf1\0006U2V0_C2\000\0006U2V7051\000\000DCD0T14US1\000\000\000\000',
|
||||
b'\xf1\x816U2V7051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V7051\x00\x00DCD0T14US1U\x867Z',
|
||||
],
|
||||
(Ecu.esp, 0x7D1, None): [b'\xf1\000CD ESC \003 102\030\b\005 58920-J7350', ],
|
||||
(Ecu.abs, 0x7D1, None): [b'\xf1\000CD ESC \003 102\030\b\005 58920-J7350', ],
|
||||
},
|
||||
CAR.KIA_FORTE: {
|
||||
(Ecu.eps, 0x7D4, None): [
|
||||
@@ -928,7 +928,7 @@ FW_VERSIONS = {
|
||||
b'\x01TBDM1NU06F200H01',
|
||||
b'391182B945\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
@@ -952,7 +952,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.03 99210-L3000 200915',
|
||||
b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.04 99210-L3000 210208',
|
||||
],
|
||||
(Ecu.esp, 0x7D1, None): [
|
||||
(Ecu.abs, 0x7D1, None): [
|
||||
b'\xf1\000DL ESC \006 101 \004\002 58910-L3200',
|
||||
b'\xf1\x8758910-L3200\xf1\000DL ESC \006 101 \004\002 58910-L3200',
|
||||
b'\xf1\x8758910-L3800\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800',
|
||||
@@ -972,7 +972,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.KONA_EV: {
|
||||
(Ecu.esp, 0x7D1, None): [
|
||||
(Ecu.abs, 0x7D1, None): [
|
||||
b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000',
|
||||
b'\xf1\x00OS IEB \x01 212 \x11\x13 58520-K4000',
|
||||
b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000',
|
||||
@@ -999,7 +999,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.KONA_EV_2022: {
|
||||
(Ecu.esp, 0x7D1, None): [
|
||||
(Ecu.abs, 0x7D1, None): [
|
||||
b'\xf1\x8758520-K4010\xf1\x00OS IEB \x02 101 \x11\x13 58520-K4010',
|
||||
b'\xf1\x8758520-K4010\xf1\x00OS IEB \x04 101 \x11\x13 58520-K4010',
|
||||
b'\xf1\x8758520-K4010\xf1\x00OS IEB \x03 101 \x11\x13 58520-K4010',
|
||||
@@ -1093,7 +1093,7 @@ FW_VERSIONS = {
|
||||
},
|
||||
CAR.KIA_SELTOS: {
|
||||
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x8799110Q5100\xf1\000SP2_ SCC FHCUP 1.01 1.05 99110-Q5100 ',],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x8758910-Q5450\xf1\000SP ESC \a 101\031\t\005 58910-Q5450',
|
||||
b'\xf1\x8758910-Q5450\xf1\000SP ESC \t 101\031\t\005 58910-Q5450',
|
||||
],
|
||||
@@ -1121,7 +1121,7 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00JF ESC \x0b 11 \x18\x030 58920-D5180',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
@@ -1158,7 +1158,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205',
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.06 99210-AA000 220111',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800',
|
||||
b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800',
|
||||
b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800',
|
||||
@@ -1204,7 +1204,7 @@ FW_VERSIONS = {
|
||||
]
|
||||
},
|
||||
CAR.KONA_HEV: {
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00OS IEB \x01 104 \x11 58520-CM000',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
@@ -1258,7 +1258,7 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00UMP LKAS AT USA LHD 1.01 1.01 95740-C6550 d01'
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00UM ESC \x0c 12 \x18\x05\x06 58910-C6330'
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7D0, None): [
|
||||
@@ -1272,7 +1272,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.KIA_EV6: {
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x8758520CV100\xf1\x00CV IEB \x02 101!\x10\x18 58520-CV100',
|
||||
],
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
@@ -1287,9 +1287,11 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.IONIQ_5: {
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00NE1 IEB \x07 106!\x11) 58520-GI010',
|
||||
b'\xf1\x8758520GI010\xf1\x00NE1 IEB \x07 106!\x11) 58520-GI010',
|
||||
b'\xf1\x00NE1 IEB \x08 104!\x04\x05 58520-GI000',
|
||||
b'\xf1\x8758520GI000\xf1\x00NE1 IEB \x08 104!\x04\x05 58520-GI000',
|
||||
],
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\x00NE MDPS R 1.00 1.06 57700GI000 4NEDR106',
|
||||
@@ -1301,6 +1303,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.02 99211-GI010 211206',
|
||||
b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813',
|
||||
],
|
||||
},
|
||||
CAR.TUCSON_HYBRID_4TH_GEN: {
|
||||
|
||||
@@ -90,7 +90,7 @@ class CarInterfaceBase(ABC):
|
||||
|
||||
@staticmethod
|
||||
@abstractmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
@@ -112,11 +112,14 @@ class CarInterfaceBase(ABC):
|
||||
ret = car.CarParams.new_message()
|
||||
ret.carFingerprint = candidate
|
||||
|
||||
# Car docs fields
|
||||
ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED']
|
||||
ret.autoResumeSng = True # describes whether car can resume from a stop automatically
|
||||
|
||||
# standard ALC params
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
ret.minSteerSpeed = 0.
|
||||
ret.wheelSpeedFactor = 1.0
|
||||
ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED']
|
||||
|
||||
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
|
||||
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
|
||||
@@ -222,7 +225,7 @@ class CarInterfaceBase(ABC):
|
||||
events.add(EventName.stockFcw)
|
||||
if cs_out.stockAeb:
|
||||
events.add(EventName.stockAeb)
|
||||
if cs_out.vEgo > MAX_CTRL_SPEED:
|
||||
if self.dragonconf.dpSpeedCheck and cs_out.vEgo > MAX_CTRL_SPEED:
|
||||
events.add(EventName.speedTooHigh)
|
||||
if self.dragonconf.dpAtl != 1 and cs_out.cruiseState.nonAdaptive:
|
||||
events.add(EventName.wrongCruiseMode)
|
||||
@@ -310,12 +313,12 @@ class CarStateBase(ABC):
|
||||
self.left_blinker_prev = False
|
||||
self.right_blinker_prev = False
|
||||
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
# Q = np.matrix([[0.0, 0.0], [0.0, 100.0]])
|
||||
# R = 0.3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, DT_CTRL], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
K=[[0.17406039], [1.65925647]])
|
||||
|
||||
def update_speed_kf(self, v_ego_raw):
|
||||
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
|
||||
@@ -15,7 +15,7 @@ class CarInterface(CarInterfaceBase):
|
||||
return float(accel) / 4.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "mazda"
|
||||
|
||||
@@ -72,7 +72,7 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x760, None): [
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
@@ -116,7 +116,7 @@ FW_VERSIONS = {
|
||||
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x760, None): [
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KBJ5-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'KL2K-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
@@ -173,7 +173,7 @@ FW_VERSIONS = {
|
||||
b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TK80-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x760, None): [
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'TK79-437K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
@@ -217,7 +217,7 @@ FW_VERSIONS = {
|
||||
b'GHP9-67Y10---41\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x760, None): [
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'B45A-437AS-0-08\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
@@ -248,7 +248,7 @@ FW_VERSIONS = {
|
||||
b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-E\000\000\000\000\000\000\000\000\000\000\000\000',
|
||||
],
|
||||
(Ecu.esp, 0x760, None): [
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'GBVH-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'GDDM-437K2-A\000\000\000\000\000\000\000\000\000\000\000\000',
|
||||
],
|
||||
@@ -275,7 +275,7 @@ FW_VERSIONS = {
|
||||
b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x760, None): [
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
|
||||
@@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase):
|
||||
return accel
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "mock"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)]
|
||||
|
||||
@@ -8,33 +8,32 @@ from selfdrive.car.nissan.values import CAR
|
||||
class CarInterface(CarInterfaceBase):
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "nissan"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)]
|
||||
ret.autoResumeSng = False
|
||||
|
||||
ret.steerLimitTimer = 1.0
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerRatio = 17
|
||||
|
||||
if candidate in (CAR.ROGUE, CAR.XTRAIL):
|
||||
ret.mass = 1610 + STD_CARGO_KG
|
||||
ret.wheelbase = 2.705
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
ret.steerRatio = 17
|
||||
elif candidate in (CAR.LEAF, CAR.LEAF_IC):
|
||||
ret.mass = 1610 + STD_CARGO_KG
|
||||
ret.wheelbase = 2.705
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
ret.steerRatio = 17
|
||||
elif candidate == CAR.ALTIMA:
|
||||
# Altima has EPS on C-CAN unlike the others that have it on V-CAN
|
||||
ret.safetyConfigs[0].safetyParam = 1 # EPS is on alternate bus
|
||||
ret.mass = 1492 + STD_CARGO_KG
|
||||
ret.wheelbase = 2.824
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
ret.steerRatio = 17
|
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
ret.radarOffCan = True
|
||||
|
||||
@@ -24,10 +24,7 @@ def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torqu
|
||||
|
||||
def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg):
|
||||
values = copy.copy(cruise_throttle_msg)
|
||||
can_bus = 2
|
||||
|
||||
if car_fingerprint == CAR.ALTIMA:
|
||||
can_bus = 1
|
||||
can_bus = 1 if car_fingerprint == CAR.ALTIMA else 2
|
||||
|
||||
values["CANCEL_BUTTON"] = 1
|
||||
values["NO_BUTTON_PRESSED"] = 0
|
||||
|
||||
@@ -95,7 +95,7 @@ FW_VERSIONS = {
|
||||
b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80',
|
||||
b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.esp, 0x740, None): [
|
||||
(Ecu.abs, 0x740, None): [
|
||||
b'476605SH1D',
|
||||
b'476605SK2A',
|
||||
],
|
||||
@@ -112,7 +112,7 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdCamera, 0x707, None): [
|
||||
b'284N86FR2A',
|
||||
],
|
||||
(Ecu.esp, 0x740, None): [
|
||||
(Ecu.abs, 0x740, None): [
|
||||
b'6FU1BD\x11\x02\x00\x02e\x95e\x80iX#\x01\x00\x00\x00\x00\x00\x80',
|
||||
b'6FU0AD\x11\x02\x00\x02e\x95e\x80iQ#\x01\x00\x00\x00\x00\x00\x80',
|
||||
],
|
||||
|
||||
@@ -9,12 +9,13 @@ from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS
|
||||
class CarInterface(CarInterfaceBase):
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "subaru"
|
||||
ret.radarOffCan = True
|
||||
ret.dashcamOnly = candidate in PREGLOBAL_CARS
|
||||
ret.autoResumeSng = False
|
||||
|
||||
if candidate in PREGLOBAL_CARS:
|
||||
ret.enableBsm = 0x25c in fingerprint[0]
|
||||
|
||||
@@ -74,7 +74,7 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.ASCENT: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa5 \x19\x02\x00',
|
||||
b'\xa5 !\002\000',
|
||||
b'\xf1\x82\xa5 \x19\x02\x00',
|
||||
@@ -104,24 +104,29 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.LEGACY: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa1\\ x04\x01',
|
||||
b'\xa1 \x03\x03'
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\x9b\xc0\x11\x00',
|
||||
b'\x9b\xc0\x11\x02'
|
||||
],
|
||||
(Ecu.fwdCamera, 0x787, None): [
|
||||
b'\x00\x00e\x80\x00\x1f@ \x19\x00',
|
||||
b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00'
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xde\"a0\x07',
|
||||
b'\xe2"aq\x07'
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xa5\xf6\x05@\x00',
|
||||
b'\xa7\xf6\x04@\x00'
|
||||
],
|
||||
},
|
||||
CAR.IMPREZA: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x7a\x94\x3f\x90\x00',
|
||||
b'\xa2 \x185\x00',
|
||||
b'\xa2 \x193\x00',
|
||||
@@ -196,7 +201,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.IMPREZA_2020: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa2 \0314\000',
|
||||
b'\xa2 \0313\000',
|
||||
b'\xa2 !i\000',
|
||||
@@ -234,7 +239,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.FORESTER: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa3 \x18\x14\x00',
|
||||
b'\xa3 \024\000',
|
||||
b'\xa3 \031\024\000',
|
||||
@@ -269,7 +274,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.FORESTER_PREGLOBAL: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x7d\x97\x14\x40',
|
||||
b'\xf1\x00\xbb\x0c\x04',
|
||||
],
|
||||
@@ -298,7 +303,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.LEGACY_PREGLOBAL: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'k\x97D\x00',
|
||||
b'[\xba\xc4\x03',
|
||||
b'{\x97D\x00',
|
||||
@@ -328,7 +333,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.OUTBACK_PREGLOBAL: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'{\x9a\xac\x00',
|
||||
b'k\x97\xac\x00',
|
||||
b'\x5b\xf7\xbc\x03',
|
||||
@@ -382,7 +387,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.OUTBACK_PREGLOBAL_2018: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x8b\x97\xac\x00',
|
||||
b'\x8b\x9a\xac\x00',
|
||||
b'\x9b\x97\xac\x00',
|
||||
@@ -425,7 +430,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.OUTBACK: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\xa1 \x06\x01',
|
||||
b'\xa1 \a\x00',
|
||||
b'\xa1 \b\001',
|
||||
|
||||
@@ -8,7 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "tesla"
|
||||
|
||||
|
||||
@@ -13,6 +13,7 @@ TESLA AP2 MODEL S: [.nan, 2.5, .nan]
|
||||
|
||||
# Guess
|
||||
FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan]
|
||||
FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan]
|
||||
FORD FOCUS 4TH GEN: [.nan, 1.5, .nan]
|
||||
###
|
||||
|
||||
|
||||
@@ -11,14 +11,17 @@ from common.conversions import Conversions as CV
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
# constants for fault workaround
|
||||
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
|
||||
MAX_STEER_RATE = 100 # deg/s
|
||||
MAX_STEER_RATE_FRAMES = 19
|
||||
MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut
|
||||
|
||||
# EPS allows user torque above threshold for 50 frames before permanently faulting
|
||||
MAX_USER_TORQUE = 500
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
UNLOCK_CMD = b'\x40\x05\x30\x11\x00\x40\x00\x00'
|
||||
LOCK_CMD = b'\x40\x05\x30\x11\x00\x80\x00\x00'
|
||||
LOCK_AT_SPEED = 25 * CV.KPH_TO_MS
|
||||
LOCK_AT_SPEED = 10 * CV.KPH_TO_MS
|
||||
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
@@ -29,6 +32,7 @@ class CarController:
|
||||
self.alert_active = False
|
||||
self.last_standstill = False
|
||||
self.standstill_req = False
|
||||
self.steer_rate_counter = 0
|
||||
|
||||
self.steer_rate_counter = 0
|
||||
|
||||
@@ -54,6 +58,7 @@ class CarController:
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
|
||||
|
||||
# gas and brake
|
||||
if self.CP.enableGasInterceptor and CC.longActive:
|
||||
@@ -77,28 +82,21 @@ class CarController:
|
||||
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits)
|
||||
|
||||
# EPS_STATUS->LKA_STATE either goes to 21 or 25 on rising edge of a steering fault and
|
||||
# the value seems to describe how many frames the steering rate was above 100 deg/s, so
|
||||
# cut torque with some margin for the lower state
|
||||
if CC.latActive and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE:
|
||||
# Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault
|
||||
if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE:
|
||||
self.steer_rate_counter += 1
|
||||
else:
|
||||
self.steer_rate_counter = 0
|
||||
|
||||
apply_steer_req = 1
|
||||
|
||||
if not CC.latActive:
|
||||
if not lat_active:
|
||||
apply_steer = 0
|
||||
apply_steer_req = 0
|
||||
elif self.steer_rate_counter >= MAX_STEER_RATE_FRAMES:
|
||||
elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES:
|
||||
apply_steer_req = 0
|
||||
self.steer_rate_counter = 0
|
||||
|
||||
# dp - try to avoid steering fault
|
||||
if self.dp_atl > 0 and abs(CS.out.steeringAngleDeg) > 500:
|
||||
apply_steer = 0
|
||||
apply_steer_req = 0
|
||||
|
||||
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
|
||||
# than CS.cruiseState.enabled. confirm they're not meaningfully different
|
||||
if not CC.enabled and CS.pcm_acc_status:
|
||||
|
||||
@@ -81,7 +81,7 @@ class CarState(CarStateBase):
|
||||
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
ret.standstill = ret.vEgoRaw < 0.001
|
||||
ret.standstill = ret.vEgoRaw == 0
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
|
||||
torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
|
||||
@@ -158,9 +158,17 @@ class CarState(CarStateBase):
|
||||
if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
|
||||
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
|
||||
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS
|
||||
cluster_set_speed = cp.vl["PCM_CRUISE_ALT"]["UI_SET_SPEED"]
|
||||
else:
|
||||
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
|
||||
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS
|
||||
cluster_set_speed = cp.vl["PCM_CRUISE_SM"]["UI_SET_SPEED"]
|
||||
|
||||
# UI_SET_SPEED is always non-zero when main is on, hide until first enable
|
||||
if ret.cruiseState.speed != 0:
|
||||
is_metric = cp.vl["BODY_CONTROL_STATE_2"]["UNITS"] in (1, 2)
|
||||
conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
|
||||
ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor
|
||||
|
||||
cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp
|
||||
|
||||
@@ -302,6 +310,7 @@ class CarState(CarStateBase):
|
||||
("DOOR_OPEN_RR", "BODY_CONTROL_STATE"),
|
||||
("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"),
|
||||
("PARKING_BRAKE", "BODY_CONTROL_STATE"),
|
||||
("UNITS", "BODY_CONTROL_STATE_2"),
|
||||
("TC_DISABLED", "ESP_CONTROL"),
|
||||
#("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"),
|
||||
("STEER_FRACTION", "STEER_ANGLE_SENSOR"),
|
||||
@@ -309,6 +318,7 @@ class CarState(CarStateBase):
|
||||
("CRUISE_ACTIVE", "PCM_CRUISE"),
|
||||
("CRUISE_STATE", "PCM_CRUISE"),
|
||||
("GAS_RELEASED", "PCM_CRUISE"),
|
||||
("UI_SET_SPEED", "PCM_CRUISE_SM"),
|
||||
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"),
|
||||
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"),
|
||||
("STEER_ANGLE", "STEER_TORQUE_SENSOR"),
|
||||
@@ -328,12 +338,14 @@ class CarState(CarStateBase):
|
||||
("LIGHT_STALK", 1),
|
||||
("BLINKERS_STATE", 0.15),
|
||||
("BODY_CONTROL_STATE", 3),
|
||||
("BODY_CONTROL_STATE_2", 2),
|
||||
("ESP_CONTROL", 3),
|
||||
("EPS_STATUS", 25),
|
||||
("BRAKE_MODULE", 40),
|
||||
("WHEEL_SPEEDS", 80),
|
||||
("STEER_ANGLE_SENSOR", 80),
|
||||
("PCM_CRUISE", 33),
|
||||
("PCM_CRUISE_SM", 1),
|
||||
("STEER_TORQUE_SENSOR", 50),
|
||||
#dp
|
||||
("ENGINE_RPM", 100),
|
||||
@@ -357,7 +369,9 @@ class CarState(CarStateBase):
|
||||
if CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
|
||||
signals.append(("MAIN_ON", "DSU_CRUISE"))
|
||||
signals.append(("SET_SPEED", "DSU_CRUISE"))
|
||||
signals.append(("UI_SET_SPEED", "PCM_CRUISE_ALT"))
|
||||
checks.append(("DSU_CRUISE", 5))
|
||||
checks.append(("PCM_CRUISE_ALT", 1))
|
||||
else:
|
||||
signals.append(("MAIN_ON", "PCM_CRUISE_2"))
|
||||
signals.append(("SET_SPEED", "PCM_CRUISE_2"))
|
||||
|
||||
@@ -26,7 +26,7 @@ class CarInterface(CarInterfaceBase):
|
||||
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disable_radar=False): # pylint: disable=dangerous-default-value
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
||||
ret.carName = "toyota"
|
||||
@@ -191,8 +191,6 @@ class CarInterface(CarInterfaceBase):
|
||||
# set_lat_tune(ret.lateralTuning, LatTunes.PID_N)
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS_TSS2)
|
||||
ret.steerActuatorDelay = 0.3
|
||||
#ret.steerRateCost = 1.25
|
||||
ret.steerLimitTimer = 0.5
|
||||
|
||||
elif candidate == CAR.MIRAI:
|
||||
stop_and_go = True
|
||||
@@ -240,6 +238,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.openpilotLongitudinalControl = True
|
||||
|
||||
if not ret.openpilotLongitudinalControl:
|
||||
ret.autoResumeSng = False
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL
|
||||
|
||||
# we can't use the fingerprint to detect this reliably, since
|
||||
@@ -258,7 +257,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.vEgoStopping = 0.2 # car is near 0.1 to 0.2 when car starts requesting stopping accel
|
||||
ret.vEgoStarting = 0.2 # needs to be > or == vEgoStopping
|
||||
ret.stopAccel = -2.0 # Toyota requests -0.4 when stopped
|
||||
ret.stoppingDecelRate = 0.8 # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4
|
||||
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4
|
||||
ret.longitudinalActuatorDelayLowerBound = 0.3
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.3
|
||||
else:
|
||||
|
||||
@@ -33,14 +33,14 @@ def set_long_tune(tune, name):
|
||||
if name == LongTunes.TSS2 or name == LongTunes.PEDAL:
|
||||
tune.deadzoneBP = [0., 8.05]
|
||||
tune.deadzoneV = [.0, .14]
|
||||
tune.kpBP = [0., 5., 20.]
|
||||
tune.kpV = [1.3, 1.0, 0.7]
|
||||
tune.kiBP = [0., 5., 12., 20., 27.]
|
||||
tune.kiV = [.35, .23, .20, .17, .1]
|
||||
tune.kpBP = [0., 5., 20., 30.]
|
||||
tune.kpV = [1.3, 1.0, 0.7, 0.1]
|
||||
tune.kiBP = [0., 0.07, 8, 13., 18., 20., 24., 25., 27., 33.]
|
||||
tune.kiV = [.01, .1, .15, .21, .175, .18, .2, .01, .001, .0001]
|
||||
# Default longitudinal tune
|
||||
elif name == LongTunes.TSS:
|
||||
tune.deadzoneBP = [0., 9.]
|
||||
tune.deadzoneV = [0., .15]
|
||||
tune.deadzoneV = [.0, .15]
|
||||
tune.kpBP = [0., 5., 35.]
|
||||
tune.kiBP = [0., 35.]
|
||||
tune.kpV = [3.6, 2.4, 1.5]
|
||||
@@ -54,14 +54,22 @@ def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_de
|
||||
|
||||
if name == LatTunes.INDI_PRIUS_TSS2:
|
||||
tune.init('indi')
|
||||
tune.indi.innerLoopGainBP = [20, 24, 30]
|
||||
tune.indi.innerLoopGainV = [7.25, 7.5, 9]
|
||||
tune.indi.outerLoopGainBP = [20, 24, 30]
|
||||
tune.indi.outerLoopGainV = [6, 7.25, 6]
|
||||
tune.indi.timeConstantBP = [20, 24]
|
||||
tune.indi.timeConstantV = [2.0, 2.2]
|
||||
tune.indi.actuatorEffectivenessBP = [20, 24]
|
||||
tune.indi.actuatorEffectivenessV = [2, 3]
|
||||
#tune.indi.innerLoopGainBP = [20, 24, 30]
|
||||
#tune.indi.innerLoopGainV = [7.25, 7.5, 9]
|
||||
#tune.indi.outerLoopGainBP = [20, 24, 30]
|
||||
#tune.indi.outerLoopGainV = [6, 7.25, 6]
|
||||
#tune.indi.timeConstantBP = [20, 24]
|
||||
#tune.indi.timeConstantV = [2.0, 2.2]
|
||||
#tune.indi.actuatorEffectivenessBP = [20, 24]
|
||||
#tune.indi.actuatorEffectivenessV = [2, 3]
|
||||
tune.indi.innerLoopGainBP = [0.]
|
||||
tune.indi.innerLoopGainV = [15]
|
||||
tune.indi.outerLoopGainBP = [0.]
|
||||
tune.indi.outerLoopGainV = [17]
|
||||
tune.indi.timeConstantBP = [0.]
|
||||
tune.indi.timeConstantV = [4.5]
|
||||
tune.indi.actuatorEffectivenessBP = [0.]
|
||||
tune.indi.actuatorEffectivenessV = [15]
|
||||
elif 'PID' in str(name):
|
||||
tune.init('pid')
|
||||
tune.pid.kiBP = [0.0]
|
||||
|
||||
@@ -200,9 +200,9 @@ STATIC_DSU_MSGS = [
|
||||
]
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.CHR_TSS2: {(Ecu.esp, 0xfff, None): [b'\x00']},
|
||||
CAR.CHR_TSS2: {(Ecu.eps, 0xfff, None): [b'\x00']},
|
||||
CAR.AVALON: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152607060\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
@@ -227,7 +227,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.AVALON_2019: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152607140\x00\x00\x00\x00\x00\x00',
|
||||
b'F152607171\x00\x00\x00\x00\x00\x00',
|
||||
b'F152607110\x00\x00\x00\x00\x00\x00',
|
||||
@@ -255,7 +255,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.AVALONH_2019: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152641040\x00\x00\x00\x00\x00\x00',
|
||||
b'F152641061\x00\x00\x00\x00\x00\x00',
|
||||
b'F152641050\x00\x00\x00\x00\x00\x00',
|
||||
@@ -282,7 +282,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.AVALON_TSS2: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F152607240\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152607280\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -302,7 +302,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.AVALONH_TSS2: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152641080\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
@@ -358,7 +358,7 @@ FW_VERSIONS = {
|
||||
b'8821F0608200 ',
|
||||
b'8821F0609100 ',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152606210\x00\x00\x00\x00\x00\x00',
|
||||
b'F152606230\x00\x00\x00\x00\x00\x00',
|
||||
b'F152606270\x00\x00\x00\x00\x00\x00',
|
||||
@@ -422,7 +422,7 @@ FW_VERSIONS = {
|
||||
b'\x028966306S0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
|
||||
b'\x028966306S1100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152633214\x00\x00\x00\x00\x00\x00',
|
||||
b'F152633660\x00\x00\x00\x00\x00\x00',
|
||||
b'F152633712\x00\x00\x00\x00\x00\x00',
|
||||
@@ -488,7 +488,7 @@ FW_VERSIONS = {
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B33630\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F152606370\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152606390\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152606400\x00\x00\x00\x00\x00\x00',
|
||||
@@ -514,7 +514,7 @@ FW_VERSIONS = {
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B33630\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152633D00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x700, None): [
|
||||
@@ -549,7 +549,7 @@ FW_VERSIONS = {
|
||||
b'8821FF406000 ',
|
||||
b'8821FF407100 ',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152610020\x00\x00\x00\x00\x00\x00',
|
||||
b'F152610153\x00\x00\x00\x00\x00\x00',
|
||||
b'F152610210\x00\x00\x00\x00\x00\x00',
|
||||
@@ -601,7 +601,7 @@ FW_VERSIONS = {
|
||||
b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
|
||||
b'\x0189663F438000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152610012\x00\x00\x00\x00\x00\x00',
|
||||
b'F152610013\x00\x00\x00\x00\x00\x00',
|
||||
b'F152610014\x00\x00\x00\x00\x00\x00',
|
||||
@@ -660,7 +660,7 @@ FW_VERSIONS = {
|
||||
b'881510201100\x00\x00\x00\x00',
|
||||
b'881510201200\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152602190\x00\x00\x00\x00\x00\x00',
|
||||
b'F152602191\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -720,6 +720,7 @@ FW_VERSIONS = {
|
||||
b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
|
||||
b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
|
||||
b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
|
||||
b'\x03312N6200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
|
||||
b'\x02312K4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x02312U5000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -736,7 +737,7 @@ FW_VERSIONS = {
|
||||
b'\x018965B12510\x00\x00\x00\x00\x00\x00',
|
||||
b'\x018965B1256000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F152602280\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152602560\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152602590\x00\x00\x00\x00\x00\x00',
|
||||
@@ -814,7 +815,7 @@ FW_VERSIONS = {
|
||||
b'\x018965B12520\x00\x00\x00\x00\x00\x00',
|
||||
b'\x018965B12530\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152612590\x00\x00\x00\x00\x00\x00',
|
||||
b'F152612691\x00\x00\x00\x00\x00\x00',
|
||||
b'F152612692\x00\x00\x00\x00\x00\x00',
|
||||
@@ -886,7 +887,7 @@ FW_VERSIONS = {
|
||||
b'8965B48150\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B48210\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [b'F15260E011\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.abs, 0x7b0, None): [b'F15260E011\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
b'881510E01100\x00\x00\x00\x00',
|
||||
b'881510E01200\x00\x00\x00\x00',
|
||||
@@ -904,7 +905,7 @@ FW_VERSIONS = {
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B48160\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152648541\x00\x00\x00\x00\x00\x00',
|
||||
b'F152648542\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -929,7 +930,7 @@ FW_VERSIONS = {
|
||||
b'8965B48310\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B48320\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F15260E051\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F15260E061\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F15260E110\x00\x00\x00\x00\x00\x00',
|
||||
@@ -948,7 +949,9 @@ FW_VERSIONS = {
|
||||
b'\x01896630EB2200\x00\x00\x00\x00',
|
||||
b'\x01896630EC4000\x00\x00\x00\x00',
|
||||
b'\x01896630ED9000\x00\x00\x00\x00',
|
||||
b'\x01896630ED9100\x00\x00\x00\x00',
|
||||
b'\x01896630EE1000\x00\x00\x00\x00',
|
||||
b'\x01896630EE1100\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'\x018821F3301400\x00\x00\x00\x00',
|
||||
@@ -966,7 +969,7 @@ FW_VERSIONS = {
|
||||
b'8965B48241\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B48310\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F15264872300\x00\x00\x00\x00',
|
||||
b'\x01F15264872400\x00\x00\x00\x00',
|
||||
b'\x01F15264872500\x00\x00\x00\x00',
|
||||
@@ -976,15 +979,16 @@ FW_VERSIONS = {
|
||||
b'\x01F152648J6000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x01896630EE4000\x00\x00\x00\x00',
|
||||
b'\x01896630EE6000\x00\x00\x00\x00',
|
||||
b'\x01896630E67000\x00\x00\x00\x00',
|
||||
b'\x01896630EA1000\x00\x00\x00\x00',
|
||||
b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
b'\x01896630EE4000\x00\x00\x00\x00',
|
||||
b'\x01896630EE4100\x00\x00\x00\x00',
|
||||
b'\x01896630EE6000\x00\x00\x00\x00',
|
||||
b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'\x018821F3301400\x00\x00\x00\x00',
|
||||
@@ -1013,7 +1017,7 @@ FW_VERSIONS = {
|
||||
b'\x02353P7000\x00\x00\x00\x00\x00\x00\x00\x00530J5000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x02353P9000\x00\x00\x00\x00\x00\x00\x00\x00553C1000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152653300\x00\x00\x00\x00\x00\x00',
|
||||
b'F152653301\x00\x00\x00\x00\x00\x00',
|
||||
b'F152653310\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1097,7 +1101,7 @@ FW_VERSIONS = {
|
||||
b'8965B47050\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B47060\x00\x00\x00\x00\x00\x00', # This is the EPS with good angle sensor
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152647290\x00\x00\x00\x00\x00\x00',
|
||||
b'F152647300\x00\x00\x00\x00\x00\x00',
|
||||
b'F152647310\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1138,7 +1142,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.PRIUS_V: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152647280\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
@@ -1171,7 +1175,7 @@ FW_VERSIONS = {
|
||||
b'8965B42082\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B42083\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F15260R102\x00\x00\x00\x00\x00\x00',
|
||||
b'F15260R103\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642493\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1208,7 +1212,7 @@ FW_VERSIONS = {
|
||||
b'8965B42162\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B42163\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152642090\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642110\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642120\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1270,7 +1274,7 @@ FW_VERSIONS = {
|
||||
b'\x02896634A47000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00',
|
||||
b'\x028966342Z8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F15260R210\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F15260R220\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F15260R290\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1311,7 +1315,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.RAV4_TSS2_2022: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F15260R350\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F15260R361\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -1351,7 +1355,7 @@ FW_VERSIONS = {
|
||||
b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152642291\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642290\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642322\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1390,7 +1394,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.RAV4H_TSS2_2022: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F15264283100\x00\x00\x00\x00',
|
||||
b'\x01F15264286200\x00\x00\x00\x00',
|
||||
b'\x01F15264286100\x00\x00\x00\x00',
|
||||
@@ -1438,7 +1442,7 @@ FW_VERSIONS = {
|
||||
b'8965B45080\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B45082\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152608130\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
@@ -1457,7 +1461,7 @@ FW_VERSIONS = {
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
b'881517601100\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152676144\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
@@ -1479,7 +1483,7 @@ FW_VERSIONS = {
|
||||
b'\x018966333X6000\x00\x00\x00\x00',
|
||||
b'\x01896633T07000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F152606281\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152606340\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152606461\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1516,7 +1520,7 @@ FW_VERSIONS = {
|
||||
b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00',
|
||||
b'\x01896633T38000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152633423\x00\x00\x00\x00\x00\x00',
|
||||
b'F152633680\x00\x00\x00\x00\x00\x00',
|
||||
b'F152633681\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1549,7 +1553,7 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\x02333M4200\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152633171\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
@@ -1575,7 +1579,7 @@ FW_VERSIONS = {
|
||||
b'\x01896637854000\x00\x00\x00\x00',
|
||||
b'\x01896637878000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152678130\x00\x00\x00\x00\x00\x00',
|
||||
b'F152678140\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -1602,7 +1606,7 @@ FW_VERSIONS = {
|
||||
b'\x018966378B3000\x00\x00\x00\x00',
|
||||
b'\x018966378G3000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F152678221\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
@@ -1622,7 +1626,7 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152678210\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
@@ -1643,7 +1647,7 @@ FW_VERSIONS = {
|
||||
b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152678160\x00\x00\x00\x00\x00\x00',
|
||||
b'F152678170\x00\x00\x00\x00\x00\x00',
|
||||
b'F152678171\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1670,7 +1674,7 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\x0232484000\x00\x00\x00\x00\x00\x00\x00\x0052422000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152624221\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
@@ -1706,7 +1710,7 @@ FW_VERSIONS = {
|
||||
b'\x018966348R8500\x00\x00\x00\x00',
|
||||
b'\x018966348W1300\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152648472\x00\x00\x00\x00\x00\x00',
|
||||
b'F152648473\x00\x00\x00\x00\x00\x00',
|
||||
b'F152648492\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1753,7 +1757,7 @@ FW_VERSIONS = {
|
||||
b'\x02348V6000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x02348Z3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152648361\x00\x00\x00\x00\x00\x00',
|
||||
b'F152648501\x00\x00\x00\x00\x00\x00',
|
||||
b'F152648502\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1801,7 +1805,7 @@ FW_VERSIONS = {
|
||||
b'\x01896634D43000\x00\x00\x00\x00',
|
||||
b'\x01896634D44000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F15260E041\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152648781\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1831,7 +1835,7 @@ FW_VERSIONS = {
|
||||
b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x02348X4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152648831\x00\x00\x00\x00\x00\x00',
|
||||
b'F152648891\x00\x00\x00\x00\x00\x00',
|
||||
b'F152648D00\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1861,7 +1865,7 @@ FW_VERSIONS = {
|
||||
b'\x038966347C5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707101\x00\x00\x00\x00',
|
||||
b'\x038966347C5100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707101\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152647500\x00\x00\x00\x00\x00\x00',
|
||||
b'F152647510\x00\x00\x00\x00\x00\x00',
|
||||
b'F152647520\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1881,8 +1885,8 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.MIRAI: {
|
||||
(Ecu.esp, 0x7D1, None): [b'\x01898A36203000\x00\x00\x00\x00',],
|
||||
(Ecu.esp, 0x7B0, None): [ # a second ESP ECU
|
||||
(Ecu.abs, 0x7D1, None): [b'\x01898A36203000\x00\x00\x00\x00',],
|
||||
(Ecu.abs, 0x7B0, None): [ # a second ABS ECU
|
||||
b'\x01F15266203200\x00\x00\x00\x00',
|
||||
b'\x01F15266203500\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -1915,7 +1919,7 @@ FW_VERSIONS = {
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B58040\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
(Ecu.abs, 0x7b0, None): [
|
||||
b'F152658341\x00\x00\x00\x00\x00\x00'
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
|
||||
@@ -5,7 +5,6 @@ import traceback
|
||||
|
||||
import cereal.messaging as messaging
|
||||
import panda.python.uds as uds
|
||||
from panda.python.uds import FUNCTIONAL_ADDRS
|
||||
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
from system.swaglog import cloudlog
|
||||
|
||||
@@ -24,10 +23,11 @@ def is_valid_vin(vin: str):
|
||||
|
||||
|
||||
def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
|
||||
addrs = [0x7e0, 0x7e2, 0x18da10f1, 0x18da0ef1] # engine, VMCU, 29-bit engine, PGM-FI
|
||||
for i in range(retry):
|
||||
for request, response in ((UDS_VIN_REQUEST, UDS_VIN_RESPONSE), (OBD_VIN_REQUEST, OBD_VIN_RESPONSE)):
|
||||
try:
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [request, ], [response, ], functional_addr=True, debug=debug)
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, addrs, [request, ], [response, ], debug=debug)
|
||||
for (addr, rx_addr), vin in query.get_data(timeout).items():
|
||||
|
||||
# Honda Bosch response starts with a length, trim to correct length
|
||||
|
||||
@@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase):
|
||||
self.cp_ext = self.cp_cam
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "volkswagen"
|
||||
ret.radarOffCan = True
|
||||
@@ -49,9 +49,10 @@ class CarInterface(CarInterfaceBase):
|
||||
# Panda ALLOW_DEBUG firmware required.
|
||||
ret.dashcamOnly = True
|
||||
|
||||
if disable_radar and ret.networkLocation == NetworkLocation.gateway:
|
||||
if experimental_long and ret.networkLocation == NetworkLocation.gateway:
|
||||
# Proof-of-concept, prep for E2E only. No radar points available. Follow-to-stop not yet supported, but should
|
||||
# be simple to add when a suitable test car becomes available. Panda ALLOW_DEBUG firmware required.
|
||||
ret.experimentalLongitudinalAvailable = True
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
|
||||
|
||||
@@ -197,6 +198,7 @@ class CarInterface(CarInterfaceBase):
|
||||
else:
|
||||
raise ValueError(f"unsupported car {candidate}")
|
||||
|
||||
ret.autoResumeSng = ret.minEnableSpeed == -1
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
ret.centerToFront = ret.wheelbase * 0.45
|
||||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
|
||||
@@ -174,7 +174,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
|
||||
VWCarInfo("Volkswagen CC 2018-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
],
|
||||
CAR.ATLAS_MK1: [
|
||||
VWCarInfo("Volkswagen Atlas 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Atlas 2018-23", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Atlas Cross Sport 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Teramont 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Teramont Cross Sport 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
@@ -275,6 +275,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x8703H906026AA\xf1\x899970',
|
||||
b'\xf1\x8703H906026AJ\xf1\x890638',
|
||||
b'\xf1\x8703H906026AT\xf1\x891922',
|
||||
b'\xf1\x8703H906026BC\xf1\x892664',
|
||||
b'\xf1\x8703H906026F \xf1\x896696',
|
||||
b'\xf1\x8703H906026F \xf1\x899970',
|
||||
b'\xf1\x8703H906026J \xf1\x896026',
|
||||
@@ -287,12 +288,14 @@ FW_VERSIONS = {
|
||||
b'\xf1\x8709G927158DR\xf1\x893536',
|
||||
b'\xf1\x8709G927158DR\xf1\x893742',
|
||||
b'\xf1\x8709G927158FT\xf1\x893835',
|
||||
b'\xf1\x8709G927158GL\xf1\x893939',
|
||||
],
|
||||
(Ecu.srs, 0x715, None): [
|
||||
b'\xf1\x873Q0959655BC\xf1\x890503\xf1\x82\0161914151912001103111122031200',
|
||||
b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\0162214152212001105141122052900',
|
||||
b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\0162214152212001105141122052900',
|
||||
b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105161122052J00',
|
||||
b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1115151112001105171122052J00',
|
||||
],
|
||||
(Ecu.eps, 0x712, None): [
|
||||
b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1',
|
||||
@@ -300,6 +303,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x757, None): [
|
||||
b'\xf1\x872Q0907572AA\xf1\x890396',
|
||||
b'\xf1\x872Q0907572R \xf1\x890372',
|
||||
b'\xf1\x872Q0907572T \xf1\x890383',
|
||||
b'\xf1\x875Q0907572H \xf1\x890620',
|
||||
@@ -370,6 +374,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x870D9300012 \xf1\x895045',
|
||||
b'\xf1\x870D9300014M \xf1\x895004',
|
||||
b'\xf1\x870D9300014Q \xf1\x895006',
|
||||
b'\xf1\x870D9300020Q \xf1\x895201',
|
||||
b'\xf1\x870D9300020S \xf1\x895201',
|
||||
b'\xf1\x870D9300040A \xf1\x893613',
|
||||
b'\xf1\x870D9300040S \xf1\x894311',
|
||||
@@ -421,6 +426,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1',
|
||||
b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A01A18A1',
|
||||
b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\x0571A0JA16A1',
|
||||
b'\xf1\x873QM909144 \xf1\x895072\xf1\x82\x0571A01714A1',
|
||||
b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820519A9040203',
|
||||
b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00441A1',
|
||||
b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00608A1',
|
||||
@@ -738,6 +744,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x8704E906023BL\xf1\x895190',
|
||||
b'\xf1\x8704E906027CJ\xf1\x897798',
|
||||
b'\xf1\x8704L997022N \xf1\x899459',
|
||||
b'\xf1\x875G0906259A \xf1\x890004',
|
||||
b'\xf1\x875G0906259L \xf1\x890002',
|
||||
b'\xf1\x875G0906259Q \xf1\x890002',
|
||||
b'\xf1\x878V0906259F \xf1\x890002',
|
||||
@@ -750,6 +757,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x870CW300044T \xf1\x895245',
|
||||
b'\xf1\x870CW300048 \xf1\x895201',
|
||||
b'\xf1\x870D9300012 \xf1\x894912',
|
||||
b'\xf1\x870D9300012K \xf1\x894513',
|
||||
b'\xf1\x870D9300013B \xf1\x894931',
|
||||
b'\xf1\x870D9300041N \xf1\x894512',
|
||||
b'\xf1\x870D9300043T \xf1\x899699',
|
||||
@@ -766,6 +774,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x875Q0959655AM\xf1\x890315\xf1\x82\x1311111111111111311411011231129321212100',
|
||||
b'\xf1\x875Q0959655AM\xf1\x890318\xf1\x82\x1311111111111112311411011531159321212100',
|
||||
b'\xf1\x875Q0959655BJ\xf1\x890339\xf1\x82\x1311110011131100311111011731179321342100',
|
||||
b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\x13111112111111--241115141112221291163221',
|
||||
b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\023111112111111--171115141112221291163221',
|
||||
b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221',
|
||||
b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13121111111111--341117141212231291163221',
|
||||
@@ -779,6 +788,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521G0G809A1',
|
||||
b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00303A0',
|
||||
b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00803A0',
|
||||
b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\x0503G0G803A0',
|
||||
b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\00516G00804A1',
|
||||
b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1',
|
||||
],
|
||||
|
||||
@@ -12,10 +12,10 @@ import cereal.messaging as messaging
|
||||
from common.conversions import Conversions as CV
|
||||
from panda import ALTERNATIVE_EXPERIENCE
|
||||
from system.swaglog import cloudlog
|
||||
from system.version import get_short_branch
|
||||
from system.version import is_tested_branch, get_short_branch
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
|
||||
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
|
||||
from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET
|
||||
from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, update_v_cruise, initialize_v_cruise
|
||||
from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature
|
||||
from selfdrive.controls.lib.latcontrol import LatControl
|
||||
@@ -39,7 +39,7 @@ LANE_DEPARTURE_THRESHOLD = 0.1
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
SIMULATION = "SIMULATION" in os.environ
|
||||
NOSENSOR = "NOSENSOR" in os.environ
|
||||
IGNORE_PROCESSES = {"uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "statsd", "mapd",
|
||||
IGNORE_PROCESSES = {"uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "statsd", "mapd", "navd"
|
||||
"logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad", "laikad"} | \
|
||||
{k for k, v in managed_processes.items() if not v.enabled}
|
||||
|
||||
@@ -115,9 +115,8 @@ class Controls:
|
||||
|
||||
# dp
|
||||
self.sm['dragonConf'].dpAtl = int(Params().get('dp_atl', encoding='utf8'))
|
||||
self.sm['dragonConf'].dpCameraOffset = CAMERA_OFFSET * 100 # 0.04 x 100 = 4
|
||||
self.dp_camera_offset_last = None
|
||||
self.camera_offset = CAMERA_OFFSET
|
||||
self.dp_temp_check = Params().get_bool('dp_temp_check')
|
||||
self.dp_vag_resume_fix = Params().get_bool('dp_vag_resume_fix')
|
||||
|
||||
# set alternative experiences from parameters
|
||||
self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator")
|
||||
@@ -148,10 +147,14 @@ class Controls:
|
||||
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
|
||||
self.CP.safetyConfigs = [safety_config]
|
||||
|
||||
if is_tested_branch():
|
||||
self.CP.experimentalLongitudinalAvailable = False
|
||||
|
||||
# Write CarParams for radard
|
||||
cp_bytes = self.CP.to_bytes()
|
||||
params.put("CarParams", cp_bytes)
|
||||
put_nonblocking("CarParamsCache", cp_bytes)
|
||||
put_nonblocking("CarParamsPersistent", cp_bytes)
|
||||
|
||||
self.CC = car.CarControl.new_message()
|
||||
self.CS_prev = car.CarState.new_message()
|
||||
@@ -268,7 +271,7 @@ class Controls:
|
||||
self.events.add_from_msg(CS.events)
|
||||
|
||||
# Create events for temperature, disk space, and memory
|
||||
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
|
||||
if self.dp_temp_check and self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
|
||||
self.events.add(EventName.overheat)
|
||||
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
|
||||
# under 7% of space free no enable allowed
|
||||
@@ -696,9 +699,6 @@ class Controls:
|
||||
self.button_timers[b.type.raw] = 1 if b.pressed else 0
|
||||
|
||||
def publish_logs(self, CS, start_time, CC, lac_log):
|
||||
if self.sm['dragonConf'].dpCameraOffset != self.dp_camera_offset_last:
|
||||
self.camera_offset = self.sm['dragonConf'].dpCameraOffset * 0.01
|
||||
self.dp_camera_offset_last = self.sm['dragonConf'].dpCameraOffset
|
||||
|
||||
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
|
||||
|
||||
@@ -715,11 +715,11 @@ class Controls:
|
||||
if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
|
||||
CC.cruiseControl.cancel = True
|
||||
|
||||
speeds = self.sm['longitudinalPlan'].speeds
|
||||
if len(speeds):
|
||||
if self.sm['dragonConf'].dpAtl == 1 and self.sm['longitudinalPlan'].hasLead:
|
||||
CC.cruiseControl.resume = CS.cruiseActualEnabled and CS.cruiseState.standstill and speeds[-1] > 0.1
|
||||
else:
|
||||
if self.dp_vag_resume_fix or self.sm['dragonConf'].dpAtl == 1:
|
||||
CC.cruiseControl.resume = CS.cruiseActualEnabled and CS.cruiseState.standstill
|
||||
else:
|
||||
speeds = self.sm['longitudinalPlan'].speeds
|
||||
if len(speeds):
|
||||
CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
|
||||
|
||||
hudControl = CC.hudControl
|
||||
@@ -738,14 +738,14 @@ class Controls:
|
||||
model_v2 = self.sm['modelV2']
|
||||
desire_prediction = model_v2.meta.desirePrediction
|
||||
if len(desire_prediction) and ldw_allowed:
|
||||
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
|
||||
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
|
||||
right_lane_visible = model_v2.laneLineProbs[2] > 0.5
|
||||
left_lane_visible = model_v2.laneLineProbs[1] > 0.5
|
||||
l_lane_change_prob = desire_prediction[Desire.laneChangeLeft - 1]
|
||||
r_lane_change_prob = desire_prediction[Desire.laneChangeRight - 1]
|
||||
|
||||
lane_lines = model_v2.laneLines
|
||||
l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + self.camera_offset))
|
||||
r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - self.camera_offset))
|
||||
l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET))
|
||||
r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET))
|
||||
|
||||
hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
|
||||
hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
|
||||
|
||||
@@ -5,7 +5,7 @@ from common.realtime import DT_MDL, sec_since_boot
|
||||
LaneChangeState = log.LateralPlan.LaneChangeState
|
||||
LaneChangeDirection = log.LateralPlan.LaneChangeDirection
|
||||
|
||||
LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS
|
||||
LANE_CHANGE_SPEED_MIN = 15 * CV.MPH_TO_MS
|
||||
LANE_CHANGE_TIME_MAX = 10.
|
||||
|
||||
DESIRES = {
|
||||
|
||||
@@ -1,113 +0,0 @@
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from common.filter_simple import FirstOrderFilter
|
||||
from common.numpy_fast import interp
|
||||
from common.realtime import DT_MDL
|
||||
from system.swaglog import cloudlog
|
||||
from system.hardware import EON, TICI
|
||||
|
||||
|
||||
TRAJECTORY_SIZE = 33
|
||||
# camera offset is meters from center car to camera
|
||||
# model path is in the frame of the camera. Empirically
|
||||
# the model knows the difference between TICI and EON
|
||||
# so a path offset is not needed
|
||||
PATH_OFFSET = 0.00
|
||||
if EON:
|
||||
CAMERA_OFFSET = -0.06
|
||||
elif TICI:
|
||||
CAMERA_OFFSET = 0.04
|
||||
else:
|
||||
CAMERA_OFFSET = 0.0
|
||||
|
||||
|
||||
class LanePlanner:
|
||||
def __init__(self, wide_camera=False):
|
||||
self.ll_t = np.zeros((TRAJECTORY_SIZE,))
|
||||
self.ll_x = np.zeros((TRAJECTORY_SIZE,))
|
||||
self.lll_y = np.zeros((TRAJECTORY_SIZE,))
|
||||
self.rll_y = np.zeros((TRAJECTORY_SIZE,))
|
||||
self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL)
|
||||
self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL)
|
||||
self.lane_width = 3.7
|
||||
|
||||
self.lll_prob = 0.
|
||||
self.rll_prob = 0.
|
||||
self.d_prob = 0.
|
||||
|
||||
self.lll_std = 0.
|
||||
self.rll_std = 0.
|
||||
|
||||
self.l_lane_change_prob = 0.
|
||||
self.r_lane_change_prob = 0.
|
||||
|
||||
self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET
|
||||
self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET
|
||||
|
||||
self.dp_camera_offset = None
|
||||
self.wide_camera = wide_camera
|
||||
|
||||
def update_offsets(self, camera_offset):
|
||||
if self.dp_camera_offset != camera_offset:
|
||||
self.dp_camera_offset = camera_offset
|
||||
self.camera_offset = (camera_offset - 8 if self.wide_camera else camera_offset) * 0.01
|
||||
|
||||
def parse_model(self, md):
|
||||
lane_lines = md.laneLines
|
||||
if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE:
|
||||
self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2
|
||||
# left and right ll x is the same
|
||||
self.ll_x = lane_lines[1].x
|
||||
self.lll_y = np.array(lane_lines[1].y) + self.camera_offset
|
||||
self.rll_y = np.array(lane_lines[2].y) + self.camera_offset
|
||||
self.lll_prob = md.laneLineProbs[1]
|
||||
self.rll_prob = md.laneLineProbs[2]
|
||||
self.lll_std = md.laneLineStds[1]
|
||||
self.rll_std = md.laneLineStds[2]
|
||||
|
||||
desire_state = md.meta.desireState
|
||||
if len(desire_state):
|
||||
self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
|
||||
self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
|
||||
|
||||
def get_d_path(self, v_ego, path_t, path_xyz):
|
||||
# Reduce reliance on lanelines that are too far apart or
|
||||
# will be in a few seconds
|
||||
path_xyz[:, 1] += self.path_offset
|
||||
l_prob, r_prob = self.lll_prob, self.rll_prob
|
||||
width_pts = self.rll_y - self.lll_y
|
||||
prob_mods = []
|
||||
for t_check in (0.0, 1.5, 3.0):
|
||||
width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts)
|
||||
prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
|
||||
mod = min(prob_mods)
|
||||
l_prob *= mod
|
||||
r_prob *= mod
|
||||
|
||||
# Reduce reliance on uncertain lanelines
|
||||
l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0])
|
||||
r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0])
|
||||
l_prob *= l_std_mod
|
||||
r_prob *= r_std_mod
|
||||
|
||||
# Find current lanewidth
|
||||
self.lane_width_certainty.update(l_prob * r_prob)
|
||||
current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
|
||||
self.lane_width_estimate.update(current_lane_width)
|
||||
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
|
||||
self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \
|
||||
(1 - self.lane_width_certainty.x) * speed_lane_width
|
||||
|
||||
clipped_lane_width = min(4.0, self.lane_width)
|
||||
path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
|
||||
path_from_right_lane = self.rll_y - clipped_lane_width / 2.0
|
||||
|
||||
self.d_prob = l_prob + r_prob - l_prob * r_prob
|
||||
lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
|
||||
safe_idxs = np.isfinite(self.ll_t)
|
||||
if safe_idxs[0]:
|
||||
lane_path_y_interp = np.interp(path_t, self.ll_t[safe_idxs], lane_path_y[safe_idxs])
|
||||
path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
|
||||
else:
|
||||
cloudlog.warning("Lateral mpc - NaNs in laneline times, ignoring")
|
||||
return path_xyz
|
||||
@@ -0,0 +1,450 @@
|
||||
{
|
||||
"acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
|
||||
"acados_lib_path": "/data/openpilot/third_party/acados/include/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": [
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vu_0": [
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vx": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vx_0": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vx_e": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vz": [],
|
||||
"Vz_0": [],
|
||||
"W": [
|
||||
[
|
||||
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
|
||||
]
|
||||
],
|
||||
"W_e": [
|
||||
[
|
||||
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
|
||||
],
|
||||
"yref_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_e": [
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zl": [],
|
||||
"zl_e": [],
|
||||
"zu": [],
|
||||
"zu_e": []
|
||||
},
|
||||
"cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
|
||||
"dims": {
|
||||
"N": 16,
|
||||
"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": 3,
|
||||
"ny_0": 3,
|
||||
"ny_e": 2,
|
||||
"nz": 0
|
||||
},
|
||||
"model": {
|
||||
"dyn_disc_fun": null,
|
||||
"dyn_disc_fun_jac": null,
|
||||
"dyn_disc_fun_jac_hess": null,
|
||||
"dyn_ext_fun_type": "casadi",
|
||||
"dyn_source_discrete": null,
|
||||
"gnsf": {
|
||||
"nontrivial_f_LO": 1,
|
||||
"purely_linear": 0
|
||||
},
|
||||
"name": "lat"
|
||||
},
|
||||
"parameter_values": [
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"problem_class": "OCP",
|
||||
"simulink_opts": {
|
||||
"inputs": {
|
||||
"cost_W": 0,
|
||||
"cost_W_0": 0,
|
||||
"cost_W_e": 0,
|
||||
"lbu": 1,
|
||||
"lbx": 1,
|
||||
"lbx_0": 1,
|
||||
"lbx_e": 1,
|
||||
"lg": 1,
|
||||
"lh": 1,
|
||||
"parameter_traj": 1,
|
||||
"reset_solver": 0,
|
||||
"u_init": 0,
|
||||
"ubu": 1,
|
||||
"ubx": 1,
|
||||
"ubx_0": 1,
|
||||
"ubx_e": 1,
|
||||
"ug": 1,
|
||||
"uh": 1,
|
||||
"x_init": 0,
|
||||
"y_ref": 1,
|
||||
"y_ref_0": 1,
|
||||
"y_ref_e": 1
|
||||
},
|
||||
"outputs": {
|
||||
"CPU_time": 1,
|
||||
"CPU_time_lin": 0,
|
||||
"CPU_time_qp": 0,
|
||||
"CPU_time_sim": 0,
|
||||
"KKT_residual": 1,
|
||||
"solver_status": 1,
|
||||
"sqp_iter": 1,
|
||||
"u0": 1,
|
||||
"utraj": 0,
|
||||
"x1": 1,
|
||||
"xtraj": 0
|
||||
},
|
||||
"samplingtime": "t0"
|
||||
},
|
||||
"solver_options": {
|
||||
"Tsim": 0.009765625,
|
||||
"alpha_min": 0.05,
|
||||
"alpha_reduction": 0.7,
|
||||
"collocation_type": "GAUSS_LEGENDRE",
|
||||
"eps_sufficient_descent": 0.0001,
|
||||
"exact_hess_constr": 1,
|
||||
"exact_hess_cost": 1,
|
||||
"exact_hess_dyn": 1,
|
||||
"ext_cost_num_hess": 0,
|
||||
"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_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_iter_max": 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,
|
||||
"sim_method_jac_reuse": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"sim_method_newton_iter": 3,
|
||||
"sim_method_num_stages": [
|
||||
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
|
||||
],
|
||||
"tf": 2.5,
|
||||
"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,0 +1,278 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
// acados
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/print.h"
|
||||
|
||||
|
||||
// example specific
|
||||
#include "lat_model/lat_model.h"
|
||||
#include "acados_sim_solver_lat.h"
|
||||
|
||||
|
||||
// ** solver data **
|
||||
|
||||
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
|
||||
{
|
||||
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
|
||||
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
|
||||
|
||||
return capsule;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
|
||||
{
|
||||
free(capsule);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_create(sim_solver_capsule * capsule)
|
||||
{
|
||||
// initialize
|
||||
const int nx = LAT_NX;
|
||||
const int nu = LAT_NU;
|
||||
const int nz = LAT_NZ;
|
||||
const int np = LAT_NP;
|
||||
bool tmp_bool;
|
||||
|
||||
|
||||
double Tsim = 0.009765625;
|
||||
|
||||
|
||||
// explicit ode
|
||||
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
|
||||
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
|
||||
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
|
||||
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
|
||||
|
||||
|
||||
|
||||
// sim plan & config
|
||||
sim_solver_plan_t plan;
|
||||
plan.sim_solver = ERK;
|
||||
|
||||
// create correct config based on plan
|
||||
sim_config * lat_sim_config = sim_config_create(plan);
|
||||
capsule->acados_sim_config = lat_sim_config;
|
||||
|
||||
// sim dims
|
||||
void *lat_sim_dims = sim_dims_create(lat_sim_config);
|
||||
capsule->acados_sim_dims = lat_sim_dims;
|
||||
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
|
||||
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
|
||||
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
|
||||
|
||||
|
||||
// sim opts
|
||||
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
|
||||
capsule->acados_sim_opts = lat_sim_opts;
|
||||
int tmp_int = 3;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
|
||||
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
|
||||
|
||||
|
||||
tmp_int = 4;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = 1;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
|
||||
tmp_bool = 0;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
|
||||
|
||||
|
||||
// sim in / out
|
||||
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
|
||||
capsule->acados_sim_in = lat_sim_in;
|
||||
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
|
||||
capsule->acados_sim_out = lat_sim_out;
|
||||
|
||||
sim_in_set(lat_sim_config, lat_sim_dims,
|
||||
lat_sim_in, "T", &Tsim);
|
||||
|
||||
// model functions
|
||||
lat_sim_config->model_set(lat_sim_in->model,
|
||||
"expl_vde_for", capsule->sim_forw_vde_casadi);
|
||||
lat_sim_config->model_set(lat_sim_in->model,
|
||||
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
// sim solver
|
||||
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
|
||||
lat_sim_dims, lat_sim_opts);
|
||||
capsule->acados_sim_solver = lat_sim_solver;
|
||||
|
||||
|
||||
/* initialize parameter values */
|
||||
double* p = calloc(np, sizeof(double));
|
||||
|
||||
|
||||
lat_acados_sim_update_params(capsule, p, np);
|
||||
free(p);
|
||||
|
||||
|
||||
/* initialize input */
|
||||
// x
|
||||
double x0[4];
|
||||
for (int ii = 0; ii < 4; ii++)
|
||||
x0[ii] = 0.0;
|
||||
|
||||
sim_in_set(lat_sim_config, lat_sim_dims,
|
||||
lat_sim_in, "x", x0);
|
||||
|
||||
|
||||
// u
|
||||
double u0[1];
|
||||
for (int ii = 0; ii < 1; ii++)
|
||||
u0[ii] = 0.0;
|
||||
|
||||
sim_in_set(lat_sim_config, lat_sim_dims,
|
||||
lat_sim_in, "u", u0);
|
||||
|
||||
// S_forw
|
||||
double S_forw[20];
|
||||
for (int ii = 0; ii < 20; ii++)
|
||||
S_forw[ii] = 0.0;
|
||||
for (int ii = 0; ii < 4; ii++)
|
||||
S_forw[ii + ii * 4 ] = 1.0;
|
||||
|
||||
|
||||
sim_in_set(lat_sim_config, lat_sim_dims,
|
||||
lat_sim_in, "S_forw", S_forw);
|
||||
|
||||
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_solve(sim_solver_capsule *capsule)
|
||||
{
|
||||
// integrate dynamics using acados sim_solver
|
||||
int status = sim_solve(capsule->acados_sim_solver,
|
||||
capsule->acados_sim_in, capsule->acados_sim_out);
|
||||
if (status != 0)
|
||||
printf("error in lat_acados_sim_solve()! Exiting.\n");
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_free(sim_solver_capsule *capsule)
|
||||
{
|
||||
// free memory
|
||||
sim_solver_destroy(capsule->acados_sim_solver);
|
||||
sim_in_destroy(capsule->acados_sim_in);
|
||||
sim_out_destroy(capsule->acados_sim_out);
|
||||
sim_opts_destroy(capsule->acados_sim_opts);
|
||||
sim_dims_destroy(capsule->acados_sim_dims);
|
||||
sim_config_destroy(capsule->acados_sim_config);
|
||||
|
||||
// free external function
|
||||
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
|
||||
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
|
||||
{
|
||||
int status = 0;
|
||||
int casadi_np = LAT_NP;
|
||||
|
||||
if (casadi_np != np) {
|
||||
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
|
||||
" External function has %i parameters. Exiting.\n", np, casadi_np);
|
||||
exit(1);
|
||||
}
|
||||
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
|
||||
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/* getters pointers to C objects*/
|
||||
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_config;
|
||||
};
|
||||
|
||||
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_in;
|
||||
};
|
||||
|
||||
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_out;
|
||||
};
|
||||
|
||||
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_dims;
|
||||
};
|
||||
|
||||
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_opts;
|
||||
};
|
||||
|
||||
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_solver;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_SIM_lat_H_
|
||||
#define ACADOS_SIM_lat_H_
|
||||
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define LAT_NX 4
|
||||
#define LAT_NZ 0
|
||||
#define LAT_NU 1
|
||||
#define LAT_NP 2
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// ** capsule for solver data **
|
||||
typedef struct sim_solver_capsule
|
||||
{
|
||||
// acados objects
|
||||
sim_in *acados_sim_in;
|
||||
sim_out *acados_sim_out;
|
||||
sim_solver *acados_sim_solver;
|
||||
sim_opts *acados_sim_opts;
|
||||
sim_config *acados_sim_config;
|
||||
void *acados_sim_dims;
|
||||
|
||||
/* external functions */
|
||||
// ERK
|
||||
external_function_param_casadi * sim_forw_vde_casadi;
|
||||
external_function_param_casadi * sim_expl_ode_fun_casadi;
|
||||
external_function_param_casadi * sim_expl_ode_hess;
|
||||
|
||||
// IRK
|
||||
external_function_param_casadi * sim_impl_dae_fun;
|
||||
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
|
||||
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
|
||||
external_function_param_casadi * sim_impl_dae_hess;
|
||||
|
||||
// GNSF
|
||||
external_function_param_casadi * sim_gnsf_phi_fun;
|
||||
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
|
||||
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
|
||||
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
external_function_param_casadi * sim_gnsf_get_matrices_fun;
|
||||
|
||||
} sim_solver_capsule;
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_lat_H_
|
||||
+264
@@ -0,0 +1,264 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#define S_FUNCTION_NAME acados_solver_sfunction_lat
|
||||
#define S_FUNCTION_LEVEL 2
|
||||
|
||||
#define MDL_START
|
||||
|
||||
// acados
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
// example specific
|
||||
#include "lat_model/lat_model.h"
|
||||
#include "acados_solver_lat.h"
|
||||
|
||||
#include "simstruc.h"
|
||||
|
||||
#define SAMPLINGTIME 0.009765625
|
||||
|
||||
static void mdlInitializeSizes (SimStruct *S)
|
||||
{
|
||||
// specify the number of continuous and discrete states
|
||||
ssSetNumContStates(S, 0);
|
||||
ssSetNumDiscStates(S, 0);// specify the number of input ports
|
||||
if ( !ssSetNumInputPorts(S, 8) )
|
||||
return;
|
||||
|
||||
// specify the number of output ports
|
||||
if ( !ssSetNumOutputPorts(S, 6) )
|
||||
return;
|
||||
|
||||
// specify dimension information for the input ports
|
||||
// lbx_0
|
||||
ssSetInputPortVectorDimension(S, 0, 4);
|
||||
// ubx_0
|
||||
ssSetInputPortVectorDimension(S, 1, 4);
|
||||
// parameters
|
||||
ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
|
||||
// y_ref_0
|
||||
ssSetInputPortVectorDimension(S, 3, 3);
|
||||
// y_ref
|
||||
ssSetInputPortVectorDimension(S, 4, 45);
|
||||
// y_ref_e
|
||||
ssSetInputPortVectorDimension(S, 5, 2);
|
||||
// lbx
|
||||
ssSetInputPortVectorDimension(S, 6, 30);
|
||||
// ubx
|
||||
ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
|
||||
ssSetOutputPortVectorDimension(S, 0, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 1, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 2, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
|
||||
ssSetOutputPortVectorDimension(S, 4, 1);
|
||||
ssSetOutputPortVectorDimension(S, 5, 1 );
|
||||
|
||||
// specify the direct feedthrough status
|
||||
// should be set to 1 for all inputs used in mdlOutputs
|
||||
ssSetInputPortDirectFeedThrough(S, 0, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 1, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 2, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 3, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 4, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 5, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 6, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 7, 1);
|
||||
|
||||
// one sample time
|
||||
ssSetNumSampleTimes(S, 1);
|
||||
}
|
||||
|
||||
|
||||
#if defined(MATLAB_MEX_FILE)
|
||||
|
||||
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
|
||||
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
|
||||
|
||||
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
#endif /* MATLAB_MEX_FILE */
|
||||
|
||||
|
||||
static void mdlInitializeSampleTimes(SimStruct *S)
|
||||
{
|
||||
ssSetSampleTime(S, 0, SAMPLINGTIME);
|
||||
ssSetOffsetTime(S, 0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
static void mdlStart(SimStruct *S)
|
||||
{
|
||||
lat_solver_capsule *capsule = lat_acados_create_capsule();
|
||||
lat_acados_create(capsule);
|
||||
|
||||
ssSetUserData(S, (void*)capsule);
|
||||
}
|
||||
|
||||
|
||||
static void mdlOutputs(SimStruct *S, int_T tid)
|
||||
{
|
||||
lat_solver_capsule *capsule = ssGetUserData(S);
|
||||
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
|
||||
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
|
||||
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
|
||||
|
||||
InputRealPtrsType in_sign;
|
||||
|
||||
// local buffer
|
||||
real_t buffer[4];
|
||||
|
||||
/* go through inputs */
|
||||
// lbx_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
|
||||
for (int i = 0; i < 4; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
|
||||
// ubx_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
|
||||
for (int i = 0; i < 4; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
|
||||
// parameters - stage-variant !!!
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
|
||||
|
||||
// update value of parameters
|
||||
for (int ii = 0; ii <= 16; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 2; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*2+jj]);
|
||||
lat_acados_update_params(capsule, ii, buffer, 2);
|
||||
}
|
||||
|
||||
|
||||
// y_ref_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
|
||||
|
||||
|
||||
// y_ref - for stages 1 to N-1
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
|
||||
|
||||
for (int ii = 1; ii < 16; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 3; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
|
||||
}
|
||||
|
||||
|
||||
// y_ref_e
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
|
||||
|
||||
for (int i = 0; i < 2; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
|
||||
// lbx
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
|
||||
for (int ii = 1; ii < 16; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 2; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
|
||||
}
|
||||
// ubx
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
|
||||
for (int ii = 1; ii < 16; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 2; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
|
||||
}
|
||||
|
||||
/* call solver */
|
||||
int rti_phase = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
|
||||
int acados_status = lat_acados_solve(capsule);
|
||||
|
||||
|
||||
/* set outputs */
|
||||
// assign pointers to output signals
|
||||
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
|
||||
int tmp_int;
|
||||
out_u0 = ssGetOutputPortRealSignal(S, 0);
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
|
||||
|
||||
|
||||
out_status = ssGetOutputPortRealSignal(S, 1);
|
||||
*out_status = (real_t) acados_status;
|
||||
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
|
||||
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
|
||||
out_x1 = ssGetOutputPortRealSignal(S, 3);
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
|
||||
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
|
||||
// get solution time
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
|
||||
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
|
||||
// get sqp iter
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
|
||||
*out_sqp_iter = (real_t) tmp_int;
|
||||
|
||||
}
|
||||
|
||||
static void mdlTerminate(SimStruct *S)
|
||||
{
|
||||
lat_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
lat_acados_free(capsule);
|
||||
lat_acados_free_capsule(capsule);
|
||||
}
|
||||
|
||||
|
||||
#ifdef MATLAB_MEX_FILE
|
||||
#include "simulink.c"
|
||||
#else
|
||||
#include "cg_sfun.h"
|
||||
#endif
|
||||
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_sim_solver_lat.h"
|
||||
|
||||
#define NX LAT_NX
|
||||
#define NZ LAT_NZ
|
||||
#define NU LAT_NU
|
||||
#define NP LAT_NP
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int status = 0;
|
||||
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
|
||||
status = lat_acados_sim_create(capsule);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
|
||||
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
|
||||
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
|
||||
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
|
||||
|
||||
// initial condition
|
||||
double x_current[NX];
|
||||
x_current[0] = 0.0;
|
||||
x_current[1] = 0.0;
|
||||
x_current[2] = 0.0;
|
||||
x_current[3] = 0.0;
|
||||
|
||||
|
||||
x_current[0] = 0;
|
||||
x_current[1] = 0;
|
||||
x_current[2] = 0;
|
||||
x_current[3] = 0;
|
||||
|
||||
|
||||
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
u0[0] = 0.0;
|
||||
// set parameters
|
||||
double p[NP];
|
||||
p[0] = 0;
|
||||
p[1] = 0;
|
||||
|
||||
lat_acados_sim_update_params(capsule, p, NP);
|
||||
|
||||
|
||||
int n_sim_steps = 3;
|
||||
// solve ocp in loop
|
||||
for (int ii = 0; ii < n_sim_steps; ii++)
|
||||
{
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "x", x_current);
|
||||
status = lat_acados_sim_solve(capsule);
|
||||
|
||||
if (status != ACADOS_SUCCESS)
|
||||
{
|
||||
printf("acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
sim_out_get(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_out, "x", x_current);
|
||||
|
||||
printf("\nx_current, %d\n", ii);
|
||||
for (int jj = 0; jj < NX; jj++)
|
||||
{
|
||||
printf("%e\n", x_current[jj]);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
|
||||
|
||||
// free solver
|
||||
status = lat_acados_sim_free(capsule);
|
||||
if (status) {
|
||||
printf("lat_acados_sim_free() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
lat_acados_sim_solver_free_capsule(capsule);
|
||||
|
||||
return status;
|
||||
}
|
||||
@@ -0,0 +1,125 @@
|
||||
%
|
||||
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
%
|
||||
% This file is part of acados.
|
||||
%
|
||||
% The 2-Clause BSD License
|
||||
%
|
||||
% Redistribution and use in source and binary forms, with or without
|
||||
% modification, are permitted provided that the following conditions are met:
|
||||
%
|
||||
% 1. Redistributions of source code must retain the above copyright notice,
|
||||
% this list of conditions and the following disclaimer.
|
||||
%
|
||||
% 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
% this list of conditions and the following disclaimer in the documentation
|
||||
% and/or other materials provided with the distribution.
|
||||
%
|
||||
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% POSSIBILITY OF SUCH DAMAGE.;
|
||||
%
|
||||
|
||||
SOURCES = { ...
|
||||
'lat_model/lat_expl_ode_fun.c', ...
|
||||
'lat_model/lat_expl_vde_forw.c',...
|
||||
'lat_cost/lat_cost_y_0_fun.c',...
|
||||
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
|
||||
'lat_cost/lat_cost_y_0_hess.c',...
|
||||
'lat_cost/lat_cost_y_fun.c',...
|
||||
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
|
||||
'lat_cost/lat_cost_y_hess.c',...
|
||||
'lat_cost/lat_cost_y_e_fun.c',...
|
||||
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
|
||||
'lat_cost/lat_cost_y_e_hess.c',...
|
||||
'acados_solver_sfunction_lat.c', ...
|
||||
'acados_solver_lat.c'
|
||||
};
|
||||
|
||||
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
|
||||
|
||||
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
|
||||
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
|
||||
['-I', fullfile(INC_PATH, 'acados')], ...
|
||||
['-I', fullfile(INC_PATH)]};
|
||||
|
||||
|
||||
|
||||
CFLAGS = 'CFLAGS=$CFLAGS';
|
||||
LDFLAGS = 'LDFLAGS=$LDFLAGS';
|
||||
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
|
||||
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
|
||||
|
||||
|
||||
|
||||
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
|
||||
|
||||
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
|
||||
|
||||
% acados linking libraries and flags
|
||||
|
||||
|
||||
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
|
||||
LIB_PATH, LIBS{:}, SOURCES{:}, ...
|
||||
'-output', 'acados_solver_sfunction_lat' );
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
|
||||
eval('mexext')] );
|
||||
|
||||
|
||||
%% print note on usage of s-function
|
||||
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
|
||||
input_note = 'Inputs are:\n';
|
||||
i_in = 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
|
||||
' size [4]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
|
||||
' size [4]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
|
||||
' size [34]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
|
||||
' size [45]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
|
||||
i_in = i_in + 1;
|
||||
|
||||
fprintf(input_note)
|
||||
|
||||
disp(' ')
|
||||
|
||||
output_note = 'Outputs are:\n';
|
||||
i_out = 0;
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
|
||||
|
||||
fprintf(output_note)
|
||||
@@ -4,16 +4,15 @@ from common.numpy_fast import interp
|
||||
from system.swaglog import cloudlog
|
||||
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
|
||||
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N
|
||||
from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
|
||||
from selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
|
||||
TRAJECTORY_SIZE = 33
|
||||
CAMERA_OFFSET = 0.04
|
||||
|
||||
class LateralPlanner:
|
||||
def __init__(self, CP, use_lanelines=True, wide_camera=False):
|
||||
self.use_lanelines = use_lanelines
|
||||
self.LP = LanePlanner(wide_camera)
|
||||
def __init__(self, CP):
|
||||
self.DH = DesireHelper()
|
||||
|
||||
# Vehicle model parameters used to calculate lateral movement of car
|
||||
@@ -28,13 +27,12 @@ class LateralPlanner:
|
||||
self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,))
|
||||
self.t_idxs = np.arange(TRAJECTORY_SIZE)
|
||||
self.y_pts = np.zeros(TRAJECTORY_SIZE)
|
||||
self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
|
||||
#dp: mf vision controller
|
||||
#self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
|
||||
|
||||
self.lat_mpc = LateralMpc()
|
||||
self.reset_mpc(np.zeros(4))
|
||||
|
||||
self.dp_dlp_lanelines = True
|
||||
|
||||
def reset_mpc(self, x0=np.zeros(4)):
|
||||
self.x0 = x0
|
||||
self.lat_mpc.reset(x0=self.x0)
|
||||
@@ -43,12 +41,8 @@ class LateralPlanner:
|
||||
v_ego = sm['carState'].vEgo
|
||||
measured_curvature = sm['controlsState'].curvature
|
||||
|
||||
# dp - update camera offset
|
||||
self.LP.update_offsets(sm['dragonConf'].dpCameraOffset)
|
||||
|
||||
# Parse model predictions
|
||||
md = sm['modelV2']
|
||||
self.LP.parse_model(md)
|
||||
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
|
||||
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
|
||||
self.t_idxs = np.array(md.position.t)
|
||||
@@ -57,31 +51,17 @@ class LateralPlanner:
|
||||
self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
|
||||
|
||||
# Lane change logic
|
||||
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
|
||||
desire_state = md.meta.desireState
|
||||
if len(desire_state):
|
||||
self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
|
||||
self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
|
||||
lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
|
||||
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['dragonConf'])
|
||||
|
||||
# Turn off lanes during lane change
|
||||
if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft:
|
||||
self.LP.lll_prob *= self.DH.lane_change_ll_prob
|
||||
self.LP.rll_prob *= self.DH.lane_change_ll_prob
|
||||
self.d_path_w_lines_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
|
||||
|
||||
if sm.updated['dragonConf'] and sm['dragonConf'].dpUseLanelines:
|
||||
if self.LP.lll_prob < 0.3 and self.LP.rll_prob < 0.3:
|
||||
self.dp_dlp_lanelines = False
|
||||
elif self.LP.lll_prob > 0.5 and self.LP.rll_prob > 0.5:
|
||||
self.dp_dlp_lanelines = True
|
||||
self.use_lanelines = self.dp_dlp_lanelines
|
||||
|
||||
# Calculate final driving path and set MPC costs
|
||||
if self.use_lanelines:
|
||||
d_path_xyz = self.d_path_w_lines_xyz
|
||||
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE)
|
||||
else:
|
||||
d_path_xyz = self.path_xyz
|
||||
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
|
||||
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
|
||||
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE)
|
||||
d_path_xyz = self.path_xyz
|
||||
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
|
||||
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
|
||||
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE)
|
||||
|
||||
y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
|
||||
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
|
||||
@@ -126,24 +106,21 @@ class LateralPlanner:
|
||||
|
||||
lateralPlan = plan_send.lateralPlan
|
||||
lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
||||
lateralPlan.laneWidth = float(self.LP.lane_width)
|
||||
lateralPlan.dPathPoints = self.y_pts.tolist()
|
||||
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
|
||||
lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist()
|
||||
lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
|
||||
lateralPlan.lProb = float(self.LP.lll_prob)
|
||||
lateralPlan.rProb = float(self.LP.rll_prob)
|
||||
lateralPlan.dProb = float(self.LP.d_prob)
|
||||
|
||||
lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
|
||||
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
|
||||
|
||||
lateralPlan.desire = self.DH.desire
|
||||
lateralPlan.useLaneLines = self.use_lanelines
|
||||
lateralPlan.useLaneLines = False
|
||||
lateralPlan.laneChangeState = self.DH.lane_change_state
|
||||
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
|
||||
|
||||
plan_send.lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
|
||||
plan_send.lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
|
||||
#dp: mf vts
|
||||
#plan_send.lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
|
||||
#plan_send.lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
|
||||
|
||||
pm.send('lateralPlan', plan_send)
|
||||
|
||||
@@ -11,16 +11,21 @@ LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
ACCEL_MIN_ISO = -3.5 # m/s^2
|
||||
ACCEL_MAX_ISO = 2.0 # m/s^2
|
||||
|
||||
|
||||
def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
|
||||
v_target_future, brake_pressed, cruise_standstill):
|
||||
"""Update longitudinal control state machine"""
|
||||
accelerating = v_target_future > v_target
|
||||
stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
|
||||
(v_ego < CP.vEgoStopping and
|
||||
((v_target_future < CP.vEgoStopping and not accelerating) or brake_pressed))
|
||||
v_target_1sec, brake_pressed, cruise_standstill):
|
||||
accelerating = v_target_1sec > v_target
|
||||
planned_stop = (v_target < CP.vEgoStopping and
|
||||
v_target_1sec < CP.vEgoStopping and
|
||||
not accelerating)
|
||||
stay_stopped = (v_ego < CP.vEgoStopping and
|
||||
(brake_pressed or cruise_standstill))
|
||||
stopping_condition = planned_stop or stay_stopped
|
||||
|
||||
starting_condition = v_target_future > CP.vEgoStarting and accelerating and not cruise_standstill
|
||||
starting_condition = (v_target_1sec > CP.vEgoStarting and
|
||||
accelerating and
|
||||
not cruise_standstill and
|
||||
not brake_pressed)
|
||||
started_condition = v_ego > CP.vEgoStarting
|
||||
|
||||
if not active:
|
||||
long_control_state = LongCtrlState.off
|
||||
@@ -34,9 +39,21 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
|
||||
long_control_state = LongCtrlState.stopping
|
||||
|
||||
elif long_control_state == LongCtrlState.stopping:
|
||||
if starting_condition:
|
||||
if starting_condition and CP.startingState:
|
||||
long_control_state = LongCtrlState.starting
|
||||
elif starting_condition:
|
||||
long_control_state = LongCtrlState.pid
|
||||
|
||||
elif long_control_state == LongCtrlState.starting:
|
||||
if stopping_condition:
|
||||
long_control_state = LongCtrlState.stopping
|
||||
elif started_condition:
|
||||
long_control_state = LongCtrlState.pid
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
return long_control_state
|
||||
|
||||
|
||||
@@ -60,64 +77,62 @@ class LongControl:
|
||||
# Interp control trajectory
|
||||
speeds = long_plan.speeds
|
||||
if len(speeds) == CONTROL_N:
|
||||
v_target = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds)
|
||||
a_target = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels)
|
||||
v_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds)
|
||||
a_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels)
|
||||
|
||||
v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
|
||||
a_target_lower = 2 * (v_target_lower - v_target) / self.CP.longitudinalActuatorDelayLowerBound - a_target
|
||||
a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now
|
||||
|
||||
v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
|
||||
a_target_upper = 2 * (v_target_upper - v_target) / self.CP.longitudinalActuatorDelayUpperBound - a_target
|
||||
a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now
|
||||
|
||||
v_target = min(v_target_lower, v_target_upper)
|
||||
a_target = min(a_target_lower, a_target_upper)
|
||||
|
||||
v_target_future = speeds[-1]
|
||||
v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds)
|
||||
else:
|
||||
v_target = 0.0
|
||||
v_target_future = 0.0
|
||||
v_target_1sec = 0.0
|
||||
a_target = 0.0
|
||||
|
||||
# TODO: This check is not complete and needs to be enforced by MPC
|
||||
a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO)
|
||||
|
||||
self.pid.neg_limit = accel_limits[0]
|
||||
self.pid.pos_limit = accel_limits[1]
|
||||
|
||||
# Update state machine
|
||||
output_accel = self.last_output_accel
|
||||
self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
|
||||
v_target, v_target_future, CS.brakePressed,
|
||||
v_target, v_target_1sec, CS.brakePressed,
|
||||
CS.cruiseState.standstill)
|
||||
|
||||
if self.long_control_state == LongCtrlState.off:
|
||||
self.reset(CS.vEgo)
|
||||
output_accel = 0.
|
||||
|
||||
# tracking objects and driving
|
||||
elif self.long_control_state == LongCtrlState.stopping:
|
||||
if output_accel > self.CP.stopAccel:
|
||||
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
|
||||
self.reset(CS.vEgo)
|
||||
|
||||
elif self.long_control_state == LongCtrlState.starting:
|
||||
output_accel = self.CP.startAccel
|
||||
self.reset(CS.vEgo)
|
||||
|
||||
elif self.long_control_state == LongCtrlState.pid:
|
||||
self.v_pid = v_target
|
||||
self.v_pid = v_target_now
|
||||
|
||||
# Toyota starts braking more when it thinks you want to stop
|
||||
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
|
||||
prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
|
||||
# TODO too complex, needs to be simplified and tested on toyotas
|
||||
prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < self.v_pid
|
||||
deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV)
|
||||
freeze_integrator = prevent_overshoot
|
||||
|
||||
error = self.v_pid - CS.vEgo
|
||||
error_deadzone = apply_deadzone(error, deadzone)
|
||||
output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, feedforward=a_target, freeze_integrator=freeze_integrator)
|
||||
output_accel = self.pid.update(error_deadzone, speed=CS.vEgo,
|
||||
feedforward=a_target,
|
||||
freeze_integrator=freeze_integrator)
|
||||
|
||||
if prevent_overshoot:
|
||||
output_accel = min(output_accel, 0.0)
|
||||
|
||||
# Intention is to stop, switch to a different brake control until we stop
|
||||
elif self.long_control_state == LongCtrlState.stopping:
|
||||
# Keep applying brakes until the car is stopped
|
||||
if not CS.standstill or output_accel > self.CP.stopAccel:
|
||||
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
|
||||
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
self.reset(CS.vEgo)
|
||||
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
|
||||
self.last_output_accel = output_accel
|
||||
final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
|
||||
return final_accel
|
||||
return self.last_output_accel
|
||||
|
||||
@@ -0,0 +1,627 @@
|
||||
{
|
||||
"acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
|
||||
"acados_lib_path": "/data/openpilot/third_party/acados/include/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": [
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vu_0": [
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vx": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vx_0": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Vx_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
|
||||
]
|
||||
],
|
||||
"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": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
|
||||
"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
|
||||
},
|
||||
"model": {
|
||||
"dyn_disc_fun": null,
|
||||
"dyn_disc_fun_jac": null,
|
||||
"dyn_disc_fun_jac_hess": null,
|
||||
"dyn_ext_fun_type": "casadi",
|
||||
"dyn_source_discrete": null,
|
||||
"gnsf": {
|
||||
"nontrivial_f_LO": 1,
|
||||
"purely_linear": 0
|
||||
},
|
||||
"name": "long"
|
||||
},
|
||||
"parameter_values": [
|
||||
-1.2,
|
||||
1.2,
|
||||
0.0,
|
||||
0.0,
|
||||
1.45,
|
||||
0.75
|
||||
],
|
||||
"problem_class": "OCP",
|
||||
"simulink_opts": {
|
||||
"inputs": {
|
||||
"cost_W": 0,
|
||||
"cost_W_0": 0,
|
||||
"cost_W_e": 0,
|
||||
"lbu": 1,
|
||||
"lbx": 1,
|
||||
"lbx_0": 1,
|
||||
"lbx_e": 1,
|
||||
"lg": 1,
|
||||
"lh": 1,
|
||||
"parameter_traj": 1,
|
||||
"reset_solver": 0,
|
||||
"u_init": 0,
|
||||
"ubu": 1,
|
||||
"ubx": 1,
|
||||
"ubx_0": 1,
|
||||
"ubx_e": 1,
|
||||
"ug": 1,
|
||||
"uh": 1,
|
||||
"x_init": 0,
|
||||
"y_ref": 1,
|
||||
"y_ref_0": 1,
|
||||
"y_ref_e": 1
|
||||
},
|
||||
"outputs": {
|
||||
"CPU_time": 1,
|
||||
"CPU_time_lin": 0,
|
||||
"CPU_time_qp": 0,
|
||||
"CPU_time_sim": 0,
|
||||
"KKT_residual": 1,
|
||||
"solver_status": 1,
|
||||
"sqp_iter": 1,
|
||||
"u0": 1,
|
||||
"utraj": 0,
|
||||
"x1": 1,
|
||||
"xtraj": 0
|
||||
},
|
||||
"samplingtime": "t0"
|
||||
},
|
||||
"solver_options": {
|
||||
"Tsim": 0.06944444444444445,
|
||||
"alpha_min": 0.05,
|
||||
"alpha_reduction": 0.7,
|
||||
"collocation_type": "GAUSS_LEGENDRE",
|
||||
"eps_sufficient_descent": 0.0001,
|
||||
"exact_hess_constr": 1,
|
||||
"exact_hess_cost": 1,
|
||||
"exact_hess_dyn": 1,
|
||||
"ext_cost_num_hess": 0,
|
||||
"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_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_iter_max": 10,
|
||||
"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,
|
||||
"sim_method_jac_reuse": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"sim_method_newton_iter": 3,
|
||||
"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
|
||||
]
|
||||
}
|
||||
}
|
||||
+282
@@ -0,0 +1,282 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
// acados
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/print.h"
|
||||
|
||||
|
||||
// example specific
|
||||
#include "long_model/long_model.h"
|
||||
#include "acados_sim_solver_long.h"
|
||||
|
||||
|
||||
// ** solver data **
|
||||
|
||||
sim_solver_capsule * long_acados_sim_solver_create_capsule()
|
||||
{
|
||||
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
|
||||
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
|
||||
|
||||
return capsule;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
|
||||
{
|
||||
free(capsule);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_create(sim_solver_capsule * capsule)
|
||||
{
|
||||
// initialize
|
||||
const int nx = LONG_NX;
|
||||
const int nu = LONG_NU;
|
||||
const int nz = LONG_NZ;
|
||||
const int np = LONG_NP;
|
||||
bool tmp_bool;
|
||||
|
||||
|
||||
double Tsim = 0.06944444444444445;
|
||||
|
||||
|
||||
// explicit ode
|
||||
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
capsule->sim_forw_vde_casadi->casadi_fun = &long_expl_vde_forw;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_in = &long_expl_vde_forw_n_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_out = &long_expl_vde_forw_n_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &long_expl_vde_forw_sparsity_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &long_expl_vde_forw_sparsity_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_work = &long_expl_vde_forw_work;
|
||||
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
|
||||
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_fun = &long_expl_ode_fun;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &long_expl_ode_fun_n_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &long_expl_ode_fun_n_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &long_expl_ode_fun_sparsity_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &long_expl_ode_fun_sparsity_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_work = &long_expl_ode_fun_work;
|
||||
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
|
||||
|
||||
|
||||
|
||||
// sim plan & config
|
||||
sim_solver_plan_t plan;
|
||||
plan.sim_solver = ERK;
|
||||
|
||||
// create correct config based on plan
|
||||
sim_config * long_sim_config = sim_config_create(plan);
|
||||
capsule->acados_sim_config = long_sim_config;
|
||||
|
||||
// sim dims
|
||||
void *long_sim_dims = sim_dims_create(long_sim_config);
|
||||
capsule->acados_sim_dims = long_sim_dims;
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nx", &nx);
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nu", &nu);
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nz", &nz);
|
||||
|
||||
|
||||
// sim opts
|
||||
sim_opts *long_sim_opts = sim_opts_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_opts = long_sim_opts;
|
||||
int tmp_int = 3;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "newton_iter", &tmp_int);
|
||||
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "collocation_type", &collocation_type);
|
||||
|
||||
|
||||
tmp_int = 4;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = 1;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "num_steps", &tmp_int);
|
||||
tmp_bool = 0;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "jac_reuse", &tmp_bool);
|
||||
|
||||
|
||||
// sim in / out
|
||||
sim_in *long_sim_in = sim_in_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_in = long_sim_in;
|
||||
sim_out *long_sim_out = sim_out_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_out = long_sim_out;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "T", &Tsim);
|
||||
|
||||
// model functions
|
||||
long_sim_config->model_set(long_sim_in->model,
|
||||
"expl_vde_for", capsule->sim_forw_vde_casadi);
|
||||
long_sim_config->model_set(long_sim_in->model,
|
||||
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
// sim solver
|
||||
sim_solver *long_sim_solver = sim_solver_create(long_sim_config,
|
||||
long_sim_dims, long_sim_opts);
|
||||
capsule->acados_sim_solver = long_sim_solver;
|
||||
|
||||
|
||||
/* initialize parameter values */
|
||||
double* p = calloc(np, sizeof(double));
|
||||
|
||||
p[0] = -1.2;
|
||||
p[1] = 1.2;
|
||||
p[4] = 1.45;
|
||||
p[5] = 0.75;
|
||||
|
||||
long_acados_sim_update_params(capsule, p, np);
|
||||
free(p);
|
||||
|
||||
|
||||
/* initialize input */
|
||||
// x
|
||||
double x0[3];
|
||||
for (int ii = 0; ii < 3; ii++)
|
||||
x0[ii] = 0.0;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "x", x0);
|
||||
|
||||
|
||||
// u
|
||||
double u0[1];
|
||||
for (int ii = 0; ii < 1; ii++)
|
||||
u0[ii] = 0.0;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "u", u0);
|
||||
|
||||
// S_forw
|
||||
double S_forw[12];
|
||||
for (int ii = 0; ii < 12; ii++)
|
||||
S_forw[ii] = 0.0;
|
||||
for (int ii = 0; ii < 3; ii++)
|
||||
S_forw[ii + ii * 3 ] = 1.0;
|
||||
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "S_forw", S_forw);
|
||||
|
||||
int status = sim_precompute(long_sim_solver, long_sim_in, long_sim_out);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_solve(sim_solver_capsule *capsule)
|
||||
{
|
||||
// integrate dynamics using acados sim_solver
|
||||
int status = sim_solve(capsule->acados_sim_solver,
|
||||
capsule->acados_sim_in, capsule->acados_sim_out);
|
||||
if (status != 0)
|
||||
printf("error in long_acados_sim_solve()! Exiting.\n");
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_free(sim_solver_capsule *capsule)
|
||||
{
|
||||
// free memory
|
||||
sim_solver_destroy(capsule->acados_sim_solver);
|
||||
sim_in_destroy(capsule->acados_sim_in);
|
||||
sim_out_destroy(capsule->acados_sim_out);
|
||||
sim_opts_destroy(capsule->acados_sim_opts);
|
||||
sim_dims_destroy(capsule->acados_sim_dims);
|
||||
sim_config_destroy(capsule->acados_sim_config);
|
||||
|
||||
// free external function
|
||||
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
|
||||
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
|
||||
{
|
||||
int status = 0;
|
||||
int casadi_np = LONG_NP;
|
||||
|
||||
if (casadi_np != np) {
|
||||
printf("long_acados_sim_update_params: trying to set %i parameters for external functions."
|
||||
" External function has %i parameters. Exiting.\n", np, casadi_np);
|
||||
exit(1);
|
||||
}
|
||||
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
|
||||
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/* getters pointers to C objects*/
|
||||
sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_config;
|
||||
};
|
||||
|
||||
sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_in;
|
||||
};
|
||||
|
||||
sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_out;
|
||||
};
|
||||
|
||||
void * long_acados_get_sim_dims(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_dims;
|
||||
};
|
||||
|
||||
sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_opts;
|
||||
};
|
||||
|
||||
sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_solver;
|
||||
};
|
||||
|
||||
+103
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_SIM_long_H_
|
||||
#define ACADOS_SIM_long_H_
|
||||
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define LONG_NX 3
|
||||
#define LONG_NZ 0
|
||||
#define LONG_NU 1
|
||||
#define LONG_NP 6
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// ** capsule for solver data **
|
||||
typedef struct sim_solver_capsule
|
||||
{
|
||||
// acados objects
|
||||
sim_in *acados_sim_in;
|
||||
sim_out *acados_sim_out;
|
||||
sim_solver *acados_sim_solver;
|
||||
sim_opts *acados_sim_opts;
|
||||
sim_config *acados_sim_config;
|
||||
void *acados_sim_dims;
|
||||
|
||||
/* external functions */
|
||||
// ERK
|
||||
external_function_param_casadi * sim_forw_vde_casadi;
|
||||
external_function_param_casadi * sim_expl_ode_fun_casadi;
|
||||
external_function_param_casadi * sim_expl_ode_hess;
|
||||
|
||||
// IRK
|
||||
external_function_param_casadi * sim_impl_dae_fun;
|
||||
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
|
||||
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
|
||||
external_function_param_casadi * sim_impl_dae_hess;
|
||||
|
||||
// GNSF
|
||||
external_function_param_casadi * sim_gnsf_phi_fun;
|
||||
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
|
||||
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
|
||||
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
external_function_param_casadi * sim_gnsf_get_matrices_fun;
|
||||
|
||||
} sim_solver_capsule;
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_create(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_solve(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_free(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT void * long_acados_get_sim_dims(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_solver_capsule * long_acados_sim_solver_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_long_H_
|
||||
@@ -306,7 +306,7 @@ void long_acados_create_3_create_and_set_functions(long_solver_capsule* capsule)
|
||||
capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \
|
||||
capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \
|
||||
capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \
|
||||
external_function_param_casadi_create(&capsule->__CAPSULE_FNC__ , 5); \
|
||||
external_function_param_casadi_create(&capsule->__CAPSULE_FNC__ , 6); \
|
||||
}while(false)
|
||||
|
||||
|
||||
@@ -376,6 +376,7 @@ void long_acados_create_4_set_default_parameters(long_solver_capsule* capsule) {
|
||||
p[0] = -1.2;
|
||||
p[1] = 1.2;
|
||||
p[4] = 1.45;
|
||||
p[5] = 0.75;
|
||||
|
||||
for (int i = 0; i <= N; i++) {
|
||||
long_acados_update_params(capsule, i, p, NP);
|
||||
@@ -863,7 +864,7 @@ int long_acados_update_params(long_solver_capsule* capsule, int stage, double *p
|
||||
{
|
||||
int solver_status = 0;
|
||||
|
||||
int casadi_np = 5;
|
||||
int casadi_np = 6;
|
||||
if (casadi_np != np) {
|
||||
printf("acados_update_params: trying to set %i parameters for external functions."
|
||||
" External function has %i parameters. Exiting.\n", np, casadi_np);
|
||||
|
||||
@@ -42,7 +42,7 @@
|
||||
#define LONG_NX 3
|
||||
#define LONG_NZ 0
|
||||
#define LONG_NU 1
|
||||
#define LONG_NP 5
|
||||
#define LONG_NP 6
|
||||
#define LONG_NBX 0
|
||||
#define LONG_NBX0 3
|
||||
#define LONG_NBU 0
|
||||
|
||||
+264
@@ -0,0 +1,264 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#define S_FUNCTION_NAME acados_solver_sfunction_long
|
||||
#define S_FUNCTION_LEVEL 2
|
||||
|
||||
#define MDL_START
|
||||
|
||||
// acados
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
// example specific
|
||||
#include "long_model/long_model.h"
|
||||
#include "acados_solver_long.h"
|
||||
|
||||
#include "simstruc.h"
|
||||
|
||||
#define SAMPLINGTIME 0.06944444444444445
|
||||
|
||||
static void mdlInitializeSizes (SimStruct *S)
|
||||
{
|
||||
// specify the number of continuous and discrete states
|
||||
ssSetNumContStates(S, 0);
|
||||
ssSetNumDiscStates(S, 0);// specify the number of input ports
|
||||
if ( !ssSetNumInputPorts(S, 8) )
|
||||
return;
|
||||
|
||||
// specify the number of output ports
|
||||
if ( !ssSetNumOutputPorts(S, 6) )
|
||||
return;
|
||||
|
||||
// specify dimension information for the input ports
|
||||
// lbx_0
|
||||
ssSetInputPortVectorDimension(S, 0, 3);
|
||||
// ubx_0
|
||||
ssSetInputPortVectorDimension(S, 1, 3);
|
||||
// parameters
|
||||
ssSetInputPortVectorDimension(S, 2, (12+1) * 6);
|
||||
// y_ref_0
|
||||
ssSetInputPortVectorDimension(S, 3, 6);
|
||||
// y_ref
|
||||
ssSetInputPortVectorDimension(S, 4, 66);
|
||||
// y_ref_e
|
||||
ssSetInputPortVectorDimension(S, 5, 5);
|
||||
// lh
|
||||
ssSetInputPortVectorDimension(S, 6, 4);
|
||||
// uh
|
||||
ssSetInputPortVectorDimension(S, 7, 4);/* specify dimension information for the OUTPUT ports */
|
||||
ssSetOutputPortVectorDimension(S, 0, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 1, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 2, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 3, 3 ); // state at shooting node 1
|
||||
ssSetOutputPortVectorDimension(S, 4, 1);
|
||||
ssSetOutputPortVectorDimension(S, 5, 1 );
|
||||
|
||||
// specify the direct feedthrough status
|
||||
// should be set to 1 for all inputs used in mdlOutputs
|
||||
ssSetInputPortDirectFeedThrough(S, 0, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 1, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 2, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 3, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 4, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 5, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 6, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 7, 1);
|
||||
|
||||
// one sample time
|
||||
ssSetNumSampleTimes(S, 1);
|
||||
}
|
||||
|
||||
|
||||
#if defined(MATLAB_MEX_FILE)
|
||||
|
||||
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
|
||||
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
|
||||
|
||||
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
#endif /* MATLAB_MEX_FILE */
|
||||
|
||||
|
||||
static void mdlInitializeSampleTimes(SimStruct *S)
|
||||
{
|
||||
ssSetSampleTime(S, 0, SAMPLINGTIME);
|
||||
ssSetOffsetTime(S, 0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
static void mdlStart(SimStruct *S)
|
||||
{
|
||||
long_solver_capsule *capsule = long_acados_create_capsule();
|
||||
long_acados_create(capsule);
|
||||
|
||||
ssSetUserData(S, (void*)capsule);
|
||||
}
|
||||
|
||||
|
||||
static void mdlOutputs(SimStruct *S, int_T tid)
|
||||
{
|
||||
long_solver_capsule *capsule = ssGetUserData(S);
|
||||
ocp_nlp_config *nlp_config = long_acados_get_nlp_config(capsule);
|
||||
ocp_nlp_dims *nlp_dims = long_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_in *nlp_in = long_acados_get_nlp_in(capsule);
|
||||
ocp_nlp_out *nlp_out = long_acados_get_nlp_out(capsule);
|
||||
|
||||
InputRealPtrsType in_sign;
|
||||
|
||||
// local buffer
|
||||
real_t buffer[6];
|
||||
|
||||
/* go through inputs */
|
||||
// lbx_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
|
||||
for (int i = 0; i < 3; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
|
||||
// ubx_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
|
||||
for (int i = 0; i < 3; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
|
||||
// parameters - stage-variant !!!
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
|
||||
|
||||
// update value of parameters
|
||||
for (int ii = 0; ii <= 12; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 6; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*6+jj]);
|
||||
long_acados_update_params(capsule, ii, buffer, 6);
|
||||
}
|
||||
|
||||
|
||||
// y_ref_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
|
||||
|
||||
for (int i = 0; i < 6; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
|
||||
|
||||
|
||||
// y_ref - for stages 1 to N-1
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
|
||||
|
||||
for (int ii = 1; ii < 12; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 6; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*6+jj]);
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
|
||||
}
|
||||
|
||||
|
||||
// y_ref_e
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
|
||||
|
||||
for (int i = 0; i < 5; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 12, "yref", (void *) buffer);
|
||||
// lh
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 0; ii < 12; ii++)
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
|
||||
// uh
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 0; ii < 12; ii++)
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
|
||||
|
||||
/* call solver */
|
||||
int rti_phase = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
|
||||
int acados_status = long_acados_solve(capsule);
|
||||
|
||||
|
||||
/* set outputs */
|
||||
// assign pointers to output signals
|
||||
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
|
||||
int tmp_int;
|
||||
out_u0 = ssGetOutputPortRealSignal(S, 0);
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
|
||||
|
||||
|
||||
out_status = ssGetOutputPortRealSignal(S, 1);
|
||||
*out_status = (real_t) acados_status;
|
||||
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
|
||||
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
|
||||
out_x1 = ssGetOutputPortRealSignal(S, 3);
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
|
||||
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
|
||||
// get solution time
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
|
||||
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
|
||||
// get sqp iter
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
|
||||
*out_sqp_iter = (real_t) tmp_int;
|
||||
|
||||
}
|
||||
|
||||
static void mdlTerminate(SimStruct *S)
|
||||
{
|
||||
long_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
long_acados_free(capsule);
|
||||
long_acados_free_capsule(capsule);
|
||||
}
|
||||
|
||||
|
||||
#ifdef MATLAB_MEX_FILE
|
||||
#include "simulink.c"
|
||||
#else
|
||||
#include "cg_sfun.h"
|
||||
#endif
|
||||
BIN
Binary file not shown.
+3
-3
@@ -52,10 +52,10 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
|
||||
/* long_constr_h_fun:(i0[3],i1,i2[],i3[5])->(o0[4]) */
|
||||
/* long_constr_h_fun:(i0[3],i1,i2[],i3[6])->(o0[4]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
@@ -70,7 +70,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
|
||||
a2=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a2=(a2-a1);
|
||||
a1=7.5000000000000000e-01;
|
||||
a1=arg[3]? arg[3][5] : 0;
|
||||
a3=casadi_sq(a0);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
|
||||
+3
-3
@@ -54,12 +54,12 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s5[12] = {4, 4, 0, 1, 2, 3, 5, 2, 3, 3, 1, 2};
|
||||
static const casadi_int casadi_s6[3] = {4, 0, 0};
|
||||
|
||||
/* long_constr_h_fun_jac_uxt_zt:(i0[3],i1,i2[],i3[5])->(o0[4],o1[4x4,5nz],o2[4x0]) */
|
||||
/* long_constr_h_fun_jac_uxt_zt:(i0[3],i1,i2[],i3[6])->(o0[4],o1[4x4,5nz],o2[4x0]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
@@ -74,7 +74,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
|
||||
a2=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a2=(a2-a1);
|
||||
a1=7.5000000000000000e-01;
|
||||
a1=arg[3]? arg[3][5] : 0;
|
||||
a3=casadi_sq(a0);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
|
||||
+3
-5
@@ -28,7 +28,6 @@ extern "C" {
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -50,10 +49,9 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
|
||||
/* long_cost_y_0_fun:(i0[3],i1,i2[5])->(o0[6]) */
|
||||
/* long_cost_y_0_fun:(i0[3],i1,i2[6])->(o0[6]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4;
|
||||
a0=arg[2]? arg[2][2] : 0;
|
||||
@@ -150,7 +148,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_in(casadi_int
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 0: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
+5
-7
@@ -29,7 +29,6 @@ extern "C" {
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -51,11 +50,10 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
|
||||
|
||||
/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[5])->(o0[6],o1[4x6,7nz]) */
|
||||
/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[6])->(o0[6],o1[4x6,7nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[2]? arg[2][2] : 0;
|
||||
@@ -171,8 +169,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_in(c
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
case 0: return casadi_s2;
|
||||
case 1: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
+4
-6
@@ -29,7 +29,6 @@ extern "C" {
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -52,10 +51,9 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
|
||||
static const casadi_int casadi_s3[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
|
||||
|
||||
/* long_cost_y_0_hess:(i0[3],i1,i2[6],i3[5])->(o0[4x4,3nz]) */
|
||||
/* long_cost_y_0_hess:(i0[3],i1,i2[6],i3[6])->(o0[4x4,3nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
@@ -168,14 +166,14 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_in(casadi_int
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
case 3: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
+5
-3
@@ -28,6 +28,7 @@ extern "C" {
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -49,9 +50,10 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
|
||||
/* long_cost_y_e_fun:(i0[3],i1[],i2[5])->(o0[5]) */
|
||||
/* long_cost_y_e_fun:(i0[3],i1[],i2[6])->(o0[5]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4;
|
||||
a0=arg[2]? arg[2][2] : 0;
|
||||
@@ -146,7 +148,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_in(casadi_int
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s2;
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
+7
-5
@@ -29,6 +29,7 @@ extern "C" {
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -50,10 +51,11 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[14] = {3, 5, 0, 2, 3, 4, 5, 6, 0, 1, 0, 1, 2, 2};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s4[14] = {3, 5, 0, 2, 3, 4, 5, 6, 0, 1, 0, 1, 2, 2};
|
||||
|
||||
/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[5])->(o0[5],o1[3x5,6nz]) */
|
||||
/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[6])->(o0[5],o1[3x5,6nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[2]? arg[2][2] : 0;
|
||||
@@ -166,8 +168,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_in(c
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s2;
|
||||
case 1: return casadi_s3;
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
+6
-4
@@ -29,6 +29,7 @@ extern "C" {
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -51,9 +52,10 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[9] = {3, 3, 0, 1, 3, 3, 1, 0, 1};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[9] = {3, 3, 0, 1, 3, 3, 1, 0, 1};
|
||||
|
||||
/* long_cost_y_e_hess:(i0[3],i1[],i2[5],i3[5])->(o0[3x3,3nz]) */
|
||||
/* long_cost_y_e_hess:(i0[3],i1[],i2[5],i3[6])->(o0[3x3,3nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
@@ -166,14 +168,14 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_in(casadi_int
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 0: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
+3
-5
@@ -28,7 +28,6 @@ extern "C" {
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -50,10 +49,9 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
|
||||
/* long_cost_y_fun:(i0[3],i1,i2[5])->(o0[6]) */
|
||||
/* long_cost_y_fun:(i0[3],i1,i2[6])->(o0[6]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4;
|
||||
a0=arg[2]? arg[2][2] : 0;
|
||||
@@ -150,7 +148,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_in(casadi_int i)
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 0: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
+5
-7
@@ -29,7 +29,6 @@ extern "C" {
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -51,11 +50,10 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
|
||||
|
||||
/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[5])->(o0[6],o1[4x6,7nz]) */
|
||||
/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[6])->(o0[6],o1[4x6,7nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[2]? arg[2][2] : 0;
|
||||
@@ -171,8 +169,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_in(cas
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
case 0: return casadi_s2;
|
||||
case 1: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
+4
-6
@@ -29,7 +29,6 @@ extern "C" {
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
@@ -52,10 +51,9 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
|
||||
static const casadi_int casadi_s3[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
|
||||
|
||||
/* long_cost_y_hess:(i0[3],i1,i2[6],i3[5])->(o0[4x4,3nz]) */
|
||||
/* long_cost_y_hess:(i0[3],i1,i2[6],i3[6])->(o0[4x4,3nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
@@ -168,14 +166,14 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_in(casadi_int i
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
case 3: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user